Advanced Question Calculating optimal thruster choice...

Hlynkacg · Jan 26, 2014

Apologies for the double post but this conundrum that has started to consume an annoying amount of my development time/effort.

When I started re-writing AAPO's LM flight control code I used elements of Darrenc's Simplex-based thruster control project. However as I began to model some of the more esoteric components of LM flight control (SAS modes, thruster 'Pulses', and thruster count) things quickly got out of hand. As Such I find myself going back to the drawing-board.

What I need is a function/algorithm that will take a desired torque vector in n-meters, force vector in newtons, and a desired number of thrusters to be used And select the optimal thrusters for the job and required throttle settings.

I've already got code that creates a table of available thrusters along with their torque and force vectors that I adapted from darrenc's. What I need are ideas of ways to consult that table and quickly arrive at the optimal thruster firing solution for the given inputs

Constraints:

Use thrusters contained in provided table.
Returned throttle settings must fall between 0.0 and 1.0
If exact solution is impossible (insufficient thrust) get "next best" with focus on matching vector direction rather than magnitude
number of thrusters utilized in the solution must be <= number of thrusters designated in the input.
Minimal iterations/number crunching, this function may be called multiple times per timestep.

Any suggestions/ideas would be greatly appreciated :hailprobe:

Hlynkacg · Jan 28, 2014

Still need help here.

I've got structure for storing data on each of my RCS thrusters shown below and am trying to figure out how to build a framework for turning this into a matrix problem.

Code:

	union THRUSTER
	{
		THRUSTER_HANDLE	th;		// Thruster handle

		struct
		{
			VECTOR3		F;		// Force vector        
			VECTOR3		T;      // Torque vector
			double		Mdot;   // The propellant flow rate [GetThrusterIsp() * GetThrusterMax()] // for propellant optimization **NOT YET IMPLIMENTED**
		};

		double	data[6]; //  For array access to F and T vectors */
	};

I've tried dissecting DarrenC's code but despite being thoroughly commented I'm having trouble following his thought process and PM's requesting clarification have gone unanswered.

---------- Post added at 12:22 ---------- Previous post was at 12:08 ----------

So further thinking...

I've created a set for storing RCS thruster handles.

Code:

typedef std::set <THRUSTER_HANDLE> THRUSTER_SET;

This set is populated by a function that resides in my vessel's control code.

Code:

