void URDF_RBDL_Model::process(const urdf::Link& link, unsigned int parent, const Math::SpatialTransform& parentJointFrame)
{
RigidBodyDynamics::Body body;
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;
double mass = link.inertial->mass;
if(mass == 0)
mass = 0.000001;
body = RigidBodyDynamics::Body(mass, toRBDL(link.inertial->origin.position), 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(link.parent_joint)
{
jointFrame = toRBDL(link.parent_joint->parent_to_joint_origin_transform);
jointFrame.r = jointFrame.r - parentJointFrame.r;
joint = toRBDL(*link.parent_joint);
}
// 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, 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);
#endif
// Set up the associated joint if required
if(link.parent_joint && link.parent_joint->type != urdf::Joint::FIXED)
{
setupJoint(id, *link.parent_joint, false);
#if DISPLAY_DEBUG_INFO
ROS_WARN("Added joint %u: '%s' (%s -> %s)", id - 1, link.parent_joint->name.c_str(), link.parent_joint->parent_link_name.c_str(), link.parent_joint->child_link_name.c_str());
#endif
}
if(link.name == m_nameURDFRoot)
m_indexURDFRoot = (dof_count == 0 ? 0 : id);
for(size_t i = 0; i < link.child_links.size(); ++i)
process(*link.child_links[i], id, Math::SpatialTransform());
}
//-----------------------------------------------------------------------------
// setupJoint()
//-----------------------------------------------------------------------------
void LLViewerJointMesh::setupJoint(LLViewerJoint* current_joint)
{
// llinfos << "Mesh: " << getName() << llendl;
// S32 joint_count = 0;
U32 sj;
for (sj=0; sj<mNumSkinJoints; sj++)
{
LLSkinJoint &js = mSkinJoints[sj];
if (js.mJoint != current_joint)
{
continue;
}
// we've found a skinjoint for this joint..
// is the last joint in the array our parent?
if(mMesh->mJointRenderData.count() && mMesh->mJointRenderData[mMesh->mJointRenderData.count() - 1]->mWorldMatrix == ¤t_joint->getParent()->getWorldMatrix())
{
// ...then just add ourselves
LLViewerJoint* jointp = js.mJoint;
mMesh->mJointRenderData.put(new LLJointRenderData(&jointp->getWorldMatrix(), &js));
// llinfos << "joint " << joint_count << js.mJoint->getName() << llendl;
// joint_count++;
}
// otherwise add our parent and ourselves
else
{
mMesh->mJointRenderData.put(new LLJointRenderData(¤t_joint->getParent()->getWorldMatrix(), NULL));
// llinfos << "joint " << joint_count << current_joint->getParent()->getName() << llendl;
// joint_count++;
mMesh->mJointRenderData.put(new LLJointRenderData(¤t_joint->getWorldMatrix(), &js));
// llinfos << "joint " << joint_count << current_joint->getName() << llendl;
// joint_count++;
}
}
// depth-first traversal
for (LLJoint::child_list_t::iterator iter = current_joint->mChildren.begin();
iter != current_joint->mChildren.end(); ++iter)
{
LLViewerJoint* child_joint = (LLViewerJoint*)(*iter);
setupJoint(child_joint);
}
}
void URDF_RBDL_Model::process(const urdf::Link& link, int parent, const Math::SpatialTransform& parentJointFrame)
{
RigidBodyDynamics::Body body;
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;
double mass = link.inertial->mass;
if(mass == 0)
mass = 0.000001;
body = RigidBodyDynamics::Body(mass, toRBDL(link.inertial->origin.position), 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(link.parent_joint)
{
jointFrame = toRBDL(link.parent_joint->parent_to_joint_origin_transform);
jointFrame.r = jointFrame.r - parentJointFrame.r;
joint = toRBDL(*link.parent_joint);
}
int id = AddBody(parent, jointFrame, joint, body, link.name);
if(link.parent_joint && link.parent_joint->type != urdf::Joint::FIXED)
setupJoint(id, *link.parent_joint, false);
for(size_t i = 0; i < link.child_links.size(); ++i)
{
process(*link.child_links[i], id, Math::SpatialTransform());
}
}
//-----------------------------------------------------------------------------
// LLAvatarJointMesh::setMesh()
//-----------------------------------------------------------------------------
void LLAvatarJointMesh::setMesh( LLPolyMesh *mesh )
{
// set the mesh pointer
mMesh = mesh;
// release any existing skin joints
freeSkinData();
if ( mMesh == NULL )
{
return;
}
// acquire the transform from the mesh object
setPosition( mMesh->getPosition() );
setRotation( mMesh->getRotation() );
setScale( mMesh->getScale() );
// create skin joints if necessary
if ( mMesh->hasWeights() && !mMesh->isLOD())
{
U32 numJointNames = mMesh->getNumJointNames();
allocateSkinData( numJointNames );
std::string *jointNames = mMesh->getJointNames();
U32 jn;
for (jn = 0; jn < numJointNames; jn++)
{
//LL_INFOS() << "Setting up joint " << jointNames[jn] << LL_ENDL;
LLAvatarJoint* joint = (LLAvatarJoint*)(getRoot()->findJoint(jointNames[jn]) );
mSkinJoints[jn].setupSkinJoint( joint );
}
}
// setup joint array
if (!mMesh->isLOD())
{
setupJoint((LLAvatarJoint*)getRoot());
}
LL_DEBUGS() << "joint render entries: " << mMesh->mJointRenderData.size() << LL_ENDL;
}
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);
}
}