GCC Code Coverage Report

Directory: ./
File: include/pinocchio/bindings/python/spatial/inertia.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 70 100 70.0%
Branches: 92 284 32.4%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2024 CNRS INRIA
3 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5
6 #ifndef __pinocchio_python_spatial_inertia_hpp__
7 #define __pinocchio_python_spatial_inertia_hpp__
8
9 #include <eigenpy/exception.hpp>
10 #include <eigenpy/eigenpy.hpp>
11 #include <eigenpy/memory.hpp>
12 #include <boost/python/tuple.hpp>
13
14 #include "pinocchio/spatial/inertia.hpp"
15
16 #include "pinocchio/bindings/python/utils/cast.hpp"
17 #include "pinocchio/bindings/python/utils/copyable.hpp"
18 #include "pinocchio/bindings/python/utils/printable.hpp"
19
20 #if EIGENPY_VERSION_AT_MOST(2, 8, 1)
21 EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Inertia)
22 #endif
23
24 namespace pinocchio
25 {
26 namespace python
27 {
28 namespace bp = boost::python;
29
30 template<typename Inertia>
31 struct InertiaPythonVisitor : public boost::python::def_visitor<InertiaPythonVisitor<Inertia>>
32 {
33 enum
34 {
35 Options = Inertia::Options
36 };
37 typedef typename Inertia::Scalar Scalar;
38 typedef typename Inertia::Vector3 Vector3;
39 typedef typename Inertia::Matrix3 Matrix3;
40 typedef typename Inertia::Vector6 Vector6;
41 typedef typename Inertia::Matrix6 Matrix6;
42
43 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
44 typedef MotionTpl<Scalar, Options> Motion;
45 typedef ForceTpl<Scalar, Options> Force;
46
47 public:
48 template<class PyClass>
49 20 void visit(PyClass & cl) const
50 {
51 static const Scalar dummy_precision = Eigen::NumTraits<Scalar>::dummy_precision();
52 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
53 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_SELF_ASSIGN_OVERLOADED
54
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 20 cl.def(
55 "__init__",
56 bp::make_constructor(
57 20 &InertiaPythonVisitor::makeFromMCI, bp::default_call_policies(),
58 bp::args("mass", "lever", "inertia")),
59 "Initialize from mass, lever and 3d inertia.")
60
61
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 40 .def(bp::init<>(bp::arg("self"), "Default constructor."))
62
5/10
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
 40 .def(bp::init<const Inertia &>((bp::arg("self"), bp::arg("clone")), "Copy constructor"))
63
64
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .add_property(
65 "mass", &InertiaPythonVisitor::getMass, &InertiaPythonVisitor::setMass,
66 "Mass of the Spatial Inertia.")
67
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 .add_property(
68 "lever",
69 bp::make_function(
70 (typename Inertia::Vector3 & (Inertia::*)()) & Inertia::lever,
71 20 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.")
75
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .add_property(
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.")
79
80
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 .def("matrix", (Matrix6(Inertia::*)() const) & Inertia::matrix, bp::arg("self"))
81
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def("inverse", (Matrix6(Inertia::*)() const) & Inertia::inverse, bp::arg("self"))
82
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
83 "se3Action", &Inertia::template se3Action<Scalar, Options>, bp::args("self", "M"),
84 "Returns the result of the action of M on *this.")
85
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
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.")
88
89
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
90
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 "setIdentity", &Inertia::setIdentity, bp::arg("self"),
91 "Set *this to be the Identity inertia.")
92
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
93
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 "setZero", &Inertia::setZero, bp::arg("self"),
94 "Set all the components of *this to zero.")
95
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
96
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 "setRandom", &Inertia::setRandom, bp::arg("self"),
97 "Set all the components of *this to random values.")
98
99
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 20 .def(bp::self + bp::self)
100
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self += bp::self)
101
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self - bp::self)
102
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self -= bp::self)
103
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self * bp::other<Motion>())
104
105 40 .add_property("np", (Matrix6(Inertia::*)() const) & Inertia::matrix)
106
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
107 "vxiv", &Inertia::template vxiv<Motion>, bp::args("self", "v"),
108 "Returns the result of v x Iv.")
109
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
110 "vtiv", &Inertia::template vtiv<Motion>, bp::args("self", "v"),
111 "Returns the result of v.T * Iv.")
112
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
113 "vxi",
114 (Matrix6(Inertia::*)(const MotionDense<Motion> &) const)
115 & Inertia::template vxi<Motion>,
116 bp::args("self", "v"), "Returns the result of v x* I, a 6x6 matrix.")
117
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
118 "ivx",
119 (Matrix6(Inertia::*)(const MotionDense<Motion> &) const)
120 & Inertia::template ivx<Motion>,
121 bp::args("self", "v"), "Returns the result of I vx, a 6x6 matrix.")
122
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 40 .def(
123 "variation",
124 (Matrix6(Inertia::*)(const MotionDense<Motion> &) const)
125 & Inertia::template variation<Motion>,
126 bp::args("self", "v"), "Returns the time derivative of the inertia.")
127
128 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
129
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self == bp::self)
130
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self != bp::self)
131 #endif
132
133 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
134
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
135 "isApprox", &Inertia::isApprox,
136
5/10
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
 60 (bp::arg("self"), bp::arg("other"), bp::arg("prec") = dummy_precision),
