Add moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink function (#…

…1193)

* Add moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink function definition and tests

src/core/include/iDynTree/Axis.h

@@ -132,6 +132,11 @@ namespace iDynTree
          */
         Axis reverse() const;
 
+        /**
+         * Compute the point on the axis that is closest to a given opoint
+         */
+        Position getPointOnAxisClosestToGivenPoint(const iDynTree::Position& point) const;
+
         /**
          * Compute distance between the axis and a given point
          */

src/core/src/Axis.cpp

@@ -261,21 +261,29 @@ namespace iDynTree
 
     double Axis::getDistanceBetweenAxisAndPoint(const iDynTree::Position& point) const
     {
-        Eigen::Vector3d direction = iDynTree::toEigen(this->getDirection()).normalized();
-
-        // Vector from the offset point of the to the given point
-        Eigen::Vector3d axisOrigin_p_point = iDynTree::toEigen(point) - iDynTree::toEigen(this->getOrigin());
-
-        // Project the axisOrigin_p_point onto the axis direction
-        Eigen::Vector3d projection = direction * axisOrigin_p_point.dot(direction);
-
         // Calculate the closest point on the axis to the given point
-        Eigen::Vector3d closestPointOnAxis = iDynTree::toEigen(this->getOrigin()) + projection;
+        iDynTree::Position closestPointOnAxis = getPointOnAxisClosestToGivenPoint(point);
 
         // Calculate the distance between the closest point on the axis and the given point
-        return (closestPointOnAxis - iDynTree::toEigen(point)).norm();
+        return (iDynTree::toEigen(closestPointOnAxis) - iDynTree::toEigen(point)).norm();
     }
 
+    iDynTree::Position Axis::getPointOnAxisClosestToGivenPoint(const iDynTree::Position& point) const
+    {
+            Eigen::Vector3d direction = iDynTree::toEigen(this->getDirection()).normalized();
+
+            // Vector from the offset point of the to the given point
+            Eigen::Vector3d axisOrigin_p_point = iDynTree::toEigen(point) - iDynTree::toEigen(this->getOrigin());
+
+            // Project the axisOrigin_p_point onto the axis direction
+            Eigen::Vector3d projection = direction * axisOrigin_p_point.dot(direction);
+
+            // Calculate the closest point on the axis to the given point
+            iDynTree::Position closestPointOnAxis;
+            iDynTree::toEigen(closestPointOnAxis) = iDynTree::toEigen(this->getOrigin()) + projection;
+
+            return closestPointOnAxis;
+    }
 
     std::string Axis::toString() const
     {

src/model/include/iDynTree/ModelTransformers.h

@@ -145,6 +145,27 @@ bool extractSubModel(const iDynTree::Model& fullModel, const iDynTree::Traversal
 bool addValidNamesToAllSolidShapes(const iDynTree::Model& inputModel,
                                    iDynTree::Model& outputModel);
 
+/**
+ * Transform the input model in model that can be exported as URDF with the given base link.
+ *
+ * In iDynTree, the link frame can be placed without constraint w.r.t. to the joints to which
+ * the link is connected. On the other hand, in the URDF format, the origin of the frame of the child link
+ * connected to its parent with a non-fixed joint **must** lay on the axis of the joint.
+ *
+ * That means that if you want to export a model with an arbitrary base link, some link frame will need
+ * to be moved to respect the constraint given by the URDF format. This function perform exactly this
+ * transformation, ensuring that inertia, visual and collision information is probably accounted for,
+ * and leaving the original link frames in the model as "additional frames" with the naming scheme
+ * <linkName>_original_frame .
+ *
+ * Note that the operation done depends on the base link used, if you want to use a different
+ * base link, change the default base link of the model via inputModel.setDefaultBaseLink method.
+ *
+ * @return true if all went well, false if there was an error in creating the sub model.
+ */
+bool moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink(const iDynTree::Model& inputModel,
+                                                           iDynTree::Model& outputModel);
+
 }
 
 

src/model/src/ModelTransformers.cpp

@@ -214,15 +214,77 @@ void buildLinkToAdditionalFramesList(const Model& fullModel,
     }
 }
 
