Permit to export as URDF iDynTree::Model models that do not respect URDF convention on link frames

[traversaro]

No description provided.

[traversaro]

This features is now needed for creo2urdf, as discussed with @Nicogene @mfussi66 .

Point of the code that need changes:
*

idyntree/src/model_io/codecs/src/URDFModelExport.cpp

Lines 362 to 370 in 07fac3a

	double distanceBetweenAxisAndOrigin = axis.getDistanceBetweenAxisAndPoint(iDynTree::Position::Zero());
	if (distanceBetweenAxisAndOrigin > 1e-7)
	{
	std::cerr << "[ERROR] URDFModelExport: Impossible to convert joint "
	<< model.getJointName(joint->getIndex()) << " to a URDF joint without moving the link frame of link "
	<< model.getLinkName(childLink->getIndex()) << " , because the axis origin is "
	<< axis.getOrigin().toString() << " the axis direction is " << axis.getDirection().toString()
	<< " and the distance between the axis and (0,0,0) is " << distanceBetweenAxisAndOrigin << std::endl;
	return false;

• We should refactor the code in

  idyntree/src/model/src/ModelTransformers.cpp

  Lines 418 to 441 in 07fac3a

  	// Compute inertia with an appropriate loop
  	computeCompositeRigidBodyInertiaSubModel(fullModel,subModels.getTraversal(linkInReducedModel),
  	jointPos,crbas);
  	// The link inertia is just its own CRBA in the submodel
  	iDynTree::SpatialInertia linkInertia = crbas(linkFullModelIndex);
  	Link newLinkForReducedModel;
  	newLinkForReducedModel.setInertia(linkInertia);
  	// Add the new link to the reducedModel
  	reducedModel.addLink(linkName,newLinkForReducedModel);
  	// Get not-base links of the submodel and transform
  	// them in additional frames in the reduced model
  	// and get additional frames and copy them to reduced model
  	reducedModelAddAdditionalFrames(fullModel,reducedModel,
  	linkName,subModels.getTraversal(linkInReducedModel),
  	jointPos,subModelBase_X_link);
  	// Lump the visual and collision shapes in the new model
  	reducedModelAddSolidShapes(fullModel,reducedModel,
  	linkName,subModels.getTraversal(linkInReducedModel),
  	jointPos,subModelBase_X_link);

  and

  idyntree/src/model/src/ModelTransformers.cpp

  Lines 449 to 531 in 07fac3a

  	size_t nrOfJointsInReducedModel = jointsInReducedModel.size();
  	for(size_t jointInReducedModel = 0;
  	jointInReducedModel < nrOfJointsInReducedModel;
  	jointInReducedModel++)
  	{
  	std::string jointName =
  	jointsInReducedModel[jointInReducedModel];
  	// We get the two links that the joint connected in the old
  	// full model
  	JointIndex fullModelJointIndex = fullModel.getJointIndex(jointName);
  	IJointConstPtr oldJoint = fullModel.getJoint(fullModelJointIndex);
  	LinkIndex oldLink1 = oldJoint->getFirstAttachedLink();
  	LinkIndex oldLink2 = oldJoint->getSecondAttachedLink();
  	// We get the new link that the joint connects, after the lumping
  	// (consider that the subModel index not matches the indices of the new links
  	// in the new reduced model
  	LinkIndex newLink1 = (LinkIndex) subModels.getSubModelOfLink(oldLink1);
  	LinkIndex newLink2 = (LinkIndex) subModels.getSubModelOfLink(oldLink2);
  	Transform newLink1_X_oldLink1 = subModelBase_X_link(oldLink1);
  	Transform newLink2_X_oldLink2 = subModelBase_X_link(oldLink2);
  	// \todo TODO handle this in a joint-agnostic way,
  	// possibly extending the joint interface
  	IJointPtr newJoint = 0;
  	if( dynamic_cast<const FixedJoint*>(oldJoint) )
  	{
  	const FixedJoint* oldJointFixed = dynamic_cast<const FixedJoint*>(oldJoint);
  	Transform oldLink1_X_oldLink2 = oldJointFixed->getRestTransform(oldLink1,oldLink2);
  	Transform newLink1_X_newLink2 = newLink1_X_oldLink1*oldLink1_X_oldLink2*newLink2_X_oldLink2.inverse();
  	FixedJoint* newJointFixed = new FixedJoint(newLink1,newLink2,newLink1_X_newLink2);
  	newJoint = (IJointPtr) newJointFixed;
  	}
  	else if( dynamic_cast<const RevoluteJoint*>(oldJoint) )
  	{
  	const RevoluteJoint* oldJointRevolute = dynamic_cast<const RevoluteJoint*>(oldJoint);
  	Transform oldLink1_X_oldLink2 = oldJointRevolute->getRestTransform(oldLink1,oldLink2);
  	Transform newLink1_X_newLink2 = newLink1_X_oldLink1*oldLink1_X_oldLink2*newLink2_X_oldLink2.inverse();
  	Axis rotationAxis_wrt_newLink2 = newLink2_X_oldLink2*oldJointRevolute->getAxis(oldLink2);
  	RevoluteJoint* newJointRevolute = new RevoluteJoint(*oldJointRevolute);
  	newJointRevolute->setAttachedLinks(newLink1,newLink2);
  	newJointRevolute->setRestTransform(newLink1_X_newLink2);
  	newJointRevolute->setAxis(rotationAxis_wrt_newLink2, newLink2);
  	newJoint = (IJointPtr) newJointRevolute;
  	}
  	else if( dynamic_cast<const PrismaticJoint*>(oldJoint) )
  	{
  	const PrismaticJoint* oldJointPrismatic = dynamic_cast<const PrismaticJoint*>(oldJoint);
  	Transform oldLink1_X_oldLink2 = oldJointPrismatic->getRestTransform(oldLink1,oldLink2);
  	Transform newLink1_X_newLink2 = newLink1_X_oldLink1*oldLink1_X_oldLink2*newLink2_X_oldLink2.inverse();
  	Axis prismaticAxis_wrt_newLink2 = newLink2_X_oldLink2*oldJointPrismatic->getAxis(oldLink2);
  	PrismaticJoint* newJointPrismatic = new PrismaticJoint(*oldJointPrismatic);
  	newJointPrismatic->setAttachedLinks(newLink1,newLink2);
  	newJointPrismatic->setRestTransform(newLink1_X_newLink2);
  	newJointPrismatic->setAxis(prismaticAxis_wrt_newLink2, newLink2);
  	newJoint = (IJointPtr) newJointPrismatic;
  	}
  	else
  	{
  	std::cerr << "[ERROR] createReducedModel error : "
  	<< " processing joint that is not revolute, prismatic or fixed. "
  	<< std::endl;
  	return false;
  	}
  	reducedModel.addJoint(jointName,newJoint);

  to permit to move link frame, instead of just handling the case of multiple links that are lumped in a parent one.

[traversaro]

I added the definition and the (failing) test in 271e68a. Now I will proceed with the implementation.