Spaceship Thruster Controller using PID

Howdy all!

I have a bit of code I've been working on, it's probably iteration # 120,238,124 at this point and I'm looking for some smarter folks than I to critique/QC this.

Here's the setup:

Box Colliders currently act as stand-ins for all of the future at assets. These are triggers (required for future damage functionality)

-One monolithic script (which is going to be broken down into sub scripts..this thing even includes input control...it's ugly, don't crush my soul to bad...I know it's ugly, this just made testing easier ) currently contains all the information about the thrusters, desired velocities, etc.
-ALL thrusters are either ON or OFF (apply the maximum thrust they've been assigned)
-Desired velocities are defined.
-Desired thrust levels of each thruster are assigned.
-Angular and Linear velocities are all transformed to local values, which are then used to feed into the PID
-The PID outputs are fed into the respective 'ApplyThrust' functions and all velocities are nulled out

So, questions I have:

1. Am I using the PID controller correctly?
2. Rather than manually coding each thruster to thrust on a specific axis, is there a way to make each thruster 'smart enough' to know when it should be fired? As an example, I explicitly tell all four thrusters to fire up or down to get a translation. Is there a better way to just say...when velocities are this, use transform up/down and the correct thrust modifier?
3. How would you handle firing the main and reverse thrusters to handle asymmetric thrust? Say...one of the two main thrusters is blown out, clearly there's a HUGE thrust differential there, the ship will then start spinning. The maneuvering jets are strong enough to overcome that on their own, the opposite side reverse thruster has to get involved...whats the best way to approach that? I suppose I could just hard code that if the opposite side thruster is destroyed, allow the rotational controller to fire the opposite reverse thruster...then I have to get throttling involved...because ideally the main and reverse thrusters will throttle between 20% and 120% of maximum (affecting fuel flow in the process...to be coded)

I hope those questions and brief description make sense. As is, this thing works really nicely. There isn't to much 'wobble' with the PID set as is. I could probably further dampen it out, but it works well as is..so...there's that.

Anyway, code and a screen shot to show how things are setup. The thrusters are all triggers, I have one central cube that isn't a trigger just to aid the rigidbody with it's MOI stuff.

V/R

Brad

Spoiler: Ship

Code (CSharp):

1. using System.Collections;
2. using System.Collections.Generic;
3. using UnityEngine;
4.  
5. public class Ship : MonoBehaviour
6. {
7.     public Transform[] mainThrusters        = new Transform[2];
8.     public Transform[] reverseThrusters     = new Transform[2];
9.     public Transform[] maneuveringThrusters = new Transform[4];
10.  
11.     public float cruiseVelocity         = 60.0f;   //m/s
12.     public float translationVelocity    = 10.0f;   //m/s
13.     public float setVelocity            = 0.0f;    //m/s
14.     public float setLatTransVel         = 0.0f;
15.     public float setVertTransVel        = 0.0f;
16.  
17.     public float yawRate        = 100.0f;       //deg/s
18.     public float rollRate       = 100.0f;       //deg/s
19.     public float pitchRate      = 100.0f;       //deg/s
20.  
21.     float setYawRate,
22.           setRollRate,
23.           setPitchRate;
24.  
25.     public float mainThrust     = 120000.0f;    //N
26.     public float reverseThrust  = 30000.0f;     //N
27.     public float maneuverThrust = 5000.0f;      //N
28.  
29.     public Vector3 localLinearVelocity,
30.                    localAngularVelocity;
31.  
32.     public Vector3 velocityError,
33.                    velocityOutput;
34.  
35.     public Vector3 angularVelocityError,
36.                    angularVelocityOutput;
37.  
38.     private Rigidbody rb;
39.  
40.     void Start()
41.     {
42.         rb = gameObject.GetComponent<Rigidbody>();
43.         rb.useGravity = false;
44.         //Drag!
45.         rb.drag = 0.0f;
46.         rb.angularDrag = 0.0f;
47.  
48.     }
49.  
50.     void Update()
51.     {
52.         localLinearVelocity = this.transform.InverseTransformDirection(rb.velocity);
53.         localAngularVelocity = this.transform.InverseTransformDirection(rb.angularVelocity);
54.  
55.         velocityError.x = setLatTransVel  + localLinearVelocity.x;
56.         velocityError.y = setVertTransVel + localLinearVelocity.y;
57.         velocityError.z = setVelocity - localLinearVelocity.z;
58.  
59.         velocityOutput.x = PID(velocityError.x, Time.fixedDeltaTime);
60.         velocityOutput.y = PID(velocityError.y, Time.fixedDeltaTime);
61.         velocityOutput.z = PID(velocityError.z, Time.fixedDeltaTime);
62.  
63.         angularVelocityError.x = setPitchRate + localAngularVelocity.x;
64.         angularVelocityError.y = setYawRate   + localAngularVelocity.y;
65.         angularVelocityError.z = setRollRate  + localAngularVelocity.z;
66.  
67.         angularVelocityOutput.x = PID(angularVelocityError.x, Time.fixedDeltaTime);
68.         angularVelocityOutput.y = PID(angularVelocityError.y, Time.fixedDeltaTime);
69.         angularVelocityOutput.z = PID(angularVelocityError.z, Time.fixedDeltaTime);
70.  
71.         //Debug.Log(velocityOutput);
72.  
73.         GetInput();
74.         SetShipVelocity();
75.         SetShipRotationVelocity();
76.         SetShipTranslationVelocity();
77.     }
78.  
79.     void FixedUpdate()
80.     {
81.         ApplyMainThrust();
82.         ApplyManeuverThrust();
83.         ApplyTranslationThrust();
84.     }
85.  
86.     #region PID
87.  
88.     float PID(float _currentError, float _deltaTime)
89.     {
90.         float Kp = 1.0f;
91.         float Ki = 0.0f;
92.         float Kd = 0.1f;
93.  
94.         float P = 0.0f,
95.               I = 0.0f,
96.               D = 0.0f,
97.               prevError = 0.0f;
98.  
99.         P = _currentError;
100.         I += P * _deltaTime;
101.         D = (P - prevError) / _deltaTime;
102.         prevError = _currentError;
103.  
104.         return P * Kp + I * Ki + D * Kd;
105.     }
106.  
107.     #endregion
108.  
109.  
110.     #region Ship Forces
111.  
112.     void SetShipVelocity()
113.     {
114.         //Velocity
115.         if (key_1)
116.         {
117.             setVelocity = cruiseVelocity * 0.0f;
118.         }
119.  
120.         if (key_2)
121.         {
122.             setVelocity = cruiseVelocity * 0.33f;
123.         }
124.  
125.         if (key_3)
126.         {
127.             setVelocity = cruiseVelocity * 0.66f;
128.         }
129.         if (key_4)
130.         {
131.             setVelocity = cruiseVelocity * 1.0f;
132.         }
133.     }
134.  
135.     void SetShipRotationVelocity()
136.     {
137.         //Pitch velocities in rads/sec
138.         if (key_w)
139.             setPitchRate = -pitchRate * Mathf.Deg2Rad;
140.  
141.         if (key_s)
142.             setPitchRate = pitchRate * Mathf.Deg2Rad;
143.  
144.         if (!key_s && !key_w)
145.             setPitchRate = 0.0f;
146.  
147.         //Yaw velocities in rads/sec
148.         if (key_a)
149.             setYawRate = yawRate * Mathf.Deg2Rad;
150.  
151.         if (key_d)
152.             setYawRate = -yawRate * Mathf.Deg2Rad;
153.  
154.         if (!key_a && !key_d)
155.             setYawRate = 0.0f;
156.  
157.         //Roll velocities in rads/sec
158.         if (key_q)
159.             setRollRate = -rollRate * Mathf.Deg2Rad;
160.  
161.         if (key_e)
162.             setRollRate = rollRate * Mathf.Deg2Rad;
163.  
164.         if (!key_q && !key_e)
165.             setRollRate = 0.0f;
166.     }
167.  
168.     void SetShipTranslationVelocity()
169.     {
170.         //Left translation
171.         if (key_z)
172.             setLatTransVel = translationVelocity;
173.  
174.         //Right translation
175.         if (key_c)
176.             setLatTransVel = -translationVelocity;
177.  
178.         if (!key_z && !key_c)
179.             setLatTransVel = 0.0f;
180.  
181.         //Up translation
182.         if (key_r)
183.             setVertTransVel = -translationVelocity;
184.  
185.         //Down translation
186.         if (key_f)
187.             setVertTransVel = translationVelocity;
188.  
189.         if (!key_r && !key_f)
190.             setVertTransVel = 0.0f;
191.     }
192.  
193.     void ApplyMainThrust()
194.     {
195.         if (velocityOutput.z > 0.0f)
196.         {
197.             rb.AddForceAtPosition(transform.forward * mainThrust, mainThrusters[0].position);
198.             //rb.AddForceAtPosition(transform.forward * mainThrust, mainThrusters[1].position);
199.         }
200.  
201.         if (velocityOutput.z < 0.0f)
202.         {
203.             rb.AddForceAtPosition(transform.forward * -reverseThrust, reverseThrusters[0].position);
204.             rb.AddForceAtPosition(transform.forward * -reverseThrust, reverseThrusters[1].position);
205.         }
206.     }
207.  
208.     void ApplyManeuverThrust()
209.     {
210.         //Yaw left
211.         if (angularVelocityOutput.y > 0.0f)
212.         {
213.             rb.AddForceAtPosition(transform.right * -maneuverThrust, maneuveringThrusters[0].position);
214.             rb.AddForceAtPosition(transform.right * maneuverThrust, maneuveringThrusters[3].position);
215.         }
216.         //Yaw right
217.         if (angularVelocityOutput.y < 0.0f)
218.         {
219.             rb.AddForceAtPosition(transform.right * maneuverThrust, maneuveringThrusters[1].position);
220.             rb.AddForceAtPosition(transform.right * -maneuverThrust, maneuveringThrusters[2].position);
221.         }
222.         //Pitch down
223.         if (angularVelocityOutput.x < 0.0f)
224.         {
225.             rb.AddForceAtPosition(transform.up * -maneuverThrust, maneuveringThrusters[0].position);
226.             rb.AddForceAtPosition(transform.up * -maneuverThrust, maneuveringThrusters[1].position);
227.  
228.             rb.AddForceAtPosition(transform.up * maneuverThrust, maneuveringThrusters[2].position);
229.             rb.AddForceAtPosition(transform.up * maneuverThrust, maneuveringThrusters[3].position);
230.         }
231.  
232.         //Pitch up
233.         if (angularVelocityOutput.x > 0.0f)
234.         {
235.             rb.AddForceAtPosition(transform.up * maneuverThrust, maneuveringThrusters[0].position);
236.             rb.AddForceAtPosition(transform.up * maneuverThrust, maneuveringThrusters[1].position);
237.  
238.             rb.AddForceAtPosition(transform.up * -maneuverThrust, maneuveringThrusters[2].position);
239.             rb.AddForceAtPosition(transform.up * -maneuverThrust, maneuveringThrusters[3].position);
240.         }
241.  
242.         //Roll right
243.         if (angularVelocityOutput.z > 0.0f)
244.         {
245.             rb.AddForceAtPosition(transform.up * -maneuverThrust, maneuveringThrusters[2].position);
246.             rb.AddForceAtPosition(transform.up * maneuverThrust, maneuveringThrusters[3].position);
247.         }
248.  
249.         //Roll left
250.         if (angularVelocityOutput.z < 0.0f)
251.         {
252.             rb.AddForceAtPosition(transform.up * maneuverThrust, maneuveringThrusters[2].position);
253.             rb.AddForceAtPosition(transform.up * -maneuverThrust, maneuveringThrusters[3].position);
254.         }
255.     }
256.  
257.     void ApplyTranslationThrust()
258.     {
259.         //Translate up
260.         if (velocityOutput.y < 0.0f)
261.         {
262.             rb.AddForceAtPosition(transform.up * maneuverThrust, maneuveringThrusters[0].position);
263.             rb.AddForceAtPosition(transform.up * maneuverThrust, maneuveringThrusters[1].position);
264.             rb.AddForceAtPosition(transform.up * maneuverThrust, maneuveringThrusters[2].position);
265.             rb.AddForceAtPosition(transform.up * maneuverThrust, maneuveringThrusters[3].position);
266.         }
267.  
268.         //Translate down
269.         //if (key_f)
270.         if (velocityOutput.y > 0.0f)
271.         {
272.             rb.AddForceAtPosition(transform.up * -maneuverThrust, maneuveringThrusters[0].position);
273.             rb.AddForceAtPosition(transform.up * -maneuverThrust, maneuveringThrusters[1].position);
274.             rb.AddForceAtPosition(transform.up * -maneuverThrust, maneuveringThrusters[2].position);
275.             rb.AddForceAtPosition(transform.up * -maneuverThrust, maneuveringThrusters[3].position);
276.         }
277.  
278.         //Translate left
279.         if (velocityOutput.x > 0.0f)
280.         {
281.             rb.AddForceAtPosition(transform.right * -maneuverThrust, maneuveringThrusters[0].position);
282.             rb.AddForceAtPosition(transform.right * -maneuverThrust, maneuveringThrusters[2].position);
283.         }
284.        
285.         //Translate right
286.         if (velocityOutput.x < 0.0f)
287.         {
288.             rb.AddForceAtPosition(transform.right * maneuverThrust, maneuveringThrusters[1].position);
289.             rb.AddForceAtPosition(transform.right * maneuverThrust, maneuveringThrusters[3].position);
290.         }
291.     }
292.  
293.     #endregion
294.  
295.     #region Input
296.  
297.     //Throttle
298.     bool key_1,
299.          key_2,
300.          key_3,
301.          key_4;
302.     //Maneuvering
303.     bool key_w,
304.          key_a,
305.          key_s,
306.          key_d,  
307.          key_q,
308.          key_e,
309.          key_z,
310.          key_c,
311.          key_r,
312.          key_f;
313.  
314.     void GetInput()
315.     {
316.         //Throttle Keys
317.         if (Input.GetKeyDown(KeyCode.Alpha1))
318.             key_1 = true;
319.         else
320.             key_1 = false;
321.  
322.         if (Input.GetKeyDown(KeyCode.Alpha2))
323.             key_2 = true;
324.         else
325.             key_2 = false;
326.  
327.         if (Input.GetKeyDown(KeyCode.Alpha3))
328.             key_3 = true;
329.         else
330.             key_3 = false;
331.  
332.         if (Input.GetKeyDown(KeyCode.Alpha4))
333.             key_4 = true;
334.         else
335.             key_4 = false;
336.  
337.         //Maneuvering Keys
338.         if (Input.GetKey(KeyCode.W))
339.             key_w = true;
340.         else
341.             key_w = false;
342.  
343.         if (Input.GetKey(KeyCode.A))
344.             key_a = true;
345.         else
346.             key_a = false;
347.  
348.         if (Input.GetKey(KeyCode.S))
349.             key_s = true;
350.         else
351.             key_s = false;
352.  
353.         if (Input.GetKey(KeyCode.D))
354.             key_d = true;
355.         else
356.             key_d = false;
357.  
358.         if (Input.GetKey(KeyCode.Q))
359.             key_q = true;
360.         else
361.             key_q = false;
362.  
363.         if (Input.GetKey(KeyCode.E))
364.             key_e = true;
365.         else
366.             key_e = false;
367.  
368.         //Translation Keys
369.         if (Input.GetKey(KeyCode.Z))
370.             key_z = true;
371.         else
372.             key_z = false;
373.  
374.         if (Input.GetKey(KeyCode.C))
375.             key_c = true;
376.         else
377.             key_c = false;
378.  
379.         if (Input.GetKey(KeyCode.R))
380.             key_r = true;
381.         else
382.             key_r = false;
383.  
384.         if (Input.GetKey(KeyCode.F))
385.             key_f = true;
386.         else
387.             key_f = false;
388.     }
389.  
390.     #endregion
391.  
392.  
393.     #region Testing Hud
394.  
395.     Rect textRect = new Rect(16.0f, 16.0f, 64.0f, 64.0f);
396.     Rect angularRect = new Rect(16.0f, 128.0f, 64.0f, 64.0f);
397.  
398.     void OnGUI()
399.     {
400.         GUI.Label(textRect, "Linear Velocities: " + localLinearVelocity.ToString());
401.        
402.         GUI.Label(angularRect, "Angular Velocities: " + localAngularVelocity.ToString());
403.     }
404.  
405.     #endregion
406. }

 

I love these sorts of things... I'd like to try and fly your ship a bit but I'm unclear how to set up your engines and thrusters. I tried a sorta naive approach of main thrusters aft, reverse thrusters counter them, then four directions around, but no matter what, there is no motion, or it goes completely wild and crazy.

I'd love it if you saved your scene setup out as a .unitypackage and added it to your post, just to see what your control system is doing. I've always done open-loop controllers (as in, the HUMAN provides the control loop), except when it's a pure AI situation, at which point I don't even try to make the AI use real controls: the AI has a separate way of controlling the ships that is more tailored to its internal state (distance from goal, heading, time to arrive, etc.)

And as for the code, looks fine to me. It's very tidy, nice and in one package, must be very easy to work with and iterate with. You can always break it apart later if you want, OR NOT!

 

Regular PID controllers should be able to 'compensate' for things going wrong in the process. I would approach this type of problem by defining a PID for each individual thruster. That way if one of the thrusters dies, the others will automatically compensate (if they can).

This will make the tuning problem more difficult, but you can live with it.

This is harder. PID controllers can't do this on their own. But you could figure out a general mathematical formula to solve this.

 

Kurt, ask and ye' shall receive!

 

@Kiwasi Okay if I understand what you're saying...basically then...each thruster would have to be more or less aware of it's own local velocities..both linear and angular, and then depending on what it's reading, that would determine if it fires or not. So instead of the master saying 'hey, by the way i'm reading these velocities' the thruster says 'hey, i'm reading these velocities and I can fire in 'x' number of directions to try and do something about it'...more or less?

V/R

Brad

 

Yup. That's how I would do it.

 

This sounds like a job for a neural net AI! There we go, we have now exceeded the sum total of my entire formal knowledge about neural nets. Good luck!

But seriously, thanks for the little scene and demo... I liked it so much I threw a particle system in the mix on the main engines, only when they're turned on... enclosed is the unitypackage. I duped your script and your scene so it should be completely unrelated to your existing assets, but I still urge you to perhaps put my assets in a completely-new empty project, JUST IN CASE.

That brings up: I hope you are using source control, btw, because if you are then you don't care about mistakes you just revert! I use git for everything, and it lets me try changes fearlessly becaue I can always reset the files back.

Meanwhile, enjoy the ShipWithCandles.unitypackage!

 

So here's another idea that I'm going to make an attempt at tackling:

First we define the baseline objects moment of inertia, since we're talking about a spaceship that has to be mass balanced, we'll define our body thusly:



This allows us to define (which theoretically as I understand unity's RigidBody dynamics can be accomplished by adding a cube of said dimensions...can't say for certain) the basic volume/shape of our spacecraft.

So with that defined, we can still define thruster positions, calculate their distance from the center of mass and figure out torques. We do that with:

Torque = Distance(Vector A) X Force(Vector B)

Cross product being (Ay * Bz - Az * By, Az * Bx - Ax * Bz, Ax * By - Ay * Bx)

Where X denotes the cross product and distance is the vector measuring the distance from the center of mass to the thruster. I've got an excel book (attached) where I can plug in numbers for force, but what I'd really like is for force to be calculated by the system. So you position the thrusters, you define the moment of inertia, it spits out a torque and then you. So what I then did was solve for the value I needed, in this case Force where:

Torque / Az - Ax * Bz = Bx

Now I can position my thruster wherever I want and figure determine the force given a torque. So how does this help me? Well, I need to define some things. What is the PID trying to control? I've decided I will define the angular velocities. So these are Pitch, Yaw and Roll rates. Using my custom moment of inertia solver I can now figure out the Torque required per axis using:

Torque = I * A

Where I is the moment of Inertia for the axis and A is the angular acceleration in rad/s (for easier reading it'll actually be degrees per second and then converted). Now, there are issues here because there are probably some cases where the torque will exceed the thrusters thrust capability, which may not be all bad, but might be annoying, especially if you're trying to shoot at another fighter. You want crisp control of where you're pointing the nose. So now we have to cap the torque so that it doesn't exceed what the thrusters can handle.

There are issues here though still. The first one is, if the total axis torque is say...20k N-m and we have 2 thrusters, we have to divide that torque by the number of thrusters. Shouldn't be to hard, because we can define the number of thrusters available per side as well, or really per axis. Generally in the setups I've played with yaw is 2, one each on the forward right and rear left sides of the ship (thanks to the reversed equations, i can now position those anywhere...they don't have to be equidistant!...well, maybe they still should be, quick experiments show other motion occuring...dangit!, but that can be damped by the other thrusters though too so...it's a holistic system after all!) Pitch is generally 4 thrusters, 2 front and 2 back firing opposite and so on.

So now...is this long winded post of semi-coherent math meanderings anywhere near sane? Is this the way to approach a system that'll better do what is desired? Once the numbers are calculated, they'll be stored in the thruster. If that thruster is destroyed, then that number will output a 0 force instead of the calculated value until it's repaired (if it can be). I'm going to start attempting to code this here tonight...probably closer to tomorrow, it's late and I'm a little tired. Can someone way smarter at physics than I QC this? I may even end up bypassing the rigid body entirely, though I suppose I could switch to an add torque instead of using add force at position.

And actually...I would be defining a velocity, not an acceleration...so I'd have to work out in how many seconds I'd want the ship to be able to reach the desired velocity...probably one second to be able to start/stop a rotation. So there's another equation to play with. 120 degrees per second is a velocity, though...i guess in that case it would be the desired acceleration as well since I want to travel that many degrees in one second...Right?

Anyway, enough of all that! Hope this rambling post helps someone else out and hopefully I'm not completely off base!

V/R

Brad

 

You could, but it would be a painful way to do it. Just some basic trigonometry and physics should be enough.

 

So learned some things...that long ol' post above? Turns out no...No I don't have to have a custom Inertia solver as it turns out (and because I learned some new terms) the RigidBody Inertia Tensor is exactly what I need. Provided ALL other (in so far as I understand at this time) colliders are marked as Triggers, they aren't used in the calculation of the RigidBody Inertia Tensor. So so long as you go with a single box collider that isn't setup as a trigger, you can define the dimensions of the Inertia Tensor. I also backed up that it's using a solid cuboid inertia tensor formula for the cube. I manually did the math, compared numbers and bammo! They all checked out.

So now, I can define the desired pitch/yaw/roll rates which allows me to then determine the torque on each axis, run the torques for each axis and then figure out what each thruster will output force wise. and in which direction. Then I can define which directions the thruster can fire, and also cap the max force of each thruster (spaceshuttles main RCS jets were around 3800N, so I bumped it up to 5000N positing 'future tech(tm)'.

So now to start coding some stuff and making things work! Will post an update when I can.

V/R

Brad

 

Hey @BradMick how did your thruster control stuff work out? I have a similar problem ahead of me and am interested in hearing about your solution. Did you ever get it to work ?