se3.hpp

//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2018-2025 INRIA
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#pragma once

#include <eigenpy/eigenpy.hpp>
#include <eigenpy/memory.hpp>
#include <eigenpy/copyable.hpp>
#include <boost/python/tuple.hpp>

#include "pinocchio/spatial.hpp"

#include "pinocchio/utils/string.hpp"

#include "pinocchio/bindings/python/utils/cast.hpp"
#include "pinocchio/bindings/python/utils/printable.hpp"
#include "pinocchio/bindings/python/utils/namespace.hpp"

namespace pinocchio
{
 namespace python
 {
 namespace bp = boost::python;

 template<typename SE3>
 struct SE3PythonVisitor : public boost::python::def_visitor<SE3PythonVisitor<SE3>>
 {
 typedef typename SE3::Scalar Scalar;
 static constexpr int Options = SE3::Options;
 typedef typename SE3::Matrix3 Matrix3;
 typedef typename SE3::Vector3 Vector3;
 typedef typename SE3::Matrix4 Matrix4;
 typedef typename SE3::Quaternion Quaternion;
 typedef typename SE3::ActionMatrixType ActionMatrixType;

 typedef MotionTpl<Scalar, Options> Motion;
 typedef ForceTpl<Scalar, Options> Force;
 typedef InertiaTpl<Scalar, Options> Inertia;

 public:
 template<class PyClass>
 void visit(PyClass & cl) const
 {
 static const Scalar dummy_precision = Eigen::NumTraits<Scalar>::dummy_precision();

 cl.def(
 bp::init<const Matrix3 &, const Vector3 &>(
 (bp::arg("self"), bp::arg("rotation"), bp::arg("translation")),
 "Initialize from a rotation matrix and a translation vector."))
 .def(
 bp::init<const Quaternion &, const Vector3 &>(
 (bp::arg("self"), bp::arg("quat"), bp::arg("translation")),
 "Initialize from a quaternion and a translation vector."))
 .def(bp::init<int>((bp::arg("self"), bp::arg("int")), "Init to identity."))
 .def(
 bp::init<const Matrix4 &>(
 (bp::arg("self"), bp::arg("array")), "Initialize from an homogeneous matrix."))
 .def(bp::init<const SE3 &>((bp::arg("self"), bp::arg("clone")), "Copy constructor"))

 .add_property(
 "rotation",
 bp::make_function(
 (typename SE3::AngularRef (SE3::*)())&SE3::rotation,
 bp::return_internal_reference<>()),
 (void (SE3::*)(const Matrix3 &))&SE3::rotation,
 "The rotation part of the transformation.")
 .add_property(
 "translation",
 bp::make_function(
 (typename SE3::LinearRef (SE3::*)())&SE3::translation,
 bp::return_internal_reference<>()),
 (void (SE3::*)(const Vector3 &))&SE3::translation,
 "The translation part of the transformation.")

 .add_property(
 "homogeneous", &SE3::toHomogeneousMatrix,
 "Returns the equivalent homegeneous matrix (acting on SE3).")
 .add_property(
 "action", (ActionMatrixType (SE3::*)() const) & SE3::toActionMatrix,
 "Returns the related action matrix (acting on Motion).")
 .def(
 "toActionMatrix", (ActionMatrixType (SE3::*)() const) & SE3::toActionMatrix,
 bp::arg("self"), "Returns the related action matrix (acting on Motion).")
 .add_property(
 "actionInverse", (ActionMatrixType (SE3::*)() const) & SE3::toActionMatrixInverse,
 "Returns the inverse of the action matrix (acting on Motion).\n"
 "This is equivalent to do m.inverse().action")
 .def(
 "toActionMatrixInverse",
 (ActionMatrixType (SE3::*)() const) & SE3::toActionMatrixInverse, bp::arg("self"),
 "Returns the inverse of the action matrix (acting on Motion).\n"
 "This is equivalent to do m.inverse().toActionMatrix()")
 .add_property(
 "dualAction", (ActionMatrixType (SE3::*)() const) & SE3::toDualActionMatrix,
 "Returns the related dual action matrix (acting on Force).")
 .def(
 "toDualActionMatrix", (ActionMatrixType (SE3::*)() const) & SE3::toDualActionMatrix,
 bp::arg("self"), "Returns the related dual action matrix (acting on Force).")

 .def(
 "setIdentity", &SE3PythonVisitor::setIdentity, bp::arg("self"),
 "Set *this to the identity placement.")
 .def(
 "setRandom", &SE3PythonVisitor::setRandom, bp::arg("self"),
 "Set *this to a random placement.")

 .def("inverse", &SE3::inverse, bp::arg("self"), "Returns the inverse transform")

 .def(
 "act", (Vector3 (SE3::*)(const Vector3 &) const) & SE3::act, bp::args("self", "point"),
 "Returns a point which is the result of the entry point transforms by *this.")
 .def(
 "actInv", (Vector3 (SE3::*)(const Vector3 &) const) & SE3::actInv,
 bp::args("self", "point"),
 "Returns a point which is the result of the entry point by the inverse of *this.")

 .def(
 "act", (SE3 (SE3::*)(const SE3 & other) const) & SE3::act, bp::args("self", "M"),
 "Returns the result of *this * M.")
 .def(
 "actInv", (SE3 (SE3::*)(const SE3 & other) const) & SE3::actInv, bp::args("self", "M"),
 "Returns the result of the inverse of *this times M.")

 .def(
 "act", (Motion (SE3::*)(const Motion &) const) & SE3::act, bp::args("self", "motion"),
 "Returns the result action of *this onto a Motion.")
 .def(
 "actInv", (Motion (SE3::*)(const Motion &) const) & SE3::actInv,
 bp::args("self", "motion"), "Returns the result of the inverse of *this onto a Motion.")

 .def(
 "act", (Force (SE3::*)(const Force &) const) & SE3::act, bp::args("self", "force"),
 "Returns the result of *this onto a Force.")
 .def(
 "actInv", (Force (SE3::*)(const Force &) const) & SE3::actInv,
 bp::args("self", "force"),
 "Returns the result of the inverse of *this onto an Inertia.")

 .def(
 "act", (Inertia (SE3::*)(const Inertia &) const) & SE3::act,
 bp::args("self", "inertia"), "Returns the result of *this onto a Force.")
 .def(
 "actInv", (Inertia (SE3::*)(const Inertia &) const) & SE3::actInv,
 bp::args("self", "inertia"),
 "Returns the result of the inverse of *this onto an Inertia.")

#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
 .def(
 "isApprox", &SE3::isApprox,
 (bp::arg("self"), bp::arg("other"), bp::arg("prec") = dummy_precision),
 "Returns true if *this is approximately equal to other, within the precision given by "
 "prec.")

 .def(
 "isIdentity", &SE3::isIdentity, (bp::arg("self"), bp::arg("prec") = dummy_precision),
 "Returns true if *this is approximately equal to the identity placement, within the "
 "precision given by prec.")
#endif

 .def("__invert__", &SE3::inverse, "Returns the inverse of *this.")
 .def(bp::self * bp::self)
 .def("__mul__", &__mul__<Motion>)
 .def("__mul__", &__mul__<Force>)
 .def("__mul__", &__mul__<Inertia>)
 .def("__mul__", &__mul__<Vector3>)
 .add_property("np", &SE3::toHomogeneousMatrix)

#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
 .def(bp::self == bp::self)
 .def(bp::self != bp::self)
#endif

 .def("Identity", &SE3::Identity, "Returns the identity transformation.")
 .staticmethod("Identity")
 .def("Random", &SE3::Random, "Returns a random transformation.")
 .staticmethod("Random")
 .def(
 "Interpolate", &SE3::template Interpolate<Scalar>, bp::args("A", "B", "alpha"),
 "Linear interpolation on the SE3 manifold.\n\n"
 "This method computes the linear interpolation between A and B, such that the result C "
 "= A + (B-A)*t if it would be applied on classic Euclidian space.\n"
 "This operation is very similar to the SLERP operation on Rotations.\n"
 "Parameters:\n"
 "\tA: Initial transformation\n"
 "\tB: Target transformation\n"
 "\talpha: Interpolation factor")
 .staticmethod("Interpolate")

 .def("__array__", &SE3::toHomogeneousMatrix)
 .def(
 "__array__", &__array__,
 (bp::arg("self"), bp::arg("dtype") = bp::object(), bp::arg("copy") = bp::object()))

#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
 .def_pickle(Pickle())
#endif
 ;
 }

