Merge pull request #2786 from MegMll/topic/add_names_to_composite

Add names for joints inside a composite joint

Author: jorisv

CHANGELOG.md

@@ -15,6 +15,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - ROS: added jrl_cmakemodules dependency ([#2789](https://github.com/stack-of-tasks/pinocchio/pull/2789))
 - Removed CMake < 3.22 details ([#2790](https://github.com/stack-of-tasks/pinocchio/pull/2790))
 
+### Added
+- Add names to joints that are inside a composite joint ([#2786](https://github.com/stack-of-tasks/pinocchio/pull/2786))
+
 ## [3.8.0] - 2025-09-17
 
 ### Added

include/pinocchio/bindings/python/multibody/joint/joints-models.hpp

@@ -276,57 +276,76 @@ namespace pinocchio
     {
       context::JointModelComposite & m_joint_composite;
       const context::SE3 & m_joint_placement;
+      const std::string & m_name;
 
       JointModelCompositeAddJointVisitor(
-        context::JointModelComposite & joint_composite, const context::SE3 & joint_placement)
+        context::JointModelComposite & joint_composite,
+        const context::SE3 & joint_placement,
+        const std::string & name)
       : m_joint_composite(joint_composite)
       , m_joint_placement(joint_placement)
+      , m_name(name)
       {
       }
 
       template<typename JointModelDerived>
       context::JointModelComposite & operator()(JointModelDerived & jmodel) const
       {
-        return m_joint_composite.addJoint(jmodel, m_joint_placement);
+        return m_joint_composite.addJoint(jmodel, m_joint_placement, m_name);
       }
     }; // struct JointModelCompositeAddJointVisitor
 
     static context::JointModelComposite & addJoint_proxy(
       context::JointModelComposite & joint_composite,
       const context::JointModel & jmodel,
-      const context::SE3 & joint_placement = context::SE3::Identity())
+      const context::SE3 & joint_placement = context::SE3::Identity(),
+      const std::string & name = "")
     {
       return boost::apply_visitor(
-        JointModelCompositeAddJointVisitor(joint_composite, joint_placement), jmodel.toVariant());
+        JointModelCompositeAddJointVisitor(joint_composite, joint_placement, name),
+        jmodel.toVariant());
     }
 
     struct JointModelCompositeConstructorVisitor
     : public boost::static_visitor<context::JointModelComposite *>
     {
       const context::SE3 & m_joint_placement;
+      const std::string & m_name;
 
-      JointModelCompositeConstructorVisitor(const context::SE3 & joint_placement)
+      JointModelCompositeConstructorVisitor(
+        const context::SE3 & joint_placement, const std::string & name)
       : m_joint_placement(joint_placement)
+      , m_name(name)
       {
       }
 
       template<typename JointModelDerived>
       context::JointModelComposite * operator()(JointModelDerived & jmodel) const
       {
-        return new context::JointModelComposite(jmodel, m_joint_placement);
+        return new context::JointModelComposite(jmodel, m_joint_placement, m_name);
       }
     }; // struct JointModelCompositeConstructorVisitor
 
     static context::JointModelComposite * init_proxy1(const context::JointModel & jmodel)
     {
       return boost::apply_visitor(
-        JointModelCompositeConstructorVisitor(context::SE3::Identity()), jmodel);
+        JointModelCompositeConstructorVisitor(context::SE3::Identity(), "joint_1"), jmodel);
     }
 
     static context::JointModelComposite *
     init_proxy2(const context::JointModel & jmodel, const context::SE3 & joint_placement)
     {
-      return boost::apply_visitor(JointModelCompositeConstructorVisitor(joint_placement), jmodel);
+      return boost::apply_visitor(
+        JointModelCompositeConstructorVisitor(joint_placement, "joint_1"), jmodel);
+    }
+
+    static context::JointModelComposite * init_proxy3(
+      const context::JointModel & jmodel,
+      const context::SE3 & joint_placement,
+      const std::string & name)
+    {
+      return boost::apply_visitor(
+        JointModelCompositeConstructorVisitor(joint_placement, name), jmodel);
     }
 
     template<>
@@ -345,14 +364,23 @@ namespace pinocchio
           bp::make_constructor(
             init_proxy2, bp::default_call_policies(), bp::args("joint_model", "joint_placement")),
           "Init JointModelComposite from a joint and a placement")
+        .def(
+          "__init__",
+          bp::make_constructor(
+            init_proxy3, bp::default_call_policies(),
+            bp::args("joint_model", "joint_placement", "name")),
+          "Init JointModelComposite from a joint, a placement and a name")
         .add_property("joints", &context::JointModelComposite::joints)
         .add_property("jointPlacements", &context::JointModelComposite::jointPlacements)
         .add_property("njoints", &context::JointModelComposite::njoints)
         .def(
           "addJoint", &addJoint_proxy,
           (bp::arg("self"), bp::arg("joint_model"),
-           bp::arg("joint_placement") = context::SE3::Identity()),
+           bp::arg("joint_placement") = context::SE3::Identity(), bp::arg("name") = "joint_1"),
           "Add a joint to the vector of joints.", bp::return_internal_reference<>())