void AAPO_LM::BuildThrusterSet ()
{
	bool Powered;
	if (SimpleFlightModel ()) Powered = true;
	else Powered = true; // Place holder till electrical sub-system can be implemented

	// Clear existing set
	ts_rcs.clear ();

	// A Loop...
	if (Powered)
	{
		if (vc->GetSwitchState (SWITCH_RCS_QUADA1) == UP)
		{
			ts_rcs.insert (th_rcs[0][0]);
			ts_rcs.insert (th_rcs[0][1]);
		}
		if (vc->GetSwitchState (SWITCH_RCS_QUADA2) == UP)
		{
			ts_rcs.insert (th_rcs[0][2]);
			ts_rcs.insert (th_rcs[0][3]);		
		}

and so on...

an instance of the union I posted above is then created for each thruster in the set "ts_rcs".

The question is "where to from here?"

I'm thinking that I sould build 3 sub-sets (Pitch Roll and Yaw) based on whther the thruster's T.x/y/z values but I'm not sure if that is really the ideal solution as I could see it getting out of hand fast.

again if anyone has any ideas, please help.

meson800 · Jan 28, 2014

Hlynkacg said:

Code:

	union THRUSTER
	{
		THRUSTER_HANDLE	th;		// Thruster handle

		struct
		{
			VECTOR3		F;		// Force vector        
			VECTOR3		T;      // Torque vector
			double		Mdot;   // The propellant flow rate [GetThrusterIsp() * GetThrusterMax()] // for propellant optimization **NOT YET IMPLIMENTED**
		};

		double	data[6]; //  For array access to F and T vectors */
	};

C++ Standard said:
When a value is stored in a member of an object of union type, the bytes of the object representation that do not correspond to that member but do correspond to other members take unspecified values.

How are you accessing/setting this union? It appears from the members that you would want to set both a value for THRUSTER_HANDLE and the struct values. However, a union can only have one "active" member set at any point in time; trying to read a value from one of the inactive members causes undefined behavior.
For example

Code:

THRUSTER myThruster;
thruster.th = AwesomeTopThrusterHandle;
thruster.F = VECTOR3(0,1,0);
//somewhere later
[COLOR="Red"]ActivateThruster(thruster.th);[/COLOR]

In the above, the line ActivateThruster is just some random function that wants to use the thruster handle. Although it appears to access AwesomeTopThrusterHandle, it does not. Because the F value was last set, thruster.th is now just some random interpretation of the bytes comprising F.

This seems likely to blow up. Why are you using a union instead of a normal class/struct?

Hlynkacg said:
What I need is a function/algorithm that will take a desired torque vector in n-meters, force vector in newtons, and a desired number of thrusters to be used And select the optimal thrusters for the job and required throttle settings.

Does this mean that you want to rotate the ship while creating a force in some direction? By combining the two, it seems like you have to do more work, because if the torque causes the ship to rotate, the thrusters applying a force would have to be constantly recalculated.

Hlynkacg · Jan 28, 2014

meson800 said:
In the above, the line ActivateThruster is just some random function that wants to use the thruster handle. Although it appears to access AwesomeTopThrusterHandle, it does not. Because the F value was last set, thruster.th is now just some random interpretation of the bytes comprising F.

This seems likely to blow up. Why are you using a union instead of a normal class/struct?

The simple answer is "because I don't know any better." My c++ is entirely self-taught and I am barrowing the general format used in DarrenC's simplex solver and NAASP.

Less explosive prone ideas are always welcome.

Does this mean that you want to rotate the ship while creating a force in some direction? By combining the two, it seems like you have to do more work, because if the torque causes the ship to rotate, the thrusters applying a force would have to be constantly recalculated.

Not quite,

What it is, is that my guidance program "knows" the vessel's current state (orientation, relative velocity, angular velocity, etc...) and it knows what is desired. It then translates the difference into the amount of force and torque that must be applied to make the current state match the desired one.

So the current challenge is to take a bunch of torque and force inputs from the autopilot, guidance computer, and player, mix them and then translate the result into thruster pulses.

meson800 · Jan 28, 2014

Hlynkacg said:
The simple answer is "because I don't know any better." My c++ is entirely self-taught and I am barrowing the general format used in DarrenC's simplex solver and NAASP.

Sorry if my post came off as pretentious :facepalm: My C++ knowledge is also entirely self-taught.

In this instance, I would probably just change the union into a class, but I would have to see the code that actually manipulates the union. Because the union no longer exists, the data member would have to be redefined as a pointer to member F.

------------------------------------------------------------------
As to the issue at hand, I can't give that great of advice. My (probably bad) way to do it would be the following:

Based on the vessel's COG, calculate the torque and force vectors that the thruster would create if it was fired at full thrust.

Then, to select thrusters, I would create the following function:

Code:

float gradeVector(VECTOR3 desiredVec, thrusterVec)
{
  return dot_product(desiredVec,thrusterVec)/(magnitude(DesiredVec) * magnitude(thrusterVec));
}

The above will return a value, between 0 and 1, with how close the vectors are to each other. A value of 1 means that the two vectors are completely parallel (and therefore desired as a thruster), whereas a value of 0 means that they are completely perpendicular.

Then, the algorithm would work as follows:

Iterates over all of the thrusters, and finds the "best" thruster for desiredForce and the "best" thruster for desiredTorque using the grade function above
Checks if the thruster vector is larger than desired vector; if so, divide desiredVec/thrusterVec to find the amount of throttle
Add the thruster and throttle value to some list, like an std::vector
Subtract (thrusterVec * throttle) from each desiredVec
Loop back to the beginning to add another thruster to the list until desiredForce and desiredTorque are under a certain tolerance value

This algorithm will try to select the thruster which contributes the most to the desired Force/Torque. Then, subtracting the thruster vectors from the desired vectors means that only the remaining force/torque is examined in the next iteration through, and it automatically handles unintentional torque/force caused by a thruster. It also ends up with a vector that can be directly passed to some function which actually fires the thrusters in the vector at the desired throttle levels.

Hlynkacg · Jan 30, 2014

Code:

meson800 said:
Sorry if my post came off as pretentious :facepalm: My C++ knowledge is also entirely self-taught.

No worries.

As to the issue at hand, I can't give that great of advice. My (probably bad) way to do it would be the following...

Well seeing as I'm at loss for better ideas I say lets give it a shot.

So barrowing from my experiance with the switches and control panel (thank you for you help on that btw :tiphat

I've redefined the THRUSTER union as a struct called THRUSTER_DATA.

Code:

// --------------------------------------------------------------
// RCS thruster specification
// --------------------------------------------------------------
typedef struct
{
	THRUSTER_HANDLE	th;	// Thruster handle.
	
	VECTOR3			pos;	// Thruster reference point.
	VECTOR3			F;	// Force vector at full thrust.      
	VECTOR3			T;     // Torque vector at full thrust.
	double			Mdot;   // Propellant mass flow rate at full thrust. (for use in propellant optimization)

	double			data[6];	// Array access to F and T.

} THRUSTER_DATA;

I've also created a new "Flight Control" class called "FCS" to handle the calculations and act as interface.

Code:

	class FCS
	{
		public:
			FCS (Vessel *vessel);
			~FCS ();
			friend class	Vessel;

			// Interface Functions
			void			SetDesiredForce (VECTOR3 F);			// Set desired torque value
			void			SetDesiredTorque (VECTOR3 T);			// Set desired torque value 
			void 			AddForce (VECTOR3 F);					// Add force input
			void 			AddTorque (VECTOR3 T);					// Add torque input

			void			ClearThrusterSet (THRUSTER_SET ts);		// Clear designated thruster set
			void			AddThrusterToSet (THRUSTER_HANDLE th, THRUSTER_SET ts);	// Add designated thruster to thruster set
			THRUSTER_DATA	GetThrusterData (THRUSTER_HANDLE th);	// Generate data for assigned thruster

			// Callback Functions
			void			TimeStep ();						

		private:
			THRUSTER_SET	ts_main, ts_hover, ts_retro, ts_rcs;	// Thruster control sets
			AAPO::Vessel	*v;										// vessel interface	
			VECTOR3			DesiredForce, DesiredTorque;			// Force and Torque commands
			int				nForceCalls, nTorqueCalls;				// Number of inputs

			// Private Functions
			float	GradeVector (VECTOR3 desiredVec, VECTOR3 thrusterVec);			 

	};

---------- Post added at 16:38 ---------- Previous post was at 16:37 ----------

Here is the body of "GetThrusterData ()"

Code:

THRUSTER_DATA  AAPO::FCS::GetThrusterData (THRUSTER_HANDLE th)
{
	THRUSTER_DATA output;
	
	output.th = th;

	// Thruster position
	v->GetThrusterRef (th, output.pos);

	// Force
	v->GetThrusterDir (th, output.F);			// Get thruster direction
	output.F *=  v->GetThrusterMax (th);		// Multiply by Max Thruste to get force vector

	// Torque
	output.T.x = output.pos.x * output.F.x;
	output.T.y = output.pos.y * output.F.y;
	output.T.z = output.pos.z * output.F.z;
	
	// Mass flow rate
	output.Mdot = v->GetThrusterIsp (th) * v->GetThrusterMax (th);

	// Array access to F and T vectors
	for (int i = 0; i < 3; i++)
	{
		output.data[i] = output.F.data[i];
		output.data[i+3] = output.T.data[i];
	}

	return output;
}

---------- Post added at 17:22 ---------- Previous post was at 16:38 ----------

The plan moving forward is as follows.

Vessel will have an instance of FCS as a class member. The Vessel and guidance system will interact with FCS through the interface functions, for example the

Code:

		if (vc->GetSwitchState (SWITCH_RCS_QUADA1) == UP)
		{
			ts_rcs.insert (th_rcs[0][0]);
			ts_rcs.insert (th_rcs[0][1]);

from before becomes...

Code:

		if (vc->GetSwitchState (SWITCH_RCS_QUADA1) == UP)
		{
			fcs->AddThrusterToSet (th_rcs[0][0], ts_rcs);
			fcs->AddThrusterToSet (th_rcs[0][1], ts_rcs);

User guidance system control inputs will be handled like so...

Code:

*rotational control input* fcs->AddTorque (input);

or

Code:

*linear control input* fcs->AddForce (input);

Each simulation time-step the FCS class will average out the inputs and calculate a solution. I'm planning to follow the basic technique that you describe but could still use some help with the implementation.

---------- Post added 01-29-14 at 14:56 ---------- Previous post was 01-28-14 at 17:22 ----------

This is probably a dumb question but how do I iterate through a set?

I have the handles to all my rcs thrusters collected but now I have to step through them so that I can generate the required DATA struct and start grading vectors.

---------- Post added 01-30-14 at 11:21 ---------- Previous post was 01-29-14 at 14:56 ----------

This is what I have right now but it doesn't work.

Code:

	THRUSTER_DATA tData[1024];		// Array for storing thruster data. Arbitrarily large because we don't know how many thrusters are going to be in the set.
	int nThrusters = ts_rcs.count;	// number of thrusters available for use.

	if (nThrusters > 0)				// if nThrusters is greater than 0 populate the data array
	{
		int tmp = 0;
		std::set<THRUSTER_HANDLE>::iterator it;
		for (it = ts_rcs.begin (); it != ts_rcs.end (); it++)
		{ 
			tData[tmp] = GetThrusterData (it);
			tmp++;
		}
	}

error message...

Code:

2>..\Source\AAPO_FCS.cpp(146): error C3867: 'std::_Tree<_Traits>::count': function call missing argument list; use '&std::_Tree<_Traits>::count' to create a pointer to member
2>          with
2>          [
2>              _Traits=std::_Tset_traits<THRUSTER_HANDLE ,std::less<THRUSTER_HANDLE >,std::allocator<THRUSTER_HANDLE >,false>
2>          ]
2>..\Source\AAPO_FCS.cpp(154): error C2664: 'AAPO::FCS::GetThrusterData' : cannot convert parameter 1 from 'std::_Tree_const_iterator<_Mytree>' to 'THRUSTER_HANDLE'
2>          with
2>          [
2>              _Mytree=std::_Tree_val<std::_Tset_traits<THRUSTER_HANDLE ,std::less<THRUSTER_HANDLE >,std::allocator<THRUSTER_HANDLE >,false>>
2>          ]
2>          No user-defined-conversion operator available that can perform this conversion, or the operator cannot be called
2>..\Source\AAPO_FCS.cpp(213): warning C4244: 'return' : conversion from 'double' to 'float', possible loss of data

Seriously, there has to be a simpler way, but I don't have the vocabulary to search for it.

meson800 · Jan 30, 2014

Make the line

Code:

tData[tmp] = GetThrusterData ([COLOR="Red"]*[/COLOR]it);

Should fix the errors. The iterator is not actually of type THRUSTER_HANDLE, so you have to use a asterisk.

If you use a std::vector, you can just do a "normal" for loop

Code:

std::vector<THRUSTER_HANDLE> thrusters;
for (int i = 0; i < thrusters.size(); i++)
{
  //do stuff with thrusters[i]
}

Instead of making a static array to contain THRUSTER_DATAs, why not make it a std::set<THRUSTER_DATA> or std::vector<THRUSTER_DATA>? This would solve this:

// Array for storing thruster data. Arbitrarily large because we don't know how many thrusters are going to be in the set.

---------- Post added at 04:06 PM ---------- Previous post was at 04:05 PM ----------

By the way, a std::vector is basically just an array that doesn't have a fixed size and has a lot of nice functions like .size().

---------- Post added at 04:14 PM ---------- Previous post was at 04:06 PM ----------

Ok. My initial algorithm will not work in the current implementation, because it checks for the correct magnitude of the torque/force vectors in addition to the direction. This is how this could fail:

Imagine the DG, and a command to translate down one unit with no torque. The weighting algorithm would pick one of the top thrusters, say the top-front-center thruster. Subtracting that thruster's torque and force vectors from the desired vectors would cause the force vector to be zero (yay!), and would induce a torque. On the next iteration through, the algorithm would pick the top-back-center thruster to null out the torque. However, if the algorithm considered magnitude of force, the second thruster added would cause the force vector to be non-zero, which would cause the algorithm to continue to add thrusters to the set, when only the TFC and TBC thrusters were needed to have 0 torque and a downward force.

(Hopefully that made sense)

With that in mind, here is the "better" algorithm:

---------- Post added at 04:24 PM ---------- Previous post was at 04:14 PM ----------

Start with desired torque and force vectors.
Normalize the desired vectors and the resultant vectors (just the sum of the individual thruster torque/force vecs in the "burn" list, which starts off initially empty) and subtract them to get the "remaining" torque/force vectors.
Check if the remaining vectors are within a certain tolerance to zero. If they are, return the thruster "burn" list. If not, continue.
Use the grading method to determine the thruster that will reduce the remaining torque/force vectors the most. Add this thruster to the "burn" list; if it's thrust is beyond the required velocity, reduce this thruster's throttle before adding it to the list.
Loop back to step 1

The above should return a thruster list that, when all thrusters in the list are fired, creates a torque/force precisely(at least within the tolerance) in the same directions as desired.

Then, to account for the magnitude of the desired torque/force, have the FCS burn the thrusters for differing amounts of time.

Now onto the code...

---------- Post added at 04:49 PM ---------- Previous post was at 04:24 PM ----------

We need a couple of functions. The first several are vector normalization/add/subtract,etc. These might already be in the Orbiter API. Secondly, we need a weighting function. This will need to use the grading function from above, but account for two things.

Account for torque and force in the same function. This ensures that a thruster that creates the desired force but an excessive amount of excess torque is not picked over a thruster that reduces both force and torque.
Account for a zero vector. The above function (which is derived from the fact that dot(a,b)=mag(a) * mag(b) * cos(angle between a and b)) blows up when one of the vectors has a magnitude of zero.

In code, this is

Code:

//Returns a double of how "good" a thruster is at inducing the desired torque/force
//Higher numbers are better!
double weighThruster(THRUSTER_DATA thruster, VECTOR3 desiredTorque, VECTOR3 desiredForce)
{
	double result = 0;
	if (magnitude(desiredTorque) != 0)
	{
		//do the normal weighting function
		result += gradeVector(desiredTorque,thruster.T);
	}
	else
	{
		//aww, desired torque is zero.  As any torque that this thruster causes is bad
		//penalize the thruster based on the magnitude of the torque induced
		
		//note: this may need some "magic" constant that doesn't penalize torque too harshly
		result -= magnitude(thruster.T);
	}
	
	//same as the above, code copy/paste is generally bad, not sure how to fix it at the moment
	if (magnitude(desiredForce) != 0)
	{
		result += gradeVector(desiredForce,thruster.F);
	}
	else
	{
		result -= magnitude(thruster.F);
	}
}

Thirdly, we need a function to output an "equivalent" thruster; e.g the sum of all of the other thrusters.

Code:

//Returns a THRUSTER_DATA of the equivalent thruster, with a thruster handle of 0
//Takes a std::vector<std::pair<THRUSTER_DATA,double>>, which is the "burn" list
THRUSTER_DATA sumThrusters(std::vector<std::pair<THRUSTER_DATA,double>> thrusterBurnList)
{
	THRUSTER_DATA result;
	//set the pointer equal to zero, hopefully this means that it should not be used as an actual pointer
	result.th = 0;
	
	for (int i = 0; i < thrusterBurnList.size(); i++)
	{
		//first value in the pair is the THRUSTER_DATA
		//second value is a double, which is the thrust level, between 0 and 1
		
		//we don't care about pos, only the force/torque/fuel flow values
		
		//NOTE: this assumes that (VECTOR3 * double) returns a VECTOR3
		result.F += thrusterBurnList[i].first.F * thrusterBurnList[i].second;
		result.T += thrusterBurnList[i].first.T * thrusterBurnList[i].second;
		result.Mdot += thrusterBurnList[i].Mdot * thrusterBurnList[i].second;
	}
	return result;
}

Now we can start the algorithm! Starting with your code:

Code:

	[COLOR="Red"]std::vector<THRUSTER_DATA> tData;[/COLOR]
	int nThrusters = ts_rcs.count;	// number of thrusters available for use.

	if (nThrusters > 0)				// if nThrusters is greater than 0 populate the data array
	{
		std::set<THRUSTER_HANDLE>::iterator it;
		for (it = ts_rcs.begin (); it != ts_rcs.end (); it++)
		{ 
			[COLOR="Red"]tData.push_back(GetThrusterData(*it));[/COLOR]
		}
	}

Sorry, I can't finish this right now(in the middle of writing this, darn :lol

, so I'll just post what I wrote so far.

Hlynkacg · Jan 31, 2014

Ok back on the job...

Imagine the DG, and a command to translate down one unit with no torque. The weighting algorithm would pick one of the top thrusters, say the top-front-center thruster. Subtracting that thruster's torque and force vectors from the desired vectors would cause the force vector to be zero (yay!), and would induce a torque. On the next iteration through, the algorithm would pick the top-back-center thruster to null out the torque. However, if the algorithm considered magnitude of force, the second thruster added would cause the force vector to be non-zero, which would cause the algorithm to continue to add thrusters to the set, when only the TFC and TBC thrusters were needed to have 0 torque and a downward force.

(Hopefully that made sense)

With that in mind, here is the "better" algorithm:

---------- Post added at 04:24 PM ---------- Previous post was at 04:14 PM ----------

Start with desired torque and force vectors.
Normalize the desired vectors and the resultant vectors (just the sum of the individual thruster torque/force vecs in the "burn" list, which starts off initially empty) and subtract them to get the "remaining" torque/force vectors.
Check if the remaining vectors are within a certain tolerance to zero. If they are, return the thruster "burn" list. If not, continue.
Use the grading method to determine the thruster that will reduce the remaining torque/force vectors the most. Add this thruster to the "burn" list; if it's thrust is beyond the required velocity, reduce this thruster's throttle before adding it to the list.
Loop back to step 1

I understand, in fact that problem had already occurred to me if not the solution.

Here are the beginnings of my "Solver" function

Code:

bool AAPO::FCS::FindSolution ()
{
	// Initialize variables
	VECTOR3 ResultantTorque = NULL_VECTOR;
	VECTOR3 ResultantForce = NULL_VECTOR;

	// Get desired torque and normalize it.
	VECTOR3 tVector = DesiredTorque;		// Direction of torque
	double tMag = length (tVector);			// Magnitude of torque
	if (tMag > 0) normalise (tVector);		// Normalise vector

	// Do same for force.
	VECTOR3 fVector = DesiredForce;			// Direction of force
	double fMag = length (fVector);			// Magnitude of force
	if (fMag > 0) normalise (fVector);		// Normalise vector

	// Sanity Check: Is there a solution to find?
	if (tMag + fMag <= 0) return false;
	//else
	//{
		// <psuedo-code
		// *Iterations go here*
		
		// If desired torque/force - resultant torque/force is within set tollerances
		//{
			//for (list of thrusters in solution) FireThruster (thruster, duration, level); 
			//return true.
		//} // </psuedo-code
	//} 
	return true; // Place holder
}

I've also improved the weighting function and added safeguards to grade vector, I've also mve a bunch of my vector/transformation equations from the guidance class to the public AAPO namespace so I can use them here.

GradeVector now returns a value between 1 and -1 depending the angle of divergence between the two vectors with 1 being parallel and -1 opposing (180 degrees off), and 0 is perpendicular or anything resulting in "undefined".

The weighting function is shown below.

Code:

// --------------------------------------------------------------
// Determine how "good" a thruster is at inducing desired torque & force (Higher numbers are better!)
// --------------------------------------------------------------
double AAPO::FCS::WeighThruster (THRUSTER_DATA thruster)
{
	double result = 0;
	
	// If desired torque is greater than 0 compare desired torque value to torque induced by the thruster
	if (length (DesiredTorque) != 0) result += GradeVector (DesiredTorque, thruster.T) * length (thruster.T);

	// Otherwise if desired torque is 0, penalize the thruster based on the magnitude of torque induced.
	else result -= length (thruster.T);

	// Repeat process for desired force
	if (length (DesiredForce) != 0) result += GradeVector (DesiredForce, thruster.F) * length (thruster.F);
	else result -= length (thruster.F);

	return result;
}

NOTE: Multiplying the GradeVector's result by the vector's length (magnitude) takes care of the "magic constant" excessive penalty issue.

As for the rest...
I'm not sure where you're going with the aggregate thruster_data object.

I may be going a little struct-happy but my idea was to create a list of commanded burns "thruster pulses" like so...

Code:

typedef struct
{
	THRUSTER_HANDLE	th;			// Handle of thruster to be fired.
	double			dur;		// Duration of burn
	double			lvl;		// Throttle setting
} BURN;
std::list <BURN> BurnCommands;	// List of active firing commands

and then in clbkTimeStep have the FCS program go through the list like so...

Code:

	if (BurnList.size () > 0)
	{
		// Debuggery
		sprintf (oapiDebugString(), "%i Commanded Burns", BurnList.size ());

		//(iterate through commanded thruster pulses "burnlist")
		//{
		//	if ((pulse.dur > 0) && (pulse.lvl > 0)) // Sanity check: Are pulse duration and throttle setting greater than 0? 
		//	{
		//		double lvl = 0;									// Variable for storing throttle level
		//		if (pulse.dur > simdt) lvl = pulse.lvl;			// If pulse duration remaining is greater than the length of the timestep lvl = commanded level.
		//		else lvl = pulse.lvl * (pulse.dur / simdt);		// If not, "pro-rate" the commanded thrust level against time remaining.

		//		v->SetThrusterLevel_SingleStep (pulse.th, lvl);						// Fire thruster	
		//		pulse.dur -= simdt * (v->GetThrusterLevel (pulse.th) / pulse.lvl);	// Update pulse duration to reflect burn time remaining.
		//	}
		//	else // If pulse duration and throttle setting ARE NOT greater than 0...
		//	{
		//		v->SetThrusterLevel (pulse.th, 0);	// set thruster level to 0% 
		//		burnlist.erase(pulse);				// and remove pulse from the list
		//	}
		//}
	}

The list would be populated by the "FireThruster ()" command you see in my psuedo-code above.

meson800 · Jan 31, 2014

NOTE: Multiplying the GradeVector's result by the vector's length (magnitude) takes care of the "magic constant" excessive penalty issue.

Great idea :thumbup:

I'm not sure where you're going with the aggregate thruster_data object.

It's just an easy way to calculate and store the resultant force/torque by combining all of the thrusters in the thruster list.

Here is the setup for calculating the thruster "burn" list; e.g. the thrusters and their thrust values that would need to be fired to produce a force/torque in the desired direction without regard to the desired magnitude. Another function will need to be created which creates thruster pulses/some other method to get the desired magnitude.

I would probably make the thruster list of type

Code:

//assuming using namespace std;
vector<pair<THRUSTER_DATA,double>>

The above is a dynamic array of the special type pair. Pair allows us to store two different data types together. You operate on it like in the following examples:

Code:

//creating a pair of type THRUSTER_DATA and double
THRUSTER_DATA thruster;
double thrust;
make_pair(thruster,thrust);

//inserting a pair into a vector
vector<pair<THRUSTER_DATA,double>> thrusterList;
thrusterList.push_back(make_pair(thruster,thrust));

//accessing the individual values of a pair
thrusterList[0].first.T; //accesses the torque vector in the stored THRUSTER_LIST
thrusterList[0].second; //access the double in the pair

With that in mind, here is the algorithm code: (I hope you'll improve it...no guarantees to efficiency)

Code:

vector<pair<THRUSTER_DATA,double>> calculateThrusterList(vector<THRUSTER_DATA> thrusters,
	VECTOR3 desiredTorque, VECTOR3 desiredForce, double tolerance)
{
	//create the thruster list
	vector<pair<THRUSTER_DATA,double>> thrusterList;
	
	while (true) //probaly want some sanity check here, maybe if too many thrusters are added to the list or something
	{
		VECTOR3 remainingForce, remainingTorque;
		
		//calculate the above, it's just the difference between the
		//normalized desired vectors and the normalized sum of all of the thruster burns
		//NOTE:not sure if it would be better to make the desired vectors the same 
		//length as the sum vectors; it might work better but you'd have to handle the edge cases
		remainingForce = normalize(desiredForce) - normalize(sumThrusters(thrusterList));
		remainingTorque = nromalize(desiredTorque) - normalize(sumThrusters(thrusterList));
		
		//check if these both are within tolerances, if so return
		if (magnitude(remainingForce) < tolerance && magnitude(remainingTorque) < tolerance)
			return thrusterList;
		
		//nope, we still have to iterate
		
		//pick the best thruster
		int bestThruster;
		double highestScore = 0;
		for (int i = 0; i < thrusters.size(); i++)
			if (weightThruster(thrusters[i]) > highestScore)
			{
				highestScore = weighThruster(thrusters[i]);
				bestThruster = i;
			}
		//some sanity check here, to make sure the "best" thruster's score still isn't really low
		
		//Determine desired throttle
		double throttle = 1;
		//not sure how you want to calculate the throttle value, here is a simplistic method
		//Calculates the percent thrust needed to not "overshoot" the remaining force/torque vectors
		//and picks the lowest thrust value
		if (magnitude(thrusters[bestThruster].F) > magnitude(remainingForce) || 
			magnitude(thrusters[bestThruster].T) > magnitude(remainingTorque))
		{
			double forceDependentThrottle = magnitude(thrusters[bestThruster].F) / magnitude(remainingForce);
			double torqueDependentThrottle = magnitude(thrusters[bestThruster].T) / magnitude(remainingTorque);
			if (forceDependentThrottle < torqueDependentThrottle)
				throttle = forceDependentThrottle;
			else
				throttle = torqueDependentThrottle;
		}
		
		//add this thruster to the thruster list!
		thrusterList.push_back(make_pair(thrusters[bestThruster],throttle));
		//and loop back to the beginning
	}
	//might need this return statement so the compiler doesn't complain
	//even though you'll never hit it
	return thrusterList;
}

I may be going a little struct-happy but my idea was to create a list of commanded burns "thruster pulses" like so...

Mmm...structs :lol:

This is a good idea. The above code just produces the thruster list that would induce torque/thrust in the directions with a length of one-which means one Nm of torque/one N of force. From that thruster list, you should be able to create something which takes into account the desired magnitude and creates a vector of BURNs.

Hlynkacg · Feb 1, 2014

Code:

if (BurnCommands.size () > 0)
{
	// Debuggery
	sprintf (oapiDebugString(), "%i Commanded Burns", BurnCommands.size ());

	std::list <BURN>::iterator pulse;		
	for (pulse = BurnCommands.begin (); pulse != BurnCommands.end (); pulse++)
	{ 
		// Debuggery
		sprintf (oapiDebugString(), "Duration %0.4f Level %0.4f", pulse->dur, pulse->lvl);

		if ((pulse->dur > 0) && (pulse->lvl > 0))			// Sanity check: Are pulse duration and throttle setting greater than 0? 
		{
			double lvl = 0;									// Variable for storing throttle level
			if (pulse->dur > simdt) lvl = pulse->lvl;		// If pulse duration remaining is greater than the length of the timestep lvl = commanded level.
			else lvl = pulse->lvl * (pulse->dur / simdt);	// If not, "pro-rate" the commanded thrust level against time remaining.

			v->SetThrusterLevel_SingleStep (pulse->th, lvl);						// Fire thruster	
			pulse->dur -= simdt * (v->GetThrusterLevel (pulse->th) / pulse->lvl);	// Update pulse duration to reflect pulse time remaining.
		}
		else // If pulse duration and throttle setting ARE NOT greater than 0...
		{
			v->SetThrusterLevel (pulse->th, 0);	// set thruster level to 0% 
			[COLOR="Red"]BurnCommands.erase (pulse);			// and remove pulse from the list[/COLOR]
		}
	}
} // End "if (BurnCommands.size () > 0)"

So I've got the following code it fires the thruster as it should but crashes when it comes time to execute the highlighted line. Running through it step-by-step in the debugger reveals the following error.

Code:

"Expression::list iterator not incremental"

Obviously deleting the object from within the "for" statement is causing a problem. So with that being the case how should I go about "cleaning" up my list, removing the commanded burns that have already been completed?

---------- Post added at 18:17 ---------- Previous post was at 16:11 ----------

Altered the function to use a "while" statement rather than "for". It appears to be working properly (thrusters fire, no CTDs) but I'd still like you to take a look at it.

Code:

	if (BurnCommands.size () > 0)
	{
		std::list<BURN>::iterator pulse = BurnCommands.begin();
		while (pulse != BurnCommands.end())
		{
			if ((pulse->dur > 0) && (pulse->lvl > 0))			// Sanity check: Are pulse duration and throttle setting greater than 0? 
			{
				double lvl = 0;									// Variable for storing throttle level
				if (pulse->dur > simdt) lvl = pulse->lvl;		// If pulse duration remaining is greater than the length of the timestep lvl = commanded level.
				else lvl = pulse->lvl * (pulse->dur / simdt);	// If not, "pro-rate" the commanded thrust level against time remaining.

				v->SetThrusterLevel_SingleStep (pulse->th, lvl);						// Fire thruster	
				pulse->dur -= simdt * (v->GetThrusterLevel (pulse->th) / pulse->lvl);	// Update pulse duration to reflect pulse time remaining.
				
				pulse++;										// Continue...
			}
			else // If pulse duration and throttle setting ARE NOT greater than 0...
			{
				v->SetThrusterLevel (pulse->th, 0);		// set thruster level to 0% 
				pulse = BurnCommands.erase(pulse);		// remove pulse from list
			}	
		} // "while (pulse != BurnCommands.end())"
	} // End "if (BurnCommands.size () > 0)"

Now on to the solver itself...

meson800 · Feb 1, 2014

Yeah, I ran into the same problem when I first iterated over a map.

The issue is that the erase command invalidates the iterator, and then the invalid iterator cannot be incremented; I would use one of the solutions listed here.

Basically, you create a copy of the iterator and delete that copy, after already incrementing the original iterator:

Code:

[COLOR="Red"]std::list <BURN>::iterator pulse = BurnCommands.begin();
	while(pulse != BurnCommands.end())[/COLOR]
	{ 
		// Debuggery
		sprintf (oapiDebugString(), "Duration %0.4f Level %0.4f", pulse->dur, pulse->lvl);

		if ((pulse->dur > 0) && (pulse->lvl > 0))			// Sanity check: Are pulse duration and throttle setting greater than 0? 
		{
			double lvl = 0;									// Variable for storing throttle level
			if (pulse->dur > simdt) lvl = pulse->lvl;		// If pulse duration remaining is greater than the length of the timestep lvl = commanded level.
			else lvl = pulse->lvl * (pulse->dur / simdt);	// If not, "pro-rate" the commanded thrust level against time remaining.

			v->SetThrusterLevel_SingleStep (pulse->th, lvl);						// Fire thruster	
			pulse->dur -= simdt * (v->GetThrusterLevel (pulse->th) / pulse->lvl);	// Update pulse duration to reflect pulse time remaining.
			[COLOR="Red"]pulse++;	//increment the iterator[/COLOR]
		}
		else // If pulse duration and throttle setting ARE NOT greater than 0...
		{
			v->SetThrusterLevel (pulse->th, 0);	// set thruster level to 0% 
			[COLOR="Red"]std::list<BURN>::iterator toDelete = pulse;	//create a copy of the iterator
			pulse++;	//increment the "main" iterator
			BurnCommands.erase (toDelete);			// and remove pulse from the list[/COLOR]
		}
	}

---------- Post added at 09:24 PM ---------- Previous post was at 09:23 PM ----------

Yes, your code works, and does the same thing as mine because list::erase returns the next element. :thumbup:

meson800 · Feb 2, 2014

I thought of two possible issues with the solver code I posted above.

First, because all the vectors are normalized before calculating the "remaining" vectors, the solution will always induce one Nm of torque per N of force (except in the case where one of the desired vectors is zero). Although the function that generates the burn list based on the solution can calculate different magnitudes, the burn list will always have a 1:1 torque to force ratio. I'm not sure how to solve this. :shrug:

Secondly, my simplistic throttle calculator will fail when one of the vectors is zero. When one of them is zero, one of the throttle variables will be zero, and then zero will be set as the throttle variable.

A possible solution to this is picking which "remaining" vector will be reduced the most by the thruster, and only use that in calculating throttle. Therefore, if a thruster would reduce the remaining force to near-zero, but would cause some unwanted torque, the force vector would be picked to calculate the throttle. The unwanted torque would be reduced on a further iteration.

Hlynkacg · Feb 3, 2014

I'm still working on it but here is what I have so far. I could use your eyes and ideas.

Code:

	double tolerance = 0.01;
	
	int iterations = 0;
	int MaxIterations = 10;


	if (tMag + fMag <= 0) return false;		// Sanity Check: Is there a solution to find?
	else									// If so, continue...
	{
		bool solution = false;

		// Debuggery
		sprintf (oapiDebugString(), "Desired Torque x %0.1f, y %0.1f, z %0.1f, Desired Force x %0.1f, y %0.1f, z %0.1f", DesiredTorque.x, DesiredTorque.y, DesiredTorque.z, DesiredForce.x, DesiredForce.y, DesiredForce.z);

		// Create a list of thrusters to be used 
		std::vector <std::pair <THRUSTER_DATA, double>> ThrusterList;

		// Get to work
		while ((solution ==false) && (iterations <  MaxIterations))
		{
			VECTOR3	RemainingTorque = DesiredTorque - ResultantTorque;
			VECTOR3 RemainingForce = DesiredForce - ResultantForce;
			
			// Debuggery
			//sprintf (oapiDebugString(), "Remaining Torque x %0.1f, y %0.1f, z %0.1f, Remaining Force x %0.1f, y %0.1f, z %0.1f", RemainingTorque.x, RemainingTorque.y, RemainingTorque.z, RemainingForce.x,  RemainingForce.y,  RemainingForce.z);
		
			// Check to see if these are both are within tolerances. 
			if ((fabs (GradeVector (DesiredForce, RemainingForce)) < tolerance) && (fabs (GradeVector (DesiredTorque, RemainingTorque)) < tolerance)) solution = true; // If so, a solution has been found.
			else // If not, we need to iterate...
			{
				// Pick a thruster
				double HighScore = 0;
				int Best = 0;
				
				for (int i = 0; i < thrusters.size (); i++)
				{
					double Score = WeighThruster (thrusters[i]);	// Score thruster based on torque and force vectors
					if (Score > HighScore)							// If thruster's score is greater than the current high score...
					{
						Best = i;			// note thruster position in set
						HighScore = Score;	// and update high score

						// Debuggery
						sprintf (oapiDebugString(), "Thruster %i, Score %0.1f", Best, HighScore);
					}
				}

				if (HighScore > 0) // Sanity check
				{
					// Determine throttle setting
					double throttle = 0;
					double rF = length (RemainingTorque);
					double rT = length (RemainingForce);

					double tF = length (thrusters[Best].F);
					double tT = length (thrusters[Best].T);
				
					// Debuggery
					sprintf (oapiDebugString(), "Thruster %i, Torque %0.1f, Force %0.1f", Best, tF, tT);

					// If Thruster's Force or Torque is greater than Force or Torque remaining calculate the percent thrust needed to avoid an "overshoot"
					if ((tF > rF) || (tT > rT))
					{
						double throttleF = tF / rF; //
						double throttleT = tT / rT; //

						throttle = (throttleF < throttleT ? throttleF : throttleT); // Pick the lesser of the two values
					}
					else throttle = 1; // otherwise just set the throttle to 100%

					//inserting a pair into a vector

					ResultantTorque += thrusters[Best].T * throttle;
					ResultantForce += thrusters[Best].F * throttle;
					ThrusterList.push_back (std::make_pair (thrusters[Best], throttle));
				}

				iterations++;
		
				// Debuggery
				sprintf (oapiDebugString(), "Thrusters %i, Iterations %i", ThrusterList.size(), iterations);
			}

if (solution) ** Function that translates ThrusterList vector into "FCS::FireThruster ()" commands should go here.
		}

meson800 · Feb 4, 2014

It looks pretty good, there's some good optimizations over my code!

I still see two problems though:
When the RemainingVector values are calculated

Code:

VECTOR3	RemainingTorque = DesiredTorque - ResultantTorque;
VECTOR3 RemainingForce = DesiredForce - ResultantForce;

Nothing is normalized, so you could run into the infinite Deltaglider-thruster loop as described above.

Also:

Code:

// If Thruster's Force or Torque is greater than Force or Torque remaining calculate the percent thrust needed to avoid an "overshoot"
	if ((tF > rF) || (tT > rT))
	{
		double throttleF = tF / rF; //
		double throttleT = tT / rT; //

		[COLOR="Red"]throttle = (throttleF < throttleT ? throttleF : throttleT);[/COLOR] // Pick the lesser of the two values
	}
	else throttle = 1; // otherwise just set the throttle to 100%

This will still return a throttle value of 0 if one of the desired vectors is zero (or very small).

And I love the ternary operator

Hlynkacg · Feb 12, 2014

Apologies for letting this sit for so long but obligations in meat-space required my attention.

That said, I've been working on this for the last day and a half and despite normalizing the vectors and breaking the throttle calculation of into their own loop I have not been able to escape the Deltaglider-thruster loop conundrum.

I suspect that I'm missing something obvious but at this point I'm thinking that I might have to completely rethink my methodology.

Probe knows I could use some help. :hailprobe:

meson800 · Feb 13, 2014

meson800 said:
As to the issue at hand, I can't give that great of advice. My (probably bad) way to do it would be the following

Never said it would work :lol:

Another approach is probably better. I could look at the current algorithm and see if I see anything wrong, though.

Hlynkacg · Feb 13, 2014

Noticed something interesting in process of deconstruction my code.

THRUSTER_DATA.T was returning incorrect values.

Now the way to calculate the magnitude of a torque input is to determine the length of the lever arm and then multiply by the applied force. The lever arm is the perpendicular distance from the axis of rotation to the line of action of the force.

I translated that into...

Code:

	// Torque
	output.T.x = output.pos.x * output.F.x;
	output.T.y = output.pos.y * output.F.y;
	output.T.z = output.pos.z * output.F.z;

Bur clearly I've missed something.

the question is what?

meson800 · Feb 13, 2014

https://en.wikipedia.org/wiki/Torque

You're missing a sin(theta).

Code:

Magnitude_torque = magnitude(Force_vector) * magnitude(lever_arm_vector) * sin(angle between the two)

Or you could do the other way, depending on if you want the direction of torque (which you probably do in this case)

Code:

Torque_vector = cross_product(Force_vector, lever_arm_vector)

I would personally just do the vector method.

---------- Post added at 04:42 PM ---------- Previous post was at 04:21 PM ----------

I just saw that this was already answered in another thread. Whoops.

Hlynkacg · Feb 13, 2014

Ok, after fixing the broken torque vector i wrote the following routine that ALMOST works.

~~Control inputs are inverted and~~ T or F values that are higher than the thruster system can achieve cause the solver to break but its a step in the right direction.

Code:

bool AAPO::FCS::FindSolution ()
{
	// Get desired torque and normalize it.
	VECTOR3 tVector = DesiredTorque;		// Direction of torque
	double tMag = length (tVector);			// Magnitude of torque
	if (tMag > 0) normalise (tVector);		// Normalise vector

	// Do same for force.
	VECTOR3 fVector = DesiredForce;			// Direction of force
	double fMag = length (fVector);			// Magnitude of force
	if (fMag > 0) normalise (fVector);		// Normalise vector

	if (tMag + fMag <= 0) return false;		// Sanity Check: Is there a solution to find?

	// Debuggery
	sprintf (oapiDebugString(), "Desired Torque x %0.1f, y %0.1f, z %0.1f, Desired Force x %0.1f, y %0.1f, z %0.1f", DesiredTorque.x, DesiredTorque.y, DesiredTorque.z, DesiredForce.x, DesiredForce.y, DesiredForce.z);

	// Initialize variables
	VECTOR3 ResultantTorque = NULL_VECTOR;
	VECTOR3 ResultantForce = NULL_VECTOR;

	double tolerance = 0.1;
	int iterations = 0;
	int MaxIterations = thrusters.size ();
	bool solution = false;

	// Create a list of thrusters to be used 
	std::vector <std::pair <THRUSTER_DATA, double>> ThrusterList;

	// Get to work
	while ((solution == false) && (iterations < MaxIterations))
	{
		VECTOR3 TorqueRemaining = DesiredTorque - ResultantTorque;
		VECTOR3 ForceRemaining = DesiredForce - ResultantTorque;

		bool GoodTorque = (length (TorqueRemaining) < tolerance);
		bool GoodForce = (length (ForceRemaining) < tolerance);

		if ((GoodTorque) && (GoodForce)) solution = true;
		else // we need to iterate
		{
			VECTOR3 tr = NULL_VECTOR;
			VECTOR3 fr = NULL_VECTOR;

			if (length (TorqueRemaining) > 0)
			{
				tr = TorqueRemaining;
				normalise (tr);
			}

			if (length (ForceRemaining) > 0)
			{
				fr = ForceRemaining;
				normalise (fr);
			}

			// find all thrusters that positively contribute to desired torque
			int best = -1;
			double highscore = 0;
			std::vector<THRUSTER_DATA>::iterator i = thrusters.begin ();
			for (int i = 0; i < thrusters.size(); i++)
			{
				double score = 0;
				score = WeighThruster (thrusters[i], tr, fr);
			
				if (score > highscore)
				{
					best = i;
					highscore = score;
				}	
				i++;
			} 

			if (best >= 0) ThrusterList.push_back (std::make_pair (thrusters[best], 1));

			// Debuggery
			sprintf (oapiDebugString(), "%i Thrusters on list", ThrusterList.size ());
			sprintf (oapiDebugString(), "Desired Torque x %0.1f, y %0.1f, z %0.1f, Desired Force x %0.1f, y %0.1f, z %0.1f, %i Thrusters on list", DesiredTorque.x, DesiredTorque.y, DesiredTorque.z, DesiredForce.x, DesiredForce.y, DesiredForce.z, ThrusterList.size ());

			VECTOR3 T = NULL_VECTOR;
			VECTOR3 F = NULL_VECTOR;

			// determine total torque and force of all thrusters on list.. 
			for (int i = 0; i < ThrusterList.size(); i++)
			{		
				//we don't care about pos, only the force/torque values
				F += ThrusterList[i].first.F * ThrusterList[i].second;
				T += ThrusterList[i].first.T * ThrusterList[i].second;
			}
	
			// Scale to magnitude of desired force and torque
			double tMod = 1;
			if (length (T) > tMag) tMod = tMag / length (T);
	
			double fMod = 1;
			if (length (F) > fMag) fMod = fMag / length (F);

			double mod = (fMag > tMag ? fMod : tMod);

			// Update list and resultants
			for (int i = 0; i < ThrusterList.size(); i++)
			{
				ThrusterList[i].second *= mod;
				ResultantTorque += ThrusterList[i].first.T * ThrusterList[i].second;
				ResultantForce += ThrusterList[i].first.F * ThrusterList[i].second;
			}

			// Debuggery
			sprintf (oapiDebugString(), "Desired Torque x %0.1f, y %0.1f, z %0.1f, Desired Force x %0.1f, y %0.1f, z %0.1f, Torque x %0.1f, y %0.1f, z %0.1f, Force x %0.1f, y %0.1f, z %0.1f, %i Thrusters on list",  DesiredTorque.x, DesiredTorque.y, DesiredTorque.z, DesiredForce.x, DesiredForce.y, DesiredForce.z, T.x, T.y, T.z, F.x, F.y, F.z, ThrusterList.size ());
		} // End iteration back to the top;

		iterations++;

		// Debuggery
		sprintf (oapiDebugString(), "%i iterations", iterations);

	} // End "while ((solution == false) && (iterations < MaxIterations))"


	// Note: neet to make some sort of scaling scheme to prevent excessivly high torques or forces from breaking the solver  

	// Use "ThrusterList" to generate firing commands
	//if ((solution) && (ThrusterList.size() > 0))
	if (ThrusterList.size() > 0)
	{
		//double simdt = oapiGetSimStep ();

		for (int i = 0; i < ThrusterList.size(); i++)
		{
			double lvl = ThrusterList[i].second;
			FCS::FireThruster (ThrusterList[i].first.th, lvl);
		}
		return true;
	}
	else return false; 	
}

---------- Post added at 15:30 ---------- Previous post was at 14:21 ----------

Fixed control inversion, code above has been updated

meson800 · Feb 13, 2014

T or F values that are higher than the thruster system can achieve cause the solver to break

What does "break" mean? What happens? I have some ideas if the main loop just continues to loop until it hits MaxIterations.

Advanced Question Calculating optimal thruster choice...

Aspiring rocket scientist

Aspiring rocket scientist

Addon Developer

Aspiring rocket scientist

Addon Developer

Aspiring rocket scientist

Addon Developer

Aspiring rocket scientist

Addon Developer

Aspiring rocket scientist

Addon Developer

Addon Developer

Aspiring rocket scientist

Addon Developer

Aspiring rocket scientist

Addon Developer

Aspiring rocket scientist

Addon Developer

Aspiring rocket scientist

Addon Developer

Similar threads