GCC Code Coverage Report

Directory: ./
File: include/pinocchio/bindings/python/spatial/se3.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 87 98 88.8%
Branches: 123 288 42.7%
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_se3_hpp__
7 #define __pinocchio_python_spatial_se3_hpp__
8
9 #include <eigenpy/eigenpy.hpp>
10 #include <eigenpy/memory.hpp>
11 #include <boost/python/tuple.hpp>
12
13 #include "pinocchio/spatial/se3.hpp"
14 #include "pinocchio/spatial/motion.hpp"
15 #include "pinocchio/spatial/force.hpp"
16 #include "pinocchio/spatial/inertia.hpp"
17 #include "pinocchio/spatial/explog.hpp"
18
19 #include "pinocchio/utils/string.hpp"
20
21 #include "pinocchio/bindings/python/utils/cast.hpp"
22 #include "pinocchio/bindings/python/utils/copyable.hpp"
23 #include "pinocchio/bindings/python/utils/printable.hpp"
24 #include "pinocchio/bindings/python/utils/namespace.hpp"
25
26 #if EIGENPY_VERSION_AT_MOST(2, 8, 1)
27 EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::SE3)
28 #endif
29
30 namespace pinocchio
31 {
32 namespace python
33 {
34 namespace bp = boost::python;
35
36 template<typename SE3>
37 struct SE3PythonVisitor : public boost::python::def_visitor<SE3PythonVisitor<SE3>>
38 {
39 typedef typename SE3::Scalar Scalar;
40 enum
41 {
42 Options = SE3::Options
43 };
44 typedef typename SE3::Matrix3 Matrix3;
45 typedef typename SE3::Vector3 Vector3;
46 typedef typename SE3::Matrix4 Matrix4;
47 typedef typename SE3::Quaternion Quaternion;
48 typedef typename SE3::ActionMatrixType ActionMatrixType;
49
50 typedef MotionTpl<Scalar, Options> Motion;
51 typedef ForceTpl<Scalar, Options> Force;
52 typedef InertiaTpl<Scalar, Options> Inertia;
53
54 public:
55 template<class PyClass>
56 69 void visit(PyClass & cl) const
57 {
58
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();
59
60
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 cl.def(bp::init<const Matrix3 &, const Vector3 &>(
61
2/4
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
 207 (bp::arg("self"), bp::arg("rotation"), bp::arg("translation")),
62 "Initialize from a rotation matrix and a translation vector."))
63
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 Quaternion &, const Vector3 &>(
64
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("quat"), bp::arg("translation")),
65 "Initialize from a quaternion and a translation vector."))
66
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<int>((bp::arg("self"), bp::arg("int")), "Init to identity."))
67
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 Matrix4 &>(
68
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")), "Initialize from an homogeneous matrix."))
69
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 SE3 &>((bp::arg("self"), bp::arg("clone")), "Copy constructor"))
70
71
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .add_property(
72 "rotation",
73 bp::make_function(
74 (typename SE3::AngularRef(SE3::*)()) & SE3::rotation,
75 69 bp::return_internal_reference<>()),
76 (void(SE3::*)(const Matrix3 &)) & SE3::rotation,
77 "The rotation part of the transformation.")
78
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .add_property(
79 "translation",
80 bp::make_function(
81 (typename SE3::LinearRef(SE3::*)()) & SE3::translation,
82 69 bp::return_internal_reference<>()),
83 (void(SE3::*)(const Vector3 &)) & SE3::translation,
84 "The translation part of the transformation.")
85
86
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .add_property(
87 "homogeneous", &SE3::toHomogeneousMatrix,
88 "Returns the equivalent homegeneous matrix (acting on SE3).")
89
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .add_property(
90 "action", (ActionMatrixType(SE3::*)() const)&SE3::toActionMatrix,
91 "Returns the related action matrix (acting on Motion).")
92
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(
93 "toActionMatrix", (ActionMatrixType(SE3::*)() const)&SE3::toActionMatrix,
94
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 bp::arg("self"), "Returns the related action matrix (acting on Motion).")
95
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .add_property(
96 "actionInverse", (ActionMatrixType(SE3::*)() const)&SE3::toActionMatrixInverse,
97 "Returns the inverse of the action matrix (acting on Motion).\n"
98 "This is equivalent to do m.inverse().action")
99
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(
100 "toActionMatrixInverse", (ActionMatrixType(SE3::*)() const)&SE3::toActionMatrixInverse,
101
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 bp::arg("self"),
102 "Returns the inverse of the action matrix (acting on Motion).\n"
103 "This is equivalent to do m.inverse().toActionMatrix()")
104
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .add_property(
105 "dualAction", (ActionMatrixType(SE3::*)() const)&SE3::toDualActionMatrix,
106 "Returns the related dual action matrix (acting on Force).")
107
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(
108 "toDualActionMatrix", (ActionMatrixType(SE3::*)() const)&SE3::toDualActionMatrix,
109
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 bp::arg("self"), "Returns the related dual action matrix (acting on Force).")
110
111
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
112
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 "setIdentity", &SE3PythonVisitor::setIdentity, bp::arg("self"),
113 "Set *this to the identity placement.")
114
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(
115
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 "setRandom", &SE3PythonVisitor::setRandom, bp::arg("self"),
116 "Set *this to a random placement.")
117
118
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 .def("inverse", &SE3::inverse, bp::arg("self"), "Returns the inverse transform")
119
120
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
121 "act", (Vector3(SE3::*)(const Vector3 &) const)&SE3::act, bp::args("self", "point"),
122 "Returns a point which is the result of the entry point transforms by *this.")
123
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
124 "actInv", (Vector3(SE3::*)(const Vector3 &) const)&SE3::actInv,
125 bp::args("self", "point"),
126 "Returns a point which is the result of the entry point by the inverse of *this.")
127
128
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
129 "act", (SE3(SE3::*)(const SE3 & other) const)&SE3::act, bp::args("self", "M"),
130 "Returns the result of *this * M.")
131
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
132 "actInv", (SE3(SE3::*)(const SE3 & other) const)&SE3::actInv, bp::args("self", "M"),
133 "Returns the result of the inverse of *this times M.")
134
135
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
136 "act", (Motion(SE3::*)(const Motion &) const)&SE3::act, bp::args("self", "motion"),
137 "Returns the result action of *this onto a Motion.")
138
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
139 "actInv", (Motion(SE3::*)(const Motion &) const)&SE3::actInv,
140 bp::args("self", "motion"), "Returns the result of the inverse of *this onto a Motion.")
141
142
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
143 "act", (Force(SE3::*)(const Force &) const)&SE3::act, bp::args("self", "force"),
144 "Returns the result of *this onto a Force.")
145
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
146 "actInv", (Force(SE3::*)(const Force &) const)&SE3::actInv, bp::args("self", "force"),
147 "Returns the result of the inverse of *this onto an Inertia.")
148
149
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
150 "act", (Inertia(SE3::*)(const Inertia &) const)&SE3::act, bp::args("self", "inertia"),
151 "Returns the result of *this onto a Force.")
152
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
153 "actInv", (Inertia(SE3::*)(const Inertia &) const)&SE3::actInv,
154 bp::args("self", "inertia"),
155 "Returns the result of the inverse of *this onto an Inertia.")
156
157 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
158
2/4
✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
 130 .def(
159 "isApprox", &SE3::isApprox,
160
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),
161 "Returns true if *this is approximately equal to other, within the precision given "
162 "by prec.")
163
164
1/2
✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
 65 .def(
165
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 "isIdentity", &SE3::isIdentity, (bp::arg("self"), bp::arg("prec") = dummy_precision),
166 "Returns true if *this is approximately equal to the identity placement, within the "
167 "precision given by prec.")
168 #endif
169
170
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("__invert__", &SE3::inverse, "Returns the inverse of *this.")
171
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)
172
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("__mul__", &__mul__<Motion>)
173
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("__mul__", &__mul__<Force>)
174
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("__mul__", &__mul__<Inertia>)
175
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("__mul__", &__mul__<Vector3>)
176 138 .add_property("np", &SE3::toHomogeneousMatrix)
177
178 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
179
2/4
✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 65 times.
✗ Branch 6 not taken.
 65 .def(bp::self == bp::self)