137 "Returns true if *this is approximately equal to other, within the precision given "
138 "by prec.")
139
140
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
141
4/8
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
 40 "isZero", &Inertia::isZero, (bp::arg("self"), bp::arg("prec") = dummy_precision),
142 "Returns true if *this is approximately equal to the zero Inertia, within the "
143 "precision given by prec.")
144 #endif
145
146
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 .def("Identity", &Inertia::Identity, "Returns the identity Inertia.")
147
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("Identity")
148
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("Zero", &Inertia::Zero, "Returns the zero Inertia.")
149
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("Zero")
150
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("Random", &Inertia::Random, "Returns a random Inertia.")
151
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("Random")
152
153
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
154 "toDynamicParameters", &InertiaPythonVisitor::toDynamicParameters_proxy,
155
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 bp::arg("self"),
156 "Returns the representation of the matrix as a vector of dynamic parameters."
157 "\nThe parameters are given as v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, "
158 "I_{xz}, I_{yz}, I_{zz}]^T "
159 "where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter")
160
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
161 "FromDynamicParameters", &Inertia::template FromDynamicParameters<VectorXs>,
162 bp::args("dynamic_parameters"),
163 "Builds and inertia matrix from a vector of dynamic parameters."
164 "\nThe parameters are given as dynamic_parameters = [m, mc_x, mc_y, mc_z, I_{xx}, "
165 "I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T "
166 "where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter.")
167
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("FromDynamicParameters")
168
169
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
170 "FromSphere", &Inertia::FromSphere, bp::args("mass", "radius"),
171 "Returns the Inertia of a sphere defined by a given mass and radius.")
172
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("FromSphere")
173
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
174 "FromEllipsoid", &Inertia::FromEllipsoid,
175 bp::args("mass", "length_x", "length_y", "length_z"),
176 "Returns the Inertia of an ellipsoid shape defined by a mass and given dimensions "
177 "the semi-axis of values length_{x,y,z}.")
178
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("FromEllipsoid")
179
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
180 "FromCylinder", &Inertia::FromCylinder, bp::args("mass", "radius", "length"),
181 "Returns the Inertia of a cylinder defined by its mass, radius and length along the "
182 "Z axis.")
183
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("FromCylinder")
184
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
185 "FromBox", &Inertia::FromBox, bp::args("mass", "length_x", "length_y", "length_z"),
186 "Returns the Inertia of a box shape with a mass and of dimension the semi axis of "
187 "length_{x,y,z}.")
188
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("FromBox")
189
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
190 "FromCapsule", &Inertia::FromCapsule, bp::args("mass", "radius", "height"),
191 "Computes the Inertia of a capsule defined by its mass, radius and length along the "
192 "Z axis. Assumes a uniform density.")
193
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("FromCapsule")
194
195 40 .def("__array__", (Matrix6(Inertia::*)() const) & Inertia::matrix)
196
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 .def("__array__", &__array__)
197 #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
198
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def_pickle(Pickle())
199 #endif
200 ;
201 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
202 20 }
203
204 ✗ static Scalar getMass(const Inertia & self)
205 {
206 ✗ return self.mass();
207 }
208 ✗ static void setMass(Inertia & self, Scalar mass)
209 {
210 ✗ self.mass() = mass;
211 }
212
213 ✗ static void setLever(Inertia & self, const Vector3 & lever)
214 {
215 ✗ self.lever() = lever;
216 }
217
218 ✗ static Matrix3 getInertia(const Inertia & self)
219 {
220 ✗ return self.inertia().matrix();
221 }
222 // static void setInertia(Inertia & self, const Vector6 & minimal_inertia) {
223 // self.inertia().data() = minimal_inertia; }
224 ✗ static void setInertia(Inertia & self, const Matrix3 & symmetric_inertia)
225 {
226 ✗ if (!check_expression_if_real<Scalar>(
227 ✗ isZero(symmetric_inertia - symmetric_inertia.transpose())))
228 ✗ throw eigenpy::Exception("The 3d inertia should be symmetric.");
229 ✗ self.inertia().data() << symmetric_inertia(0, 0), symmetric_inertia(1, 0),
230 ✗ symmetric_inertia(1, 1), symmetric_inertia(0, 2), symmetric_inertia(1, 2),
231 ✗ symmetric_inertia(2, 2);
232 }
233
234 ✗ static VectorXs toDynamicParameters_proxy(const Inertia & self)
235 {
236 ✗ return self.toDynamicParameters();
237 }
238
239 static Inertia *
240 ✗ makeFromMCI(const Scalar & mass, const Vector3 & lever, const Matrix3 & inertia)
241 {
242 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
243 ✗ if (!inertia.isApprox(inertia.transpose()))
244 ✗ throw eigenpy::Exception("The 3d inertia should be symmetric.");
245 ✗ if (
246 ✗ (Vector3::UnitX().transpose() * inertia * Vector3::UnitX() < 0)
247 ✗ || (Vector3::UnitY().transpose() * inertia * Vector3::UnitY() < 0)
248 ✗ || (Vector3::UnitZ().transpose() * inertia * Vector3::UnitZ() < 0))
249 ✗ throw eigenpy::Exception("The 3d inertia should be positive.");
250 #endif
251 ✗ return new Inertia(mass, lever, inertia);
252 }
253
254 20 static void expose()
255 {
256 #if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2, 9, 0)
257 typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(Inertia) HolderType;
258 #else
259 typedef ::boost::python::detail::not_specified HolderType;
260 #endif
261 20 bp::class_<Inertia, HolderType>(
262 "Inertia",
263 "This class represenses a sparse version of a Spatial Inertia and its is defined by its "
264 "mass, its center of mass location and the rotational inertia expressed around this "
265 "center of mass.\n\n"
266 "Supported operations ...",
267 bp::no_init)
268
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(InertiaPythonVisitor<Inertia>())
269
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(CastVisitor<Inertia>())
270
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(ExposeConstructorByCastVisitor<Inertia, ::pinocchio::Inertia>())
271
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(CopyableVisitor<Inertia>())
272
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(PrintableVisitor<Inertia>());
273 20 }
274
275 private:
276 ✗ static Matrix6 __array__(const Inertia & self, bp::object)
277 {
278 ✗ return self.matrix();
279 }
280
281 struct Pickle : bp::pickle_suite
282 {
283 ✗ static boost::python::tuple getinitargs(const Inertia & I)
284 {
285 ✗ return bp::make_tuple(I.mass(), (Vector3)I.lever(), I.inertia().matrix());
286 }
287
288 20 static bool getstate_manages_dict()
289 {
290 20 return true;
291 }
292 };
293
294 }; // struct InertiaPythonVisitor
295
296 } // namespace python
297 } // namespace pinocchio
298
299 #endif // ifndef __pinocchio_python_spatial_inertia_hpp__
300