Merge branch 'devel' into patch-1

Author: jcarpent

.github/workflows/macos-linux-windows-pixi.yml

@@ -93,7 +93,7 @@ jobs:
           key: ccache-macos-linux-conda-${{ matrix.os }}-${{ matrix.build_type }}-${{ matrix.environment }}_${{ github.sha }}
           restore-keys: ccache-macos-linux-conda-${{ matrix.os }}-${{ matrix.build_type }}-${{ matrix.environment }}_
 
-      - uses: prefix-dev/[email protected].1
+      - uses: prefix-dev/[email protected].2
         with:
           cache: true
           environments: ${{ matrix.environment }}

.github/workflows/update_pixi_lockfile.yml

@@ -27,7 +27,7 @@ jobs:
           persist-credentials: false
 
       - name: Set up pixi
-        uses: prefix-dev/[email protected].1
+        uses: prefix-dev/[email protected].2
         with:
           run-install: false
 

CHANGELOG.md

@@ -10,10 +10,16 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+### Changed
 - Homogenize python viewers examples ([#2771](https://github.com/stack-of-tasks/pinocchio/pull/2771))
+- Rename hpp-fcl dependency to coal ([#2789](https://github.com/stack-of-tasks/pinocchio/pull/2794))
+- 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))
 - Add docker images ([#2776](https://github.com/stack-of-tasks/pinocchio/pull/2776))
+- Python : add overload of `BaseVisualizer::play()` to `VisualizerPythonVisitor` ([#2796](https://github.com/stack-of-tasks/pinocchio/pull/2796))
 - ROS: added jrl_cmakemodules dependency ([#2789](https://github.com/stack-of-tasks/pinocchio/pull/2789))
-- Rename hpp-fcl dependency to coal ([#2789](https://github.com/stack-of-tasks/pinocchio/pull/2794))
 
 ## [3.8.0] - 2025-09-17
 

CMakeLists.txt

@@ -86,22 +86,10 @@ include("${JRL_CMAKE_MODULES}/python.cmake")
 include("${JRL_CMAKE_MODULES}/boost.cmake")
 include("${JRL_CMAKE_MODULES}/ide.cmake")
 include("${JRL_CMAKE_MODULES}/apple.cmake")
-if(APPLE) # Use the handmade approach
-  if(${CMAKE_VERSION} VERSION_LESS "3.18.0") # Need to find the right version
-    set(CMAKE_MODULE_PATH ${JRL_CMAKE_MODULES}/find-external/OpenMP ${CMAKE_MODULE_PATH})
-  endif()
-elseif(UNIX)
-  if(${CMAKE_VERSION} VERSION_EQUAL "3.20.0")
-    set(CMAKE_MODULE_PATH ${JRL_CMAKE_MODULES}/find-external/OpenMP ${CMAKE_MODULE_PATH})
-  endif()
-endif()
 include(CMakeDependentOption)
 
 # If needed, set CMake policy for APPLE systems
 apply_default_apple_configuration()
-if(CMAKE_VERSION VERSION_GREATER "3.12")
-  cmake_policy(SET CMP0074 NEW)
-endif()
 
 # Force C++ standard to be C++11 at least
 check_minimal_cxx_standard(11 ENFORCE)

flake.nix

@@ -32,6 +32,7 @@
             default = self'.packages.pinocchio;
             pinocchio = pkgs.python3Packages.pinocchio.overrideAttrs (super: {
               propagatedBuildInputs = super.propagatedBuildInputs ++ [ pkgs.example-robot-data ];
+              nativeCheckInputs = [ pkgs.python3Packages.pybind11 ];
               src = pkgs.lib.fileset.toSource {
                 root = ./.;
                 fileset = pkgs.lib.fileset.unions [
@@ -78,6 +79,7 @@
                 pkgs.example-robot-data
                 self'.packages.libpinocchio
               ];
+              nativeCheckInputs = [ pkgs.python3Packages.pybind11 ];
               src = pkgs.lib.fileset.toSource {
                 root = ./.;
                 fileset = pkgs.lib.fileset.unions [

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/bindings/python/visualizers/visualizer-visitor.hpp

@@ -8,6 +8,7 @@
 #include "pinocchio/bindings/python/fwd.hpp"
 
 #include <eigenpy/optional.hpp>
+#include <eigenpy/std-vector.hpp>
 
 namespace pinocchio
 {
@@ -19,7 +20,6 @@ namespace pinocchio
     struct VisualizerPythonVisitor : bp::def_visitor<VisualizerPythonVisitor<Visualizer>>
     {
       typedef ::pinocchio::visualizers::BaseVisualizer Base;
-      typedef ::pinocchio::visualizers::ConstMatrixRef ConstMatrixRef;
       static_assert(
         std::is_base_of<Base, Visualizer>::value,
         "Visualizer class must be derived from pinocchio::visualizers::BaseVisualizer.");
@@ -34,7 +34,20 @@ namespace pinocchio
         vis.setCameraPose(pose);
       }
 
-      static void play_proxy2(Visualizer & vis, const ConstMatrixRef & qs, context::Scalar dt)
+      static void play_proxy(
+        Visualizer & vis, const std::vector<visualizers::VectorXs> & qs, context::Scalar dt)
+      {
+        std::vector<visualizers::ConstVectorRef> qs_;
+        qs_.reserve(qs.size());
+        for (size_t i = 0; i < qs.size(); i++)
+        {
+          qs_.emplace_back(qs[i]);
+        }
+        vis.play(qs_, dt);
+      }
+
+      static void
+      play_proxy2(Visualizer & vis, const visualizers::ConstMatrixRef & qs, context::Scalar dt)
       {
         vis.play(qs, dt);
       }
@@ -58,6 +71,7 @@ namespace pinocchio
           .def(
             "display", +[](Visualizer & v, const ConstVectorRef & q) { v.display(q); },
             (bp::arg("self"), bp::arg("q") = boost::none))
+          .def("play", play_proxy, (bp::arg("self"), "qs", "dt"))
           .def("play", play_proxy2, (bp::arg("self"), "qs", "dt"))
           .def("setCameraTarget", &Visualizer::setCameraTarget, (bp::arg("self"), "target"))
           .def("setCameraPosition", &Visualizer::setCameraPosition, (bp::arg("self"), "position"))

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()