GCC Code Coverage Report

Directory: ./
File: include/pinocchio/bindings/python/spatial/motion.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 66 88 75.0%
Branches: 82 176 46.6%
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 20 void visit(PyClass & cl) const
74 {
75 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 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 20 cl.def(bp::init<>(bp::arg("self"), "Default constructor"))
79
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 .def(bp::init<const Vector3 &, const Vector3 &>(
80
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.
 60 (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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 20 .def(bp::init<const Vector6 &>(
84
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 (bp::arg("self"), bp::arg("array")),
85 "Init from a vector 6 [linear velocity, angular velocity]"))
86
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 Motion &>((bp::arg("self"), bp::arg("clone")), "Copy constructor"))
87
88
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 .add_property(
89 "linear",
90 bp::make_function(
91 20 &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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 .add_property(
96 "angular",
97 bp::make_function(
98 20 &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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 .add_property(
103 "vector",
104 bp::make_function(
105 (typename Motion::ToVectorReturnType(Motion::*)()) & Motion::toVector,
106 20 bp::return_internal_reference<>()),
107 &MotionPythonVisitor::setVector, "Returns the components of *this as a 6d vector.")
108
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 .add_property(
109 "np", bp::make_function(
110 (typename Motion::ToVectorReturnType(Motion::*)()) & Motion::toVector,
111 20 bp::return_internal_reference<>()))
112
113
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
 20 .add_property(
121 "action", &Motion::toActionMatrix,
122 "Returns the action matrix of *this (acting on Motion).")
123
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .add_property(
124 "dualAction", &Motion::toDualActionMatrix,
125 "Returns the dual action matrix of *this (acting on Force).")
126
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .add_property(
127 "homogeneous", &Motion::toHomogeneousMatrix,
128 "Equivalent homogeneous representation of the Motion vector")
129
130
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
131
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 "setZero", &MotionPythonVisitor::setZero, bp::arg("self"),
132 "Set the linear and angular components of *this to zero.")
133
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
134
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 "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 20 times.
✗ Branch 2 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
142 "cross", (Motion(Motion::*)(const Motion &) const) & Motion::cross,
143 bp::args("self", "m"), "Action of *this onto another Motion m. Returns ¨*this x m.")
144
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 20 .def(bp::self + bp::self)
149
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self += bp::self)
150
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self - bp::self)
151
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self -= bp::self)
152
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(-bp::self)
153
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self ^ bp::self)
154
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 20 .def(bp::self ^ Force())
155
156 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
157
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self == bp::self)
158
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self != bp::self)
159 #endif
160
161
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self * Scalar())
162
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(Scalar() * bp::self)
163
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self / Scalar())
164
165 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
166
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
167 "isApprox", &call<Motion>::isApprox,
168
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),
169 "Returns true if *this is approximately equal to other, within the precision given "
170 "by prec.")
171
172
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
173
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", &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 20 times.
✗ Branch 2 not taken.
 20 .def("Random", &Motion::Random, "Returns a random Motion.")
179
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("Random")
180
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("Zero", &Motion::Zero, "Returns a zero Motion.")
181
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("Zero")
182
183
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 .def(
184 "__array__", bp::make_function(
185 (typename Motion::ToVectorReturnType(Motion::*)()) & Motion::toVector,
186 20 bp::return_internal_reference<>()))
187
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("__array__", &__array__, bp::return_internal_reference<>())
188 #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
189
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def_pickle(Pickle())
190 #endif
191 ;
192 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
193 20 }
194
195 20 static void expose()
196 {
197 typedef pinocchio::MotionBase<Motion> MotionBase;
198 20 bp::objects::register_dynamic_id<MotionBase>();
199 20 bp::objects::register_conversion<Motion, MotionBase>(false);
200
201 typedef pinocchio::MotionDense<Motion> MotionDense;
202 20 bp::objects::register_dynamic_id<MotionDense>();
203 20 bp::objects::register_conversion<Motion, MotionDense>(false);
204
205 #if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2, 9, 0)
206 typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(Motion) HolderType;
207 #else
208 typedef ::boost::python::detail::not_specified HolderType;
209 #endif
210 20 bp::class_<Motion, HolderType>(
211 "Motion",
212 "Motion vectors, in se3 == M^6.\n\n"
213 "Supported operations ...",
214 bp::no_init)
215
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(MotionPythonVisitor<Motion>())
216
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(CastVisitor<Motion>())
217
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(ExposeConstructorByCastVisitor<Motion, ::pinocchio::Motion>())
218
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(CopyableVisitor<Motion>())
219
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(PrintableVisitor<Motion>());
220 20 }
221
222 private:
223 ✗ static typename Motion::ToVectorConstReturnType __array__(const Motion & self, bp::object)
224 {
225 ✗ return self.toVector();
226 }
227
228 struct Pickle : bp::pickle_suite
229 {
230 ✗ static boost::python::tuple getinitargs(const Motion & m)
231 {
232 ✗ return bp::make_tuple((Vector3)m.linear(), (Vector3)m.angular());
233 }
234
235 20 static bool getstate_manages_dict()
236 {
237 20 return true;
238 }
239 };
240
241 ✗ static RefVector3 getLinear(Motion & self)
242 {
243 ✗ return self.linear();
244 }
245 ✗ static void setLinear(Motion & self, const Vector3 & v)
246 {
247 ✗ self.linear(v);
248 }
249 ✗ static RefVector3 getAngular(Motion & self)
250 {
251 ✗ return self.angular();
252 }
253 ✗ static void setAngular(Motion & self, const Vector3 & w)
254 {
255 ✗ self.angular(w);
256 }
257
258 ✗ static void setVector(Motion & self, const Vector6 & v)
259 {
260 ✗ self = v;
261 }
262
263 ✗ static void setZero(Motion & self)
264 {
265 ✗ self.setZero();
266 }
267 ✗ static void setRandom(Motion & self)
268 {
269 ✗ self.setRandom();
270 }
271 };
272
273 } // namespace python
274 } // namespace pinocchio
275
276 #endif // ifndef __pinocchio_python_spatial_motion_hpp__
277