refactor(model-graph-algo): AddJointBetweenBodies

Author: ipuch

include/pinocchio/multibody/joint/joint-ellipsoid.hpp

@@ -194,7 +194,6 @@ namespace pinocchio
     /// @param[out] data Joint data where M will be stored
     void computeSpatialTransform(
       const Scalar& c0, const Scalar& s0, const Scalar& c1, const Scalar& s1, const Scalar& c2, const Scalar& s2, JointDataDerived & data) const
-      const
     {
       // clang-format off
       data.M.rotation() << c1 * c2                , -c1 * s2                , s1      ,

src/parsers/graph/model-graph-algo.cpp

@@ -266,18 +266,10 @@ namespace pinocchio
         , model(model_)
         {
         }
-
-        template<typename JointGraph, typename FrameGraph>
-        void operator()(const JointGraph & /*joint*/, const FrameGraph & /*f_*/)
-        {
-          PINOCCHIO_THROW_PRETTY(
-            std::invalid_argument,
-            "Graph - Invalid joint between non body frames. Non body frames can "
-            "only be added with Fixed joint");
-        }
-
+        
+        // Default implementation for adding a joint between two body frames
         template<typename JointGraph>
-        void operator()(const JointGraph & joint, const BodyFrame & b_f)
+        void addJointBetweenBodies(const JointGraph & joint, const BodyFrame & b_f)
         {
           if (boost::get<BodyFrame>(&source_vertex.frame) == nullptr)
           {
@@ -295,14 +287,30 @@ namespace pinocchio
             edge.jlimit.friction, edge.jlimit.damping);
 
           model.addJointFrame(j_id);
-          model.appendBodyToJoint(j_id, b_f.inertia); // check this
+          model.appendBodyToJoint(j_id, b_f.inertia);
           model.addBodyFrame(target_vertex.name, j_id, body_pose);
 
           // armature
           model.armature.segment(model.joints[j_id].idx_v(), model.joints[j_id].nv()) =
             edge.jlimit.armature;
         }
 
+        template<typename JointGraph, typename FrameGraph>
+        void operator()(const JointGraph & /*joint*/, const FrameGraph & /*f_*/)
+        {
+          PINOCCHIO_THROW_PRETTY(
+            std::invalid_argument,
+            "Graph - Invalid joint between non body frames. Non body frames can "
+            "only be added with Fixed joint");
+        }
+
+        template<typename JointGraph>
+        void operator()(const JointGraph & joint, const BodyFrame & b_f)
+        {
+          // Call default implementation
+          addJointBetweenBodies(joint, b_f);
+        }
+
         template<typename FrameGraph>
         void operator()(const JointFixed & joint, const FrameGraph & f_)
         {
@@ -344,27 +352,17 @@ namespace pinocchio
           model.appendBodyToJoint(j_id, b_f.inertia); // check this
           model.addBodyFrame(target_vertex.name, j_id, body_pose);
         }
-
+        
         void operator()(const JointEllipsoid & joint, const BodyFrame & b_f)
         {
           if (!edge.forward)
             PINOCCHIO_THROW_PRETTY(
               std::invalid_argument, "Graph - JointEllipsoid cannot be reversed yet.");
+              // The ellipsoid joint cannot be reversed because of the way the motion subspace is defined.
+              // The motion subspace is defined in the parent frame, 
+              // So reversing the joint would require to change the motion subspace accordingly.
 
-          if (boost::get<BodyFrame>(&source_vertex.frame) == nullptr)
-            PINOCCHIO_THROW_PRETTY(
-              std::invalid_argument, "Graph - Invalid joint between a body and a non body frame.");
-
-          const SE3 & joint_pose = edge.source_to_joint;
-          const SE3 & body_pose = edge.joint_to_target;
-
-          const Frame previous_body = model.frames[model.getFrameId(source_vertex.name, BODY)];
-          JointIndex j_id = model.addJoint(
-            previous_body.parentJoint, cjm(joint), previous_body.placement * joint_pose, edge.name);
-
-          model.addJointFrame(j_id);
-          model.appendBodyToJoint(j_id, b_f.inertia); // check this
-          model.addBodyFrame(target_vertex.name, j_id, body_pose);
+          addJointBetweenBodies(joint, b_f);
         }
 
         void operator()(const JointFixed & joint, const BodyFrame & b_f)