Collision Force and Haptic Feedback rendering....

[Ixstala]

I've been looking around on this board searching for a solution for how to determine the resultant forces from collisions between a single object (user's hand) and possibly many other objects simultaneously. The reason for this being to feedback the collision force to a haptic device.

From what I've read people have been trying to accumulate impulses over the collision manifold and using the step rate of the dynamic world to retrieve force. Results have been poor, it seems, and I would like to offer an alternative method which works for me and will work for potentially any haptic device.

The method is to create a virtual spring/damper system between the end effector of the haptic robot (hand position in world coords) and a dynamic "proxy" hand (which is a bullet object). The user's hand drives one end of the spring/damper and the dynamic proxy hass some mass which is attracted to the real hand position. Any collisions will affect the position of the proxy and this perturbance in position will allow you to calculate the force through the spring/damper. Simply apply the opposite force through the haptic device.

Here's some pseudo code:
//Startup//
set real hand position
set proxy mass
set proxy initial position
Instantiate proxy as a rigid body
//Define virtual coupling (Not part of Bullet!!!)
Kx,Ky,Kz
//calculate damping of coupling so it's critically damped in all axis:
Bx=sqrt(4*proxymass*Kx)
By=sqrt(4*proxymass*Ky)
Bz=sqrt(4*proxymass*Kz)

//main loop
get proxy position/velocity through bullet
get real hand position (either through mouse, keyboard setup, or haptic device, etc..)
//calculate force on proxy
F=K(handpos-proxypos)-B*proxyvel
apply F to proxy object
set proxy state to active
output -F to haptic device

That's it! The stiffer you make the coupling the better tracking the proxy will have, but you will most likely have to scale the force output sufficiently for a haptic device (especially smaller desktop models). It's important to have the proxy be critically damped or it will oscillate, or not track very fast. This method does not rely on the time step of the dynamic world (directly), gives relatively smooth results, and could be extended to retrieve collision torques as well (at least for a single user controllable object).

Thoughts?

-Ixs

[Ixstala]

Well I just figured out how to extend this to torques in case anyone would like a fully dynamic character of which they can freely choose orientation and position without resorting to kinematic actors or using built in constraints from bullet.

The method for torques relies on the quarternion orientation of your rigid body and your desired orientation. An error quarternion is calculated and a simple PD controller works it's magic. Be careful of the sign of the torque! Here's some pseudo-code.

btQuarternion DesX((1,0,0),angX) //Defined by axis angle...
btQuarternion DesY((0,1,0),angY) //Defined by axis angle...
btQuarternion DesZ((0,0,1),angZ) //Defined by axis angle...
btQuarternion Des=DesX*DesY*DesZ

btQuarternion Current=rigidBody->getOrientation()

btQuarternion ErrRot=Current*Des;

Torque.X= -K*ErrRot.X*ErrRot.W - B*BodyAngVel.X
Torque.Y= -K*ErrRot.Y*ErrRot.W - B*BodyAngVel.Y
Torque.Z= -K*ErrRot.Z*ErrRot.W - B*BodyAngVel.Z

rigidBody->applyTorque(Torque)

Once again you need to have enough damping to keep it stable, but this works great for me! Now I can fully control position & orientation for something like a tennis racket and still have it interact. If you don't multiply by W you will get limit cycles around where the signs change from positive to negative.

-Ixs