physics game programming box2d - orientating a turret-like object using torques

This is a problem I hit when trying to implement a game using the LÖVE engine, which covers box2d with Lua scripting.

The objective is simple: A turret-like object (seen from the top, on a 2D environment) needs to orientate itself so it points to a target.

The turret is on the x,y coordinates, and the target is on tx, ty. We can consider that x,y are fixed, but tx, ty tend to vary from one instant to the other (i.e. they would be the mouse cursor).

The turret has a rotor that can apply a rotational force (torque) on any given moment, clockwise or counter-clockwise. The magnitude of that force has an upper limit called maxTorque.

The turret also has certain rotational inertia, which acts for angular movement the same way mass acts for linear movement. There's no friction of any kind, so the turret will keep spinning if it has an angular velocity.

The turret has a small AI function that re-evaluates its orientation to verify that it points to the right direction, and activates the rotator. This happens every dt (~60 times per second). It looks like this right now:

function Turret:update(dt)
  local x,y = self:getPositon()
  local tx,ty = self:getTarget()
  local maxTorque = self:getMaxTorque() -- max force of the turret rotor
  local inertia = self:getInertia() -- the rotational inertia
  local w = self:getAngularVelocity() -- current angular velocity of the turret
  local angle = self:getAngle() -- the angle the turret is facing currently

  -- the angle of the like that links the turret center with the target
  local targetAngle = math.atan2(oy-y,ox-x)

  local differenceAngle = _normalizeAngle(targetAngle - angle)

  if(differenceAngle <= math.pi) then -- counter-clockwise is the shortest path
    self:applyTorque(maxTorque)
  else -- clockwise is the shortest path
    self:applyTorque(-maxTorque)
  end
end

... it fails. Let me explain with two illustrative situations:

• The turret "oscillates" around the targetAngle.
• If the target is "right behind the turret, just a little clock-wise", the turret will start applying clockwise torques, and keep applying them until the instant in which it surpasses the target angle. At that moment it will start applying torques on the opposite direction. But it will have gained a significant angular velocity, so it will keep going clockwise for some time... until 开发者_开发问答the target will be "just behind, but a bit counter-clockwise". And it will start again. So the turret will oscillate or even go in round circles.

I think that my turret should start applying torques in the "opposite direction of the shortest path" before it reaches the target angle (like a car braking before stopping).

Intuitively, I think the turret should "start applying torques on the opposite direction of the shortest path when it is about half-way to the target objective". My intuition tells me that it has something to do with the angular velocity. And then there's the fact that the target is mobile - I don't know if I should take that into account somehow or just ignore it.

How do I calculate when the turret must "start braking"?

---

Think backwards. The turret must "start braking" when it has just enough room to decelerate from its current angular velocity to a dead stop, which is the same as the room it would need to accelerate from a dead stop to its current angular velocity, which is

|differenceAngle| = w^2*Inertia/2*MaxTorque.

You may also have some trouble with small oscillations around the target if your step time is too large; that'll require a little more finesse, you'll have to brake a little sooner, and more gently. Don't worry about that until you see it.

That should be good enough for now, but there's another catch that may trip you up later: deciding which way to go. Sometimes going the long way around is quicker, if you're going that way already. In that case you have to decide which way takes less time, which is not difficult, but again, cross that bridge when you come to it.

