I have a rigid body that I want to keep upright, within some tolerance. The rigid body is a modelled as a vehicle using the Bullet physics engine, but the object itself isn't a vehicle, its actually an insect, just modelled as a vehicle for convenience. I would like to allow for a certain amount of 'wobble', but if the orientation does not match the ground orientation within some threshold, I want the body to self-right. My current method is to extract the 'up' vector from the body's centre of mass matrix, then cross product that with the ground 'up' vector and use that as the torque axis. However, then I run into the problem of what the magnitude of that torque should be. Either I set it too low and it can't overcome gravity/friction enough to self-right, or I set it too high and the body bounces all over the place like crazy. So my first question is, how do I determine the magnitude of the torque? The next, secondary, problem, is that I want to prefer rotation about the 'forward' axis of the body, so it rolls over sideways from it's belly to its feet/wheels. However, fixing the torque direction to the body's forward vector causes wild spinning behaviour...? Any hints, or links to relavant material, would be most appreciated,