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