+        .def(
+          "getJointId", &context::JointModelComposite::getJointId, bp::args("joint_name"),
+          "Find the index of a joint inside a joint composite based on its name.")
 
 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
         .def(bp::self == bp::self)

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

@@ -205,6 +205,7 @@ namespace pinocchio
     , m_nv(0)
     , m_nvExtended(0)
     , njoints(0)
+    , jointNames()
     {
     }
 
@@ -216,6 +217,7 @@ namespace pinocchio
     , m_nv(0)
     , m_nvExtended(0)
     , njoints(0)
+    , jointNames()
     {
       joints.reserve(size);
       jointPlacements.reserve(size);
@@ -225,17 +227,21 @@ namespace pinocchio
       m_nqs.reserve(size);
       m_nvs.reserve(size);
       m_nvExtendeds.reserve(size);
+      jointNames.reserve(size);
     }
 
     ///
     /// \brief Constructor with one joint.
     ///
     /// \param jmodel Model of the first joint.
     /// \param placement Placement of the first joint w.r.t. the joint origin.
+    /// \param name Name of the joint inside the composite
     ///
     template<typename JointModel>
     JointModelCompositeTpl(
-      const JointModelBase<JointModel> & jmodel, const SE3 & placement = SE3::Identity())
+      const JointModelBase<JointModel> & jmodel,
+      const SE3 & placement = SE3::Identity(),
+      const std::string & name = "joint_1")
     : joints(1, (JointModelVariant)jmodel.derived())
     , jointPlacements(1, placement)
     , m_nq(jmodel.nq())
@@ -248,6 +254,7 @@ namespace pinocchio
     , m_idx_vExtended(1, 0)
     , m_nvExtendeds(1, jmodel.nvExtended())
     , njoints(1)
+    , jointNames(1, name)
     {
     }
 
@@ -270,6 +277,7 @@ namespace pinocchio
     , m_idx_vExtended(other.m_idx_vExtended)
     , m_nvExtendeds(other.m_nvExtendeds)
     , njoints(other.njoints)
+    , jointNames(other.jointNames)
     {
     }
 
@@ -278,12 +286,15 @@ namespace pinocchio
     ///
     /// \param jmodel Model of the joint to add.
     /// \param placement Placement of the joint relatively to its predecessor.
+    /// \param name name of the joint inside the composite
     ///
     /// \return A reference to *this
     ///
     template<typename JointModel>
