virtual bool getLinkTransforms(const std::string& link_name,
Ogre::Vector3& visual_position,
Ogre::Quaternion& visual_orientation,
Ogre::Vector3& collision_position,
Ogre::Quaternion& collision_orientation) const
{
unsigned int id = m_model->GetBodyId(link_name.c_str());
if(id >= m_model->X_base.size())
{
visual_position = Ogre::Vector3::ZERO;
visual_orientation = Ogre::Quaternion::IDENTITY;
return true;
}
RigidBodyDynamics::Math::SpatialTransform trans = m_model->X_base[id];
visual_position = Ogre::Vector3(
trans.r.x(), trans.r.y(), trans.r.z()
);
Ogre::Matrix3 mat(
trans.E(0, 0), trans.E(1, 0), trans.E(2, 0),
trans.E(0, 1), trans.E(1, 1), trans.E(2, 1),
trans.E(0, 2), trans.E(1, 2), trans.E(2, 2)
);
visual_orientation.FromRotationMatrix(mat);
return true;
}
inline void rbdlToTF(const Math::SpatialTransform& X, tf::Transform* t)
{
t->setOrigin(tf::Vector3(X.r.x(), X.r.y(), X.r.z()));
t->setBasis(tf::Matrix3x3(
X.E(0, 0), X.E(1, 0), X.E(2, 0),
X.E(0, 1), X.E(1, 1), X.E(2, 1),
X.E(0, 2), X.E(1, 2), X.E(2, 2)
));
}