Changed Geometry Objects bindings and added getMotionAxis function

CHANGELOG.md

@@ -6,6 +6,12 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+### Added
+- Add getMotionAxis method to helical, prismatic, revolute and ubounded revolute joint. ([#2315](https://github.com/stack-of-tasks/pinocchio/pull/2315))
+
+### Changed
+- Use eigenpy to expose `GeometryObject::meshMaterial` variant ([#2315](https://github.com/stack-of-tasks/pinocchio/pull/2315))
+
 ## [3.1.0] - 2024-07-04
 
 ### Fixed

include/pinocchio/bindings/python/multibody/geometry-object.hpp

@@ -8,6 +8,7 @@
 #include <eigenpy/memory.hpp>
 #include <eigenpy/eigen-from-python.hpp>
 #include <eigenpy/eigen-to-python.hpp>
+#include <eigenpy/variant.hpp>
 
 #include "pinocchio/bindings/python/utils/address.hpp"
 #include "pinocchio/bindings/python/utils/copyable.hpp"
@@ -28,76 +29,6 @@ namespace pinocchio
   {
     namespace bp = boost::python;
 
-    namespace
-    {
-      /// Convert GeometryMaterial boost variant to a Python object.
-      /// This converter copy the GeometryMaterial content.
-      struct GeometryMaterialValueToObject : boost::static_visitor<PyObject *>
-      {
-        static result_type convert(GeometryMaterial const & gm)
-        {
-          return apply_visitor(GeometryMaterialValueToObject(), gm);
-        }
-
-        template<typename T>
-        result_type operator()(T & t) const
-        {
-          return bp::incref(bp::object(t).ptr());
-        }
-      };
-
-      /// Convert GeometryMaterial boost variant to a Python object.
-      /// This converter return the GeometryMaterial reference.
-      /// The code the create the reference holder is taken from \see
-      /// boost::python::to_python_indirect.
-      struct GeometryMaterialRefToObject : boost::static_visitor<PyObject *>
-      {
-        static result_type convert(GeometryMaterial const & gm)
-        {
-          return apply_visitor(GeometryMaterialRefToObject(), gm);
-        }
-
-        template<typename T>
-        result_type operator()(T & t) const
-        {
-          return bp::detail::make_reference_holder::execute(&t);
-        }
-      };
-
-      /// Converter used in \see ReturnInternalVariant.
-      /// This is inspired by \see boost::python::reference_existing_object.
-      ///  It will call GeometryMaterialRefToObject to extract the variant reference.
-      struct GeometryMaterialConverter
-      {
-        template<class T>
-        struct apply
-        {
-          struct type
-          {
-            inline PyObject * operator()(const GeometryMaterial & gm) const
-            {
-              return GeometryMaterialRefToObject::convert(gm);
-            }
-
-#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
-            inline PyTypeObject const * get_pytype() const
-            {
-              return bp::converter::registered_pytype<GeometryMaterial>::get_pytype();
-            }
-#endif
-          };
-        };
-      };
-
-      /// Variant of \see boost::python::return_internal_reference that
-      /// extract GeometryMaterial variant before converting it into a PyObject*
-      struct GeometryMaterialReturnInternalVariant : bp::return_internal_reference<>
-      {
-      public:
-        typedef GeometryMaterialConverter result_converter;
-      };
-    } // namespace
-
     struct GeometryObjectPythonVisitor
     : public boost::python::def_visitor<GeometryObjectPythonVisitor>
     {
@@ -107,6 +38,9 @@ namespace pinocchio
       template<class PyClass>
       void visit(PyClass & cl) const
       {
+        typedef eigenpy::VariantConverter<GeometryMaterial> Converter;
+        Converter::registration();
+
         cl.def(bp::init<
                  std::string, JointIndex, FrameIndex, const SE3 &, CollisionGeometryPtr,
                  bp::optional<
@@ -186,7 +120,7 @@ namespace pinocchio
             "Perform a deep copy of this. It will create a copy of the underlying FCL geometry.")
           .add_property(
             "meshMaterial",
-            bp::make_getter(&GeometryObject::meshMaterial, GeometryMaterialReturnInternalVariant()),
+            bp::make_getter(&GeometryObject::meshMaterial, Converter::return_internal_reference()),
             bp::make_setter(&GeometryObject::meshMaterial),
             "Material associated to the mesh (applied only if overrideMaterial is True)")
 
@@ -288,14 +222,6 @@ namespace pinocchio
               "meshShininess", &GeometryPhongMaterial::meshShininess,
               "Shininess associated to the specular lighting model (between 0 and 1)");
         }
-
-        /// Define material conversion from C++ variant to python object
-        bp::to_python_converter<GeometryMaterial, GeometryMaterialValueToObject>();
-
-        /// Define material conversion from python object to C++ object
-        bp::implicitly_convertible<GeometryNoMaterial, GeometryMaterial>();
-        bp::implicitly_convertible<GeometryPhongMaterial, GeometryMaterial>();
-
         if (!register_symbolic_link_to_registered_type<GeometryType>())
         {
           bp::enum_<GeometryType>("GeometryType")

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

@@ -26,6 +26,37 @@ namespace pinocchio
       return cl;
     }
 
+    // specialization for JointModelRevolute
+    template<>
+    bp::class_<context::JointModelRX> &
+    expose_joint_model<context::JointModelRX>(bp::class_<context::JointModelRX> & cl)
+    {
+      return cl.def(bp::init<>(bp::args("self"), "Init JointModelRX with x as rotation axis"))
+        .def(
+          "getMotionAxis", &context::JointModelRX::getMotionAxis,
+          "Rotation axis of the JointModelRX.");
+    }
+
+    template<>
+    bp::class_<context::JointModelRY> &
+    expose_joint_model<context::JointModelRY>(bp::class_<context::JointModelRY> & cl)
+    {
+      return cl.def(bp::init<>(bp::args("self"), "Init JointModelRY with y as rotation axis"))
+        .def(
+          "getMotionAxis", &context::JointModelRY::getMotionAxis,
+          "Rotation axis of the JointModelRY.");
+    }
+
+    template<>
+    bp::class_<context::JointModelRZ> &
+    expose_joint_model<context::JointModelRZ>(bp::class_<context::JointModelRZ> & cl)
+    {
+      return cl.def(bp::init<>(bp::args("self"), "Init JointModelRZ with z as rotation axis"))
+        .def(
+          "getMotionAxis", &context::JointModelRZ::getMotionAxis,
+          "Rotation axis of the JointModelRZ.");
+    }
+
     // specialization for JointModelRevoluteUnaligned
     template<>
     bp::class_<context::JointModelRevoluteUnaligned> &
@@ -44,6 +75,68 @@ namespace pinocchio
           "Rotation axis of the JointModelRevoluteUnaligned.");
     }
 
+    // specialization for JointModelRevoluteUnbounded
+    template<>
+    bp::class_<context::JointModelRUBX> &
+    expose_joint_model<context::JointModelRUBX>(bp::class_<context::JointModelRUBX> & cl)
+    {
+      return cl.def(bp::init<>(bp::args("self"), "Init JointModelRUBX with x as rotation axis"))
+        .def(
+          "getMotionAxis", &context::JointModelRUBX::getMotionAxis,
+          "Rotation axis of the JointModelRUBX.");
+    }
+
+    template<>
+    bp::class_<context::JointModelRUBY> &
+    expose_joint_model<context::JointModelRUBY>(bp::class_<context::JointModelRUBY> & cl)
+    {
+      return cl.def(bp::init<>(bp::args("self"), "Init JointModelRUBY with y as rotation axis"))
+        .def(
+          "getMotionAxis", &context::JointModelRUBY::getMotionAxis,
+          "Rotation axis of the JointModelRUBY.");
+    }
+
+    template<>
+    bp::class_<context::JointModelRUBZ> &
+    expose_joint_model<context::JointModelRUBZ>(bp::class_<context::JointModelRUBZ> & cl)
+    {
+      return cl.def(bp::init<>(bp::args("self"), "Init JointModelRUBZ with z as rotation axis"))
+        .def(
+          "getMotionAxis", &context::JointModelRUBZ::getMotionAxis,
+          "Rotation axis of the JointModelRUBZ.");
+    }
+
+    // specialization for JointModelPrismatic
+    template<>
+    bp::class_<context::JointModelPX> &
+    expose_joint_model<context::JointModelPX>(bp::class_<context::JointModelPX> & cl)
+    {
+      return cl.def(bp::init<>(bp::args("self"), "Init JointModelPX with x as rotation axis"))
+        .def(
+          "getMotionAxis", &context::JointModelPX::getMotionAxis,
+          "Rotation axis of the JointModelPX.");
+    }
+
+    template<>
+    bp::class_<context::JointModelPY> &
+    expose_joint_model<context::JointModelPY>(bp::class_<context::JointModelPY> & cl)
+    {
+      return cl.def(bp::init<>(bp::args("self"), "Init JointModelPY with y as rotation axis"))
+        .def(
+          "getMotionAxis", &context::JointModelPY::getMotionAxis,
+          "Rotation axis of the JointModelPY.");
+    }
+
+    template<>
+    bp::class_<context::JointModelPZ> &
+    expose_joint_model<context::JointModelPZ>(bp::class_<context::JointModelPZ> & cl)
+    {
+      return cl.def(bp::init<>(bp::args("self"), "Init JointModelPZ with z as rotation axis"))
+        .def(
+          "getMotionAxis", &context::JointModelPZ::getMotionAxis,
+          "Rotation axis of the JointModelPZ.");
+    }
+
     // specialization for JointModelPrismaticUnaligned
     template<>
     bp::class_<context::JointModelPrismaticUnaligned> &
@@ -92,6 +185,9 @@ namespace pinocchio
         .def(bp::init<context::Scalar>(
           bp::args("self", "pitch"), "Init JointModelHX with pitch value"))
         .def(bp::init<>(bp::args("self"), "Init JointModelHX with pitch 0.0"))
+        .def(
+          "getMotionAxis", &context::JointModelHX::getMotionAxis,
+          "Rotation axis of the JointModelHX.")
         .def_readwrite("pitch", &context::JointModelHX::m_pitch, "Pitch h of the JointModelHX.");
     }
 
@@ -103,6 +199,9 @@ namespace pinocchio
         .def(bp::init<context::Scalar>(
           bp::args("self", "pitch"), "Init JointModelHY with pitch value."))
         .def(bp::init<>(bp::args("self"), "Init JointModelHY with pitch 0.0"))
+        .def(
+          "getMotionAxis", &context::JointModelHY::getMotionAxis,
+          "Rotation axis of the JointModelHY.")
         .def_readwrite("pitch", &context::JointModelHY::m_pitch, "Pitch h of the JointModelHY.");
     }
 
@@ -114,6 +213,9 @@ namespace pinocchio
         .def(bp::init<context::Scalar>(
           bp::args("self", "pitch"), "Init JointModelHZ with pitch value"))
         .def(bp::init<>(bp::args("self"), "Init JointModelHZ with pitch 0.0"))
+        .def(
+          "getMotionAxis", &context::JointModelHZ::getMotionAxis,
+          "Rotation axis of the JointModelHZ.")
         .def_readwrite("pitch", &context::JointModelHZ::m_pitch, "Pitch h of the JointModelHZ.");
     }
 

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

