struct b3LinkState
{
double m_worldPosition[3];//this is the inertial frame
double m_worldOrientation[4];
double m_localInertialPosition[3];//this is the local frame from inertial to link frame
double m_localInertialOrientation[4];
};
const btTransform link_frame_world =
inertial_frame_world * m_local_inertial_frame->inverse();
