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