@@ -839,6 +839,8 @@ namespace pinocchio
     using Base::idx_v;
     using Base::setIndexes;
 
+    typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
+
     JointDataDerived createData() const
     {
       return JointDataDerived();
@@ -923,6 +925,22 @@ namespace pinocchio
       return classname();
     }
 
+    Vector3 getMotionAxis() const
+    {
+      switch (axis)
+      {
+      case 0:
+        return Vector3::UnitX();
+      case 1:
+        return Vector3::UnitY();
+      case 2:
+        return Vector3::UnitZ();
+      default:
+        assert(false && "must never happen");
+        break;
+      }
+    }
+
     /// \returns An expression of *this with the Scalar type casted to NewScalar.
     template<typename NewScalar>
     JointModelHelicalTpl<NewScalar, Options, axis> cast() const

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

@@ -675,6 +675,8 @@ namespace pinocchio
     using Base::idx_v;
     using Base::setIndexes;
 
+    typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
+
     JointDataDerived createData() const
     {
       return JointDataDerived();
@@ -742,6 +744,22 @@ namespace pinocchio
       return classname();
     }
 
+    Vector3 getMotionAxis() const
+    {
+      switch (axis)
+      {
+      case 0:
+        return Vector3::UnitX();
+      case 1:
+        return Vector3::UnitY();
+      case 2:
+        return Vector3::UnitZ();
+      default:
+        assert(false && "must never happen");
+        break;
+      }
+    }
+
     /// \returns An expression of *this with the Scalar type casted to NewScalar.
     template<typename NewScalar>
     JointModelPrismaticTpl<NewScalar, Options, axis> cast() const