180
1/2
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
 65 .def(bp::self != bp::self)
181 #endif
182
183
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 69 .def("Identity", &SE3::Identity, "Returns the identity transformation.")
184
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .staticmethod("Identity")
185
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("Random", &SE3::Random, "Returns a random transformation.")
186
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .staticmethod("Random")
187
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
188 "Interpolate", &SE3::template Interpolate<Scalar>, bp::args("A", "B", "alpha"),
189 "Linear interpolation on the SE3 manifold.\n\n"
190 "This method computes the linear interpolation between A and B, such that the "
191 "result C = A + (B-A)*t if it would be applied on classic Euclidian space.\n"
192 "This operation is very similar to the SLERP operation on Rotations.\n"
193 "Parameters:\n"
194 "\tA: Initial transformation\n"
195 "\tB: Target transformation\n"
196 "\talpha: Interpolation factor")
197
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .staticmethod("Interpolate")
198
199 138 .def("__array__", &SE3::toHomogeneousMatrix)
200
2/4
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
 138 .def(
201 "__array__", &__array__,
202
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()))
203
204 #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
205
1/2
✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
 65 .def_pickle(Pickle())
206 #endif
207 ;
208 69 }
209
210 static std::string scopeName()
211 {
212 static std::string scope_name;
213 return scope_name;
214 }
215
216 69 static void expose()
217 {
218 #if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2, 9, 0)
219 typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(SE3) HolderType;
220 #else
221 typedef ::boost::python::detail::not_specified HolderType;
222 #endif
223
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 bp::class_<SE3, HolderType>(
224 "SE3", "SE3 transformation defined by a 3d vector and a rotation matrix.",
225
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 138 bp::init<>(bp::arg("self"), "Default constructor."))
226
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(SE3PythonVisitor<SE3>())
227
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(CastVisitor<SE3>())
228
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(ExposeConstructorByCastVisitor<SE3, ::pinocchio::SE3>())
229
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(CopyableVisitor<SE3>())
230
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 138 .def(bp::self_ns::str(bp::self_ns::self))
231
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("__repr__", &repr);
232 69 }
233
234 private:
235 2 static Matrix4 __array__(const SE3 & self, bp::object, bp::object)
236 {
237 2 return self.toHomogeneousMatrix();
238 }
239
240 struct Pickle : bp::pickle_suite
241 {
242 1 static boost::python::tuple getinitargs(const SE3 & M)
243 {
244
3/6
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 return bp::make_tuple((Matrix3)M.rotation(), (Vector3)M.translation());
245 }
246
247 65 static bool getstate_manages_dict()
248 {
249 65 return true;
250 }
251 };
252
253 1 static void setIdentity(SE3 & self)
254 {
255 1 self.setIdentity();
256 1 }
257 3 static void setRandom(SE3 & self)
258 {
259 3 self.setRandom();
260 3 }
261
262 template<typename Spatial>
263 14 static Spatial __mul__(const SE3 & self, const Spatial & other)
264 {
265 14 return self.act(other);
266 }
267
268 ✗ static std::string repr(const SE3 & self)
269 {
270 // bp::object
271 // py_rotation(bp::handle<>(eigenpy::EigenToPy<Matrix3,Scalar>::convert(self.rotation())));
272 // std::string rotation_repr =
273 // bp::extract<std::string>(py_rotation.attr("__repr__")());
274 //
275 // bp::object
276 // py_translation(bp::handle<>(eigenpy::EigenToPy<Vector3,Scalar>::convert(self.translation())));
277 // std::string translation_repr =
278 // bp::extract<std::string>(py_translation.attr("__repr__")());
279
280 ✗ bp::object py_homogeneous(
281 ✗ bp::handle<>(eigenpy::EigenToPy<Matrix4, Scalar>::convert(self.toHomogeneousMatrix())));
282 ✗ std::string homegeneous_repr = bp::extract<std::string>(py_homogeneous.attr("__repr__")());
283 ✗ replace(homegeneous_repr, "\n", "");
284 ✗ replace(homegeneous_repr, " ", "");
285
286 ✗ std::stringstream ss_repr;
287 ✗ ss_repr << "SE3(";
288 ✗ ss_repr << homegeneous_repr;
289 ✗ ss_repr << ")";
290
291 ✗ return ss_repr.str();
292 }
293 };
294
295 } // namespace python
296 } // namespace pinocchio
297
298 #endif // ifndef __pinocchio_python_spatial_se3_hpp__
299