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;
	}
Esempio n. 2
0
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)
	));
}