refactor(model-graph-algo): AddJointBetweenBodies

precommit

Author: ipuch

examples/CMakeLists.txt

@@ -97,8 +97,7 @@ if(BUILD_PYTHON_INTERFACE)
       inverse-dynamics-derivatives
       model-graph
       model-configuration-converter
-      ellipsoid-joint-kinematics
-      )
+      ellipsoid-joint-kinematics)
 
   if(BUILD_WITH_URDF_SUPPORT)
     list(

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

@@ -112,7 +112,6 @@ namespace pinocchio
 
   }; // struct JointDataEllipsoidTpl
 
-
   PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelEllipsoidTpl);
   /// \brief Ellipsoid joint - constrains motion to ellipsoid surface with 3-DOF.
   ///
@@ -193,8 +192,13 @@ namespace pinocchio
     /// @param[in] c2, s2 Cosine and sine of q[2]
     /// @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
+      const Scalar & c0,
+      const Scalar & s0,
+      const Scalar & c1,
+      const Scalar & s1,
+      const Scalar & c2,
+      const Scalar & s2,
+      JointDataDerived & data) const
     {
       // clang-format off
       data.M.rotation() << c1 * c2                , -c1 * s2                , s1      ,

include/pinocchio/multibody/joint/joint-free-flyer.hpp

@@ -256,7 +256,6 @@ namespace pinocchio
 
   }; // struct JointDataFreeFlyerTpl
 
-  
   PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl);
   /// @brief Free-flyer joint in \f$SE(3)\f$.
   ///

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

@@ -267,17 +267,9 @@ namespace pinocchio
         {
         }
 
-        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_)
         {
@@ -350,21 +358,11 @@ namespace pinocchio
           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)