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) )); }