GCC Code Coverage Report

Directory: ./
File: include/pinocchio/bindings/python/multibody/liegroups.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 32 98 32.7%
Branches: 28 114 24.6%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_python_lie_group_hpp__
6 #define __pinocchio_python_lie_group_hpp__
7
8 #include <eigenpy/memory.hpp>
9
10 #include "pinocchio/multibody/liegroup/liegroup.hpp"
11 #include "pinocchio/multibody/liegroup/cartesian-product-variant.hpp"
12 #include "pinocchio/multibody/liegroup/liegroup-generic.hpp"
13 #include "pinocchio/multibody/liegroup/liegroup-collection.hpp"
14
15 namespace pinocchio
16 {
17 namespace python
18 {
19 namespace bp = boost::python;
20
21 template<class LieGroupType>
22 struct LieGroupWrapperTpl
23 {
24 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, 1> ConfigVector_t;
25 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, 1> TangentVector_t;
26 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, Eigen::Dynamic> JacobianMatrix_t;
27
28 static ConfigVector_t
29 ✗ integrate(const LieGroupType & lg, const ConfigVector_t & q, const TangentVector_t & v)
30 {
31 ✗ return lg.integrate(q, v);
32 }
33
34 ✗ static ConfigVector_t interpolate(
35 const LieGroupType & lg,
36 const ConfigVector_t & q0,
37 const ConfigVector_t & q1,
38 const context::Scalar & u)
39 {
40 ✗ return lg.interpolate(q0, q1, u);
41 }
42
43 static TangentVector_t
44 ✗ difference(const LieGroupType & lg, const ConfigVector_t & q0, const ConfigVector_t & q1)
45 {
46 ✗ return lg.difference(q0, q1);
47 }
48
49 ✗ static JacobianMatrix_t dDifference1(
50 const LieGroupType & lg,
51 const ConfigVector_t & q0,
52 const ConfigVector_t & q1,
53 const ArgumentPosition arg)
54 {
55 ✗ JacobianMatrix_t J(lg.nv(), lg.nv());
56 ✗ lg.dDifference(q0, q1, J, arg);
57 ✗ return J;
58 }
59
60 ✗ static JacobianMatrix_t dDifference2(
61 const LieGroupType & lg,
62 const ConfigVector_t & q0,
63 const ConfigVector_t & q1,
64 const ArgumentPosition arg,
65 const JacobianMatrix_t & Jin,
66 int self)
67 {
68 ✗ JacobianMatrix_t J(Jin.rows(), Jin.cols());
69 ✗ switch (arg)
70 {
71 ✗ case ARG0:
72 ✗ lg.template dDifference<ARG0>(q0, q1, Jin, self, J, SETTO);
73 ✗ break;
74 ✗ case ARG1:
75 ✗ lg.template dDifference<ARG1>(q0, q1, Jin, self, J, SETTO);
76 ✗ break;
77 ✗ default:
78 ✗ throw std::invalid_argument("arg must be either ARG0 or ARG1");
79 }
80 ✗ return J;
81 }
82
83 ✗ static JacobianMatrix_t dDifference3(
84 const LieGroupType & lg,
85 const ConfigVector_t & q0,
86 const ConfigVector_t & q1,
87 const ArgumentPosition arg,
88 int self,
89 const JacobianMatrix_t & Jin)
90 {
91 ✗ JacobianMatrix_t J(Jin.rows(), Jin.cols());
92 ✗ switch (arg)
93 {
94 ✗ case ARG0:
95 ✗ lg.template dDifference<ARG0>(q0, q1, self, Jin, J, SETTO);
96 ✗ break;
97 ✗ case ARG1:
98 ✗ lg.template dDifference<ARG1>(q0, q1, self, Jin, J, SETTO);
99 ✗ break;
100 ✗ default:
101 ✗ throw std::invalid_argument("arg must be either ARG0 or ARG1");
102 }
103 ✗ return J;
104 }
105
106 ✗ static JacobianMatrix_t dIntegrate(
107 const LieGroupType & lg,
108 const ConfigVector_t & q,
109 const TangentVector_t & v,
110 const ArgumentPosition arg)
111 {
112 ✗ JacobianMatrix_t J(lg.nv(), lg.nv());
113 ✗ lg.dIntegrate(q, v, J, arg);
114 ✗ return J;
115 }
116
117 static JacobianMatrix_t
118 ✗ dIntegrate_dq1(const LieGroupType & lg, const ConfigVector_t & q, const TangentVector_t & v)
119 {
120 ✗ JacobianMatrix_t J(lg.nv(), lg.nv());
121 ✗ lg.dIntegrate_dq(q, v, J);
122 ✗ return J;
123 }
124
125 ✗ static JacobianMatrix_t dIntegrate_dq2(
126 const LieGroupType & lg,
127 const ConfigVector_t & q,
128 const TangentVector_t & v,
129 const JacobianMatrix_t & Jin,
130 int self)
131 {
132 ✗ JacobianMatrix_t J(Jin.rows(), lg.nv());
133 ✗ lg.dIntegrate_dq(q, v, Jin, self, J, SETTO);
134 ✗ return J;
135 }
136
137 ✗ static JacobianMatrix_t dIntegrate_dq3(
138 const LieGroupType & lg,
139 const ConfigVector_t & q,
140 const TangentVector_t & v,
141 int self,
142 const JacobianMatrix_t & Jin)
143 {
144 ✗ JacobianMatrix_t J(lg.nv(), Jin.cols());
145 ✗ lg.dIntegrate_dq(q, v, self, Jin, J, SETTO);
146 ✗ return J;
147 }
148
149 static JacobianMatrix_t
150 ✗ dIntegrate_dv1(const LieGroupType & lg, const ConfigVector_t & q, const TangentVector_t & v)
151 {
152 ✗ JacobianMatrix_t J(lg.nv(), lg.nv());
153 ✗ lg.dIntegrate_dv(q, v, J);
154 ✗ return J;
155 }
156
157 ✗ static JacobianMatrix_t dIntegrate_dv2(
158 const LieGroupType & lg,
159 const ConfigVector_t & q,
160 const TangentVector_t & v,
161 const JacobianMatrix_t & Jin,
162 int self)
163 {
164 ✗ JacobianMatrix_t J(Jin.rows(), lg.nv());
165 ✗ lg.dIntegrate_dv(q, v, Jin, self, J, SETTO);
166 ✗ return J;
167 }
168
169 ✗ static JacobianMatrix_t dIntegrate_dv3(
170 const LieGroupType & lg,
171 const ConfigVector_t & q,
172 const TangentVector_t & v,
173 int self,
174 const JacobianMatrix_t & Jin)
175 {
176 ✗ JacobianMatrix_t J(lg.nv(), Jin.cols());
177 ✗ lg.dIntegrate_dv(q, v, self, Jin, J, SETTO);
178 ✗ return J;
179 }
180
181 ✗ static JacobianMatrix_t dIntegrateTransport_proxy(
182 const LieGroupType & lg,
183 const ConfigVector_t & q,
184 const TangentVector_t & v,
185 const JacobianMatrix_t & J,
186 const ArgumentPosition arg)
187 {
188 ✗ JacobianMatrix_t Jout(lg.nv(), J.cols());
189 ✗ lg.dIntegrateTransport(q, v, J, Jout, arg);
190 ✗ return Jout;
191 }
192 };
193
194 template<class LieGroupType>
195 struct LieGroupPythonVisitor
196 : public boost::python::def_visitor<LieGroupPythonVisitor<LieGroupType>>
197 {
198 public:
199 /* --- Exposing C++ API to python through the handler ----------------- */
200 template<class PyClass>
201 20 void visit(PyClass & cl) const
202 {
203 typedef Eigen::Matrix<context::Scalar, Eigen::Dynamic, 1> ConfigVector_t;
204
205 typedef LieGroupWrapperTpl<LieGroupType> LieGroupWrapper;
206
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 cl.def(bp::init<>("Default constructor"))
207
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("integrate", LieGroupWrapper::integrate)
208
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("dIntegrate", LieGroupWrapper::dIntegrate)
209
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq1)
210
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq2)
211
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq3)
212
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv1)
213
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv2)
214
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv3)
215
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("dIntegrateTransport", LieGroupWrapper::dIntegrateTransport_proxy)
216
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("difference", LieGroupWrapper::difference)
217
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("dDifference", LieGroupWrapper::dDifference1)
218
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("dDifference", LieGroupWrapper::dDifference2)
219
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("dDifference", LieGroupWrapper::dDifference3)
220
221
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("interpolate", LieGroupWrapper::interpolate)
222
223 40 .def(
224 "random", static_cast<typename LieGroupType::ConfigVector_t (LieGroupType::*)() const>(
225 &LieGroupType::random))
226
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
227 "randomConfiguration",
228 &LieGroupType::template randomConfiguration<ConfigVector_t, ConfigVector_t>)
229
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("distance", &LieGroupType::template distance<ConfigVector_t, ConfigVector_t>)
230
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def(
231 "squaredDistance",
232 &LieGroupType::template squaredDistance<ConfigVector_t, ConfigVector_t>)
233
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .def("normalize", &LieGroupType::template normalize<ConfigVector_t>)
234
235
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .add_property("name", &LieGroupType::name)
236
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .add_property("neutral", &LieGroupType::neutral)
237
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .add_property("nq", &LieGroupType::nq)
238
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 .add_property("nv", &LieGroupType::nv)
239
240
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)
241
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self *= bp::self)
242 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
243
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 .def(bp::self == bp::self)
244 #endif
245 ;
246 20 }
247
248 20 static void expose(const char * name)
249 {
250
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 bp::class_<LieGroupType>(name, bp::no_init).def(LieGroupPythonVisitor<LieGroupType>());
251 20 }
252 };
253 } // namespace python
254 } // namespace pinocchio
255
256 #endif // ifndef __pinocchio_python_geometry_model_hpp__
257