+void computeTransformToTraversalBaseWithAdditionalTransform(const Model& fullModel,
+                                                            const Traversal& subModelTraversal,
+                                                            const JointPosDoubleArray& jointPos,
+                                                                  LinkPositions& traversalBase_H_link,
+                                                            const std::unordered_map<std::string, iDynTree::Transform>& newLink_H_oldLink)
+{
+    for(unsigned int traversalEl=0; traversalEl < subModelTraversal.getNrOfVisitedLinks(); traversalEl++)
+    {
+        LinkConstPtr visitedLink = subModelTraversal.getLink(traversalEl);
+        LinkConstPtr parentLink  = subModelTraversal.getParentLink(traversalEl);
+        IJointConstPtr toParentJoint = subModelTraversal.getParentJoint(traversalEl);
+
+        // If this is the traversal base
+        if( parentLink == 0 )
+        {
+            // If the visited link is the base, the base has no parent.
+            // In this case the position of the base with respect to the base is simply
+            // an identity transform, or a given transform if the link is in newLink_H_oldLink
+            auto it = newLink_H_oldLink.find(fullModel.getLinkName(visitedLink->getIndex()));
+
+            if (it != newLink_H_oldLink.end())
+            {
+                traversalBase_H_link(visitedLink->getIndex()) = it->second;
+            }
+            else
+            {
+                traversalBase_H_link(visitedLink->getIndex()) = iDynTree::Transform::Identity();
+            }
+        }
+        else
+        {
+            // Otherwise we compute the world_H_link transform as:
+            // world_H_link = world_H_parentLink * parentLink_H_link
+            traversalBase_H_link(visitedLink->getIndex()) =
+                traversalBase_H_link(parentLink->getIndex())*
+                    toParentJoint->getTransform(jointPos,parentLink->getIndex(),visitedLink->getIndex());
+        }
+    }
+
+    return;
+}
+
+void computeTransformToSubModelBaseWithAdditionalTransform(const Model& fullModel,
+                                    const SubModelDecomposition& subModelDecomposition,
+                                    const JointPosDoubleArray& jointPos,
+                                          LinkPositions& subModelBase_H_link,
+                                    const std::unordered_map<std::string, iDynTree::Transform>& newLink_H_oldLink)
+{
+    for(size_t subModel = 0;
+               subModel < subModelDecomposition.getNrOfSubModels();
+               subModel++ )
+    {
+        computeTransformToTraversalBaseWithAdditionalTransform(fullModel,
+                                        subModelDecomposition.getTraversal(subModel),
+                                        jointPos,
+                                        subModelBase_H_link,
+                                        newLink_H_oldLink);
+    }
+}
+
 void reducedModelAddAdditionalFrames(const Model& fullModel,
                                            Model& reducedModel,
                                      const std::string linkInReducedModel,
                                      const Traversal& linkSubModel,
                                      const FreeFloatingPos& pos,
-                                           LinkPositions& subModelBase_X_link)
+                                           LinkPositions& subModelBase_X_link,
+                                     const std::unordered_map<std::string, iDynTree::Transform>& newLink_H_oldLink
+                                     )
 {
     // First compute the transform between each link in the submodel and the submodel base
-    computeTransformToTraversalBase(fullModel,linkSubModel,pos.jointPos(),subModelBase_X_link);
+    computeTransformToTraversalBaseWithAdditionalTransform(fullModel,linkSubModel,pos.jointPos(),subModelBase_X_link,newLink_H_oldLink);
 
     // We then need to compute the list of additional frames for each link
     // This is a rather inefficient operation (given how frame information is stored in the Model
@@ -276,11 +338,8 @@ void reducedModelAddSolidShapes(const Model& fullModel,
                                 const std::string linkInReducedModel,
                                 const Traversal& linkSubModel,
                                 const FreeFloatingPos& pos,
-                                      LinkPositions& subModelBase_X_link)
+                                const LinkPositions& subModelBase_X_link)
 {
-    // First compute the transform between each link in the submodel and the submodel base
-    computeTransformToTraversalBase(fullModel,linkSubModel,pos.jointPos(),subModelBase_X_link);
-
     LinkIndex subModelBaseIndexInReducedModel = reducedModel.getLinkIndex(linkInReducedModel);
 
     // All the geometries collected in the traversal are lumped into the base link
@@ -328,10 +387,19 @@ void reducedModelAddSolidShapes(const Model& fullModel,
 
 }
 
-bool createReducedModel(const Model& fullModel,
-                        const std::vector< std::string >& jointsInReducedModel,
-                        Model& reducedModel,
-                        const std::unordered_map<std::string, double>& removedJointPositions)
+// This function is a private function that is not exposed in the headers, but it used as a backend
+// of both:
+//  * createReducedModel : function to create a reduced model given the specified joints
+//  * moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink: function to make sure a model is URDF compatible
+// The logic is similar to the createReducedModel, but as an additional option this function takes in input
+// a std::vector<iDynTree::Transform> newLink_H_oldLink vector (of size fullModel.getNrOfLinks() that can be used
+// to specify an optional additional transform of the final link used in the "reduced model"
+bool createReducedModelAndChangeLinkFrames(const Model& fullModel,
+                                           const std::vector< std::string >& jointsInReducedModel,
+                                           Model& reducedModel,
+                                           const std::unordered_map<std::string, double>& removedJointPositions,
+                                           const std::unordered_map<std::string, iDynTree::Transform>& newLink_H_oldLink,
+                                           bool addOriginalLinkFrameWith_original_frame_suffix)
 {
     // We use the default traversal for deciding the base links of the reduced model
     Traversal fullModelTraversal;
@@ -359,7 +427,6 @@ bool createReducedModel(const Model& fullModel,
     // We need the buffer to for each link the transform to its submodel base
     // and the composite rigid body inertia wrt to the submodel
     LinkInertias crbas(fullModel);
-    LinkPositions subModelBase_X_link(fullModel);
 
     // The position for the joint removed from the model is supposed to be 0
     FreeFloatingPos jointPos(fullModel);
@@ -404,6 +471,7 @@ bool createReducedModel(const Model& fullModel,
         }
     }
 
+    LinkPositions subModelBase_X_link(fullModel);
     for(size_t linkInReducedModel = 0;
                linkInReducedModel < nrOfLinksInReducedModel;
                linkInReducedModel++)
@@ -419,8 +487,21 @@ bool createReducedModel(const Model& fullModel,
         computeCompositeRigidBodyInertiaSubModel(fullModel,subModels.getTraversal(linkInReducedModel),
                                                  jointPos,crbas);
 
-        // The link inertia is just its own CRBA in the submodel
-        iDynTree::SpatialInertia linkInertia = crbas(linkFullModelIndex);
+        // The link inertia is just its own CRBA in the submodel, eventually considering the offset
+        // between the link frame and the new link frame
+        iDynTree::Transform newLink_H_oldLink_trans;
+        auto it = newLink_H_oldLink.find(fullModel.getLinkName(linkFullModelIndex));
+
+        if (it != newLink_H_oldLink.end())
+        {
+            newLink_H_oldLink_trans = it->second;
+        }
+        else
+        {
+            newLink_H_oldLink_trans = iDynTree::Transform::Identity();
+        }
+
+        iDynTree::SpatialInertia linkInertia = newLink_H_oldLink_trans*crbas(linkFullModelIndex);
 
         Link newLinkForReducedModel;
         newLinkForReducedModel.setInertia(linkInertia);
@@ -430,10 +511,14 @@ bool createReducedModel(const Model& fullModel,
 
         // 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
+        // and get additional frames and copy them to reduced e
+        // This function is the one that computes the subModelBase_X_link
+        // that contains the transform between the link in the original model
+        // and the corresponding link in the transformed model, that is used in the rest of the function
+        // As this quantity is influenced by newLink_H_oldLink, this is passed along
         reducedModelAddAdditionalFrames(fullModel,reducedModel,
                                         linkName,subModels.getTraversal(linkInReducedModel),
-                                        jointPos,subModelBase_X_link);
+                                        jointPos,subModelBase_X_link,newLink_H_oldLink);
 
         // Lump the visual and collision shapes in the new model
         reducedModelAddSolidShapes(fullModel,reducedModel,
@@ -506,18 +591,18 @@ bool createReducedModel(const Model& fullModel,
         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
@@ -731,6 +816,17 @@ bool createReducedModel(const Model& fullModel,
     return ok;
 }
 
+bool createReducedModel(const Model& fullModel,
+                        const std::vector< std::string >& jointsInReducedModel,
+                        Model& reducedModel,
+                        const std::unordered_map<std::string, double>& removedJointPositions)
+{
+    // We do not want to move the link frames in createReducedModel
+    std::unordered_map<std::string, iDynTree::Transform> newLink_H_oldLink;
+    bool addOriginalLinkFrameWith_original_frame_suffix = false;
+    return createReducedModelAndChangeLinkFrames(fullModel, jointsInReducedModel, reducedModel, removedJointPositions, newLink_H_oldLink, addOriginalLinkFrameWith_original_frame_suffix);
+}
+
 bool createReducedModel(const Model& fullModel,
                         const std::vector< std::string >& jointsInReducedModel,
                         Model& reducedModel)
@@ -917,4 +1013,90 @@ bool addValidNamesToAllSolidShapes(const iDynTree::Model& inputModel,
     return true;
 }
 
+bool moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink(const iDynTree::Model& inputModel,
+                                                                 iDynTree::Model& outputModel)
+{
+
+    // In this transformer we do not remove any joint, so the list of considered joints is exactly the list of joints in the input model
+    std::vector<std::string> consideredJoints;
+    for(iDynTree::JointIndex jntIdx=0; jntIdx < inputModel.getNrOfJoints(); jntIdx++)
+    {
+        consideredJoints.push_back(inputModel.getJointName(jntIdx));
+    }
+
+    // As we do not remove any joint, this map is empty
+    std::unordered_map<std::string, double> removedJointPositions;
+
+    // We now need to compute the newLink_H_oldLink transform for each link we need to transform
+    std::unordered_map<std::string, iDynTree::Transform> newLink_H_oldLink;
+
+    // First of all, compute the traversal for the default base of the model
+    iDynTree::Traversal fullTraversal;
+    bool ok = inputModel.computeFullTreeTraversal(fullTraversal);
+
+    if (!ok)
+    {
+        return false;
+    }
+
+    // We start from 1 as the first link does not have any joint to the aparent link
+    for (iDynTree::TraversalIndex trvIdx=1; trvIdx < static_cast<TraversalIndex>(fullTraversal.getNrOfVisitedLinks()); trvIdx++)
+    {
+        LinkConstPtr childLink = fullTraversal.getLink(trvIdx);
+        std::string childLinkName = inputModel.getLinkName(childLink->getIndex());
+        LinkConstPtr parentLink = fullTraversal.getParentLink(trvIdx);
+        IJointConstPtr jointToParent = fullTraversal.getParentJoint(trvIdx);
+
+
+        if (jointToParent->getNrOfDOFs() != 0)
+        {
+            iDynTree::Axis axis;
+
+            // Check that the axis of the joint is supported by URDF
+            if (dynamic_cast<const RevoluteJoint*>(jointToParent))
+            {
+                const RevoluteJoint* revJoint = dynamic_cast<const RevoluteJoint*>(jointToParent);
+                axis = revJoint->getAxis(childLink->getIndex());
+            }
+            else if (dynamic_cast<const PrismaticJoint*>(jointToParent))
+            {
+                const PrismaticJoint* prismJoint = dynamic_cast<const PrismaticJoint*>(jointToParent);
+                axis = prismJoint->getAxis(childLink->getIndex());
+            }
+
+            // If the axis is not URDF-compatible, move it to ensure that the new link frame lays on the
+            // The threshold of 1e-7 is the same used in URDFStringFromModel, if you change it here also change it there
+            double distanceBetweenAxisAndOrigin = axis.getDistanceBetweenAxisAndPoint(iDynTree::Position::Zero());
+            if (distanceBetweenAxisAndOrigin > 1e-7)
+            {
+
+                // First of all, we get the point on the child axis closest to the existing origin
+                iDynTree::Position oldLink_o_newLink = axis.getPointOnAxisClosestToGivenPoint(iDynTree::Position::Zero());
+
+                // We invert the sign
+                iDynTree::Position newLink_o_oldLink = -oldLink_o_newLink;
+
+
+                newLink_H_oldLink[childLinkName] = iDynTree::Transform(iDynTree::Rotation::Identity(), newLink_o_oldLink);
+            }
+        }
+    }
+
+    bool addOriginalLinkFrameWith_original_frame_suffix = true;
+    bool okReduced = createReducedModelAndChangeLinkFrames(inputModel, consideredJoints, outputModel,
+                                                 removedJointPositions, newLink_H_oldLink, addOriginalLinkFrameWith_original_frame_suffix);
+
+    if (okReduced)
+    {
+        // In general createReducedModelAndChangeLinkFrames do not preserve the existance of the base link,
+        // but as in this case the link existence is preserved, we also preserve the default base link
+        outputModel.setDefaultBaseLink(outputModel.getLinkIndex(inputModel.getLinkName(inputModel.getDefaultBaseLink())));
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
 }

src/tests/integration/CMakeLists.txt

@@ -42,6 +42,7 @@ endmacro()
 add_integration_test(Dynamics)
 add_integration_test(DenavitHartenberg)
 add_integration_test(ReducedModelWithFT)
+add_integration_test(ModelTransformers)
 
 # See issue https://github.com/robotology/idyntree/issues/367
 add_integration_test_no_valgrind(iCubTorqueEstimation)