GCC Code Coverage Report

Directory: ./
File: include/pinocchio/bindings/python/multibody/liegroups.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 94 98 95.9%
Branches: 56 114 49.1%
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 18 integrate(const LieGroupType & lg, const ConfigVector_t & q, const TangentVector_t & v)
30 {
31 18 return lg.integrate(q, v);
32 }
33
34 6 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 6 return lg.interpolate(q0, q1, u);
41 }
42
43 static TangentVector_t
44 6 difference(const LieGroupType & lg, const ConfigVector_t & q0, const ConfigVector_t & q1)
45 {
46 6 return lg.difference(q0, q1);
47 }
48
49 12 static JacobianMatrix_t dDifference1(
50 const LieGroupType & lg,
51 const ConfigVector_t & q0,
52 const ConfigVector_t & q1,
53 const ArgumentPosition arg)
54 {
55
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 JacobianMatrix_t J(lg.nv(), lg.nv());
56
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 lg.dDifference(q0, q1, J, arg);
57 12 return J;
58 }
59
60 12 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
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 JacobianMatrix_t J(Jin.rows(), Jin.cols());
69
2/3
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 switch (arg)
70 {
71 6 case ARG0:
72
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 lg.template dDifference<ARG0>(q0, q1, Jin, self, J, SETTO);
73 6 break;
74 6 case ARG1:
75
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 lg.template dDifference<ARG1>(q0, q1, Jin, self, J, SETTO);
76 6 break;
77 ✗ default:
78 ✗ throw std::invalid_argument("arg must be either ARG0 or ARG1");
79 }
80 12 return J;
81 }
82
83 12 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
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 JacobianMatrix_t J(Jin.rows(), Jin.cols());
92
2/3
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 switch (arg)
93 {
94 6 case ARG0:
95
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 lg.template dDifference<ARG0>(q0, q1, self, Jin, J, SETTO);
96 6 break;
97 6 case ARG1:
98
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 lg.template dDifference<ARG1>(q0, q1, self, Jin, J, SETTO);
99 6 break;
100 ✗ default:
101 ✗ throw std::invalid_argument("arg must be either ARG0 or ARG1");
102 }
103 12 return J;
104 }
105
106 12 static JacobianMatrix_t dIntegrate(
107 const LieGroupType & lg,
108 const ConfigVector_t & q,
109 const TangentVector_t & v,
110 const ArgumentPosition arg)
111 {
112
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 JacobianMatrix_t J(lg.nv(), lg.nv());
113
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 lg.dIntegrate(q, v, J, arg);
114 12 return J;
115 }
116
117 static JacobianMatrix_t
118 6 dIntegrate_dq1(const LieGroupType & lg, const ConfigVector_t & q, const TangentVector_t & v)
119 {
120
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 JacobianMatrix_t J(lg.nv(), lg.nv());
121
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 lg.dIntegrate_dq(q, v, J);
122 6 return J;
123 }
124
125 6 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
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 JacobianMatrix_t J(Jin.rows(), lg.nv());
133
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 lg.dIntegrate_dq(q, v, Jin, self, J, SETTO);
134 6 return J;
135 }
136
137 6 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
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 JacobianMatrix_t J(lg.nv(), Jin.cols());
145
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 lg.dIntegrate_dq(q, v, self, Jin, J, SETTO);
146 6 return J;
147 }
148
149 static JacobianMatrix_t
150 6 dIntegrate_dv1(const LieGroupType & lg, const ConfigVector_t & q, const TangentVector_t & v)
151 {
152
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 JacobianMatrix_t J(lg.nv(), lg.nv());
153
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 lg.dIntegrate_dv(q, v, J);
154 6 return J;
155 }
156
157 6 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
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 JacobianMatrix_t J(Jin.rows(), lg.nv());
165
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 lg.dIntegrate_dv(q, v, Jin, self, J, SETTO);
166 6 return J;
167 }
168
169 6 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
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 JacobianMatrix_t J(lg.nv(), Jin.cols());
177
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 lg.dIntegrate_dv(q, v, self, Jin, J, SETTO);
178 6 return J;
179 }
180
181 36 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
1/2
✓ Branch 3 taken 36 times.
✗ Branch 4 not taken.
 36 JacobianMatrix_t Jout(lg.nv(), J.cols());
189
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 lg.dIntegrateTransport(q, v, J, Jout, arg);
190 36 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 69 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 69 times.
✗ Branch 3 not taken.
 69 cl.def(bp::init<>("Default constructor"))
207
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("integrate", LieGroupWrapper::integrate)
208
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("dIntegrate", LieGroupWrapper::dIntegrate)
209
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq1)
210
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq2)
211
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("dIntegrate_dq", LieGroupWrapper::dIntegrate_dq3)
212
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv1)
213
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv2)
214
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("dIntegrate_dv", LieGroupWrapper::dIntegrate_dv3)
215
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("dIntegrateTransport", LieGroupWrapper::dIntegrateTransport_proxy)
216
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("difference", LieGroupWrapper::difference)
217
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("dDifference", LieGroupWrapper::dDifference1)
218
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("dDifference", LieGroupWrapper::dDifference2)
219
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("dDifference", LieGroupWrapper::dDifference3)
220
221
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("interpolate", LieGroupWrapper::interpolate)
222
223 138 .def(
224 "random", static_cast<typename LieGroupType::ConfigVector_t (LieGroupType::*)() const>(
225 &LieGroupType::random))
226
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(
227 "randomConfiguration",
228 &LieGroupType::template randomConfiguration<ConfigVector_t, ConfigVector_t>)
229
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("distance", &LieGroupType::template distance<ConfigVector_t, ConfigVector_t>)
230
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def(
231 "squaredDistance",
232 &LieGroupType::template squaredDistance<ConfigVector_t, ConfigVector_t>)
233
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .def("normalize", &LieGroupType::template normalize<ConfigVector_t>)
234
235
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .add_property("name", &LieGroupType::name)
236
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .add_property("neutral", &LieGroupType::neutral)
237
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .add_property("nq", &LieGroupType::nq)
238
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 .add_property("nv", &LieGroupType::nv)
239
240
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)
241
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 69 .def(bp::self *= bp::self)
242 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
243
1/2
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
 65 .def(bp::self == bp::self)
244 #endif
245 ;
246 69 }
247
248 69 static void expose(const char * name)
249 {
250
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 69 bp::class_<LieGroupType>(name, bp::no_init).def(LieGroupPythonVisitor<LieGroupType>());
251 69 }
252 };
253 } // namespace python
254 } // namespace pinocchio
255
256 #endif // ifndef __pinocchio_python_geometry_model_hpp__
257