GCC Code Coverage Report

Directory: ./
File: src/liegroup-space.cc
Date: 2025-05-04 12:09:19
Exec Total Coverage
Lines: 217 278 78.1%
Branches: 135 340 39.7%
Line Branch Exec Source
1 // Copyright (c) 2017, CNRS
2 // Authors: Florent Lamiraux
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <boost/serialization/export.hpp>
30 #include <boost/serialization/variant.hpp>
31 #include <boost/serialization/vector.hpp>
32 #include <boost/serialization/weak_ptr.hpp>
33 #include <hpp/pinocchio/liegroup-element.hh>
34 #include <hpp/pinocchio/liegroup-space.hh>
35 #include <hpp/pinocchio/liegroup/serialization.hh>
36 #include <hpp/pinocchio/serialization.hh>
37 #include <hpp/util/serialization.hh>
38 #include <pinocchio/serialization/eigen.hpp>
39
40 #include "../src/comparison.hh"
41 #include "../src/dintegrate-visitor.hh"
42 #include "../src/jdifference-visitor.hh"
43 #include "../src/size-visitor.hh"
44 #include "../src/visitor/interpolate.hh"
45
46 namespace hpp {
47 namespace pinocchio {
48
49 122 LiegroupSpacePtr_t LiegroupSpace::Rn(const size_type& n) {
50
1/2
✓ Branch 2 taken 122 times.
✗ Branch 3 not taken.
 122 LiegroupSpace* ptr(new LiegroupSpace(n));
51 122 LiegroupSpacePtr_t shPtr(ptr);
52
1/2
✓ Branch 2 taken 122 times.
✗ Branch 3 not taken.
 122 ptr->init(shPtr);
53 122 return shPtr;
54 }
55
56 /// Return \f$\mathbf{R}\f$ as a Lie group
57 3 LiegroupSpacePtr_t LiegroupSpace::R1(bool rotation) {
58 LiegroupSpace* ptr;
59
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 3 if (rotation) {
60 ✗ ptr = new LiegroupSpace(liegroup::VectorSpaceOperation<1, true>());
61 } else {
62
3/6
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
 3 ptr = new LiegroupSpace(liegroup::VectorSpaceOperation<1, false>());
63 }
64 3 LiegroupSpacePtr_t shPtr(ptr);
65
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 ptr->init(shPtr);
66 3 return shPtr;
67 }
68
69 /// Return \f$\mathbf{R}^2\f$ as a Lie group
70 6 LiegroupSpacePtr_t LiegroupSpace::R2() {
71 LiegroupSpace* ptr(
72
3/6
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
 6 new LiegroupSpace(liegroup::VectorSpaceOperation<2, false>()));
73 6 LiegroupSpacePtr_t shPtr(ptr);
74
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 ptr->init(shPtr);
75 6 return shPtr;
76 }
77
78 /// Return \f$\mathbf{R}^3\f$ as a Lie group
79 9 LiegroupSpacePtr_t LiegroupSpace::R3() {
80 LiegroupSpace* ptr(
81
3/6
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
 9 new LiegroupSpace(liegroup::VectorSpaceOperation<3, false>()));
82 9 LiegroupSpacePtr_t shPtr(ptr);
83
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 ptr->init(shPtr);
84 9 return shPtr;
85 }
86
87 /// Return \f$SO(2)\f$
88 1 LiegroupSpacePtr_t LiegroupSpace::SO2() {
89 LiegroupSpace* ptr(
90
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 new LiegroupSpace(liegroup::SpecialOrthogonalOperation<2>()));
91 1 LiegroupSpacePtr_t shPtr(ptr);
92
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 ptr->init(shPtr);
93 1 return shPtr;
94 }
95
96 /// Return \f$SO(3)\f$
97 1 LiegroupSpacePtr_t LiegroupSpace::SO3() {
98 LiegroupSpace* ptr(
99
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 new LiegroupSpace(liegroup::SpecialOrthogonalOperation<3>()));
100 1 LiegroupSpacePtr_t shPtr(ptr);
101
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 ptr->init(shPtr);
102 1 return shPtr;
103 }
104
105 /// Return \f$R^2\times SO(2)\f$
106 10 LiegroupSpacePtr_t LiegroupSpace::R2xSO2() {
107 LiegroupSpace* ptr(
108 10 new LiegroupSpace(liegroup::CartesianProductOperation<
109 liegroup::VectorSpaceOperation<2, false>,
110
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 10 liegroup::SpecialOrthogonalOperation<2> >()));
111 10 LiegroupSpacePtr_t shPtr(ptr);
112
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 ptr->init(shPtr);
113 10 return shPtr;
114 }
115
116 /// Return \f$R^3\times SO(3)\f$
117 15 LiegroupSpacePtr_t LiegroupSpace::R3xSO3() {
118 LiegroupSpace* ptr(
119 15 new LiegroupSpace(liegroup::CartesianProductOperation<
120 liegroup::VectorSpaceOperation<3, false>,
121
3/6
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
 15 liegroup::SpecialOrthogonalOperation<3> >()));
122 15 LiegroupSpacePtr_t shPtr(ptr);
123
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 ptr->init(shPtr);
124 15 return shPtr;
125 }
126
127 /// Return \f$SE(2)\f$
128 2 LiegroupSpacePtr_t LiegroupSpace::SE2() {
129 LiegroupSpace* ptr(
130
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 new LiegroupSpace(liegroup::SpecialEuclideanOperation<2>()));
131 2 LiegroupSpacePtr_t shPtr(ptr);
132
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 ptr->init(shPtr);
133 2 return shPtr;
134 }
135
136 /// Return \f$SE(3)\f$
137 4 LiegroupSpacePtr_t LiegroupSpace::SE3() {
138 LiegroupSpace* ptr(
139
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 4 new LiegroupSpace(liegroup::SpecialEuclideanOperation<3>()));
140 4 LiegroupSpacePtr_t shPtr(ptr);
141
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 ptr->init(shPtr);
142 4 return shPtr;
143 }
144
145 /// Return empty Lie group
146 1253 LiegroupSpacePtr_t LiegroupSpace::empty() {
147
1/2
✓ Branch 2 taken 1253 times.
✗ Branch 3 not taken.
 1253 LiegroupSpace* ptr(new LiegroupSpace());
148 1253 LiegroupSpacePtr_t shPtr(ptr);
149
1/2
✓ Branch 2 taken 1253 times.
✗ Branch 3 not taken.
 1253 ptr->init(shPtr);
150 1253 return shPtr;
151 }
152
153 118 size_type LiegroupSpace::nq(const std::size_t& rank) const {
154
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 118 times.
 118 assert(rank < liegroupTypes_.size());
155 118 liegroupType::SizeVisitor v;
156
1/2
✓ Branch 2 taken 118 times.
✗ Branch 3 not taken.
 118 boost::apply_visitor(v, liegroupTypes_[rank]);
157 118 return v.nq;
158 }
159
160 118 size_type LiegroupSpace::nv(const std::size_t& rank) const {
161
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 118 times.
 118 assert(rank < liegroupTypes_.size());
162 118 liegroupType::SizeVisitor v;
163
1/2
✓ Branch 2 taken 118 times.
✗ Branch 3 not taken.
 118 boost::apply_visitor(v, liegroupTypes_[rank]);
164 118 return v.nv;
165 }
166
167 2268 LiegroupElement LiegroupSpace::neutral() const {
168
1/2
✓ Branch 2 taken 2268 times.
✗ Branch 3 not taken.
 4536 return LiegroupElement(neutral_, weak_.lock());
169 }
170
171 1 LiegroupElement LiegroupSpace::element(vectorIn_t q) const {
172
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 return LiegroupElement(q, weak_.lock());
173 }
174
175 ✗ LiegroupElementRef LiegroupSpace::elementRef(vectorOut_t q) const {
176 ✗ return LiegroupElementRef(q, weak_.lock());
177 }
178
179 ✗ LiegroupElementConstRef LiegroupSpace::elementConstRef(vectorIn_t q) const {
180 ✗ return LiegroupElementConstRef(q, weak_.lock());
181 }
182
183 ✗ LiegroupElement LiegroupSpace::exp(vectorIn_t v) const { return neutral() + v; }
184
185 template <DerivativeProduct side>
186 ✗ void LiegroupSpace::dIntegrate_dq(LiegroupElementConstRef q, vectorIn_t v,
187 matrixOut_t Jq) const {
188 ✗ assert(q.size() == nq());
189 ✗ assert(v.size() == nv());
190 ✗ assert(Jq.rows() == nv());
191 ✗ size_type row = 0;
192 ✗ size_type configRow = 0;
193 ✗ liegroupType::dIntegrateVisitor<0, side> jiv(q.vector(), v, Jq, row,
194 configRow);
195 ✗ for (std::size_t i = 0; i < liegroupTypes_.size(); ++i) {
196 ✗ boost::apply_visitor(jiv, liegroupTypes_[i]);
197 }
198 ✗ assert(row == nv());
199 ✗ assert(configRow == nq());
200 }
201
202 template <DerivativeProduct side>
203 ✗ void LiegroupSpace::dIntegrate_dv(LiegroupElementConstRef q, vectorIn_t v,
204 matrixOut_t Jv) const {
205 ✗ assert(q.size() == nq());
206 ✗ assert(v.size() == nv());
207 ✗ assert(Jv.rows() == nv());
208 ✗ size_type row = 0;
209 ✗ size_type configRow = 0;
210 ✗ liegroupType::dIntegrateVisitor<1, side> jiv(q.vector(), v, Jv, row,
211 configRow);
212 ✗ for (std::size_t i = 0; i < liegroupTypes_.size(); ++i) {
213 ✗ boost::apply_visitor(jiv, liegroupTypes_[i]);
214 }
215 ✗ assert(row == nv());
216 ✗ assert(configRow == nq());
217 }
218
219 template void LiegroupSpace::dIntegrate_dq<DerivativeTimesInput>(
220 LiegroupElementConstRef, vectorIn_t, matrixOut_t) const;
221 template void LiegroupSpace::dIntegrate_dq<InputTimesDerivative>(
222 LiegroupElementConstRef, vectorIn_t, matrixOut_t) const;
223 template void LiegroupSpace::dIntegrate_dv<DerivativeTimesInput>(
224 LiegroupElementConstRef, vectorIn_t, matrixOut_t) const;
225 template void LiegroupSpace::dIntegrate_dv<InputTimesDerivative>(
226 LiegroupElementConstRef, vectorIn_t, matrixOut_t) const;
227
228 template <bool ApplyOnTheLeft>
229 ✗ void LiegroupSpace::Jdifference(vectorIn_t q0, vectorIn_t q1, matrixOut_t J0,
230 matrixOut_t J1) const {
231 ✗ dDifference_dq0<ApplyOnTheLeft ? DerivativeTimesInput : InputTimesDerivative>(
232 q0, q1, J0);
233 ✗ dDifference_dq1<ApplyOnTheLeft ? DerivativeTimesInput : InputTimesDerivative>(
234 q0, q1, J1);
235 }
236
237 template <DerivativeProduct side>
238 ✗ void LiegroupSpace::dDifference_dq0(vectorIn_t q0, vectorIn_t q1,
239 matrixOut_t J0) const {
240 ✗ assert(q0.size() == nq() && q1.size() == nq());
241
242 ✗ liegroupType::dDifferenceVisitor<ARG0, side> jdv(q0, q1, J0);
243 ✗ for (std::size_t i = 0; i < liegroupTypes_.size(); ++i) {
244 ✗ boost::apply_visitor(jdv, liegroupTypes_[i]);
245 }
246 ✗ assert(jdv.iq_ == nq());
247 ✗ assert(jdv.iv_ == nv());
248 }
249
250 template <DerivativeProduct side>
251 ✗ void LiegroupSpace::dDifference_dq1(vectorIn_t q0, vectorIn_t q1,
252 matrixOut_t J1) const {
253 ✗ assert(q0.size() == nq() && q1.size() == nq());
254
255 ✗ liegroupType::dDifferenceVisitor<ARG1, side> jdv(q0, q1, J1);
256 ✗ for (std::size_t i = 0; i < liegroupTypes_.size(); ++i) {
257 ✗ boost::apply_visitor(jdv, liegroupTypes_[i]);
258 }
259 ✗ assert(jdv.iq_ == nq());
260 ✗ assert(jdv.iv_ == nv());
261 }
262
263 template void LiegroupSpace::Jdifference<true>(vectorIn_t q0, vectorIn_t q1,
264 matrixOut_t J0,
265 matrixOut_t J1) const;
266 template void LiegroupSpace::Jdifference<false>(vectorIn_t q0, vectorIn_t q1,
267 matrixOut_t J0,
268 matrixOut_t J1) const;
269 template void LiegroupSpace::dDifference_dq0<DerivativeTimesInput>(
270 vectorIn_t, vectorIn_t, matrixOut_t) const;
271 template void LiegroupSpace::dDifference_dq0<InputTimesDerivative>(
272 vectorIn_t, vectorIn_t, matrixOut_t) const;
273 template void LiegroupSpace::dDifference_dq1<DerivativeTimesInput>(
274 vectorIn_t, vectorIn_t, matrixOut_t) const;
275 template void LiegroupSpace::dDifference_dq1<InputTimesDerivative>(
276 vectorIn_t, vectorIn_t, matrixOut_t) const;
277
278 ✗ void LiegroupSpace::interpolate(vectorIn_t q0, vectorIn_t q1, value_type u,
279 vectorOut_t r) const {
280 ✗ assert(q0.size() == nq());
281 ✗ assert(q1.size() == nq());
282 ✗ assert(r.size() == nq());
283
284 ✗ liegroupType::visitor::Interpolate iv(q0, q1, u, r);
285 ✗ for (LiegroupTypes::const_iterator it = liegroupTypes().begin();
286 ✗ it != liegroupTypes().end(); ++it)
287 ✗ boost::apply_visitor(iv, *it);
288 }
289
290 struct NameVisitor : public boost::static_visitor<> {
291 template <typename LgT1>
292 48 void operator()(const LgT1& lg1) {
293 48 name = lg1.name();
294 }
295 std::string name;
296 }; // struct NameVisitor
297
298 8 std::string LiegroupSpace::name() const {
299
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 std::ostringstream oss;
300 8 std::size_t size(liegroupTypes_.size());
301
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
 8 if (size == 0) {
302 ✗ return std::string("");
303 }
304
2/2
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 8 times.
 32 for (std::size_t i = 0; i < liegroupTypes_.size(); ++i) {
305 24 NameVisitor v;
306
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 24 boost::apply_visitor(v, liegroupTypes_[i]);
307
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 oss << v.name;
308
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 8 times.
 24 if (i < liegroupTypes_.size() - 1) {
309
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 oss << "*";
310 }
311 24 }
312
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 return oss.str();
313 8 }
314
315 434 bool LiegroupSpace::operator==(const LiegroupSpace& other) const {
316
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 434 times.
 434 if (liegroupTypes_.size() != other.liegroupTypes().size()) return false;
317 434 LiegroupTypes::const_iterator it1(liegroupTypes_.begin());
318 434 LiegroupTypes::const_iterator it2(other.liegroupTypes().begin());
319
2/2
✓ Branch 2 taken 436 times.
✓ Branch 3 taken 420 times.
 856 while (it1 != liegroupTypes_.end()) {
320 436 liegroup::level1::IsEqualVisitor v(*it2);
321
1/2
✓ Branch 2 taken 436 times.
✗ Branch 3 not taken.
 436 bool res = boost::apply_visitor(v, *it1);
322
2/2
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 422 times.
 436 if (!res) return false;
323 422 ++it1;
324 422 ++it2;
325 }
326 420 return true;
327 // return (liegroupTypes_ == other.liegroupTypes ());
328 }
329
330 14 bool LiegroupSpace::operator!=(const LiegroupSpace& other) const {
331 14 return !(operator==(other));
332 }
333
334 // Constructors
335
1/2
✓ Branch 2 taken 1265 times.
✗ Branch 3 not taken.
 1265 LiegroupSpace::LiegroupSpace() : nq_(0), nv_(0), neutral_(), weak_() {}
336
337 326 LiegroupSpace::LiegroupSpace(const size_type& size)
338
1/2
✓ Branch 2 taken 326 times.
✗ Branch 3 not taken.
 326 : nq_(0), nv_(0), neutral_(), weak_() {
339
2/4
✓ Branch 1 taken 326 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 326 times.
✗ Branch 5 not taken.
 326 liegroupTypes_.push_back(
340
1/2
✓ Branch 1 taken 326 times.
✗ Branch 2 not taken.
 326 liegroup::VectorSpaceOperation<Eigen::Dynamic, false>((int)size));
341
1/2
✓ Branch 1 taken 326 times.
✗ Branch 2 not taken.
 326 computeSize();
342
1/2
✓ Branch 1 taken 326 times.
✗ Branch 2 not taken.
 326 neutral_.resize(nq_);
343
1/2
✓ Branch 1 taken 326 times.
✗ Branch 2 not taken.
 326 neutral_.setZero();
344 326 }
345
346 24 LiegroupSpace::LiegroupSpace(const LiegroupSpace& other)
347 24 : liegroupTypes_(other.liegroupTypes_),
348 24 nq_(other.nq_),
349 24 nv_(other.nv_),
350
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 neutral_(other.neutral_),
351 24 weak_() {}
352
353 1165 LiegroupSpace::LiegroupSpace(const LiegroupType& type)
354
1/2
✓ Branch 2 taken 1165 times.
✗ Branch 3 not taken.
 1165 : liegroupTypes_(), neutral_(), weak_() {
355
1/2
✓ Branch 1 taken 1165 times.
✗ Branch 2 not taken.
 1165 liegroupTypes_.push_back(type);
356
1/2
✓ Branch 1 taken 1165 times.
✗ Branch 2 not taken.
 1165 computeSize();
357
1/2
✓ Branch 1 taken 1165 times.
✗ Branch 2 not taken.
 1165 computeNeutral();
358 1165 }
359
360 3777 void LiegroupSpace::computeSize() {
361 3777 nq_ = nv_ = 0;
362 3777 for (LiegroupTypes::const_iterator it = liegroupTypes_.begin();
363
2/2
✓ Branch 2 taken 4715 times.
✓ Branch 3 taken 3777 times.
 8492 it != liegroupTypes_.end(); ++it) {
364 4715 liegroupType::SizeVisitor v;
365
1/2
✓ Branch 2 taken 4715 times.
✗ Branch 3 not taken.
 4715 boost::apply_visitor(v, *it);
366 4715 nq_ += v.nq;
367 4715 nv_ += v.nv;
368 }
369 3777 }
370
371 3451 void LiegroupSpace::computeNeutral() {
372 3451 neutral_.resize(nq_);
373 3451 size_type start = 0;
374 3451 for (LiegroupTypes::const_iterator it = liegroupTypes_.begin();
375
2/2
✓ Branch 3 taken 4389 times.
✓ Branch 4 taken 3451 times.
 7840 it != liegroupTypes_.end(); ++it) {
376
1/2
✓ Branch 1 taken 4389 times.
✗ Branch 2 not taken.
 4389 liegroupType::NeutralVisitor v;
377
1/2
✓ Branch 2 taken 4389 times.
✗ Branch 3 not taken.
 4389 boost::apply_visitor(v, *it);
378
2/4
✓ Branch 2 taken 4389 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4389 times.
✗ Branch 6 not taken.
 4389 neutral_.segment(start, v.neutral.size()) = v.neutral;
379 4389 start += v.neutral.size();
380 4389 }
381
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3451 times.
 3451 assert(start == nq_);
382 3451 }
383
384 2768 void LiegroupSpace::init(const LiegroupSpaceWkPtr_t weak) { weak_ = weak; }
385
386 2274 void LiegroupSpace::mergeVectorSpaces() {
387
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2274 times.
 2274 if (liegroupTypes_.empty()) return;
388
389 2274 LiegroupTypes newLgT;
390
391 2274 liegroupType::IsVectorSpace ivs;
392 2274 liegroupType::SizeVisitor curSizes;
393 2274 size_type vsSize = 0;
394 2274 bool prevIsVectorSpace = false;
395
396 2274 for (LiegroupTypes::const_iterator _cur = liegroupTypes_.begin();
397
2/2
✓ Branch 3 taken 4223 times.
✓ Branch 4 taken 2274 times.
 6497 _cur != liegroupTypes_.end(); ++_cur) {
398
1/2
✓ Branch 2 taken 4223 times.
✗ Branch 3 not taken.
 4223 boost::apply_visitor(ivs, *_cur);
399 4223 bool curIsVectorSpace = ivs.isVectorSpace;
400
2/2
✓ Branch 0 taken 3226 times.
✓ Branch 1 taken 997 times.
 4223 if (curIsVectorSpace) {
401
1/2
✓ Branch 2 taken 3226 times.
✗ Branch 3 not taken.
 3226 boost::apply_visitor(curSizes, *_cur);
402 }
403
4/4
✓ Branch 0 taken 1056 times.
✓ Branch 1 taken 3167 times.
✓ Branch 2 taken 1013 times.
✓ Branch 3 taken 43 times.
 4223 if (prevIsVectorSpace && curIsVectorSpace) {
404 // Update previous liegroup type (which is a vector space).
405 1013 vsSize += curSizes.nq;
406
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1013 times.
 1013 assert(!newLgT.empty());
407
3/4
✗ Branch 0 not taken.
✓ Branch 1 taken 62 times.
✓ Branch 2 taken 42 times.
✓ Branch 3 taken 909 times.
 1013 switch (vsSize) {
408 ✗ case 1:
409 ✗ newLgT.back() = liegroup::VectorSpaceOperation<1, false>();
410 ✗ break;
411 62 case 2:
412
2/4
✓ Branch 2 taken 62 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 62 times.
✗ Branch 7 not taken.
 62 newLgT.back() = liegroup::VectorSpaceOperation<2, false>();
413 62 break;
414 42 case 3:
415
2/4
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 42 times.
✗ Branch 7 not taken.
 42 newLgT.back() = liegroup::VectorSpaceOperation<3, false>();
416 42 break;
417 909 default:
418
1/2
✓ Branch 1 taken 909 times.
✗ Branch 2 not taken.
 909 newLgT.back() = liegroup::VectorSpaceOperation<Eigen::Dynamic, false>(
419
1/2
✓ Branch 1 taken 909 times.
✗ Branch 2 not taken.
 909 (int)vsSize);
420 909 break;
421 }
422 } else {
423 // No merge to do.
424
2/2
✓ Branch 0 taken 2213 times.
✓ Branch 1 taken 997 times.
 3210 if (curIsVectorSpace) {
425 2213 vsSize = curSizes.nq;
426 2213 prevIsVectorSpace = true;
427 } else {
428 997 vsSize = 0;
429 997 prevIsVectorSpace = false;
430 }
431
1/2
✓ Branch 2 taken 3210 times.
✗ Branch 3 not taken.
 3210 newLgT.push_back(*_cur);
432 }
433 }
434
1/2
✓ Branch 1 taken 2274 times.
✗ Branch 2 not taken.
 2274 liegroupTypes_ = newLgT;
435
1/2
✓ Branch 1 taken 2274 times.
✗ Branch 2 not taken.
 2274 computeSize();
436
1/2
✓ Branch 1 taken 2274 times.
✗ Branch 2 not taken.
 2274 computeNeutral();
437 2274 }
438
439 ✗ LiegroupSpacePtr_t LiegroupSpace::vectorSpacesMerged() const {
440 ✗ LiegroupSpacePtr_t other(createCopy(weak_.lock()));
441 ✗ other->mergeVectorSpaces();
442 ✗ return other;
443 }
444
445 9 bool LiegroupSpace::isVectorSpace() const {
446 9 liegroupType::IsVectorSpace ivs;
447 9 for (LiegroupTypes::const_iterator _cur = liegroupTypes_.begin();
448
2/2
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 5 times.
 13 _cur != liegroupTypes_.end(); ++_cur) {
449
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 boost::apply_visitor(ivs, *_cur);
450
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
 8 if (!ivs.isVectorSpace) return false;
451 }
452 5 return true;
453 }
454
455 2250 LiegroupSpacePtr_t LiegroupSpace::operator*=(const LiegroupSpaceConstPtr_t& o) {
456
1/2
✓ Branch 6 taken 2250 times.
✗ Branch 7 not taken.
 2250 liegroupTypes_.insert(liegroupTypes_.end(), o->liegroupTypes_.begin(),
457 2250 o->liegroupTypes_.end());
458 2250 nq_ += o->nq_;
459 2250 nv_ += o->nv_;
460 2250 neutral_.conservativeResize(nq_);
461
2/4
✓ Branch 6 taken 2250 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2250 times.
✗ Branch 10 not taken.
 2250 neutral_.tail(o->nq()) = o->neutral().vector();
462 2250 mergeVectorSpaces();
463 2250 return weak_.lock();
464 }
465
466 template <class Archive>
467 48 void LiegroupSpace::serialize(Archive& ar, const unsigned int version) {
468 (void)version;
469
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 48 ar& BOOST_SERIALIZATION_NVP(liegroupTypes_);
470 if (!Archive::is_saving::value) {
471 24 computeSize();
472 24 computeNeutral();
473 }
474
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 48 ar& BOOST_SERIALIZATION_NVP(weak_);
475 }
476
477 HPP_SERIALIZATION_IMPLEMENT(LiegroupSpace);
478 } // namespace pinocchio
479 } // namespace hpp
480
481 namespace std {
482 using namespace hpp::pinocchio;
483
484 16 LiegroupSpacePtr_t operator*(const LiegroupSpaceConstPtr_t& sp1,
485 const LiegroupSpaceConstPtr_t& sp2) {
486 16 LiegroupSpacePtr_t res(LiegroupSpace::createCopy(sp1));
487
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 16 *res *= sp2;
488
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 16 res->mergeVectorSpaces();
489 16 return res;
490 }
491 /// Cartesian power by an integer
492 4 LiegroupSpacePtr_t operator^(const LiegroupSpaceConstPtr_t& sp, size_type n) {
493
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 assert(n >= 0);
494
3/4
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 4 if (n == 0) return LiegroupSpace::empty();
495
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 LiegroupSpacePtr_t result(LiegroupSpace::createCopy(sp));
496 3 --n;
497
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 3 times.
 11 while (n > 0) {
498
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 result = result * sp;
499 8 --n;
500 }
501
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 result->mergeVectorSpaces();
502 3 return result;
503 3 }
504 } // namespace std
505