EDIT:
My equation was wrong, it should be Inertia/2*maxTorque, not 2*maxTorque/Inertia (that's what I get for trying to do algebra at the keyboard). I've fixed it.

Try this:

local torque = maxTorque;
if(differenceAngle > math.pi) then -- clockwise is the shortest path
    torque = -torque;
end
if(differenceAngle < w*w*Inertia/(2*MaxTorque)) then -- brake
    torque = -torque;
end
self:applyTorque(torque)

---

This seems like a problem that can be solved with a PID controller. I use them in my work to control a heater output to set a temperature.

For the 'P' component, you apply a torque that is proportional to the difference between the turret angle and the target angle i.e.

P = P0 * differenceAngle

If this still oscillates too much (it will a bit) then add an 'I' component,

integAngle = integAngle + differenceAngle * dt
I = I0 * integAngle

If this overshoots too much then add a 'D' term

derivAngle = (prevDifferenceAngle - differenceAngle) / dt
prevDifferenceAngle = differenceAngle
D = D0 * derivAngle

P0, I0 and D0 are constants that you can tune to get the behaviour that you want (i.e. how fast the turrets respond etc.)

Just as a tip, normally P0 > I0 > D0

Use these terms to determine how much torque is applied i.e.

magnitudeAngMomentum = P + I + D

EDIT:

Here is an application written using Processing that uses PID. It actually works fine without I or D. See it working here

// Demonstration of the use of PID algorithm to 
// simulate a turret finding a target. The mouse pointer is the target

float dt = 1e-2;
float turretAngle = 0.0;
float turretMass = 1;
// Tune these to get different turret behaviour
float P0 = 5.0;
float I0 = 0.0;
float D0 = 0.0;
float maxAngMomentum = 1.0;

void setup() {
  size(500, 500);  
  frameRate(1/dt);
}

void draw() {
  background(0);
  translate(width/2, height/2);

  float angVel, angMomentum, P, I, D, diffAngle, derivDiffAngle;
  float prevDiffAngle = 0.0;
  float integDiffAngle = 0.0;

  // Find the target
  float targetX = mouseX;
  float targetY = mouseY;  
  float targetAngle = atan2(targetY - 250, targetX - 250);

  diffAngle = targetAngle - turretAngle;
  integDiffAngle = integDiffAngle + diffAngle * dt;
  derivDiffAngle = (prevDiffAngle - diffAngle) / dt;

  P = P0 * diffAngle;
  I = I0 * integDiffAngle;
  D = D0 * derivDiffAngle;

  angMomentum = P + I + D;

  // This is the 'maxTorque' equivelant
  angMomentum = constrain(angMomentum, -maxAngMomentum, maxAngMomentum);

  // Ang. Momentum = mass * ang. velocity
  // ang. velocity = ang. momentum / mass
  angVel = angMomentum / turretMass;

  turretAngle = turretAngle + angVel * dt;

  // Draw the 'turret'
  rotate(turretAngle);
  triangle(-20, 10, -20, -10, 20, 0);

  prevDiffAngle = diffAngle;
}

---

Ok I believe I got the solution.

This is based on Beta's idea, but with some necessary tweaks. Here it goes:

local twoPi = 2.0 * math.pi -- small optimisation 

-- returns -1, 1 or 0 depending on whether x>0, x<0 or x=0
function _sign(x)
  return x>0 and 1 or x<0 and -1 or 0
end

-- transforms any angle so it is on the 0-2Pi range
local _normalizeAngle = function(angle)
  angle = angle % twoPi
  return (angle < 0 and (angle + twoPi) or angle)
end

function Turret:update(dt)

  local tx, ty = self:getTargetPosition()
  local x, y = self:getPosition()
  local angle = self:getAngle()
  local maxTorque = self:getMaxTorque()
  local inertia = self:getInertia()
  local w = self:getAngularVelocity()

  local targetAngle = math.atan2(ty-y,tx-x)

  -- distance I have to cover
  local differenceAngle = _normalizeAngle(targetAngle - angle)

  -- distance it will take me to stop
  local brakingAngle = _normalizeAngle(_sign(w)*2.0*w*w*inertia/maxTorque)

  local torque = maxTorque

  -- two of these 3 conditions must be true
  local a,b,c = differenceAngle > math.pi, brakingAngle > differenceAngle, w > 0
  if( (a and b) or (a and c) or (b and c) ) then
    torque = -torque
  end

  self:applyTorque(torque)
end

The concept behind this is simple: I need to calculate how much "space" (angle) the turret needs in order to stop completely. That depends on how fast the turret moves and how much torque can it apply to itself. In a nutshell, that's what I calculate with brakingAngle.

My formula for calculating this angle is slightly different from Beta's. A friend of mine helped me out with the physics, and well, they seem to be working. Adding the sign of w was my idea.

I had to implement a "normalizing" function, which puts any angle back to the 0-2Pi zone.

Initially this was an entangled if-else-if-else. Since the conditions where very repetitive, I used some boolean logic in order to simplify the algorithm. The downside is that, even if it works ok and it is not complicated, it doesn't transpire why it works.

Once the code is a little bit more depurated I'll post a link to a demo here.

Thanks a lot.

EDIT: Working LÖVE sample is now available here. The important stuff is inside actors/AI.lua (the .love file can be opened with a zip uncompressor)

---

You could find an equation for angular velocity vs angular distance for the rotor when accelerating torque is applied, and find the same equation for when the braking torque is applied.

Then modify the breaking equation such that it intesects the angular distance axis at the required angle. With these two equations you can calculate the angular distance at which they intersect which would give you the breaking point.

Could be totally wrong though, not done any like this for a long time. Probably a simpler solution. I'm assuming that acceleration is not linear.

---

A simplified version of this problem is pretty simple to solve. Assume the motor has infinite torque, ie it can change velocity instantaneously. This is obviously not physically accurate but makes the problem much simpler to solve and in the end isn't a problem.

Focus on a target angular velocity not a target angle.

current_angle = "the turrets current angle";
target_angle = "the angle the turret should be pointing";
dt = "the timestep used for Box2D, usually 1/60";
max_omega = "the maximum speed a turret can rotate";

theta_delta = target_angle - current_angle;
normalized_delta = normalize theta_delta between -pi and pi;
delta_omega = normalized_deta / dt;
normalized_delta_omega = min( delta_omega, max_omega );

turret.SetAngularVelocity( normalized_delta_omega );

The reason this works is the turret automatically tries to move slower as it reaches its target angle.

The infinite torque is masked by the fact that the turret doesn't try to close the distance instantaneously. Instead it tries to close the distance in one timestep. Also since the range of -pi to pi is pretty small the possibly insane accelerations never show themselves. The maximum angular velocity keep the turret's rotations looking realistic.

I've never worked out the real equation for solving with torque instead of angular velocity, but I imagine it will look a lot like the PID equations.