GCC Code Coverage Report

Directory: ./
File: include/pinocchio/bindings/python/spatial/se3.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 73 94 77.7%
Branches: 107 260 41.2%
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 20 void visit(PyClass & cl) const
57 {
58 static const Scalar dummy_precision = Eigen::NumTraits<Scalar>::dummy_precision();
59
60
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 cl.def(bp::init<const Matrix3 &, const Vector3 &>(
61
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 60 (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 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 Quaternion &, const Vector3 &>(
64
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("quat"), bp::arg("translation")),
65 "Initialize from a quaternion and a translation vector."))
66
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<int>((bp::arg("self"), bp::arg("int")), "Init to identity."))
67
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 Matrix4 &>(
68
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")), "Initialize from an homogeneous matrix."))
69
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 SE3 &>((bp::arg("self"), bp::arg("clone")), "Copy constructor"))
70
71
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .add_property(
72 "rotation",
73 bp::make_function(
74 (typename SE3::AngularRef(SE3::*)()) & SE3::rotation,
75 20 bp::return_internal_reference<>()),
76 (void(SE3::*)(const Matrix3 &)) & SE3::rotation,
77 "The rotation part of the transformation.")
78
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
79 "translation",
80 bp::make_function(
81 (typename SE3::LinearRef(SE3::*)()) & SE3::translation,
82 20 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 20 times.
✗ Branch 2 not taken.
 20 .add_property(
87 "homogeneous", &SE3::toHomogeneousMatrix,
88 "Returns the equivalent homegeneous matrix (acting on SE3).")
89
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .add_property(
90 "action", (ActionMatrixType(SE3::*)() const) & SE3::toActionMatrix,
91 "Returns the related action matrix (acting on Motion).")
92
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
93 "toActionMatrix", (ActionMatrixType(SE3::*)() const) & SE3::toActionMatrix,
94
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 bp::arg("self"), "Returns the related action matrix (acting on Motion).")
95
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .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 20 times.
✗ Branch 2 not taken.
 20 .def(
100 "toActionMatrixInverse",
101
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 (ActionMatrixType(SE3::*)() const) & SE3::toActionMatrixInverse, 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 20 times.
✗ Branch 2 not taken.
 20 .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 20 times.
✗ Branch 2 not taken.
 20 .def(
108 "toDualActionMatrix", (ActionMatrixType(SE3::*)() const) & SE3::toDualActionMatrix,
109
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 bp::arg("self"), "Returns the related dual action matrix (acting on Force).")
110
111
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
112
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 "setIdentity", &SE3PythonVisitor::setIdentity, bp::arg("self"),
113 "Set *this to the identity placement.")
114
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
115
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 "setRandom", &SE3PythonVisitor::setRandom, bp::arg("self"),
116 "Set *this to a random placement.")
117
118
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 .def("inverse", &SE3::inverse, bp::arg("self"), "Returns the inverse transform")
119
120
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .def(
159 "isApprox", &SE3::isApprox,
160
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),
161 "Returns true if *this is approximately equal to other, within the precision given "
162 "by prec.")
163
164
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
165
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 "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 20 times.
✗ Branch 2 not taken.
 20 .def("__invert__", &SE3::inverse, "Returns the inverse of *this.")
171
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)
172
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("__mul__", &__mul__<Motion>)
173
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("__mul__", &__mul__<Force>)
174
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("__mul__", &__mul__<Inertia>)
175
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("__mul__", &__mul__<Vector3>)
176 40 .add_property("np", &SE3::toHomogeneousMatrix)
177
178 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
179
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)
180
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self != bp::self)
181 #endif
182
183
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("Identity", &SE3::Identity, "Returns the identity transformation.")
184
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("Identity")
185
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("Random", &SE3::Random, "Returns a random transformation.")
186
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("Random")
187
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .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 20 times.
✗ Branch 2 not taken.
 20 .staticmethod("Interpolate")
198
199 40 .def("__array__", &SE3::toHomogeneousMatrix)
200
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 .def("__array__", &__array__)
201
202 #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
203
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def_pickle(Pickle())
204 #endif
205 ;
206 20 }
207
208 static std::string scopeName()
209 {
210 static std::string scope_name;
211 return scope_name;
212 }
213
214 20 static void expose()
215 {
216 #if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2, 9, 0)
217 typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(SE3) HolderType;
218 #else
219 typedef ::boost::python::detail::not_specified HolderType;
220 #endif
221
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 bp::class_<SE3, HolderType>(
222 "SE3", "SE3 transformation defined by a 3d vector and a rotation matrix.",
223
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 40 bp::init<>(bp::arg("self"), "Default constructor."))
224
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(SE3PythonVisitor<SE3>())
225
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(CastVisitor<SE3>())
226
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(ExposeConstructorByCastVisitor<SE3, ::pinocchio::SE3>())
227
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(CopyableVisitor<SE3>())
228
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 40 .def(bp::self_ns::str(bp::self_ns::self))
229
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("__repr__", &repr);
230 20 }
231
232 private:
233 ✗ static Matrix4 __array__(const SE3 & self, bp::object)
234 {
235 ✗ return self.toHomogeneousMatrix();
236 }
237
238 struct Pickle : bp::pickle_suite
239 {
240 ✗ static boost::python::tuple getinitargs(const SE3 & M)
241 {
242 ✗ return bp::make_tuple((Matrix3)M.rotation(), (Vector3)M.translation());
243 }
244
245 20 static bool getstate_manages_dict()
246 {
247 20 return true;
248 }
249 };
250
251 ✗ static void setIdentity(SE3 & self)
252 {
253 ✗ self.setIdentity();
254 }
255 ✗ static void setRandom(SE3 & self)
256 {
257 ✗ self.setRandom();
258 }
259
260 template<typename Spatial>
261 ✗ static Spatial __mul__(const SE3 & self, const Spatial & other)
262 {
263 ✗ return self.act(other);
264 }
265
266 ✗ static std::string repr(const SE3 & self)
267 {
268 // bp::object
269 // py_rotation(bp::handle<>(eigenpy::EigenToPy<Matrix3,Scalar>::convert(self.rotation())));
270 // std::string rotation_repr =
271 // bp::extract<std::string>(py_rotation.attr("__repr__")());
272 //
273 // bp::object
274 // py_translation(bp::handle<>(eigenpy::EigenToPy<Vector3,Scalar>::convert(self.translation())));
275 // std::string translation_repr =
276 // bp::extract<std::string>(py_translation.attr("__repr__")());
277
278 ✗ bp::object py_homogeneous(
279 ✗ bp::handle<>(eigenpy::EigenToPy<Matrix4, Scalar>::convert(self.toHomogeneousMatrix())));
280 ✗ std::string homegeneous_repr = bp::extract<std::string>(py_homogeneous.attr("__repr__")());
281 ✗ replace(homegeneous_repr, "\n", "");
282 ✗ replace(homegeneous_repr, " ", "");
283
284 ✗ std::stringstream ss_repr;
285 ✗ ss_repr << "SE3(";
286 ✗ ss_repr << homegeneous_repr;
287 ✗ ss_repr << ")";
288
289 ✗ return ss_repr.str();
290 }
291 };
292
293 } // namespace python
294 } // namespace pinocchio
295
296 #endif // ifndef __pinocchio_python_spatial_se3_hpp__
297