void URDF_RBDL_Model::processReverse(const urdf::Link& link, unsigned int parent, const urdf::Link* parentLink, const Math::SpatialTransform& parentJointFrame) { RigidBodyDynamics::Body body; // Situation here: In the URDF world, we are the child of link.parent_link. // We want to become the child of parentLink, which is a child of ours in // URDF. // Our origin needs to be in the point of the joint from parentLink to us. // Determine transform from old origin to the new origin (joint of the parentLink) Math::SpatialTransform tf; boost::shared_ptr<urdf::Joint> child_joint; if(parentLink) { child_joint = parentLink->parent_joint; tf = toRBDL(child_joint->parent_to_joint_origin_transform); } else { // Just leave our origin at the child joint tf.E = Math::Matrix3d::Identity(); tf.r = Math::Vector3d::Zero(); } if(link.inertial) { const urdf::Inertial& in = *link.inertial; Math::Matrix3d I; I << in.ixx, in.ixy, in.ixz, in.ixy, in.iyy, in.iyz, in.ixz, in.iyz, in.izz; Math::Vector3d cog = toRBDL(link.inertial->origin.position); // As the coordinate system is not rotated, and the center of mass // stays where it is, the inertia tensor does not need to be // modified. // Transform cog relative to our new origin cog = cog - tf.r; double mass = link.inertial->mass; if(mass == 0) mass = 0.000001; body = RigidBodyDynamics::Body(mass, cog, I); } else { body.mCenterOfMass = Math::Vector3d::Zero(); body.mInertia = Math::Matrix3d::Zero(); body.mMass = 0.000001; } Math::SpatialTransform jointFrame = Math::SpatialTransform(); RigidBodyDynamics::Joint joint = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed); if(parentLink && parentLink->parent_joint) { // Rotate the other way around since we are reversing the joint. Math::Matrix3d E = tf.E.transpose(); // Origin of the joint in parentLink coordinates Math::Vector3d r = -parentJointFrame.r; jointFrame = Math::SpatialTransform(E, r); joint = toRBDL(*parentLink->parent_joint); // Flip joint axes for(size_t i = 0; i < joint.mDoFCount; ++i) joint.mJointAxes[i] = -joint.mJointAxes[i]; } // Add the body to the RBDL tree (Note: This function merges fixed joints into following non-fixed joints and doesn't create a new id for them) unsigned int id = AddBody(parent, jointFrame, joint, body, "?reverse_" + link.name); #if DISPLAY_DEBUG_INFO ROS_INFO("Added body link '%s' as ID %u, Parent ID %u, Joint type %d, Mass %.4f (now %d DOF)", link.name.c_str(), id, parent, joint.mJointType, body.mMass, dof_count); ROS_INFO(" -> parentLink: %p, parentLink->parentJoint: %p", parentLink, parentLink ? parentLink->parent_joint.get() : 0 ); ROS_INFO(" -> joint frame was %f, %f, %f", jointFrame.r.x(), jointFrame.r.y(), jointFrame.r.z()); if(id >= fixed_body_discriminator) { ROS_INFO(" -> fixed body has movable parent %u now, offset %f, %f, %f", mFixedBodies[id - fixed_body_discriminator].mMovableParent, mFixedBodies[id - fixed_body_discriminator].mParentTransform.r.x(), mFixedBodies[id - fixed_body_discriminator].mParentTransform.r.y(), mFixedBodies[id - fixed_body_discriminator].mParentTransform.r.z() ); } #endif // Set up the associated joint if required if(parentLink && parentLink->parent_joint && parentLink->parent_joint->type != urdf::Joint::FIXED) { setupJoint(id, *parentLink->parent_joint, true); #if DISPLAY_DEBUG_INFO ROS_WARN("Added joint %u: '%s' (%s -> %s)", id - 1, parentLink->parent_joint->name.c_str(), parentLink->parent_joint->parent_link_name.c_str(), parentLink->parent_joint->child_link_name.c_str()); #endif } if(link.name == m_nameURDFRoot) m_indexURDFRoot = (dof_count == 0 ? 0 : id); // Provide a zero-weight fixed body with the correct origin { RigidBodyDynamics::Joint fixedJoint(RigidBodyDynamics::JointTypeFixed); RigidBodyDynamics::Body fixedBody; fixedBody.mMass = 0.0; RigidBodyDynamics::Math::SpatialTransform t; t.E.setIdentity(); t.r = -tf.r; AddBody(id, t, fixedJoint, fixedBody, link.name); } for(size_t i = 0; i < link.child_links.size(); ++i) { const urdf::Link* child = link.child_links[i].get(); if(child == parentLink) continue; process(*link.child_links[i], id, tf); } boost::shared_ptr<urdf::Link> urdfParent = link.getParent(); if(urdfParent) processReverse(*urdfParent, id, &link, tf); }
void URDF_RBDL_Model::processReverse(const urdf::Link& link, int parent, const urdf::Link* parentLink, const Math::SpatialTransform& parentJointFrame) { RigidBodyDynamics::Body body; // Situation here: In the URDF world, we are the child of link.parent_link. // We want to become the child of parentLink, which is a child of ours in // URDF. // Our origin needs to be in the point of the joint from parentLink to us. // Determine transform from old origin to the new origin (joint of the parentLink) Math::SpatialTransform tf; boost::shared_ptr<urdf::Joint> child_joint; if(parentLink) { child_joint = parentLink->parent_joint; tf = toRBDL(child_joint->parent_to_joint_origin_transform); } else { // Just leave our origin at the child joint tf.E = Math::Matrix3d::Identity(); tf.r = Math::Vector3d::Zero(); } if(link.inertial) { const urdf::Inertial& in = *link.inertial; Math::Matrix3d I; I << in.ixx, in.ixy, in.ixz, in.ixy, in.iyy, in.iyz, in.ixz, in.iyz, in.izz; Math::Vector3d cog = toRBDL(link.inertial->origin.position); // As the coordinate system is not rotated, and the center of mass // stays where it is, the inertia tensor does not need to be // modified. // Transform cog relative to our new origin cog = cog - tf.r; double mass = link.inertial->mass; if(mass == 0) mass = 0.000001; body = RigidBodyDynamics::Body(mass, cog, I); } else { body.mCenterOfMass = Math::Vector3d::Zero(); body.mInertia = Math::Matrix3d::Zero(); body.mMass = 0.00001; } Math::SpatialTransform jointFrame = Math::SpatialTransform(); RigidBodyDynamics::Joint joint = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed); if(parentLink && parentLink->parent_joint) { // Rotate the other way around since we are reversing the joint. Math::Matrix3d E = tf.E.transpose(); // Origin of the joint in parentLink coordinates Math::Vector3d r = -parentJointFrame.r; jointFrame = Math::SpatialTransform(E, r); joint = toRBDL(*parentLink->parent_joint); // Flip joint axes for(size_t i = 0; i < joint.mDoFCount; ++i) { joint.mJointAxes[i] = -joint.mJointAxes[i]; } } int id = AddBody(parent, jointFrame, joint, body, link.name); if(parentLink && parentLink->parent_joint && parentLink->parent_joint->type != urdf::Joint::FIXED) setupJoint(id, *parentLink->parent_joint, true); for(size_t i = 0; i < link.child_links.size(); ++i) { const urdf::Link* child = link.child_links[i].get(); if(child == parentLink) continue; process(*link.child_links[i], id, tf); } boost::shared_ptr<urdf::Link> urdfParent = link.getParent(); if(urdfParent) { processReverse(*urdfParent, id, &link, tf); } }