I am trying to implement needle insertion using virtual fixtures. This implementation requires both orientation and position control. But I have only Phantom Omni which is 6DOF but has only 3 degrees of force feedback.
Is there is a way to get around this?
I thought of updating only the position of the virtual tool but not its orientation. Virtual tool orientation will always remain the same (precalculated value) irrespective of my phantom device.
I am not sure how to update only the position of virtual tool but not orientation.
cGenericHapticDevicePtr hapticDevice; cToolCursor* tool; // tool->updateFromDevice(); //(updates both positon, orientation, velocity etc) // read positon cVector3d position; hapticDevice->getPosition(position); toolCursor->setDeviceGlobalPos(position); //(override the postion of the haptic device.....)
I made few trails by using this code but it doesn't appear to be the one that I am looking for.
The approach that you propose should work fine, however you will need to also specify some rotation matrix.
If you a performing a needle insertion, maybe you will want to allow free orientations when the needle is in free space, and control the orientation when you begin the insertion. In this case you could track the rotation matrix of the haptic device at the time of insertion and enforce that rotation matrix during the insertion.
Since tissue is an elastic material, you may also want to allow some form of compliance. This could be done by computing a rotation matrix that is somewhere between the current rotation of the haptic device and the rotation matrix recorded at time of penetration in the skin. The interpolation could also depend of the penetration. The deeper the needle, the higher the constraint toward the insertion angle.
Interpolating between rotation matrices can be done by converting both rotation matrices to a quaternion and then using a slerp function to compute an intermediate quaternion which can then be converted again into a rotation matrix that is finally sent to the tool.