- Home /

# PID controller simulation?

Hi there,

for my robot simulation I need a simulated PID controller. In several states of the simulation the robot AI tries to rotate the robot to a specific variable angle. Since the robot body is a rigid body it behaves like the real robot with inertia and stuff. So on a constant force at two points near the playingfield (contact points of the wheels) the robot begins to rotate with increasing speed at a constant rate (integrating behaviour). The PID controller should be able to stop the rotation once the desired angle has been reached by reversing the forces. While a simple P controller would result in a permanent frequency in which the robot rotates from the left to the right and back, a PID controller should be able to reduce the forces to a value that lets the robot reach the desired angle.

Well, to cut a long story short... is it applicable to use a discrete PID simulation algorithm within Unity? Or is Unity, as I assume, by far too slow to achieve a reasonable calculation speed for a good simulation?

What other ways are there to achieve a rotation to a desired angle with the premise that it has to use a rigid body and forces (i.e. the physics engine), instead of manipulating the transform directly?

Thanks a lot!

Hendrik

Just to cross reference things: PID system available here as well: http://forum.unity3d.com/threads/68390-PID-controller

**Answer** by aldonaletto
·
Dec 22, 2011 at 01:13 AM

You can use a true PID controller in Unity for position, but not for rotation. The problem is the feedback signal: Unity stores the rotation as a quaternion, which cannot be translated to the 3-axes notation reliably (not a quaternion's fault: 3-axes notation is redundant, thus many different representations may be returned for the same quaternion).

But the show *can* go on! The quaternion->angle conversion is unreliable, but you can bet your life on the conversion angle->quaternion! The solution is to keep the current angle in a variable, calculate the proportional and differential errors (integral error only mess things up in this case), add them to generate the desired "torque" (angular acceleration, to be more precise), apply it to the angle, convert the angle to a quaternion and assign it to the object's rotation. It's simulated, but is physically correct, and works very well. The simulated PD controller (set to rotate around the Y axis) is:

```
var targetAngle: float = 0; // the desired angle
var curAngle: float; // current angle
var accel: float; // applied accel
var angSpeed: float = 0; // current ang speed
var maxAccel: float = 180; // max accel in degrees/second2
var maxASpeed: float = 90; // max angular speed in degrees/second
var pGain: float = 20; // the proportional gain
var dGain: float = 10; // differential gain
private var lastError: float;
function Start(){
targetAngle = transform.eulerAngles.y; // get the current angle just for start
curAngle = targetAngle;
}
function FixedUpdate(){
var error = targetAngle - curAngle; // generate the error signal
var diff = (error - lastError)/ Time.deltaTime; // calculate differential error
lastError = error;
// calculate the acceleration:
accel = error * pGain + diff * dGain;
// limit it to the max acceleration
accel = Mathf.Clamp(accel, -maxAccel, maxAccel);
// apply accel to angular speed:
angSpeed += accel * Time.deltaTime;
// limit max angular speed
angSpeed = Mathf.Clamp(angSpeed, -maxASpeed, maxASpeed);
curAngle += angSpeed * Time.deltaTime; // apply the rotation to the angle...
// and make the object follow the angle (must be modulo 360)
rigidbody.rotation = Quaternion.Euler(0, curAngle%360, 0);
}
```

You can set positive or negative angles, and the object will rotate to the desired angle - including angles > 360 or < -360.

**BONUS SCRIPT:** Since you're interested in PID controllers, that's a simple PID position controller entirely based on physics, that tries to reach the position set in *targetPos*. It works fine, but the parameters must be fine tuned to reach a stable operation:

```
var targetPos = Vector3.zero; // the desired position
var maxForce: float = 100; // the max force available
var pGain: float = 20; // the proportional gain
var iGain: float = 0.5; // the integral gain
var dGain: float = 0.5; // differential gain
private var integrator = Vector3.zero; // error accumulator
private var lastError = Vector2.zero;
var curPos = Vector3.zero; // actual Pos
var force = Vector3.zero; // current force
function Start(){
targetPos = transform.position;
}
function FixedUpdate(){
curPos = transform.position;
var error = targetPos - curPos; // generate the error signal
integrator += error * Time.deltaTime; // integrate error
var diff = (error - lastError)/ Time.deltaTime; // differentiate error
lastError = error;
// calculate the force summing the 3 errors with respective gains:
force = error * pGain + integrator * iGain + diff * dGain;
// clamp the force to the max value available
force = Vector3.ClampMagnitude(force, maxForce);
// apply the force to accelerate the rigidbody:
rigidbody.AddForce(force);
}
```

Alright. This works. It has been the integrating part of the PID that messed up the robot's behaviour, thanks a lot!

'cos I need to replace some parts of the simulation with "adapters" to the real world, later, I've adapted your solution to my needs regarding the exclusive use of forces.

Yes, you've read that right: later on I would like to use the AI part of the simulation to operate a real robot. So it's rather some sort of an emulation of the real robot's hardware which of course is subject to physical laws.

What I actually haven't understood is the part with the unreliable quaternion-to-angle conversation. I only use the vertical axis of the robot. The other two axes are fixed. On the other hand the vertical axis of the robot is locked against translational movement. So the body can only translate horizontally (x and z) and rotate around its vertical axis (y). As a result I assume I can rely on the eulerAngle.y value of the rigidbody's rotation to calculate the controller's error, can't I?

Well, it looks like I can. :)

You're right: fixing the other axis in 0 rotation and locking them in physics makes the eulerAngles.y reliable. The problem is that the angle returned always range from 0 to 360: if the angle is reducing, when reaching zero it suddenly becomes 360 (and vice versa), producing a completely wrong error signal that makes the robot spins forever when an angle near to 0 or 360 is specified. This would not happen with a real robot, because the encoder doesn't have this modulo 360 problem.

Yeah, alright. I just need to figure out how to deal with this 360 overflow. It doesn't exist in the real world but it can evolve into a problem when then robot has rotated, lets say, for an accumulated angle of 720 degrees and the AI needs to return it to 90 degrees for example. But that's a different story and I'm going to find out by myself (I hope I will).

Well, your answer was a great help, thanks again!

Hi,

Thanks for the solution, worked like a charm!

I had a very similar issue with the 360 overflow as well and I solved it by changing this line of code:

float error = targetPos - curPos;

to this:

float error = Mathf.DeltaAngle(curPos, targetPos);

I put the small change here just in case someone needs it :)

### Your answer

### Welcome to Unity Answers

The best place to ask and answer questions about development with Unity.

To help users navigate the site we have posted a site navigation guide.

If you are a new user to Unity Answers, check out our FAQ for more information.

Make sure to check out our Knowledge Base for commonly asked Unity questions.

If you are a moderator, see our Moderator Guidelines page.

We are making improvements to UA, see the list of changes.