GCC Code Coverage Report

Directory: ./
File: include/hpp/constraints/symbolic-calculus.hh
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 277 382 72.5%
Branches: 180 545 33.0%
Line Branch Exec Source
1 // Copyright (c) 2014, LAAS-CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
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 #ifndef HPP_CONSTRAINTS_SYMBOLIC_CALCULUS_HH
30 #define HPP_CONSTRAINTS_SYMBOLIC_CALCULUS_HH
31
32 #define HPP_CONSTRAINTS_CB_REF shared_ptr
33 #define HPP_CONSTRAINTS_CB_WKREF shared_ptr
34
35 #define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(op, InType, OutType) \
36 template <typename RhsType> \
37 typename Traits<OutType<RhsType> >::Ptr_t op( \
38 const InType& lhs, const HPP_CONSTRAINTS_CB_REF<RhsType>& rhs) { \
39 typedef OutType<RhsType> Op_t; \
40 return Op_t::create(lhs, rhs); \
41 }
42 #define HPP_CONSTRAINTS_CB_FRIEND_OPERATOR1(op, InType, OutType) \
43 template <typename RhsType> \
44 friend typename Traits<OutType<RhsType> >::Ptr_t op( \
45 const InType& lhs, const HPP_CONSTRAINTS_CB_REF<RhsType>& rhs);
46
47 #define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(op, OutType) \
48 template <typename LhsType, typename RhsType> \
49 typename Traits<OutType<LhsType, RhsType> >::Ptr_t op( \
50 const HPP_CONSTRAINTS_CB_REF<LhsType>& lhs, \
51 const HPP_CONSTRAINTS_CB_REF<RhsType>& rhs) { \
52 typedef OutType<LhsType, RhsType> Op_t; \
53 return Op_t::create(lhs, rhs); \
54 }
55
56 #define HPP_CONSTRAINTS_CB_FRIEND_OPERATOR2(op, OutType) \
57 template <typename LhsType, typename RhsType> \
58 friend typename Traits<OutType<LhsType, RhsType> >::Ptr_t op( \
59 const HPP_CONSTRAINTS_CB_REF<LhsType>& lhs, \
60 const HPP_CONSTRAINTS_CB_REF<RhsType>& rhs);
61
62 #define HPP_CONSTRAINTS_CB_CREATE1(Class, Arg0Type) \
63 static typename Traits<Class>::Ptr_t create(Arg0Type arg0) { \
64 typename Traits<Class>::Ptr_t ptr(new Class(arg0)); \
65 ptr->init(ptr); \
66 return ptr; \
67 }
68
69 #define HPP_CONSTRAINTS_CB_CREATE2(Class, Arg0Type, Arg1Type) \
70 static typename Traits<Class>::Ptr_t create(Arg0Type arg0, Arg1Type arg1) { \
71 typename Traits<Class>::Ptr_t ptr(new Class(arg0, arg1)); \
72 ptr->init(ptr); \
73 return ptr; \
74 }
75
76 #define HPP_CONSTRAINTS_CB_CREATE3(Class, Arg0Type, Arg1Type, Arg2Type) \
77 static typename Traits<Class>::Ptr_t create(Arg0Type arg0, Arg1Type arg1, \
78 Arg2Type arg2) { \
79 typename Traits<Class>::Ptr_t ptr(new Class(arg0, arg1, arg2)); \
80 ptr->init(ptr); \
81 return ptr; \
82 }
83
84 #include <Eigen/SVD>
85 #include <hpp/constraints/fwd.hh>
86 #include <hpp/constraints/macros.hh>
87 #include <hpp/constraints/svd.hh>
88 #include <hpp/constraints/tools.hh>
89 #include <hpp/pinocchio/center-of-mass-computation.hh>
90 #include <hpp/pinocchio/joint.hh>
91 #include <hpp/pinocchio/liegroup-element.hh>
92
93 namespace hpp {
94 namespace constraints {
95 /// \defgroup symbolic_calculus Symbolic calculus
96
97 /// \addtogroup symbolic_calculus
98 /// \{
99
100 template <typename ValueType, typename JacobianType>
101 class CalculusBaseAbstract;
102 template <typename T>
103 class Traits;
104
105 template <typename LhsValue, typename RhsValue>
106 class Expression;
107 template <typename LhsValue, typename RhsValue>
108 class CrossProduct;
109 template <typename LhsValue, typename RhsValue>
110 class ScalarProduct;
111 template <typename LhsValue, typename RhsValue>
112 class Difference;
113 template <typename LhsValue, typename RhsValue>
114 class Sum;
115 template <typename RhsValue>
116 class ScalarMultiply;
117 template <typename RhsValue>
118 class RotationMultiply;
119 typedef eigen::matrix3_t CrossMatrix;
120 typedef Eigen::Matrix<value_type, 1, Eigen::Dynamic, Eigen::RowMajor>
121 RowJacobianMatrix;
122 typedef Eigen::Matrix<value_type, 3, Eigen::Dynamic, Eigen::RowMajor>
123 JacobianMatrix;
124
125 template <typename Class>
126 struct Traits {
127 typedef HPP_CONSTRAINTS_CB_REF<Class> Ptr_t;
128 typedef HPP_CONSTRAINTS_CB_WKREF<Class> WkPtr_t;
129 };
130 template <>
131 struct Traits<value_type> {
132 typedef value_type Ptr_t;
133 typedef value_type WkPtr_t;
134 };
135 template <>
136 struct Traits<pinocchio::Joint> {
137 typedef JointPtr_t Ptr_t;
138 typedef JointPtr_t WkPtr_t;
139 };
140
141 struct JointTranspose {
142 1 JointTranspose(const JointPtr_t& joint) : j_(joint) {}
143 const JointPtr_t j_;
144 };
145 template <>
146 struct Traits<JointTranspose> {
147 typedef JointTranspose Ptr_t;
148 typedef JointTranspose WkPtr_t;
149 };
150
151 /// Abstract class defining a basic common interface.
152 ///
153 /// The purpose of this class is to allow the user to define an expression
154 /// without requiring to explicitly write its type. The type will be
155 /// automatically deduced by the compiler.
156 ///
157 /// \code
158 /// // First define a, b and c with basic elements.
159 /// CalculusBaseAbstract<OutValueType, OutJacobianType>::Ptr_t
160 /// myExpression_ptr =
161 /// CalculusBaseAbstract<OutValueType, OutJacobianType>::create (a + b * c)
162 /// \endcode
163 template <class ValueType = eigen::vector3_t,
164 class JacobianType = JacobianMatrix>
165 class CalculusBaseAbstract {
166 public:
167 typedef ValueType ValueType_t;
168 typedef JacobianType JacobianType_t;
169
170 virtual const ValueType& value() const = 0;
171 virtual const JacobianType& jacobian() const = 0;
172 virtual void computeValue(const ConfigurationIn_t arg) = 0;
173 virtual void computeJacobian(const ConfigurationIn_t arg) = 0;
174 virtual void invalidate() = 0;
175 };
176
177 /// Main abstract class.
178 ///
179 /// The framework is using CRTP to virtual function calls overload.
180 /// Keep in mind, so far, no mathematical simplification is done on the
181 /// expression you provide. It means that you may have a more efficient
182 /// expression by calculating and simplifying the jacobian yourself.
183 /// These classes provide:
184 /// \li a fast way of defining new constraints,
185 /// \li a robust way of calculation of jacobians -
186 /// hand calculation error proof
187 /// \li a fast way to check the results of your calculations.
188 template <class T, class ValueType = vector3_t,
189 class JacobianType = JacobianMatrix, class CrossType = CrossMatrix>
190 class CalculusBase : public CalculusBaseAbstract<ValueType, JacobianType> {
191 public:
192
3/5
✓ Branch 4 taken 49 times.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 49 times.
✗ Branch 8 not taken.
 118 CalculusBase() : cross_(CrossMatrix::Zero()) {}
193
194 5 CalculusBase(const ValueType& value, const JacobianType& jacobian)
195
5/8
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 5 : value_(value), jacobian_(jacobian), cross_(CrossMatrix::Zero()) {}
196
197 CalculusBase(const CalculusBase& o)
198 : value_(o.value()), jacobian_(o.jacobian_), cross_(o.cross()) {}
199
200 37270 inline const ValueType& value() const { return value_; }
201 109638 inline const JacobianType& jacobian() const { return jacobian_; }
202 21004 void computeValue(const ConfigurationIn_t arg) {
203
2/2
✓ Branch 0 taken 5520 times.
✓ Branch 1 taken 15484 times.
 42008 if (vValid_) return;
204
1/2
✓ Branch 2 taken 14714 times.
✗ Branch 3 not taken.
 30968 static_cast<T*>(this)->impl_value(arg);
205 30968 vValid_ = true;
206 }
207 48638 void computeJacobian(const ConfigurationIn_t arg) {
208
2/2
✓ Branch 0 taken 41806 times.
✓ Branch 1 taken 6832 times.
 97276 if (jValid_) return;
209
1/2
✓ Branch 2 taken 5016 times.
✗ Branch 3 not taken.
 13664 static_cast<T*>(this)->impl_jacobian(arg);
210 13664 jValid_ = true;
211 }
212 22026 void invalidate() {
213 44052 vValid_ = false;
214 44052 jValid_ = false;
215 44052 cValid_ = false;
216 44052 }
217 5059 inline const CrossType& cross() const { return cross_; }
218 5059 void computeCrossValue(const ConfigurationIn_t arg) {
219
2/2
✓ Branch 0 taken 1284 times.
✓ Branch 1 taken 3775 times.
 10018 if (cValid_) return;
220
1/2
✓ Branch 2 taken 3775 times.
✗ Branch 3 not taken.
 7450 computeValue(arg);
221 7450 computeCrossMatrix(value_, cross_);
222 7450 cValid_ = true;
223 }
224
225 protected:
226 ValueType value_;
227 JacobianType jacobian_;
228 CrossType cross_;
229
230 bool vValid_, jValid_, cValid_;
231
232 122 void init(const typename Traits<T>::Ptr_t& ptr) { wkPtr_ = ptr; }
233
234 private:
235 typename Traits<T>::WkPtr_t wkPtr_;
236
237 public:
238 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
239 };
240
241 15 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(operator-, Difference)
242 2 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(operator+, Sum)
243 29 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(operator*, ScalarProduct)
244 15 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(operator^, CrossProduct)
245
246 1 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(operator*, value_type, ScalarMultiply)
247 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(operator*, JointPtr_t, RotationMultiply)
248 1 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(operator*, JointTranspose, RotationMultiply)
249
250 /// Base class for classes representing an operation.
251 template <typename LhsValue, typename RhsValue>
252 class Expression {
253 public:
254 typedef shared_ptr<Expression<LhsValue, RhsValue> > Ptr_t;
255 typedef weak_ptr<Expression<LhsValue, RhsValue> > WkPtr_t;
256
257 static Ptr_t create() {
258 Ptr_t p(new Expression());
259 p->init(p);
260 return p;
261 }
262
263 80 static Ptr_t create(const typename Traits<LhsValue>::Ptr_t& lhs,
264 const typename Traits<RhsValue>::Ptr_t& rhs) {
265
1/2
✓ Branch 3 taken 40 times.
✗ Branch 4 not taken.
 80 Ptr_t p(new Expression(lhs, rhs));
266 80 p->init(p);
267 80 return p;
268 }
269
270 const LhsValue& lhs() const { return lhs_; }
271
272 const RhsValue& rhs() const { return rhs_; }
273
274 Expression() {}
275
276 Expression(const Expression& other) : rhs_(other.rhs()), lhs_(other.lhs()) {}
277
278 80 Expression(const typename Traits<LhsValue>::Ptr_t& lhs,
279 const typename Traits<RhsValue>::Ptr_t& rhs)
280 80 : rhs_(rhs), lhs_(lhs) {}
281
282 80 inline void init(Ptr_t self) { self_ = self; }
283
284 typename Traits<RhsValue>::Ptr_t rhs_;
285 typename Traits<LhsValue>::Ptr_t lhs_;
286 typename Traits<Expression>::WkPtr_t self_;
287
288 public:
289 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
290 };
291
292 /// Cross product of two expressions.
293 template <typename LhsValue, typename RhsValue>
294 class CrossProduct : public CalculusBase<CrossProduct<LhsValue, RhsValue> > {
295 public:
296 typedef CalculusBase<CrossProduct<LhsValue, RhsValue> > Parent_t;
297
298
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
 26 HPP_CONSTRAINTS_CB_CREATE2(CrossProduct,
299 const typename Traits<LhsValue>::Ptr_t&,
300 const typename Traits<RhsValue>::Ptr_t&)
301
302 CrossProduct() {}
303
304 CrossProduct(const CalculusBase<CrossProduct>& other)
305 : Parent_t(other), e_(static_cast<const CrossProduct&>(other).e_) {}
306
307 26 CrossProduct(const typename Traits<LhsValue>::Ptr_t& lhs,
308 const typename Traits<RhsValue>::Ptr_t& rhs)
309
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 26 : e_(Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
310
311 5742 void impl_value(const ConfigurationIn_t arg) {
312
1/2
✓ Branch 4 taken 2871 times.
✗ Branch 5 not taken.
 5742 e_->lhs_->computeCrossValue(arg);
313
1/2
✓ Branch 4 taken 2871 times.
✗ Branch 5 not taken.
 5742 e_->rhs_->computeValue(arg);
314
1/2
✓ Branch 8 taken 2871 times.
✗ Branch 9 not taken.
 5742 this->value_ = e_->lhs_->cross() * e_->rhs_->value();
315 5742 }
316 1044 void impl_jacobian(const ConfigurationIn_t arg) {
317
1/2
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
 2088 e_->lhs_->computeCrossValue(arg);
318
1/2
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
 2088 e_->rhs_->computeCrossValue(arg);
319
1/2
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
 2088 e_->lhs_->computeJacobian(arg);
320
1/2
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
 2088 e_->rhs_->computeJacobian(arg);
321
4/8
✓ Branch 3 taken 1044 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 1044 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1044 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1044 times.
✗ Branch 16 not taken.
 2088 this->jacobian_ = e_->lhs_->cross() * e_->rhs_->jacobian() -
322 2088 e_->rhs_->cross() * e_->lhs_->jacobian();
323 2088 }
324 2871 void invalidate() {
325 5742 Parent_t::invalidate();
326 5742 e_->rhs_->invalidate();
327 5742 e_->lhs_->invalidate();
328 5742 }
329
330 protected:
331 typename Expression<LhsValue, RhsValue>::Ptr_t e_;
332
333 friend class Expression<LhsValue, RhsValue>;
334 };
335
336 /// Scalar product of two expressions.
337 template <typename LhsValue, typename RhsValue>
338 class ScalarProduct
339 : public CalculusBase<ScalarProduct<LhsValue, RhsValue>,
340 Eigen::Matrix<value_type, 1, 1>, RowJacobianMatrix> {
341 public:
342 typedef CalculusBase<ScalarProduct<LhsValue, RhsValue>,
343 Eigen::Matrix<value_type, 1, 1>, RowJacobianMatrix>
344 Parent_t;
345
346
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 29 HPP_CONSTRAINTS_CB_CREATE2(ScalarProduct,
347 const typename Traits<LhsValue>::Ptr_t&,
348 const typename Traits<RhsValue>::Ptr_t&)
349
350 ScalarProduct() {}
351
352 ScalarProduct(const CalculusBase<ScalarProduct>& other)
353 : CalculusBase<ScalarProduct>(other),
354 e_(static_cast<const ScalarProduct&>(other).e_) {}
355
356 29 ScalarProduct(const typename Traits<LhsValue>::Ptr_t& lhs,
357 const typename Traits<RhsValue>::Ptr_t& rhs)
358
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 29 : e_(Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
359
360 2718 void impl_value(const ConfigurationIn_t arg) {
361
1/2
✓ Branch 4 taken 1409 times.
✗ Branch 5 not taken.
 2718 e_->lhs_->computeValue(arg);
362
1/2
✓ Branch 4 taken 1409 times.
✗ Branch 5 not taken.
 2718 e_->rhs_->computeValue(arg);
363 2718 this->value_[0] = e_->lhs_->value().dot(e_->rhs_->value());
364 2718 }
365 706 void impl_jacobian(const ConfigurationIn_t arg) {
366
1/2
✓ Branch 4 taken 706 times.
✗ Branch 5 not taken.
 1312 e_->lhs_->computeValue(arg);
367
1/2
✓ Branch 4 taken 706 times.
✗ Branch 5 not taken.
 1312 e_->rhs_->computeValue(arg);
368
1/2
✓ Branch 4 taken 706 times.
✗ Branch 5 not taken.
 1312 e_->lhs_->computeJacobian(arg);
369
1/2
✓ Branch 4 taken 706 times.
✗ Branch 5 not taken.
 1312 e_->rhs_->computeJacobian(arg);
370
6/12
✓ Branch 3 taken 706 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 706 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 706 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 706 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 706 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 706 times.
✗ Branch 21 not taken.
 1312 this->jacobian_ = e_->lhs_->value().transpose() * e_->rhs_->jacobian() +
371
1/2
✓ Branch 8 taken 706 times.
✗ Branch 9 not taken.
 1312 e_->rhs_->value().transpose() * e_->lhs_->jacobian();
372 1312 }
373 2015 void invalidate() {
374 3930 Parent_t::invalidate();
375 3930 e_->rhs_->invalidate();
376 3930 e_->lhs_->invalidate();
377 3930 }
378
379 protected:
380 typename Expression<LhsValue, RhsValue>::Ptr_t e_;
381
382 friend class Expression<LhsValue, RhsValue>;
383 };
384
385 /// Difference of two expressions.
386 template <typename LhsValue, typename RhsValue>
387 class Difference : public CalculusBase<Difference<LhsValue, RhsValue> > {
388 public:
389 typedef CalculusBase<Difference<LhsValue, RhsValue> > Parent_t;
390
391
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 15 HPP_CONSTRAINTS_CB_CREATE2(Difference,
392 const typename Traits<LhsValue>::Ptr_t&,
393 const typename Traits<RhsValue>::Ptr_t&)
394
395 Difference() {}
396
397 Difference(const CalculusBase<Difference>& other)
398 : CalculusBase<Difference>(other),
399 e_(static_cast<const Difference&>(other).e_) {}
400
401 15 Difference(const typename Traits<LhsValue>::Ptr_t& lhs,
402 const typename Traits<RhsValue>::Ptr_t& rhs)
403
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 15 : e_(Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
404
405 6614 void impl_value(const ConfigurationIn_t arg) {
406
1/2
✓ Branch 4 taken 3357 times.
✗ Branch 5 not taken.
 6614 e_->lhs_->computeValue(arg);
407
1/2
✓ Branch 4 taken 3357 times.
✗ Branch 5 not taken.
 6614 e_->rhs_->computeValue(arg);
408
1/2
✓ Branch 8 taken 3357 times.
✗ Branch 9 not taken.
 6614 this->value_ = e_->lhs_->value() - e_->rhs_->value();
409 6614 }
410 1266 void impl_jacobian(const ConfigurationIn_t arg) {
411
1/2
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
 2432 e_->lhs_->computeJacobian(arg);
412
1/2
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
 2432 e_->rhs_->computeJacobian(arg);
413
1/2
✓ Branch 8 taken 1266 times.
✗ Branch 9 not taken.
 2432 this->jacobian_ = e_->lhs_->jacobian() - e_->rhs_->jacobian();
414 2432 }
415 4117 void invalidate() {
416 8134 Parent_t::invalidate();
417 8134 e_->rhs_->invalidate();
418 8134 e_->lhs_->invalidate();
419 8134 }
420
421 protected:
422 typename Expression<LhsValue, RhsValue>::Ptr_t e_;
423
424 friend class Expression<LhsValue, RhsValue>;
425 };
426
427 /// Sum of two expressions.
428 template <typename LhsValue, typename RhsValue>
429 class Sum : public CalculusBase<Sum<LhsValue, RhsValue> > {
430 public:
431 typedef CalculusBase<Sum<LhsValue, RhsValue> > Parent_t;
432
433
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 4 HPP_CONSTRAINTS_CB_CREATE2(Sum, const typename Traits<LhsValue>::Ptr_t&,
434 const typename Traits<RhsValue>::Ptr_t&)
435
436 Sum() {}
437
438 Sum(const CalculusBase<Sum>& other)
439 : CalculusBase<Sum>(other), e_(static_cast<const Sum&>(other).e_) {}
440
441 4 Sum(const typename Traits<RhsValue>::Ptr_t& rhs,
442 const typename Traits<LhsValue>::Ptr_t& lhs)
443
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 : e_(Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
444
445 400 void impl_value(const ConfigurationIn_t arg) {
446
1/2
✓ Branch 4 taken 200 times.
✗ Branch 5 not taken.
 400 e_->lhs_->computeValue(arg);
447
1/2
✓ Branch 4 taken 200 times.
✗ Branch 5 not taken.
 400 e_->rhs_->computeValue(arg);
448
1/2
✓ Branch 8 taken 200 times.
✗ Branch 9 not taken.
 400 this->value_ = e_->lhs_->value() + e_->rhs_->value();
449 400 }
450 200 void impl_jacobian(const ConfigurationIn_t arg) {
451
1/2
✓ Branch 4 taken 200 times.
✗ Branch 5 not taken.
 400 e_->lhs_->computeJacobian(arg);
452
1/2
✓ Branch 4 taken 200 times.
✗ Branch 5 not taken.
 400 e_->rhs_->computeJacobian(arg);
453
1/2
✓ Branch 8 taken 200 times.
✗ Branch 9 not taken.
 400 this->jacobian_ = e_->lhs_->jacobian() + e_->rhs_->jacobian();
454 400 }
455 200 void invalidate() {
456 400 Parent_t::invalidate();
457 400 e_->rhs_->invalidate();
458 400 e_->lhs_->invalidate();
459 400 }
460
461 protected:
462 typename Expression<LhsValue, RhsValue>::Ptr_t e_;
463
464 friend class Expression<LhsValue, RhsValue>;
465 };
466
467 /// Multiplication of an expression by a scalar.
468 template <typename RhsValue>
469 class ScalarMultiply : public CalculusBase<ScalarMultiply<RhsValue> > {
470 public:
471 typedef CalculusBase<ScalarMultiply<RhsValue> > Parent_t;
472
473
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 HPP_CONSTRAINTS_CB_CREATE2(ScalarMultiply,
474 const typename Traits<value_type>::Ptr_t&,
475 const typename Traits<RhsValue>::Ptr_t&)
476
477 ScalarMultiply() {}
478
479 ScalarMultiply(const CalculusBase<ScalarMultiply>& other)
480 : CalculusBase<ScalarMultiply>(other),
481 e_(static_cast<const ScalarMultiply&>(other).e_) {}
482
483 1 ScalarMultiply(const typename Traits<value_type>::Ptr_t& scalar,
484 const typename Traits<RhsValue>::Ptr_t& rhs)
485
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 : e_(Expression<value_type, RhsValue>::create(scalar, rhs)) {}
486
487 100 void impl_value(const ConfigurationIn_t arg) {
488
1/2
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 100 e_->rhs_->computeValue(arg);
489
1/2
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
 100 this->value_ = e_->lhs_ * e_->rhs_->value();
490 100 }
491 100 void impl_jacobian(const ConfigurationIn_t arg) {
492
1/2
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 100 e_->rhs_->computeJacobian(arg);
493
1/2
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
 100 this->jacobian_ = e_->lhs_ * e_->rhs_->jacobian();
494 100 }
495 100 void invalidate() {
496 100 Parent_t::invalidate();
497 100 e_->rhs_->invalidate();
498 100 }
499
500 protected:
501 typename Expression<value_type, RhsValue>::Ptr_t e_;
502
503 friend class Expression<value_type, RhsValue>;
504 };
505
506 /// Multiplication of an expression by a rotation matrix.
507 template <typename RhsValue>
508 class RotationMultiply : public CalculusBase<RotationMultiply<RhsValue> > {
509 public:
510 typedef CalculusBase<RotationMultiply<RhsValue> > Parent_t;
511
512 RotationMultiply() {}
513
514 RotationMultiply(const CalculusBase<RotationMultiply>& other)
515 : CalculusBase<RotationMultiply>(other),
516 e_(static_cast<const RotationMultiply&>(other).e_),
517 transpose_(other.transpose_) {}
518
519 HPP_CONSTRAINTS_CB_CREATE2(RotationMultiply,
520 const typename Traits<pinocchio::Joint>::Ptr_t&,
521 const typename Traits<RhsValue>::Ptr_t&)
522
523
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 HPP_CONSTRAINTS_CB_CREATE2(RotationMultiply,
524 const typename Traits<JointTranspose>::Ptr_t&,
525 const typename Traits<RhsValue>::Ptr_t&)
526
527 1 RotationMultiply(const typename Traits<JointTranspose>::Ptr_t& joint,
528 const typename Traits<RhsValue>::Ptr_t& rhs)
529 1 : e_(Expression<pinocchio::Joint, RhsValue>::create(joint.j_, rhs)),
530
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 transpose_(true) {}
531
532 RotationMultiply(const typename Traits<pinocchio::Joint>::Ptr_t& joint,
533 const typename Traits<RhsValue>::Ptr_t& rhs,
534 bool transpose = false)
535 : e_(Expression<pinocchio::Joint, RhsValue>::create(joint, rhs)),
536 transpose_(transpose) {}
537
538 100 void impl_value(const ConfigurationIn_t arg) {
539
1/2
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 100 e_->rhs_->computeValue(arg);
540 100 const matrix3_t& R = e_->lhs_->currentTransformation().rotation();
541
1/2
✓ Branch 0 taken 100 times.
✗ Branch 1 not taken.
 100 if (transpose_)
542
2/4
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
 100 this->value_ = R.transpose() * e_->rhs_->value();
543 else
544 ✗ this->value_ = R * e_->rhs_->value();
545 100 }
546 100 void impl_jacobian(const ConfigurationIn_t arg) {
547
1/2
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 100 e_->rhs_->computeJacobian(arg);
548
1/2
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 100 e_->rhs_->computeCrossValue(arg);
549 100 const JointJacobian_t& J = e_->lhs_->jacobian();
550 100 const matrix3_t& R = e_->lhs_->currentTransformation().rotation();
551
1/2
✓ Branch 0 taken 100 times.
✗ Branch 1 not taken.
 100 if (transpose_)
552
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 this->jacobian_ =
553
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 R.transpose() *
554
2/4
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
 200 ((e_->rhs_->cross() * R) * J.bottomRows<3>() + e_->rhs_->jacobian());
555 else
556 ✗ this->jacobian_ = R * ((e_->rhs_->cross() * R) * J.bottomRows<3>() +
557 ✗ e_->rhs_->jacobian());
558 100 }
559 100 void invalidate() {
560 100 Parent_t::invalidate();
561 100 e_->rhs_->invalidate();
562 100 }
563
564 protected:
565 typename Expression<pinocchio::Joint, RhsValue>::Ptr_t e_;
566
567 friend class Expression<pinocchio::Joint, RhsValue>;
568
569 private:
570 bool transpose_;
571 };
572
573 /// Basic expression representing a point in a joint frame.
574 class PointInJoint : public CalculusBase<PointInJoint> {
575 public:
576 typedef CalculusBase<PointInJoint> Parent_t;
577
578
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 HPP_CONSTRAINTS_CB_CREATE2(PointInJoint, const JointPtr_t&, const vector3_t&)
579 ✗ HPP_CONSTRAINTS_CB_CREATE3(PointInJoint, const JointPtr_t&, const vector3_t&,
580 const size_type&)
581
582 PointInJoint() {}
583
584 PointInJoint(const CalculusBase<PointInJoint>& other)
585 : CalculusBase<PointInJoint>(other) {
586 const PointInJoint& o = static_cast<const PointInJoint&>(other);
587 joint_ = o.joint();
588 local_ = o.local();
589 center_ = local_.isZero();
590 }
591
592 PointInJoint(const PointInJoint& pointInJoint)
593 : CalculusBase<PointInJoint>(pointInJoint),
594 joint_(pointInJoint.joint()),
595 local_(pointInJoint.local()),
596 center_(local_.isZero()) {}
597
598 2 PointInJoint(const JointPtr_t& joint, const vector3_t& pointInLocalFrame)
599 4 : joint_(joint),
600
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 local_(pointInLocalFrame),
601
1/2
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 center_(pointInLocalFrame.isZero()) {
602
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(joint_ != NULL);
603 2 }
604
605 ✗ PointInJoint(const JointPtr_t& joint, const vector3_t& pointInLocalFrame,
606 size_type nbDof)
607 ✗ : joint_(joint),
608 ✗ local_(pointInLocalFrame),
609 ✗ center_(pointInLocalFrame.isZero()) {
610 ✗ if (joint_ == NULL) {
611 ✗ for (int i = 0; i < 3; ++i) this->value_[i] = local_[i];
612 ✗ this->jacobian_.resize(3, nbDof);
613 ✗ this->jacobian_.setZero();
614 ✗ this->cross_.setZero();
615 }
616 }
617
618 ✗ const JointPtr_t& joint() const { return joint_; }
619 ✗ const vector3_t& local() const { return local_; }
620 200 void impl_value(const ConfigurationIn_t) {
621
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 200 times.
 200 if (joint_ == NULL) return;
622 200 this->value_ = joint_->currentTransformation().act(local_);
623 }
624 200 void impl_jacobian(const ConfigurationIn_t) {
625
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 200 times.
 200 if (joint_ == NULL) return;
626 200 const JointJacobian_t& J(joint_->jacobian());
627 200 const matrix3_t& R = joint_->currentTransformation().rotation();
628
3/6
✓ Branch 2 taken 200 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 200 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 200 times.
✗ Branch 9 not taken.
 200 this->jacobian_.noalias() = R * J.topRows<3>();
629
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 200 times.
 200 if (!center_) {
630 ✗ computeCrossRXl();
631 ✗ this->jacobian_.noalias() -= (this->cross_ * R) * J.bottomRows<3>();
632 }
633 }
634 ✗ void computeCrossRXl() {
635 ✗ if (joint_ == NULL) return;
636 ✗ if (center_) {
637 ✗ this->cross_.setZero();
638 ✗ return;
639 }
640 ✗ computeCrossMatrix(joint_->currentTransformation().rotation() * local_,
641 ✗ this->cross_);
642 }
643
644 protected:
645 JointPtr_t joint_;
646 vector3_t local_;
647 bool center_;
648
649 public:
650 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
651 };
652
653 /// Basic expression representing a vector in a joint frame.
654 class VectorInJoint : public CalculusBase<VectorInJoint> {
655 public:
656 HPP_CONSTRAINTS_CB_CREATE2(VectorInJoint, const JointPtr_t&, const vector3_t&)
657 ✗ HPP_CONSTRAINTS_CB_CREATE3(VectorInJoint, const JointPtr_t&, const vector3_t&,
658 const size_type&)
659
660 VectorInJoint() {}
661
662 VectorInJoint(const CalculusBase<VectorInJoint>& other)
663 : CalculusBase<VectorInJoint>(other),
664 joint_(static_cast<const VectorInJoint&>(other).joint()),
665 vector_(static_cast<const VectorInJoint&>(other).vector()) {}
666
667 VectorInJoint(const VectorInJoint& vectorInJoint)
668 : CalculusBase<VectorInJoint>(vectorInJoint),
669 joint_(vectorInJoint.joint()),
670 vector_(vectorInJoint.vector()) {}
671
672 VectorInJoint(const JointPtr_t& joint, const vector3_t& vectorInLocalFrame)
673 : joint_(joint), vector_(vectorInLocalFrame) {}
674
675 ✗ VectorInJoint(const JointPtr_t& joint, const vector3_t& vectorInLocalFrame,
676 const size_type& nbDof)
677 ✗ : joint_(joint), vector_(vectorInLocalFrame) {
678 ✗ if (joint_ == NULL) {
679 ✗ for (int i = 0; i < 3; ++i) this->value_[i] = vector_[i];
680 ✗ this->jacobian_.resize(3, nbDof);
681 ✗ this->jacobian_.setZero();
682 ✗ this->cross_.setZero();
683 }
684 }
685
686 const JointPtr_t& joint() const { return joint_; }
687 const vector3_t& vector() const { return vector_; }
688 ✗ void impl_value(const ConfigurationIn_t) {
689 ✗ if (joint_ == NULL) return;
690 ✗ this->value_ = joint_->currentTransformation().rotation() * vector_;
691 }
692 ✗ void impl_jacobian(const ConfigurationIn_t) {
693 ✗ if (joint_ == NULL) return;
694 ✗ const JointJacobian_t& J(joint_->jacobian());
695 ✗ const matrix3_t& R = joint_->currentTransformation().rotation();
696 ✗ computeCrossRXl();
697 ✗ this->jacobian_.noalias() = (-this->cross_ * R) * J.bottomRows<3>();
698 }
699 ✗ void computeCrossRXl() {
700 ✗ if (joint_ == NULL) return;
701 ✗ computeCrossMatrix(joint_->currentTransformation().rotation() * vector_,
702 ✗ this->cross_);
703 }
704
705 protected:
706 JointPtr_t joint_;
707 vector3_t vector_;
708
709 public:
710 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
711 };
712
713 /// Basic expression mapping a function as an expression
714 template <typename FunctionType>
715 class FunctionExp
716 : public CalculusBase<FunctionExp<FunctionType>, vector_t, matrix_t> {
717 public:
718 typedef CalculusBase<FunctionExp<FunctionType>, vector_t, matrix_t> Parent_t;
719 typedef shared_ptr<FunctionType> FunctionTypePtr_t;
720
721
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 HPP_CONSTRAINTS_CB_CREATE1(FunctionExp<FunctionType>,
722 const FunctionTypePtr_t&)
723
724 FunctionExp() {}
725
726 FunctionExp(const Parent_t& other)
727 : Parent_t(other), f_(other.f_), lge_(other.lge_) {}
728
729 FunctionExp(const FunctionExp& other)
730 : Parent_t(other), f_(other.f_), lge_(other.lge_) {}
731
732 /// Constructor
733 ///
734 /// \param func the inner function
735 1 FunctionExp(const FunctionTypePtr_t& func)
736 1 : Parent_t(vector_t::Zero(func->outputSize()),
737 1 matrix_t::Zero(func->outputDerivativeSize(),
738 1 func->inputDerivativeSize())),
739 1 f_(func),
740
5/10
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
 3 lge_(func->outputSpace()) {}
741
742 100 void impl_value(const ConfigurationIn_t arg) {
743
2/4
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
 100 f_->value(lge_, arg);
744 100 this->value_ = lge_.vector();
745 100 }
746 100 void impl_jacobian(const ConfigurationIn_t arg) {
747
2/4
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
 100 f_->jacobian(this->jacobian_, arg);
748 100 }
749
750 private:
751 FunctionTypePtr_t f_;
752 LiegroupElement lge_;
753 };
754
755 /// Basic expression representing a static point
756 ///
757 /// Its value is constant and its jacobian is a zero matrix.
758 class Point : public CalculusBase<Point, vector3_t, JacobianMatrix> {
759 public:
760
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 HPP_CONSTRAINTS_CB_CREATE2(Point, const vector3_t&, const size_t&)
761
762 Point() {}
763
764 Point(const CalculusBase<Point, vector3_t, JacobianMatrix>& other)
765 : CalculusBase<Point, eigen::vector3_t, JacobianMatrix>(other) {}
766
767 Point(const Point& point)
768 : CalculusBase<Point, vector3_t, JacobianMatrix>(point) {}
769
770 /// Constructor
771 ///
772 /// \param point the static point
773 /// \param jacobianNbCols number of column of the jacobian
774 1 Point(const vector3_t& point, size_t jacobianNbCols)
775 1 : CalculusBase<Point, vector3_t, JacobianMatrix>(
776
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 point, JacobianMatrix::Zero(3, jacobianNbCols)) {}
777
778 770 void impl_value(const ConfigurationIn_t) {}
779 244 void impl_jacobian(const ConfigurationIn_t) {}
780 };
781
782 /// Basic expression representing a COM.
783 class PointCom : public CalculusBase<PointCom, vector3_t, ComJacobian_t> {
784 public:
785 typedef CalculusBase<PointCom, vector3_t, ComJacobian_t> Parent_t;
786 ✗ HPP_CONSTRAINTS_CB_CREATE1(PointCom, const CenterOfMassComputationPtr_t&)
787
788 PointCom() {}
789
790 PointCom(const Parent_t& other)
791 : Parent_t(other),
792 comc_(static_cast<const PointCom&>(other).centerOfMassComputation()) {}
793
794 ✗ PointCom(const CenterOfMassComputationPtr_t& comc) : comc_(comc) {}
795
796 ✗ inline const vector3_t& value() const { return comc_->com(); }
797 ✗ inline const ComJacobian_t& jacobian() const { return comc_->jacobian(); }
798
799 ✗ const CenterOfMassComputationPtr_t& centerOfMassComputation() const {
800 ✗ return comc_;
801 }
802 ✗ void impl_value(const ConfigurationIn_t) {
803 ✗ comc_->compute(hpp::pinocchio::COM);
804 }
805 ✗ void impl_jacobian(const ConfigurationIn_t) {
806 ✗ comc_->compute(hpp::pinocchio::COMPUTE_ALL);
807 // TODO: there is memory and time to be saved here as this copy is
808 // not important.
809 // this->jacobian_ = comc_->jacobian ();
810 }
811
812 protected:
813 CenterOfMassComputationPtr_t comc_;
814 };
815
816 class JointFrame
817 : public CalculusBase<JointFrame, Eigen::Matrix<value_type, 6, 1>,
818 Eigen::Matrix<value_type, 6, Eigen::Dynamic> > {
819 public:
820 typedef CalculusBase<JointFrame, ValueType_t, JacobianType_t> Parent_t;
821
822
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 HPP_CONSTRAINTS_CB_CREATE1(JointFrame, const JointPtr_t&)
823
824 JointFrame() {}
825
826 JointFrame(const Parent_t& other)
827 : Parent_t(other), joint_(static_cast<const JointFrame&>(other).joint_) {}
828
829 JointFrame(const JointFrame& jf) : Parent_t(jf), joint_(jf.joint()) {}
830
831 1 JointFrame(const JointPtr_t& joint) : joint_(joint) {
832
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(joint_ != NULL);
833
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 this->jacobian_.resize(6,
834
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 joint->robot()->numberDof() -
835 2 joint->robot()->extraConfigSpace().dimension());
836 1 }
837
838 const JointPtr_t& joint() const { return joint_; }
839 200 void impl_value(const ConfigurationIn_t) {
840 200 const Transform3s& M = joint_->currentTransformation();
841
1/2
✓ Branch 3 taken 200 times.
✗ Branch 4 not taken.
 200 this->value_.head<3>() = M.translation();
842
2/4
✓ Branch 2 taken 200 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 200 times.
✗ Branch 6 not taken.
 200 logSO3(M.rotation(), theta_, this->value_.tail<3>());
843 200 }
844 100 void impl_jacobian(const ConfigurationIn_t arg) {
845
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 100 computeValue(arg);
846
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 100 const JointJacobian_t& J(joint_->jacobian());
847
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 100 const matrix3_t& R(joint_->currentTransformation().rotation());
848 // Compute vector r
849
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 eigen::matrix3_t Jlog;
850
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 100 times.
 100 assert(theta_ >= 0);
851
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 100 JlogSO3(theta_, this->value_.tail<3>(), Jlog);
852
5/10
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
 100 this->jacobian_.topRows<3>().noalias() = R * J.topRows<3>();
853
5/10
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
 100 this->jacobian_.bottomRows<3>().noalias() = Jlog * J.bottomRows<3>();
854 100 }
855
856 protected:
857 JointPtr_t joint_;
858 double theta_;
859
860 public:
861 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
862 };
863
864 /// Matrix having Expression elements
865 template <typename ValueType = eigen::vector3_t,
866 typename JacobianType = JacobianMatrix>
867 class MatrixOfExpressions
868 : public CalculusBase<
869 MatrixOfExpressions<ValueType, JacobianType>,
870 Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic>,
871 Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic> > {
872 public:
873 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic> Value_t;
874 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic> Jacobian_t;
875 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic> PseudoInv_t;
876 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic>
877 PseudoInvJacobian_t;
878 typedef CalculusBase<MatrixOfExpressions, Value_t, Jacobian_t> Parent_t;
879 typedef CalculusBaseAbstract<ValueType, JacobianType> Element_t;
880 typedef typename Traits<Element_t>::Ptr_t ElementPtr_t;
881 typedef Eigen::JacobiSVD<Value_t> SVD_t;
882
883 HPP_CONSTRAINTS_CB_CREATE2(MatrixOfExpressions,
884 const Eigen::Ref<const Value_t>&,
885 const Eigen::Ref<const Jacobian_t>&)
886
887 1 MatrixOfExpressions(const Eigen::Ref<const Value_t>& value,
888 const Eigen::Ref<const Jacobian_t>& jacobian)
889 : Parent_t(value, jacobian),
890 1 nRows_(0),
891 1 nCols_(0),
892
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 svd_(value.rows(), value.cols(),
893 Eigen::ComputeFullU | Eigen::ComputeFullV),
894 1 piValid_(false),
895
6/12
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
 2 svdValid_(false) {}
896
897 MatrixOfExpressions(const Parent_t& other)
898 : Parent_t(other),
899 nRows_(static_cast<const MatrixOfExpressions&>(other).nRows_),
900 nCols_(static_cast<const MatrixOfExpressions&>(other).nCols_),
901 elements_(static_cast<const MatrixOfExpressions&>(other).elements_),
902 svd_(static_cast<const MatrixOfExpressions&>(other).svd_),
903 piValid_(static_cast<const MatrixOfExpressions&>(other).piValid_),
904 svdValid_(static_cast<const MatrixOfExpressions&>(other).svdValid_) {}
905
906 MatrixOfExpressions(const MatrixOfExpressions& matrix)
907 : Parent_t(matrix),
908 nRows_(matrix.nRows_),
909 nCols_(matrix.nCols_),
910 elements_(matrix.elements_),
911 svd_(matrix.svd_),
912 piValid_(matrix.piValid_),
913 svdValid_(matrix.svdValid_) {}
914
915 1 void setSize(std::size_t nRows, std::size_t nCols) {
916 1 nRows_ = nRows;
917 1 nCols_ = nCols;
918 1 elements_.resize(nRows_);
919
2/2
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 1 times.
 3 for (std::size_t i = 0; i < nRows; ++i) elements_[i].resize(nCols);
920 1 }
921
922 ✗ ElementPtr_t& operator()(std::size_t i, std::size_t j) {
923 ✗ return elements_[i][j];
924 }
925
926 4 void set(std::size_t i, std::size_t j, const ElementPtr_t ptr) {
927 4 elements_[i][j] = ptr;
928 4 }
929
930 100 void impl_value(const ConfigurationIn_t arg) {
931 100 size_type r = 0, c = 0, nr = 0, nc = 0;
932
2/2
✓ Branch 0 taken 200 times.
✓ Branch 1 taken 100 times.
 300 for (std::size_t i = 0; i < nRows_; ++i) {
933 200 c = 0;
934 200 nr = elements_[i][0]->value().rows();
935
2/2
✓ Branch 0 taken 400 times.
✓ Branch 1 taken 200 times.
 600 for (std::size_t j = 0; j < nCols_; ++j) {
936
1/2
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
 400 elements_[i][j]->computeValue(arg);
937
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 400 times.
 400 assert(nr == elements_[i][j]->value().rows());
938 400 nc = elements_[i][j]->value().cols();
939
1/2
✓ Branch 6 taken 400 times.
✗ Branch 7 not taken.
 400 this->value_.block(r, c, nr, nc) = elements_[i][j]->value();
940 400 c += nc;
941 }
942 200 r += nr;
943 }
944 100 }
945 100 void impl_jacobian(const ConfigurationIn_t arg) {
946 100 size_type r = 0, c = 0, nr = 0, nc = 0;
947
2/2
✓ Branch 0 taken 200 times.
✓ Branch 1 taken 100 times.
 300 for (std::size_t i = 0; i < nRows_; ++i) {
948 200 c = 0;
949 200 nr = elements_[i][0]->jacobian().rows();
950
2/2
✓ Branch 0 taken 400 times.
✓ Branch 1 taken 200 times.
 600 for (std::size_t j = 0; j < nCols_; ++j) {
951
1/2
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
 400 elements_[i][j]->computeJacobian(arg);
952
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 400 times.
 400 assert(nr == elements_[i][j]->jacobian().rows());
953 400 nc = elements_[i][j]->jacobian().cols();
954
1/2
✓ Branch 6 taken 400 times.
✗ Branch 7 not taken.
 400 this->jacobian_.block(r, c, nr, nc) = elements_[i][j]->jacobian();
955 400 c += nc;
956 }
957 200 r += nr;
958 }
959 100 }
960
961 ✗ inline const PseudoInv_t& pinv() const { return pi_; }
962 ✗ inline const PseudoInvJacobian_t& pinvJacobian() const { return pij_; }
963 ✗ void computeSVD(const ConfigurationIn_t arg) {
964 ✗ if (svdValid_) return;
965 ✗ this->computeValue(arg);
966 ✗ svd_.compute(this->value_);
967 HPP_DEBUG_SVDCHECK(svd_);
968 ✗ svdValid_ = true;
969 }
970 ✗ void computePseudoInverse(const ConfigurationIn_t arg) {
971 ✗ if (piValid_) return;
972 ✗ this->computeValue(arg);
973 ✗ this->computeSVD(arg);
974 ✗ pi_.resize(this->value_.cols(), this->value_.rows());
975 ✗ pseudoInverse<SVD_t>(svd_, pi_);
976 ✗ piValid_ = true;
977 }
978 ✗ void computePseudoInverseJacobian(
979 const ConfigurationIn_t arg,
980 const Eigen::Ref<const Eigen::Matrix<value_type, Eigen::Dynamic, 1> >&
981 rhs) {
982 ✗ this->computeJacobian(arg);
983 ✗ computePseudoInverse(arg);
984 ✗ const std::size_t nbDof = elements_[0][0]->jacobian().cols();
985 ✗ const std::size_t inSize = this->value_.cols();
986 ✗ const vector_t piTrhs = svd_.solve(rhs);
987 ✗ assert(pi_.rows() == (size_type)inSize);
988
989 ✗ Jacobian_t cache(this->jacobian_.rows(), nbDof);
990 ✗ jacobianTimes(arg, piTrhs, cache);
991 ✗ pij_.noalias() = -pi_ * cache;
992 ✗ cache.resize(inSize, nbDof);
993
994 ✗ pkInv_.resize(pi_.cols(), pi_.cols());
995 ✗ projectorOnKernelOfInv<SVD_t>(svd_, pkInv_, true);
996 ✗ jacobianTransposeTimes(arg, pkInv_ * rhs, cache);
997 ✗ pij_.noalias() += (pi_ * pi_.transpose()) * cache;
998
999 ✗ jacobianTransposeTimes(arg, pi_.transpose() * piTrhs, cache);
1000 ✗ pk_.resize(inSize, inSize);
1001 ✗ projectorOnKernel<SVD_t>(svd_, pk_, true);
1002 ✗ pij_.noalias() += pk_ * cache;
1003 }
1004
1005 10000 void jacobianTimes(
1006 const ConfigurationIn_t arg,
1007 const Eigen::Ref<const Eigen::Matrix<value_type, Eigen::Dynamic, 1> >&
1008 rhs,
1009 Eigen::Ref<Jacobian_t> cache) const {
1010 10000 size_type r = 0, c = 0, nr = 0, nc = 0;
1011 10000 cache.setZero();
1012
2/2
✓ Branch 0 taken 20000 times.
✓ Branch 1 taken 10000 times.
 30000 for (std::size_t i = 0; i < nRows_; ++i) {
1013 20000 c = 0;
1014 20000 nr = elements_[i][0]->jacobian().rows();
1015
2/2
✓ Branch 0 taken 40000 times.
✓ Branch 1 taken 20000 times.
 60000 for (std::size_t j = 0; j < nCols_; ++j) {
1016
1/2
✓ Branch 5 taken 40000 times.
✗ Branch 6 not taken.
 40000 elements_[i][j]->computeJacobian(arg);
1017
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 40000 times.
 40000 assert(nr == elements_[i][j]->jacobian().rows());
1018 40000 nc = elements_[i][j]->jacobian().cols();
1019
4/8
✓ Branch 1 taken 40000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 40000 times.
✗ Branch 11 not taken.
 40000 cache.middleRows(r, nr).noalias() +=
1020
1/2
✓ Branch 2 taken 40000 times.
✗ Branch 3 not taken.
 40000 this->jacobian_.block(r, c, nr, nc) * rhs[j];
1021 40000 c += nc;
1022 }
1023 20000 r += nr;
1024 }
1025 10000 }
1026
1027 ✗ void jacobianTransposeTimes(
1028 const ConfigurationIn_t arg,
1029 const Eigen::Ref<const Eigen::Matrix<value_type, Eigen::Dynamic, 1> >&
1030 rhs,
1031 Eigen::Ref<Jacobian_t> cache) const {
1032 ✗ size_type r = 0, c = 0, nr = 0, nc = 0;
1033 ✗ cache.setZero();
1034 ✗ for (std::size_t i = 0; i < nRows_; ++i) {
1035 ✗ c = 0;
1036 ✗ nr = elements_[i][0]->jacobian().rows();
1037 ✗ for (std::size_t j = 0; j < nCols_; ++j) {
1038 ✗ elements_[i][j]->computeJacobian(arg);
1039 ✗ assert(nr == elements_[i][j]->jacobian().rows());
1040 ✗ nc = elements_[i][j]->jacobian().cols();
1041 ✗ cache.row(j) += rhs.segment(r, nr).transpose() *
1042 ✗ this->jacobian_.block(r, c, nr, nc);
1043 ✗ c += nc;
1044 }
1045 ✗ r += nr;
1046 }
1047 }
1048
1049 ✗ Eigen::JacobiSVD<Value_t>& svd() { return svd_; }
1050
1051 100 void invalidate() {
1052 100 Parent_t::invalidate();
1053
2/2
✓ Branch 0 taken 200 times.
✓ Branch 1 taken 100 times.
 300 for (std::size_t i = 0; i < nRows_; ++i)
1054
2/2
✓ Branch 4 taken 400 times.
✓ Branch 5 taken 200 times.
 600 for (std::size_t j = 0; j < nCols_; ++j) elements_[i][j]->invalidate();
1055 100 piValid_ = false;
1056 100 svdValid_ = false;
1057 100 }
1058
1059 std::size_t nRows_, nCols_;
1060 std::vector<std::vector<ElementPtr_t> > elements_;
1061
1062 private:
1063 SVD_t svd_;
1064 matrix_t pkInv_, pk_;
1065 PseudoInv_t pi_;
1066 PseudoInvJacobian_t pij_;
1067 bool piValid_, svdValid_;
1068
1069 public:
1070 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1071 };
1072
1073 /// \}
1074 } // namespace constraints
1075 } // namespace hpp
1076
1077 #endif // HPP_CONSTRAINTS_SYMBOLIC_CALCULUS_HH
1078