6#ifndef __pinocchio_python_spatial_inertia_hpp__
7#define __pinocchio_python_spatial_inertia_hpp__
9#include <eigenpy/exception.hpp>
10#include <eigenpy/eigenpy.hpp>
11#include <eigenpy/memory.hpp>
12#include <boost/python/tuple.hpp>
14#include "pinocchio/spatial/inertia.hpp"
16#include "pinocchio/bindings/python/utils/cast.hpp"
17#include "pinocchio/bindings/python/utils/copyable.hpp"
18#include "pinocchio/bindings/python/utils/printable.hpp"
20#if EIGENPY_VERSION_AT_MOST(2, 8, 1)
21EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Inertia)
30 namespace bp = boost::python;
32 template<
typename Inertia>
33 struct InertiaPythonVisitor :
public boost::python::def_visitor<InertiaPythonVisitor<Inertia>>
37 Options = Inertia::Options
39 typedef typename Inertia::Scalar Scalar;
40 typedef typename Inertia::Vector3 Vector3;
41 typedef typename Inertia::Matrix3 Matrix3;
42 typedef typename Inertia::Vector6 Vector6;
43 typedef typename Inertia::Matrix6 Matrix6;
45 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
46 typedef MotionTpl<Scalar, Options> Motion;
47 typedef ForceTpl<Scalar, Options> Force;
50 template<
class PyClass>
51 void visit(PyClass & cl)
const
53 static const Scalar dummy_precision = Eigen::NumTraits<Scalar>::dummy_precision();
57 &InertiaPythonVisitor::makeFromMCI, bp::default_call_policies(),
58 bp::args(
"mass",
"lever",
"inertia")),
59 "Initialize from mass, lever and 3d inertia.")
61 .def(bp::init<>(bp::arg(
"self"),
"Default constructor."))
62 .def(bp::init<const Inertia &>((bp::arg(
"self"), bp::arg(
"clone")),
"Copy constructor"))
65 "mass", &InertiaPythonVisitor::getMass, &InertiaPythonVisitor::setMass,
66 "Mass of the Spatial Inertia.")
70 (
typename Inertia::Vector3 & (Inertia::*)()) & Inertia::lever,
71 bp::return_internal_reference<>()),
72 &InertiaPythonVisitor::setLever,
73 "Center of mass location of the Spatial Inertia. It corresponds to the location of the "
74 "center of mass regarding to the frame where the Spatial Inertia is expressed.")
76 "inertia", &InertiaPythonVisitor::getInertia, &InertiaPythonVisitor::setInertia,
77 "Rotational part of the Spatial Inertia, i.e. a symmetric matrix "
78 "representing the rotational inertia around the center of mass.")
80 .def(
"matrix", (Matrix6(Inertia::*)() const)&Inertia::matrix, bp::arg(
"self"))
81 .def(
"inverse", (Matrix6(Inertia::*)() const)&Inertia::inverse, bp::arg(
"self"))
83 "se3Action", &Inertia::template se3Action<Scalar, Options>, bp::args(
"self",
"M"),
84 "Returns the result of the action of M on *this.")
86 "se3ActionInverse", &Inertia::template se3ActionInverse<Scalar, Options>,
87 bp::args(
"self",
"M"),
"Returns the result of the action of the inverse of M on *this.")
90 "setIdentity", &Inertia::setIdentity, bp::arg(
"self"),
91 "Set *this to be the Identity inertia.")
93 "setZero", &Inertia::setZero, bp::arg(
"self"),
94 "Set all the components of *this to zero.")
96 "setRandom", &Inertia::setRandom, bp::arg(
"self"),
97 "Set all the components of *this to random values.")
99 .def(bp::self + bp::self)
100 .def(bp::self += bp::self)
101 .def(bp::self - bp::self)
102 .def(bp::self -= bp::self)
103 .def(bp::self * bp::other<Motion>())
105 .add_property(
"np", (Matrix6(Inertia::*)() const)&Inertia::matrix)
107 "vxiv", &Inertia::template vxiv<Motion>, bp::args(
"self",
"v"),
108 "Returns the result of v x Iv.")
110 "vtiv", &Inertia::template vtiv<Motion>, bp::args(
"self",
"v"),
111 "Returns the result of v.T * Iv.")
114 (Matrix6(Inertia::*)(const MotionDense<Motion> &) const)&Inertia::template vxi<Motion>,
115 bp::args(
"self",
"v"),
"Returns the result of v x* I, a 6x6 matrix.")
118 (Matrix6(Inertia::*)(const MotionDense<Motion> &) const)&Inertia::template ivx<Motion>,
119 bp::args(
"self",
"v"),
"Returns the result of I vx, a 6x6 matrix.")
122 (Matrix6(Inertia::*)(const MotionDense<Motion> &)
123 const)&Inertia::template variation<Motion>,
124 bp::args(
"self",
"v"),
"Returns the time derivative of the inertia.")
126#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
127 .def(bp::self == bp::self)
128 .def(bp::self != bp::self)
131#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
133 "isApprox", &Inertia::isApprox,
134 (bp::arg(
"self"), bp::arg(
"other"), bp::arg(
"prec") = dummy_precision),
135 "Returns true if *this is approximately equal to other, within the precision given "
139 "isZero", &Inertia::isZero, (bp::arg(
"self"), bp::arg(
"prec") = dummy_precision),
140 "Returns true if *this is approximately equal to the zero Inertia, within the "
141 "precision given by prec.")
144 .def(
"Identity", &Inertia::Identity,
"Returns the identity Inertia.")
145 .staticmethod(
"Identity")
146 .def(
"Zero", &Inertia::Zero,
"Returns the zero Inertia.")
147 .staticmethod(
"Zero")
148 .def(
"Random", &Inertia::Random,
"Returns a random Inertia.")
149 .staticmethod(
"Random")
152 "toDynamicParameters", &InertiaPythonVisitor::toDynamicParameters_proxy,
154 "Returns the representation of the matrix as a vector of dynamic parameters."
155 "\nThe parameters are given as v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, "
156 "I_{xz}, I_{yz}, I_{zz}]^T "
157 "where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter")
159 "FromDynamicParameters", &InertiaPythonVisitor::fromDynamicParameters_proxy,
160 bp::args(
"dynamic_parameters"),
161 "Builds and inertia matrix from a vector of dynamic parameters."
162 "\nThe parameters are given as dynamic_parameters = [m, mc_x, mc_y, mc_z, I_{xx}, "
163 "I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T "
164 "where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter.")
165 .staticmethod(
"FromDynamicParameters")
168 "FromSphere", &Inertia::FromSphere, bp::args(
"mass",
"radius"),
169 "Returns the Inertia of a sphere defined by a given mass and radius.")
170 .staticmethod(
"FromSphere")
172 "FromEllipsoid", &Inertia::FromEllipsoid,
173 bp::args(
"mass",
"length_x",
"length_y",
"length_z"),
174 "Returns the Inertia of an ellipsoid shape defined by a mass and given dimensions "
175 "the semi-axis of values length_{x,y,z}.")
176 .staticmethod(
"FromEllipsoid")
178 "FromCylinder", &Inertia::FromCylinder, bp::args(
"mass",
"radius",
"length"),
179 "Returns the Inertia of a cylinder defined by its mass, radius and length along the "
181 .staticmethod(
"FromCylinder")
183 "FromBox", &Inertia::FromBox, bp::args(
"mass",
"length_x",
"length_y",
"length_z"),
184 "Returns the Inertia of a box shape with a mass and of dimension the semi axis of "
186 .staticmethod(
"FromBox")
188 "FromCapsule", &Inertia::FromCapsule, bp::args(
"mass",
"radius",
"height"),
189 "Computes the Inertia of a capsule defined by its mass, radius and length along the "
190 "Z axis. Assumes a uniform density.")
191 .staticmethod(
"FromCapsule")
194 "FromPseudoInertia", &Inertia::FromPseudoInertia, bp::args(
"pseudo_inertia"),
195 "Returns the Inertia created from a pseudo inertia object.")
196 .staticmethod(
"FromPseudoInertia")
199 "toPseudoInertia", &Inertia::toPseudoInertia, bp::arg(
"self"),
200 "Returns the pseudo inertia representation of the inertia.")
203 "FromLogCholeskyParameters", &Inertia::FromLogCholeskyParameters,
204 bp::args(
"log_cholesky_parameters"),
205 "Returns the Inertia created from log Cholesky parameters.")
206 .staticmethod(
"FromLogCholeskyParameters")
208 .def(
"__array__", (Matrix6(Inertia::*)() const)&Inertia::matrix)
210 "__array__", &__array__,
211 (bp::arg(
"self"), bp::arg(
"dtype") = bp::object(), bp::arg(
"copy") = bp::object()))
212#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
213 .def_pickle(Pickle())
218 static Scalar getMass(
const Inertia & self)
222 static void setMass(Inertia & self, Scalar mass)
227 static void setLever(Inertia & self,
const Vector3 & lever)
229 self.lever() = lever;
232 static Matrix3 getInertia(
const Inertia & self)
234 return self.inertia().matrix();
238 static void setInertia(Inertia & self,
const Matrix3 & symmetric_inertia)
240 if (!check_expression_if_real<Scalar>(
241 isZero(symmetric_inertia - symmetric_inertia.transpose())))
242 throw eigenpy::Exception(
"The 3d inertia should be symmetric.");
243 self.inertia().data() << symmetric_inertia(0, 0), symmetric_inertia(1, 0),
244 symmetric_inertia(1, 1), symmetric_inertia(0, 2), symmetric_inertia(1, 2),
245 symmetric_inertia(2, 2);
248 static VectorXs toDynamicParameters_proxy(
const Inertia & self)
250 return self.toDynamicParameters();
253 static Inertia fromDynamicParameters_proxy(
const VectorXs & params)
255 if (params.rows() != 10 || params.cols() != 1)
257 std::ostringstream shape;
258 shape <<
"(" << params.rows() <<
", " << params.cols() <<
")";
259 throw std::invalid_argument(
260 "Wrong size: params" + shape.str() +
" but should have the following shape (10, 1)");
262 return Inertia::FromDynamicParameters(params);
266 makeFromMCI(
const Scalar & mass,
const Vector3 & lever,
const Matrix3 & inertia)
268#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
269 if (!inertia.isApprox(inertia.transpose()))
270 throw eigenpy::Exception(
"The 3d inertia should be symmetric.");
272 (Vector3::UnitX().transpose() * inertia * Vector3::UnitX() < 0)
273 || (Vector3::UnitY().transpose() * inertia * Vector3::UnitY() < 0)
274 || (Vector3::UnitZ().transpose() * inertia * Vector3::UnitZ() < 0))
275 throw eigenpy::Exception(
"The 3d inertia should be positive.");
277 return new Inertia(mass, lever, inertia);
282#if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2, 9, 0)
283 typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(Inertia) HolderType;
285 typedef ::boost::python::detail::not_specified HolderType;
287 bp::class_<Inertia, HolderType>(
289 "This class represenses a sparse version of a Spatial Inertia and its is defined by its "
290 "mass, its center of mass location and the rotational inertia expressed around this "
291 "center of mass.\n\n"
292 "Supported operations ...",
294 .def(InertiaPythonVisitor<Inertia>())
295 .def(CastVisitor<Inertia>())
296 .def(ExposeConstructorByCastVisitor<Inertia, ::pinocchio::Inertia>())
297 .def(CopyableVisitor<Inertia>())
298 .def(PrintableVisitor<Inertia>());
302 static Matrix6 __array__(
const Inertia & self, bp::object, bp::object)
304 return self.matrix();
307 struct Pickle : bp::pickle_suite
309 static boost::python::tuple getinitargs(
const Inertia & I)
311 return bp::make_tuple(I.mass(), (Vector3)I.lever(), I.inertia().matrix());
314 static bool getstate_manages_dict()
322 template<
typename PseudoInertia>
323 struct PseudoInertiaPythonVisitor
324 :
public boost::python::def_visitor<PseudoInertiaPythonVisitor<PseudoInertia>>
328 Options = PseudoInertia::Options
330 typedef typename PseudoInertia::Scalar Scalar;
331 typedef typename PseudoInertia::Vector3 Vector3;
332 typedef typename PseudoInertia::Matrix3 Matrix3;
333 typedef typename PseudoInertia::Vector10 Vector10;
334 typedef typename PseudoInertia::Matrix4 Matrix4;
335 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
338 template<
class PyClass>
339 void visit(PyClass & cl)
const
341 cl.def(bp::init<const Scalar &, const Vector3 &, const Matrix3 &>(
342 (bp::arg(
"self"), bp::arg(
"mass"), bp::arg(
"h"), bp::arg(
"sigma")),
343 "Initialize from mass, vector part of the pseudo inertia and matrix part of the "
345 .def(bp::init<const PseudoInertia &>(
346 (bp::arg(
"self"), bp::arg(
"clone")),
"Copy constructor"))
348 "mass", &PseudoInertiaPythonVisitor::getMass, &PseudoInertiaPythonVisitor::setMass,
349 "Mass of the Pseudo Inertia.")
351 "h", &PseudoInertiaPythonVisitor::getH, &PseudoInertiaPythonVisitor::setH,
352 "Vector part of the Pseudo Inertia.")
354 "sigma", &PseudoInertiaPythonVisitor::getSigma, &PseudoInertiaPythonVisitor::setSigma,
355 "Matrix part of the Pseudo Inertia.")
358 "toMatrix", &PseudoInertia::toMatrix, bp::arg(
"self"),
359 "Returns the pseudo inertia as a 4x4 matrix.")
361 "toDynamicParameters", &PseudoInertiaPythonVisitor::toDynamicParameters_proxy,
362 bp::arg(
"self"),
"Returns the dynamic parameters representation.")
364 "toInertia", &PseudoInertia::toInertia, bp::arg(
"self"),
365 "Returns the inertia representation.")
367 "FromDynamicParameters", &PseudoInertiaPythonVisitor::fromDynamicParameters_proxy,
368 bp::args(
"dynamic_parameters"),
369 "Builds a pseudo inertia matrix from a vector of dynamic parameters."
370 "\nThe parameters are given as dynamic_parameters = [m, h_x, h_y, h_z, I_{xx}, "
371 "I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T.")
372 .staticmethod(
"FromDynamicParameters")
375 "FromMatrix", &PseudoInertia::FromMatrix, bp::args(
"pseudo_inertia_matrix"),
376 "Returns the Pseudo Inertia from a 4x4 matrix.")
377 .staticmethod(
"FromMatrix")
380 "FromInertia", &PseudoInertia::FromInertia, bp::args(
"inertia"),
381 "Returns the Pseudo Inertia from an Inertia object.")
382 .staticmethod(
"FromInertia")
384 .def(
"__array__", &PseudoInertia::toMatrix)
386 "__array__", &__array__,
387 (bp::arg(
"self"), bp::arg(
"dtype") = bp::object(), bp::arg(
"copy") = bp::object()))
388#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
389 .def_pickle(Pickle())
394 static Scalar getMass(
const PseudoInertia & self)
398 static void setMass(PseudoInertia & self, Scalar mass)
403 static Vector3 getH(
const PseudoInertia & self)
407 static void setH(PseudoInertia & self,
const Vector3 & h)
412 static Matrix3 getSigma(
const PseudoInertia & self)
416 static void setSigma(PseudoInertia & self,
const Matrix3 & sigma)
421 static VectorXs toDynamicParameters_proxy(
const PseudoInertia & self)
423 return self.toDynamicParameters();
426 static PseudoInertia fromDynamicParameters_proxy(
const VectorXs & params)
428 if (params.rows() != 10 || params.cols() != 1)
430 std::ostringstream shape;
431 shape <<
"(" << params.rows() <<
", " << params.cols() <<
")";
432 throw std::invalid_argument(
433 "Wrong size: params" + shape.str() +
" but should have the following shape (10, 1)");
435 return PseudoInertia::FromDynamicParameters(params);
440#if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2, 9, 0)
441 typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(PseudoInertia) HolderType;
443 typedef ::boost::python::detail::not_specified HolderType;
445 bp::class_<PseudoInertia, HolderType>(
447 "This class represents a pseudo inertia matrix and it is defined by its mass, vector "
448 "part, and 3x3 matrix part.\n\n"
449 "Supported operations ...",
451 .def(PseudoInertiaPythonVisitor<PseudoInertia>())
452 .def(CopyableVisitor<PseudoInertia>())
453 .def(PrintableVisitor<PseudoInertia>())
454 .def(CastVisitor<PseudoInertia>())
455 .def(ExposeConstructorByCastVisitor<PseudoInertia, ::pinocchio::PseudoInertia>());
459 static Matrix4 __array__(
const PseudoInertia & self, bp::object, bp::object)
461 return self.toMatrix();
464 struct Pickle : bp::pickle_suite
466 static boost::python::tuple getinitargs(
const PseudoInertia & pi)
468 return bp::make_tuple(pi.mass, pi.h, pi.sigma);
471 static bool getstate_manages_dict()
479 template<
typename LogCholeskyParameters>
480 struct LogCholeskyParametersPythonVisitor
481 :
public boost::python::def_visitor<LogCholeskyParametersPythonVisitor<LogCholeskyParameters>>
485 Options = LogCholeskyParameters::Options
487 typedef typename LogCholeskyParameters::Scalar Scalar;
488 typedef typename LogCholeskyParameters::Vector10 Vector10;
489 typedef typename LogCholeskyParameters::Matrix10 Matrix10;
490 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
491 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
494 template<
class PyClass>
495 void visit(PyClass & cl)
const
499 bp::make_constructor(
500 &LogCholeskyParametersPythonVisitor::makeFromParameters, bp::default_call_policies(),
501 bp::args(
"log_cholesky_parameters")),
502 "Initialize from log cholesky parameters.")
503 .def(bp::init<const LogCholeskyParameters &>(
504 (bp::arg(
"self"), bp::arg(
"clone")),
"Copy constructor"))
507 "parameters", &LogCholeskyParametersPythonVisitor::getParameters,
508 &LogCholeskyParametersPythonVisitor::setParameters,
"Log Cholesky parameters.")
511 "toDynamicParameters", &LogCholeskyParametersPythonVisitor::toDynamicParameters_proxy,
512 bp::arg(
"self"),
"Returns the dynamic parameters representation.")
514 "toPseudoInertia", &LogCholeskyParameters::toPseudoInertia, bp::arg(
"self"),
515 "Returns the Pseudo Inertia representation.")
517 "toInertia", &LogCholeskyParameters::toInertia, bp::arg(
"self"),
518 "Returns the Inertia representation.")
520 "calculateJacobian", &LogCholeskyParametersPythonVisitor::calculateJacobian_proxy,
521 bp::arg(
"self"),
"Calculates the Jacobian of the log Cholesky parameters.")
523 .def(
"__array__", &LogCholeskyParametersPythonVisitor::getParameters)
525 "__array__", &__array__,
526 (bp::arg(
"self"), bp::arg(
"dtype") = bp::object(), bp::arg(
"copy") = bp::object()))
527#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
528 .def_pickle(Pickle())
533 static LogCholeskyParameters * makeFromParameters(
const VectorXs & params)
535 if (params.rows() != 10 || params.cols() != 1)
537 std::ostringstream shape;
538 shape <<
"(" << params.rows() <<
", " << params.cols() <<
")";
539 throw std::invalid_argument(
540 "Wrong size: params" + shape.str() +
" but should have the following shape (10, 1)");
542 return new LogCholeskyParameters(params);
545 static VectorXs getParameters(
const LogCholeskyParameters & self)
547 return self.parameters;
549 static void setParameters(LogCholeskyParameters & self,
const Vector10 & parameters)
551 self.parameters = parameters;
554 static VectorXs toDynamicParameters_proxy(
const LogCholeskyParameters & self)
556 return self.toDynamicParameters();
559 static MatrixXs calculateJacobian_proxy(
const LogCholeskyParameters & self)
561 return self.calculateJacobian();
566#if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2, 9, 0)
567 typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(LogCholeskyParameters) HolderType;
569 typedef ::boost::python::detail::not_specified HolderType;
571 bp::class_<LogCholeskyParameters, HolderType>(
572 "LogCholeskyParameters",
573 "This class represents log Cholesky parameters.\n\n"
574 "Supported operations ...",
576 .def(LogCholeskyParametersPythonVisitor<LogCholeskyParameters>())
577 .def(CopyableVisitor<LogCholeskyParameters>())
578 .def(PrintableVisitor<LogCholeskyParameters>())
579 .def(CastVisitor<LogCholeskyParameters>())
580 .def(ExposeConstructorByCastVisitor<
585 static VectorXs __array__(
const LogCholeskyParameters & self, bp::object, bp::object)
587 return self.parameters;
590 struct Pickle : bp::pickle_suite
592 static boost::python::tuple getinitargs(
const LogCholeskyParameters & lcp)
594 return bp::make_tuple(lcp.parameters);
597 static bool getstate_manages_dict()
Main pinocchio namespace.
A structure representing log Cholesky parameters.
A structure representing a pseudo inertia matrix.