-    JointModelDerived &
-    addJoint(const JointModelBase<JointModel> & jmodel, const SE3 & placement = SE3::Identity())
+    JointModelDerived & addJoint(
+      const JointModelBase<JointModel> & jmodel,
+      const SE3 & placement = SE3::Identity(),
+      const std::string & name = "")
     {
       joints.push_back((JointModelVariant)jmodel.derived());
       jointPlacements.push_back(placement);
@@ -295,6 +306,13 @@ namespace pinocchio
       updateJointIndexes();
       njoints++;
 
+      std::string final_name = name;
+      if (name.empty())
+      {
+        final_name = "joint_" + std::to_string(njoints + 1);
+      }
+      jointNames.push_back(final_name);
+
       return *this;
     }
 
@@ -414,6 +432,7 @@ namespace pinocchio
       joints = other.joints;
       jointPlacements = other.jointPlacements;
       njoints = other.njoints;
+      jointNames = other.jointNames;
 
       return *this;
     }
@@ -432,7 +451,8 @@ namespace pinocchio
              && internal::comparison_eq(m_nvExtendeds, other.m_nvExtendeds)
              && internal::comparison_eq(joints, other.joints)
              && internal::comparison_eq(jointPlacements, other.jointPlacements)
-             && internal::comparison_eq(njoints, other.njoints);
+             && internal::comparison_eq(njoints, other.njoints)
+             && internal::comparison_eq(jointNames, other.jointNames);
     }
 
     /// \returns An expression of *this with the Scalar type casted to NewScalar.
@@ -452,6 +472,7 @@ namespace pinocchio
       res.m_nvs = m_nvs;
       res.m_nvExtendeds = m_nvExtendeds;
       res.njoints = njoints;
+      res.jointNames = jointNames;
 
       res.joints.resize(joints.size());
       res.jointPlacements.resize(jointPlacements.size());
@@ -464,6 +485,16 @@ namespace pinocchio
       return res;
     }
 
+    JointIndex getJointId(const std::string & joint_name) const
+    {
+      auto it = std::find(jointNames.begin(), jointNames.end(), joint_name);
+
+      if (it == jointNames.end())
+        PINOCCHIO_THROW_PRETTY(std::invalid_argument, "JointComposite - joint_name not found");
+
+      return JointIndex(std::distance(jointNames.begin(), it));
+    }
+
     /// \brief Vector of joints contained in the joint composite.
     JointModelVector joints;
     /// \brief Vector of joint placements. Those placements correspond to the origin of the joint
@@ -688,6 +719,8 @@ namespace pinocchio
     std::vector<int> m_idx_vExtended;
     /// \brief Dimension of the segment in the jacobian matrix
     std::vector<int> m_nvExtendeds;
+    /// \brief Vector of names for each joint inside the composite
+    std::vector<std::string> jointNames;
 
   public:
     /// \brief Number of joints contained in the JointModelComposite

include/pinocchio/serialization/joints-model.hpp

@@ -29,6 +29,7 @@ namespace pinocchio
 
       ar & make_nvp("joints", joint.joints);
       ar & make_nvp("jointPlacements", joint.jointPlacements);
+      ar & make_nvp("jointNames", joint.jointNames);
     }
   };
 } // namespace pinocchio

unittest/joint-composite.cpp

@@ -344,4 +344,16 @@ BOOST_AUTO_TEST_CASE(test_kinematics)
   BOOST_CHECK(data.a.back().isApprox(data_c.a.back()));
 }
 
+BOOST_AUTO_TEST_CASE(checkJointNames)
+{
+  JointModelComposite jmodel_composite;
+  jmodel_composite.addJoint(JointModelRZ())
+    .addJoint(JointModelRY(), SE3::Random(), "penultinum_joint")
+    .addJoint(JointModelRX(), SE3::Random(), "last_joint");
+
+  BOOST_CHECK(jmodel_composite.getJointId("last_joint") == 2);
+  BOOST_CHECK(jmodel_composite.getJointId("penultinum_joint") == 1);
+  BOOST_CHECK_THROW(jmodel_composite.getJointId("wrong_name"), std::invalid_argument);
+}
+
 BOOST_AUTO_TEST_SUITE_END()