GCC Code Coverage Report

Directory: ./
File: include/pinocchio/bindings/python/spatial/motion.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 90 96 93.8%
Branches: 102 211 48.3%
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_motion_hpp__
7 #define __pinocchio_python_spatial_motion_hpp__
8
9 #include <eigenpy/eigenpy.hpp>
10 #include <eigenpy/memory.hpp>
11 #include <boost/python/tuple.hpp>
12 #include <boost/python/implicit.hpp>
13
14 #include "pinocchio/spatial/se3.hpp"
15 #include "pinocchio/spatial/motion.hpp"
16 #include "pinocchio/spatial/force.hpp"
17
18 #include "pinocchio/bindings/python/utils/cast.hpp"
19 #include "pinocchio/bindings/python/utils/copyable.hpp"
20 #include "pinocchio/bindings/python/utils/printable.hpp"
21
22 #if EIGENPY_VERSION_AT_MOST(2, 8, 1)
23 EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Motion)
24 #endif
25
26 namespace pinocchio
27 {
28 namespace python
29 {
30 namespace bp = boost::python;
31
32 template<typename T>
33 struct call;
34
35 template<typename Scalar, int Options>
36 struct call<MotionTpl<Scalar, Options>>
37 {
38 typedef MotionTpl<Scalar, Options> Motion;
39
40 ✗ static bool isApprox(
41 const Motion & self,
42 const Motion & other,
43 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
44 {
45 ✗ return self.isApprox(other, prec);
46 }
47
48 static bool
49 ✗ isZero(const Motion & self, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
50 {
51 ✗ return self.isZero(prec);
52 }
53 };
54
55 template<typename Motion>
56 struct MotionPythonVisitor : public boost::python::def_visitor<MotionPythonVisitor<Motion>>
57 {
58 enum
59 {
60 Options = traits<Motion>::Options
61 };
62
63 typedef typename Motion::Scalar Scalar;
64 typedef ForceTpl<Scalar, Options> Force;
65 typedef typename Motion::Vector6 Vector6;
66 typedef typename Motion::Vector3 Vector3;
67
68 typedef typename Eigen::Map<Vector3> MapVector3;
69 typedef typename Eigen::Ref<Vector3> RefVector3;
70
71 public:
72 template<class PyClass>
73 69 void visit(PyClass & cl) const
74 {
75
3/8
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 4 static const Scalar dummy_precision = Eigen::NumTraits<Scalar>::dummy_precision();
76 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
77 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_SELF_ASSIGN_OVERLOADED
78
2/4
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
 69 cl.def(bp::init<>(bp::arg("self"), "Default constructor"))
79
4/8
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 69 times.
✗ Branch 11 not taken.
 138 .def(bp::init<const Vector3 &, const Vector3 &>(
80
3/6
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
 207 (bp::arg("self"), bp::arg("linear"), bp::arg("angular")),
81 "Initialize from linear and angular components of a Motion vector (don't mix the "
82 "order)."))
83
3/6
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
 69 .def(bp::init<const Vector6 &>(
84
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 (bp::arg("self"), bp::arg("array")),
85 "Init from a vector 6 [linear velocity, angular velocity]"))
86
5/10
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 69 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 69 times.
✗ Branch 14 not taken.
 138 .def(bp::init<const Motion &>((bp::arg("self"), bp::arg("clone")), "Copy constructor"))
87
88
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 69 .add_property(
89 "linear",
90 bp::make_function(
91 69 &MotionPythonVisitor::getLinear, bp::with_custodian_and_ward_postcall<0, 1>()),
92 &MotionPythonVisitor::setLinear,
93 "Linear part of a *this, corresponding to the linear velocity in case of a "
94 "Spatial velocity.")
95
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 69 .add_property(
96 "angular",
97 bp::make_function(
98 69 &MotionPythonVisitor::getAngular, bp::with_custodian_and_ward_postcall<0, 1>()),
99 &MotionPythonVisitor::setAngular,
100 "Angular part of a *this, corresponding to the angular velocity in case of "
101 "a Spatial velocity.")
102
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 69 .add_property(
103 "vector",
104 bp::make_function(
105 (typename Motion::ToVectorReturnType(Motion::*)()) & Motion::toVector,
106 69 bp::return_internal_reference<>()),
107 &MotionPythonVisitor::setVector, "Returns the components of *this as a 6d vector.")
108
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 69 .add_property(
109 "np", bp::make_function(
110 (typename Motion::ToVectorReturnType(Motion::*)()) & Motion::toVector,
111 69 bp::return_internal_reference<>()))
112
113
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 .def(
114 "se3Action", &Motion::template se3Action<Scalar, Options>, bp::args("self", "M"),
115 "Returns the result of the action of M on *this.")
116
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
117 "se3ActionInverse", &Motion::template se3ActionInverse<Scalar, Options>,
118 bp::args("self", "M"), "Returns the result of the action of the inverse of M on *this.")
119
120
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .add_property(
121 "action", &Motion::toActionMatrix,
122 "Returns the action matrix of *this (acting on Motion).")
123
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .add_property(
124 "dualAction", &Motion::toDualActionMatrix,
125 "Returns the dual action matrix of *this (acting on Force).")
126
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .add_property(
127 "homogeneous", &Motion::toHomogeneousMatrix,
128 "Equivalent homogeneous representation of the Motion vector")
129
130
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
131
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 "setZero", &MotionPythonVisitor::setZero, bp::arg("self"),
132 "Set the linear and angular components of *this to zero.")
133
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(
134
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 "setRandom", &MotionPythonVisitor::setRandom, bp::arg("self"),
135 "Set the linear and angular components of *this to random values.")
136
137
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 .def(
138 "dot", (Scalar(Motion::*)(const ForceBase<Force> &) const)&Motion::dot,
139 bp::args("self", "f"), "Dot product between *this and a Force f.")
140
141
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
142 "cross", (Motion(Motion::*)(const Motion &) const)&Motion::cross, bp::args("self", "m"),
143 "Action of *this onto another Motion m. Returns ¨*this x m.")
144
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
145 "cross", (Force(Motion::*)(const Force &) const)&Motion::cross, bp::args("self", "f"),
146 "Dual action of *this onto a Force f. Returns *this x* f.")
147
148
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
 69 .def(bp::self + bp::self)
149
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 69 .def(bp::self += bp::self)
150
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 69 .def(bp::self - bp::self)
151
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 69 .def(bp::self -= bp::self)
152
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 69 .def(-bp::self)
153
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 69 .def(bp::self ^ bp::self)
154
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
 73 .def(bp::self ^ Force())
155
156 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
157
1/2
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
 65 .def(bp::self == bp::self)
158
1/2
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
 65 .def(bp::self != bp::self)
159 #endif
160
161
3/5
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 73 .def(bp::self * Scalar())
162
3/5
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 73 .def(Scalar() * bp::self)
163
3/5
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 73 .def(bp::self / Scalar())
164
165 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
166
2/4
✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
 130 .def(
167 "isApprox", &call<Motion>::isApprox,
168
5/10
✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 65 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 65 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 65 times.
✗ Branch 14 not taken.
 195 (bp::arg("self"), bp::arg("other"), bp::arg("prec") = dummy_precision),
169 "Returns true if *this is approximately equal to other, within the precision given "
170 "by prec.")
171
172
1/2
✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
 65 .def(
173
4/8
✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 65 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 65 times.
✗ Branch 11 not taken.
 130 "isZero", &call<Motion>::isZero, (bp::arg("self"), bp::arg("prec") = dummy_precision),
174 "Returns true if *this is approximately equal to the zero Motion, within the "
175 "precision given by prec.")
176 #endif
177
178
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("Random", &Motion::Random, "Returns a random Motion.")
179
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .staticmethod("Random")
180
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("Zero", &Motion::Zero, "Returns a zero Motion.")
181
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .staticmethod("Zero")
182
183
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 69 .def(
184 "__array__", bp::make_function(
185 (typename Motion::ToVectorReturnType(Motion::*)()) & Motion::toVector,
186 69 bp::return_internal_reference<>()))
187
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(
188 "__array__", &__array__,
189
9/18
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 69 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 69 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 69 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 69 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 69 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 69 times.
✗ Branch 26 not taken.
 138 (bp::arg("self"), bp::arg("dtype") = bp::object(), bp::arg("copy") = bp::object()),
190 69 bp::return_internal_reference<>())
191 #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
192
1/2
✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
 65 .def_pickle(Pickle())
193 #endif
194 ;
195 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
196 69 }
197
198 69 static void expose()
199 {
200 typedef pinocchio::MotionBase<Motion> MotionBase;
201 69 bp::objects::register_dynamic_id<MotionBase>();
202 69 bp::objects::register_conversion<Motion, MotionBase>(false);
203
204 typedef pinocchio::MotionDense<Motion> MotionDense;
205 69 bp::objects::register_dynamic_id<MotionDense>();
206 69 bp::objects::register_conversion<Motion, MotionDense>(false);
207
208 #if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2, 9, 0)
209 typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(Motion) HolderType;
210 #else
211 typedef ::boost::python::detail::not_specified HolderType;
212 #endif
213 69 bp::class_<Motion, HolderType>(
214 "Motion",
215 "Motion vectors, in se3 == M^6.\n\n"
216 "Supported operations ...",
217 bp::no_init)
218
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(MotionPythonVisitor<Motion>())
219
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(CastVisitor<Motion>())
220
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(ExposeConstructorByCastVisitor<Motion, ::pinocchio::Motion>())
221
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(CopyableVisitor<Motion>())
222
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(PrintableVisitor<Motion>());
223 69 }
224
225 private:
226 static typename Motion::ToVectorConstReturnType
227 3 __array__(const Motion & self, bp::object, bp::object)
228 {
229 3 return self.toVector();
230 }
231
232 struct Pickle : bp::pickle_suite
233 {
234 ✗ static boost::python::tuple getinitargs(const Motion & m)
235 {
236 ✗ return bp::make_tuple((Vector3)m.linear(), (Vector3)m.angular());
237 }
238
239 65 static bool getstate_manages_dict()
240 {
241 65 return true;
242 }
243 };
244
245 7 static RefVector3 getLinear(Motion & self)
246 {
247
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 14 return self.linear();
248 }
249 1 static void setLinear(Motion & self, const Vector3 & v)
250 {
251 1 self.linear(v);
252 1 }
253 7 static RefVector3 getAngular(Motion & self)
254 {
255
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 14 return self.angular();
256 }
257 1 static void setAngular(Motion & self, const Vector3 & w)
258 {
259 1 self.angular(w);
260 1 }
261
262 1 static void setVector(Motion & self, const Vector6 & v)
263 {
264 1 self = v;
265 1 }
266
267 1 static void setZero(Motion & self)
268 {
269 1 self.setZero();
270 1 }
271 2 static void setRandom(Motion & self)
272 {
273 2 self.setRandom();
274 2 }
275 };
276
277 } // namespace python
278 } // namespace pinocchio
279
280 #endif // ifndef __pinocchio_python_spatial_motion_hpp__
281