Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Changed Geometry Objects bindings and added getMotionAxis function #2315

Merged
merged 8 commits into from
Jul 10, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .pre-commit-config.yaml
Original file line number Diff line number Diff line change
@@ -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
Expand Down
6 changes: 6 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
84 changes: 5 additions & 79 deletions include/pinocchio/bindings/python/multibody/geometry-object.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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>
{
Expand All @@ -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<
Expand Down Expand Up @@ -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)")

Expand Down Expand Up @@ -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")
Expand Down
141 changes: 135 additions & 6 deletions include/pinocchio/bindings/python/multibody/joint/joints-models.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,43 @@ 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 the X axis ([1, 0, 0]) 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 the Y axis ([0, 1, 0]) 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 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_<context::JointModelRevoluteUnaligned> &
Expand All @@ -44,6 +81,80 @@ 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 the X axis ([1, 0, 0]) 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 the Y axis ([0, 1, 0]) 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 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_<context::JointModelPX> &
expose_joint_model<context::JointModelPX>(bp::class_<context::JointModelPX> & 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_<context::JointModelPY> &
expose_joint_model<context::JointModelPY>(bp::class_<context::JointModelPY> & 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_<context::JointModelPZ> &
expose_joint_model<context::JointModelPZ>(bp::class_<context::JointModelPZ> & 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_<context::JointModelPrismaticUnaligned> &
Expand Down Expand Up @@ -90,8 +201,14 @@ namespace pinocchio
{
return cl
.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"))
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.");
}

Expand All @@ -101,8 +218,14 @@ namespace pinocchio
{
return cl
.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"))
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.");
}

Expand All @@ -112,8 +235,14 @@ namespace pinocchio
{
return cl
.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"))
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.");
}

Expand Down
18 changes: 18 additions & 0 deletions include/pinocchio/multibody/joint/joint-helical.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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
Expand Down
18 changes: 18 additions & 0 deletions include/pinocchio/multibody/joint/joint-prismatic.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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
Expand Down
Loading
Loading