There are four ways to control the positions of rigidbodies in Unity.
- Set rigidbody.position or transform.position
- Set rigidbody.velocity
- Set acceleration by using AddForce
- Use a Joint to constrain a Rigidbody so that it follows another body
This article is about using the third option.
Note that the first three options correspond to:
- controlling the position directly
- controlling the first derivative of position
- controlling the second derivative of position
Method one should be used sparingly. Setting the transform directly triggers a big Solve routine in PhysX which will seriously affect the performance of your game. It also reduces the stability of the physics simulation. It is effectively a teleport jump in the physics world.
Method two is the way PhysX would like you to move objects around. However it overrides other forces such as collisions that are trying to affect an object as it moves. Objects will appear infinitely stiff and strong as they move around.
Method three is used when we want the control forces to be mixed with other forces. Picture a car trying to cling to the road as it rounds a corner but is knocked into a skid when it is sideswiped by another vehicle.
Force And Torque Needed To Achieve A Desired Position And Rotation
Imagine that we want to move an object from p to pNew in time dt. The high school physics formula for this is (a is acceleration, v is velocity):
pNew = a * dt *dt / 2 + v * dt + p
DON’T USE THIS IN PhysX. This formula is for the real world where time is continuous. In PhysX time is like a strobelight, jumping instantaneously from frame to frame. In PhysX the formula is:
pNew = (a * dt + v) * dt + p
Solve for the acceleration
a = (pNew – p – v * dt) / (dt^2)
Multiply times mass to get the necessary force F = ma:
Force Needed To Achieve A Desired Translation
float dt = Time.fixedDeltaTime; Vector3 pNew = new Vector3(1,2,3); //our new desired position Vector3 p = transform.position; //our current position Vector3 v = rigidbody.velocity; //our current velocity Vector3 force = rigidbody.mass * (pNew - p - v * dt) / (dt); rigidbody.AddForce(force);
Computing the torque needed to achieve a desired rotation is similar. We use Quaternions and angle/axis representation to represent the angle and and use the inertia tensor instead of mass.
Torque Needed to Achieve a Desired Rotation
public Vector3 ComputeTorque(Quaternion desiredRotation){
//q will rotate from our current rotation to desired rotation
Quaternion q = desiredRotation * Quaternion.Inverse(transform.rotation);
//convert to angle axis representation so we can do math with angular velocity
Vector3 x;
float xMag;
q.ToAngleAxis (out xMag, out x);
x.Normalize ();
//w is the angular velocity we need to achieve
Vector3 w = x * xMag * Mathf.Deg2Rad / Time.fixedDeltaTime;
w -= rigidbody.angularVelocity;
//to multiply with inertia tensor local then rotationTensor coords
Vector3 wl = transform.InverseTransformDirection (w);
Vector3 Tl;
Vector3 wll = wl;
wll = rigidbody.inertiaTensorRotation * wll;
wll.Scale(rigidbody.inertiaTensor);
Tl = Quaternion.Inverse(rigidbody.inertiaTensorRotation) * wll;
Vector3 T = transform.TransformDirection (Tl);
return T;
}
Note that this code can produce a force and torque that is too large for the Physics engine to handle (it tries to do the entire move in one frame). You may need to clamp it. Also note that you will need to stop the moving object on the next frame after accelerating it in this frame).
Forward PD Controllers
PD Controllers are mathematical spring models. They are based on the physics equations for an ideal spring. Most programmers try these first because the idea is intuitive and simple. I will spend more time talking about backward (stable) PD Controllers because the forward PD controllers suck. Here is the theory:
F = (Pdes – P) * kp
The applied force is proportional to how far an object is from the desired position. Just like a spring. kp is a constant with a value chosen by the programmer.
One reason these suck because they oscillate. They continue to accelerate an object toward the desired position right up to the moment the desired position is reached (like a child on a swing). The the object then shoots out the other side and the PD controller begins to decelerate it. The normal way to fix this is to add a velocity damping term. Think of a spring or pendulum in molasses.
F = (Pdes – P) * kp – (Vdes – V) * kd
Forward PD Controllers Suck Because
- They are not stable
- They lag when tracking a target
- They require laborious trial and error to pick the constants
A Better PD Controller, David Wu’s Stable Backwards PD Controller
The stability of PD controllers is affected by the choice for kp and kd. David Wu models backwards PD Controllers which are unconditionally stable. Instead of calculating the force needed to attract to an object to the desired location in the current timestep, why not calculate the force needed to attract to the desired location in the next time step. This is after all where we want to be after the force is applied.
F = (Pdes – Pt1)* ks + (Vdes – Vt1)* kd
Pt1 and Vt1 are the position and velocity one time step into the future. How do we know values in the future? These are estimated by using Euler integration with the force F we are generating. After skipping many steps this results in:
float dt = Time.fixedDeltaTime; float g = 1 / (1 + kd * dt + kp * dt * dt); float ksg = kp * g; float kdg = (kd + kp * dt) * g; Vector3 Pt0 = transform.position; Vector3 Vt0 = rigidbody.velocity; Vector3 F = (Pdes - Pt0) * ksg + (Vdes - Vt0) * kdg; rigidbody.AddForce (F);
This has the same form as the forward PD controller! and will be stable for all values of ks and kd. ksg and kdg can be precomputed so it has the same performance cost as the normal PD controller.
Stable Torque PD Controller
Quaternion desiredRotation = Quaternion.Euler(45f,45f,34f);
kp = (6f*frequency)*(6f*frequency)* 0.25f;
kd = 4.5f*frequency*damping;
float dt = Time.fixedDeltaTime;
float g = 1 / (1 + kd * dt + kp * dt * dt);
float ksg = kp * g;
float kdg = (kd + kp * dt) * g;
Vector3 x;
float xMag;
Quaternion q = desiredRotation * Quaternion.Inverse(transform.rotation);
// Q can be the-long-rotation-around-the-sphere eg. 350 degrees
// We want the equivalant short rotation eg. -10 degrees
// Check if rotation is greater than 190 degees == q.w is negative
if (q.w < 0)
{
// Convert the quaterion to eqivalent "short way around" quaterion
q.x = -q.x;
q.y = -q.y;
q.z = -q.z;
q.w = -q.w;
}
q.ToAngleAxis (out xMag, out x);
x.Normalize ();
x *= Mathf.Deg2Rad;
Vector3 pidv = kp * x * xMag - kd * rigidbody.angularVelocity;
Quaternion rotInertia2World = rigidbody.inertiaTensorRotation * transform.rotation;
pidv = Quaternion.Inverse(rotInertia2World) * pidv;
pidv.Scale(rigidbody.inertiaTensor);
pidv = rotInertia2World * pidv;
rigidbody.AddTorque (pidv);
Choosing kp and kd
Most people adjust kp and kd by trial and error. David Wu suggests a better approach. Instead of adjusting kp and kd, adjust frequency and damping which are more intuitive and use these to compute kp and kd.
kp = (6f*frequency)*(6f*frequency)* 0.25f; kd = 4.5f*frequency*damping;
- damping = 1, the system is critically damped
- damping > 1 the system is over damped (sluggish)
- damping is < 1 the system is under damped (it will oscillate a little)
- Frequency is the speed of convergence. If damping is 1, frequency is the 1/time taken to reach ~95% of the target value. i.e. a frequency of 6 will bring you very close to your target within 1/6 seconds.
SPD Controllers
Some smart people at the University of Georgia have published a paper describing Stable Proportional Derivative Controllers. These are similar to David Wu’s Backward PD Controllers except they include an acceleration term. They are a little more complicated to use but are probably more accurate as it is possible to feed existing forces that we want cancelled into the controller.