 static std::string scopeName()
 {
 static std::string scope_name;
 return scope_name;
 }

 static void expose()
 {
 typedef ::boost::python::detail::not_specified HolderType;
 bp::class_<SE3, HolderType>(
 "SE3", "SE3 transformation defined by a 3d vector and a rotation matrix.",
 bp::init<>(bp::arg("self"), "Default constructor."))
 .def(SE3PythonVisitor<SE3>())
 .def(CastVisitor<SE3>())
 .def(ExposeConstructorByCastVisitor<SE3, ::pinocchio::SE3>())
 .def(::eigenpy::CopyableVisitor<SE3>())
 .def(bp::self_ns::str(bp::self_ns::self))
 .def("__repr__", &repr);
 }

 private:
 static Matrix4 __array__(const SE3 & self, bp::object, bp::object)
 {
 return self.toHomogeneousMatrix();
 }

 struct Pickle : bp::pickle_suite
 {
 static boost::python::tuple getinitargs(const SE3 & M)
 {
 return bp::make_tuple((Matrix3)M.rotation(), (Vector3)M.translation());
 }

 static bool getstate_manages_dict()
 {
 return true;
 }
 };

 static void setIdentity(SE3 & self)
 {
 self.setIdentity();
 }
 static void setRandom(SE3 & self)
 {
 self.setRandom();
 }

 template<typename Spatial>
 static Spatial __mul__(const SE3 & self, const Spatial & other)
 {
 return self.act(other);
 }

 static std::string repr(const SE3 & self)
 {
 // bp::object
 // py_rotation(bp::handle<>(eigenpy::EigenToPy<Matrix3,Scalar>::convert(self.rotation())));
 // std::string rotation_repr =
 // bp::extract<std::string>(py_rotation.attr("__repr__")());
 //
 // bp::object
 // py_translation(bp::handle<>(eigenpy::EigenToPy<Vector3,Scalar>::convert(self.translation())));
 // std::string translation_repr =
 // bp::extract<std::string>(py_translation.attr("__repr__")());

 bp::object py_homogeneous(
 bp::handle<>(eigenpy::EigenToPy<Matrix4, Scalar>::convert(self.toHomogeneousMatrix())));
 std::string homegeneous_repr = bp::extract<std::string>(py_homogeneous.attr("__repr__")());
 replace(homegeneous_repr, "\n", "");
 replace(homegeneous_repr, " ", "");

 std::stringstream ss_repr;
 ss_repr << "SE3(";
 ss_repr << homegeneous_repr;
 ss_repr << ")";

 return ss_repr.str();
 }
 };

 } // namespace python
} // namespace pinocchio