Model-graph - Add spline joint to model graph

Author: MegMll

include/pinocchio/parsers/graph/graph-visitor.hpp

@@ -129,6 +129,13 @@ namespace pinocchio
             std::invalid_argument,
             "Graph - Joint Mimic cannot have a q_ref. Please use the joint offset directly.");
         }
+
+        SE3 operator()(const JointSpline &) const
+        {
+          PINOCCHIO_THROW_PRETTY(
+            std::invalid_argument,
+            "Graph - Joint Spline cannot have a q_ref. Please use the joint offset directly.");
+        }
       };
 
       /// Return qref transformation.

include/pinocchio/parsers/graph/joints.hpp

@@ -229,6 +229,43 @@ namespace pinocchio
       }
     };
 
+    struct JointSpline
+    {
+      std::vector<SE3> ctrlFrames;
+      int degree = 3;
+
+      static constexpr int nq = 1;
+      static constexpr int nv = 1;
+
+      JointSpline() = default;
+      JointSpline(const int degree)
+      : degree(degree)
+      {
+      }
+
+      JointSpline(const SE3 & ctrlFrame, const int degree = 3)
+      : degree(degree)
+      {
+        ctrlFrames.push_back(ctrlFrame);
+      }
+
+      JointSpline(const std::vector<SE3> & ctrlFrames, const int degree = 3)
+      : degree(degree)
+      , ctrlFrames(ctrlFrames)
+      {
+      }
+
+      void addCtrlFrame(const SE3 & ctrlFrame)
+      {
+        ctrlFrames.push_back(ctrlFrame);
+      }
+
+      bool operator==(const JointSpline & other) const
+      {
+        return degree == other.degree && ctrlFrames == other.ctrlFrames;
+      }
+    };
+
     // Forward declare
     struct JointComposite;
     struct JointMimic;
@@ -245,6 +282,7 @@ namespace pinocchio
       JointPlanar,
       JointHelical,
       JointUniversal,
+      JointSpline,
       boost::recursive_wrapper<JointComposite>,
       boost::recursive_wrapper<JointMimic>>
       JointVariant;

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

@@ -117,6 +117,9 @@ namespace pinocchio
         // Joint Universal
         typedef typename JointCollectionDefault::JointModelUniversal JointModelUniversal;
 
+        // JointSpline
+        typedef typename JointCollectionDefault::JointModelSpline JointModelSpline;
+
         typedef JointModel ReturnType;
 
         ReturnType operator()(const JointFixed & /*joint*/) const
@@ -229,6 +232,17 @@ namespace pinocchio
         {
           return boost::apply_visitor(*this, joint.secondary_joint);
         }
+        ReturnType operator()(const JointSpline & joint) const
+        {
+          JointModelSpline jmodel(joint.degree);
+
+          for (size_t i = 0; i < joint.ctrlFrames.size(); i++)
+          {
+            jmodel.addControlFrame(joint.ctrlFrames[i]);
+          }
+          jmodel.buildJoint();
+          return jmodel;
+        }
         ReturnType operator()(const JointComposite & joint) const
         {
           JointModelComposite jmodel;

src/parsers/graph/model-graph.cpp

@@ -85,6 +85,10 @@ namespace pinocchio
         {
           return {joint, SE3::Identity()};
         }
+        ReturnType operator()(const JointSpline & joint) const
+        {
+          return {joint, SE3::Identity()};
+        }
         ReturnType operator()(const JointComposite & joint) const
         {
           JointComposite jReturn;