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