What torque should I apply in Bullet to maintain a vertical orientation?

Question

I have a RigidBody in Bullet. Each frame, I want to apply torque to correct its orientation toward an upright position (i.e. near-zero pitch and roll). I know its angular velocity and orientation.

Bullet is not very well documented. I don't even know what the parameters to btRigidBody::applyTorque() mean.

How should I approach this?

Answer 1

This is a classical control problem. You want to create a feedback loop that takes the divergence from optimal position and applies the appropriate torque to nudge it back into position.

btQuaternion targetOrientation  = // whatever you need
btQuaternion currentOrientation = myObject->getOrientation();

Getting the delta orientation is quite simple, you need to find the "difference" between two the quaternions target and current orientation.

btQuaternion deltaOrientation = targetOrientation *  currentOrientation.inverse();

Unfortunately this quaterion does not say much about any divergence that we can use. But if you convert this delta orientation into euler angles.

btVector3 deltaEuler = QuaternionToEulerXYZ(deltaOrientation );

You basically get the scaled inverse of the torque you want to apply. Now you "just" need to find an appropriate amount to ease it in.

btVector torqueToApply = control(-deltaEuler);

The simple solution is to multiply it with a factor. You may need to play around with the factor until you find a good value.

// simple proportional controller
btVector control(btVector in) 
{
    // play around with the factor until you find a matching one
    static Kp = 0.5;
    return in * Kp;
}

You may also want to look into a PID-Controller to get a smoother more responsive easing.

Answer 2

I approached with a little Googlesearch ;)

From what I found bt:RigidBody::applyTorque(btVector3 & torque) takes a vector in WorldSpace and uses it as axis to apply a torque which has a strength of the length of the vector.
The LocalSpace-Torque seems to be solved in this answer, even though the provided code looks like there has to be an easier version. The author confirms this.

To actually compute the torque needed to be applied, think of the following:

1. You have two phases:
   1. Acceleration - Apply Torque to increase rotation speed
   2. Decceleration - Apply Torque to decrease rotation speed (usually opposite of Acceleration-Torque)
2. Do you want a maximum rotation speed (for example to not make it immediatly visible that a Bullet is rotating)?
   1. Set Torque to 0 inbetween (or simply don't apply it).

You have to carefully time these phases, if you for example have a maximum Torque you can/want apply, accelerating too long will result in overshooting. Ideally your acceleration- and decceleration-Torque are of the same length along the same axis.

Rest is up to you, and I hope I could help.

EDIT: Long live the related question: changing orientation by applying torques.

Answer 3

The accepted answer works for me and is a good starting point.

It might be helpful to have a complete example that works with the most recent Bullet version.

btRigidBody* rigid_body;

btQuaternion current_orientation = bt_rigid_body->getOrientation();

btVector3 axis(0, 0, 1);
btScalar angle = 0.1;
btQuaternion target_orientation(axis, angle);

btQuaternion delta = target_orientation * current_orientation.inverse();

// Get Euler angles
float x, y, z;
delta.getEulerZYX(z, y, x);

// Feed torque into control function (see accepted answer)
btVector3 local_torque = control(btVector3(x, y, z));

// Transform torque to world space
btVector3 world_torque = bt_rigid_body->getWorldTransform().getBasis() * local_torque;
bt_rigid_body->applyTorque(world_torque);