GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2023 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 |
|||
13 |
#include "pinocchio/spatial/se3.hpp" |
||
14 |
#include "pinocchio/spatial/motion.hpp" |
||
15 |
#include "pinocchio/spatial/force.hpp" |
||
16 |
#include "pinocchio/bindings/python/fwd.hpp" |
||
17 |
#include "pinocchio/bindings/python/utils/copyable.hpp" |
||
18 |
#include "pinocchio/bindings/python/utils/printable.hpp" |
||
19 |
|||
20 |
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Motion) |
||
21 |
|||
22 |
namespace pinocchio |
||
23 |
{ |
||
24 |
namespace python |
||
25 |
{ |
||
26 |
namespace bp = boost::python; |
||
27 |
|||
28 |
template<typename T> struct call; |
||
29 |
|||
30 |
template<typename Scalar, int Options> |
||
31 |
struct call< MotionTpl<Scalar,Options> > |
||
32 |
{ |
||
33 |
typedef MotionTpl<Scalar,Options> Motion; |
||
34 |
|||
35 |
static bool isApprox(const Motion & self, const Motion & other, |
||
36 |
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) |
||
37 |
{ |
||
38 |
return self.isApprox(other,prec); |
||
39 |
} |
||
40 |
|||
41 |
static bool isZero(const Motion & self, |
||
42 |
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) |
||
43 |
{ |
||
44 |
return self.isZero(prec); |
||
45 |
} |
||
46 |
}; |
||
47 |
|||
48 |
✗✗ | 38 |
BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxMotion_overload,call<Motion>::isApprox,2,3) |
49 |
BOOST_PYTHON_FUNCTION_OVERLOADS(isZero_overload,call<Motion>::isZero,1,2) |
||
50 |
|||
51 |
template<typename Motion> |
||
52 |
struct MotionPythonVisitor |
||
53 |
: public boost::python::def_visitor< MotionPythonVisitor<Motion> > |
||
54 |
{ |
||
55 |
enum { Options = traits<Motion>::Options }; |
||
56 |
|||
57 |
typedef typename Motion::Scalar Scalar; |
||
58 |
typedef ForceTpl<Scalar,traits<Motion>::Options> Force; |
||
59 |
typedef typename Motion::Vector6 Vector6; |
||
60 |
typedef typename Motion::Vector3 Vector3; |
||
61 |
|||
62 |
typedef typename Eigen::Map<Vector3> MapVector3; |
||
63 |
typedef typename Eigen::Ref<Vector3> RefVector3; |
||
64 |
|||
65 |
public: |
||
66 |
|||
67 |
template<class PyClass> |
||
68 |
19 |
void visit(PyClass& cl) const |
|
69 |
{ |
||
70 |
cl |
||
71 |
.def(bp::init<>(bp::arg("self"),"Default constructor")) |
||
72 |
✓✗✓✗ |
19 |
.def(bp::init<Vector3,Vector3> |
73 |
(bp::args("self","linear","angular"), |
||
74 |
"Initialize from linear and angular components of a Motion vector (don't mix the order).")) |
||
75 |
✓✗✓✗ ✓✗ |
38 |
.def(bp::init<Vector6>((bp::arg("self"),bp::arg("array")),"Init from a vector 6 [linear velocity, angular velocity]")) |
76 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
38 |
.def(bp::init<Motion>((bp::arg("self"),bp::arg("other")),"Copy constructor.")) |
77 |
|||
78 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
38 |
.add_property("linear", |
79 |
bp::make_function(&MotionPythonVisitor::getLinear, |
||
80 |
bp::with_custodian_and_ward_postcall<0,1>()), |
||
81 |
&MotionPythonVisitor::setLinear, |
||
82 |
"Linear part of a *this, corresponding to the linear velocity in case of a Spatial velocity.") |
||
83 |
✓✗✓✗ |
38 |
.add_property("angular", |
84 |
bp::make_function(&MotionPythonVisitor::getAngular, |
||
85 |
bp::with_custodian_and_ward_postcall<0,1>()), |
||
86 |
&MotionPythonVisitor::setAngular, |
||
87 |
"Angular part of a *this, corresponding to the angular velocity in case of a Spatial velocity.") |
||
88 |
✓✗✓✗ |
38 |
.add_property("vector", |
89 |
bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector, |
||
90 |
bp::return_internal_reference<>()), |
||
91 |
&MotionPythonVisitor::setVector, |
||
92 |
"Returns the components of *this as a 6d vector.") |
||
93 |
✓✗✓✗ |
38 |
.add_property("np", |
94 |
bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector, |
||
95 |
bp::return_internal_reference<>())) |
||
96 |
|||
97 |
✓✗✓✗ |
38 |
.def("se3Action",&Motion::template se3Action<Scalar,Options>, |
98 |
bp::args("self","M"),"Returns the result of the action of M on *this.") |
||
99 |
✓✗ | 38 |
.def("se3ActionInverse",&Motion::template se3ActionInverse<Scalar,Options>, |
100 |
bp::args("self","M"),"Returns the result of the action of the inverse of M on *this.") |
||
101 |
|||
102 |
✓✗✓✗ |
38 |
.add_property("action",&Motion::toActionMatrix,"Returns the action matrix of *this (acting on Motion).") |
103 |
✓✗ | 19 |
.add_property("dualAction",&Motion::toDualActionMatrix,"Returns the dual action matrix of *this (acting on Force).") |
104 |
✓✗ | 19 |
.add_property("homogeneous", &Motion::toHomogeneousMatrix, "Equivalent homogeneous representation of the Motion vector") |
105 |
|||
106 |
✓✗ | 19 |
.def("setZero",&MotionPythonVisitor::setZero,bp::arg("self"), |
107 |
"Set the linear and angular components of *this to zero.") |
||
108 |
✓✗✓✗ ✓✗ |
38 |
.def("setRandom",&MotionPythonVisitor::setRandom,bp::arg("self"), |
109 |
"Set the linear and angular components of *this to random values.") |
||
110 |
|||
111 |
✓✗✓✗ |
38 |
.def("cross",(Motion (Motion::*)(const Motion &) const) &Motion::cross, |
112 |
bp::args("self","m"),"Action of *this onto another Motion m. Returns ยจ*this x m.") |
||
113 |
✓✗ | 38 |
.def("cross",(Force (Motion::*)(const Force &) const) &Motion::cross, |
114 |
bp::args("self","f"),"Dual action of *this onto a Force f. Returns *this x* f.") |
||
115 |
|||
116 |
✓✗✓✗ |
57 |
.def(bp::self + bp::self) |
117 |
✓✗✓✗ |
38 |
.def(bp::self += bp::self) |
118 |
✓✗ | 19 |
.def(bp::self - bp::self) |
119 |
✓✗ | 19 |
.def(bp::self -= bp::self) |
120 |
✓✗ | 19 |
.def(-bp::self) |
121 |
✓✗ | 19 |
.def(bp::self ^ bp::self) |
122 |
✓✗✓✗ |
19 |
.def(bp::self ^ Force()) |
123 |
|||
124 |
✓✗ | 19 |
.def(bp::self == bp::self) |
125 |
✓✗ | 19 |
.def(bp::self != bp::self) |
126 |
|||
127 |
✓✗ | 19 |
.def(bp::self * Scalar()) |
128 |
✓✗ | 19 |
.def(Scalar() * bp::self) |
129 |
✓✗ | 19 |
.def(bp::self / Scalar()) |
130 |
|||
131 |
✓✗ | 19 |
.def("isApprox", |
132 |
call<Motion>::isApprox, |
||
133 |
isApproxMotion_overload(bp::args("self","other","prec"), |
||
134 |
"Returns true if *this is approximately equal to other, within the precision given by prec.")) |
||
135 |
|||
136 |
✓✗✓✗ ✓✗ |
38 |
.def("isZero", |
137 |
call<Motion>::isZero, |
||
138 |
isZero_overload(bp::args("self","prec"), |
||
139 |
"Returns true if *this is approximately equal to the zero Motion, within the precision given by prec.")) |
||
140 |
|||
141 |
✓✗✓✗ ✓✗ |
38 |
.def("Random",&Motion::Random,"Returns a random Motion.") |
142 |
✓✗ | 19 |
.staticmethod("Random") |
143 |
✓✗ | 19 |
.def("Zero",&Motion::Zero,"Returns a zero Motion.") |
144 |
✓✗ | 19 |
.staticmethod("Zero") |
145 |
|||
146 |
✓✗ | 19 |
.def("__array__",bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector, |
147 |
bp::return_internal_reference<>())) |
||
148 |
|||
149 |
✓✗✓✗ ✓✗ |
19 |
.def_pickle(Pickle()) |
150 |
; |
||
151 |
19 |
} |
|
152 |
|||
153 |
19 |
static void expose() |
|
154 |
{ |
||
155 |
#if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2,9,0) |
||
156 |
typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(Motion) HolderType; |
||
157 |
#else |
||
158 |
typedef ::boost::python::detail::not_specified HolderType; |
||
159 |
#endif |
||
160 |
bp::class_<Motion,HolderType>("Motion", |
||
161 |
"Motion vectors, in se3 == M^6.\n\n" |
||
162 |
"Supported operations ...", |
||
163 |
bp::no_init) |
||
164 |
.def(MotionPythonVisitor<Motion>()) |
||
165 |
✓✗ | 19 |
.def(CopyableVisitor<Motion>()) |
166 |
✓✗✓✗ |
19 |
.def(PrintableVisitor<Motion>()) |
167 |
; |
||
168 |
19 |
} |
|
169 |
|||
170 |
private: |
||
171 |
|||
172 |
struct Pickle : bp::pickle_suite |
||
173 |
{ |
||
174 |
static |
||
175 |
boost::python::tuple |
||
176 |
getinitargs(const Motion & m) |
||
177 |
{ return bp::make_tuple((Vector3)m.linear(),(Vector3)m.angular()); } |
||
178 |
|||
179 |
19 |
static bool getstate_manages_dict() { return true; } |
|
180 |
}; |
||
181 |
|||
182 |
static RefVector3 getLinear(Motion & self) { return self.linear(); } |
||
183 |
static void setLinear (Motion & self, const Vector3 & v) { self.linear(v); } |
||
184 |
static RefVector3 getAngular(Motion & self) { return self.angular(); } |
||
185 |
static void setAngular(Motion & self, const Vector3 & w) { self.angular(w); } |
||
186 |
|||
187 |
static void setVector(Motion & self, const Vector6 & v) { self = v; } |
||
188 |
|||
189 |
static void setZero(Motion & self) { self.setZero(); } |
||
190 |
static void setRandom(Motion & self) { self.setRandom(); } |
||
191 |
|||
192 |
}; |
||
193 |
|||
194 |
}} // namespace pinocchio::python |
||
195 |
|||
196 |
#endif // ifndef __pinocchio_python_spatial_motion_hpp__ |
Generated by: GCOVR (Version 4.2) |