diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1ce621fc31..e3fb22218b 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,6 +1,7 @@ ci: autoupdate_branch: devel autofix_prs: false + autoupdate_schedule: quarterly repos: - repo: https://github.com/pre-commit/mirrors-clang-format rev: v18.1.8 diff --git a/CHANGELOG.md b/CHANGELOG.md index 5216097f66..ac675cff9f 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/include/pinocchio/bindings/python/multibody/geometry-object.hpp b/include/pinocchio/bindings/python/multibody/geometry-object.hpp index 4396ad9e65..3b4e78e37b 100644 --- a/include/pinocchio/bindings/python/multibody/geometry-object.hpp +++ b/include/pinocchio/bindings/python/multibody/geometry-object.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #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 - { - static result_type convert(GeometryMaterial const & gm) - { - return apply_visitor(GeometryMaterialValueToObject(), gm); - } - - template - 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 - { - static result_type convert(GeometryMaterial const & gm) - { - return apply_visitor(GeometryMaterialRefToObject(), gm); - } - - template - 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 - 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::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 { @@ -107,6 +38,9 @@ namespace pinocchio template void visit(PyClass & cl) const { + typedef eigenpy::VariantConverter 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(); - - /// Define material conversion from python object to C++ object - bp::implicitly_convertible(); - bp::implicitly_convertible(); - if (!register_symbolic_link_to_registered_type()) { bp::enum_("GeometryType") diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp index 0f7b15e765..c6253ade15 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp @@ -26,6 +26,43 @@ namespace pinocchio return cl; } + // specialization for JointModelRevolute + template<> + bp::class_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def(bp::init<>( + bp::args("self"), "Init JointModelRX with the X axis ([1, 0, 0]) as rotation axis.")) + .def( + "getMotionAxis", &context::JointModelRX::getMotionAxis, + "Rotation axis of the JointModelRX."); + } + + template<> + bp::class_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def(bp::init<>( + bp::args("self"), "Init JointModelRY with the Y axis ([0, 1, 0]) as rotation axis.")) + .def( + "getMotionAxis", &context::JointModelRY::getMotionAxis, + "Rotation axis of the JointModelRY."); + } + + template<> + bp::class_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def(bp::init<>( + bp::args("self"), "Init JointModelRZ with the Z axis ([0, 0, 1]) as rotation axis")) + .def( + "getMotionAxis", &context::JointModelRZ::getMotionAxis, + "Rotation axis of the JointModelRZ."); + } + // specialization for JointModelRevoluteUnaligned template<> bp::class_ & @@ -44,6 +81,80 @@ namespace pinocchio "Rotation axis of the JointModelRevoluteUnaligned."); } + // specialization for JointModelRevoluteUnbounded + template<> + bp::class_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def(bp::init<>( + bp::args("self"), "Init JointModelRUBX with the X axis ([1, 0, 0]) as rotation axis")) + .def( + "getMotionAxis", &context::JointModelRUBX::getMotionAxis, + "Rotation axis of the JointModelRUBX."); + } + + template<> + bp::class_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def(bp::init<>( + bp::args("self"), "Init JointModelRUBY with the Y axis ([0, 1, 0]) as rotation axis")) + .def( + "getMotionAxis", &context::JointModelRUBY::getMotionAxis, + "Rotation axis of the JointModelRUBY."); + } + + template<> + bp::class_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def(bp::init<>( + bp::args("self"), "Init JointModelRUBZ with the Z axis ([0, 0, 1]) as rotation axis")) + .def( + "getMotionAxis", &context::JointModelRUBZ::getMotionAxis, + "Rotation axis of the JointModelRUBZ."); + } + + // specialization for JointModelPrismatic + template<> + bp::class_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def(bp::init<>( + bp::args("self"), "Init JointModelPX with the X axis ([1, 0, 0]) as rotation axis")) + .def( + "getMotionAxis", &context::JointModelPX::getMotionAxis, + "Rotation axis of the JointModelPX."); + } + + template<> + bp::class_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def(bp::init<>( + bp::args("self"), "Init JointModelPY with the Y axis ([0, 1, 0]) as rotation axis")) + .def( + "getMotionAxis", &context::JointModelPY::getMotionAxis, + "Rotation axis of the JointModelPY."); + } + + template<> + bp::class_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def(bp::init<>( + bp::args("self"), "Init JointModelPZ with the Z axis ([0, 0, 1]) as rotation axis")) + .def( + "getMotionAxis", &context::JointModelPZ::getMotionAxis, + "Rotation axis of the JointModelPZ."); + } + // specialization for JointModelPrismaticUnaligned template<> bp::class_ & @@ -90,8 +201,14 @@ namespace pinocchio { return cl .def(bp::init( - bp::args("self", "pitch"), "Init JointModelHX with pitch value")) - .def(bp::init<>(bp::args("self"), "Init JointModelHX with pitch 0.0")) + bp::args("self", "pitch"), + "Init JointModelHX with pitch value and the X axis ([1, 0, 0]) as a rotation axis.")) + .def(bp::init<>( + bp::args("self"), + "Init JointModelHX with pitch 0.0 and the X axis ([1, 0, 0]) as a rotation axis.")) + .def( + "getMotionAxis", &context::JointModelHX::getMotionAxis, + "Rotation axis of the JointModelHX.") .def_readwrite("pitch", &context::JointModelHX::m_pitch, "Pitch h of the JointModelHX."); } @@ -101,8 +218,14 @@ namespace pinocchio { return cl .def(bp::init( - bp::args("self", "pitch"), "Init JointModelHY with pitch value.")) - .def(bp::init<>(bp::args("self"), "Init JointModelHY with pitch 0.0")) + bp::args("self", "pitch"), + "Init JointModelHY with pitch value and the Y axis ([0, 1, 0]) as a rotation axis.")) + .def(bp::init<>( + bp::args("self"), + "Init JointModelHY with pitch 0.0 and the Y axis ([0, 1, 0]) as a rotation axis.")) + .def( + "getMotionAxis", &context::JointModelHY::getMotionAxis, + "Rotation axis of the JointModelHY.") .def_readwrite("pitch", &context::JointModelHY::m_pitch, "Pitch h of the JointModelHY."); } @@ -112,8 +235,14 @@ namespace pinocchio { return cl .def(bp::init( - bp::args("self", "pitch"), "Init JointModelHZ with pitch value")) - .def(bp::init<>(bp::args("self"), "Init JointModelHZ with pitch 0.0")) + bp::args("self", "pitch"), + "Init JointModelHZ with pitch value and the Z axis ([0, 0, 1]) as a rotation axis.")) + .def(bp::init<>( + bp::args("self"), + "Init JointModelHZ with pitch 0.0 and the Z axis ([0, 0, 1]) as a rotation axis.")) + .def( + "getMotionAxis", &context::JointModelHZ::getMotionAxis, + "Rotation axis of the JointModelHZ.") .def_readwrite("pitch", &context::JointModelHZ::m_pitch, "Pitch h of the JointModelHZ."); } diff --git a/include/pinocchio/multibody/joint/joint-helical.hpp b/include/pinocchio/multibody/joint/joint-helical.hpp index 0923579795..dc92b2fb44 100644 --- a/include/pinocchio/multibody/joint/joint-helical.hpp +++ b/include/pinocchio/multibody/joint/joint-helical.hpp @@ -839,6 +839,8 @@ namespace pinocchio using Base::idx_v; using Base::setIndexes; + typedef Eigen::Matrix 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 JointModelHelicalTpl cast() const diff --git a/include/pinocchio/multibody/joint/joint-prismatic.hpp b/include/pinocchio/multibody/joint/joint-prismatic.hpp index 46522a8f7c..2a8233cbfc 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic.hpp @@ -675,6 +675,8 @@ namespace pinocchio using Base::idx_v; using Base::setIndexes; + typedef Eigen::Matrix 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 JointModelPrismaticTpl cast() const diff --git a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp index f9e4f10cad..fb635e4319 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp @@ -129,6 +129,8 @@ namespace pinocchio using Base::idx_v; using Base::setIndexes; + typedef Eigen::Matrix 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 JointModelRevoluteUnboundedTpl cast() const diff --git a/include/pinocchio/multibody/joint/joint-revolute.hpp b/include/pinocchio/multibody/joint/joint-revolute.hpp index 1a77dd170a..9eaeeebe62 100644 --- a/include/pinocchio/multibody/joint/joint-revolute.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute.hpp @@ -763,6 +763,8 @@ namespace pinocchio using Base::idx_v; using Base::setIndexes; + typedef Eigen::Matrix 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 JointModelRevoluteTpl cast() const