include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp

@@ -129,6 +129,8 @@ namespace pinocchio
     using Base::idx_v;
     using Base::setIndexes;
 
+    typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
+
     JointDataDerived createData() const
     {
       return JointDataDerived();
@@ -200,6 +202,22 @@ namespace pinocchio
       return classname();
     }
 
+    Vector3 getMotionAxis() const
+    {
+      switch (axis)
+      {
+      case 0:
+        return Vector3::UnitX();
+      case 1:
+        return Vector3::UnitY();
+      case 2:
+        return Vector3::UnitZ();
+      default:
+        assert(false && "must never happen");
+        break;
+      }
+    }
+
     /// \returns An expression of *this with the Scalar type casted to NewScalar.
     template<typename NewScalar>
     JointModelRevoluteUnboundedTpl<NewScalar, Options, axis> cast() const

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

@@ -763,6 +763,8 @@ namespace pinocchio
     using Base::idx_v;
     using Base::setIndexes;
 
+    typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
+
     JointDataDerived createData() const
     {
       return JointDataDerived();
@@ -838,6 +840,22 @@ namespace pinocchio
       return classname();
     }
 
+    Vector3 getMotionAxis() const
+    {
+      switch (axis)
+      {
+      case 0:
+        return Vector3::UnitX();
+      case 1:
+        return Vector3::UnitY();
+      case 2:
+        return Vector3::UnitZ();
+      default:
+        assert(false && "must never happen");
+        break;
+      }
+    }
+
     /// \returns An expression of *this with the Scalar type casted to NewScalar.
     template<typename NewScalar>
     JointModelRevoluteTpl<NewScalar, Options, axis> cast() const