hpp-constraints: include/hpp/constraints/symbolic-calculus.hh Source File

hpp-constraints 9.0.2 Definition of basic geometric constraints for motion planning

Loading...
Searching...
No Matches
symbolic-calculus.hh
Go to the documentation of this file.
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 {
96
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 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
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
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 CalculusBase() : cross_(CrossMatrix::Zero()) {}
193
194 CalculusBase(const ValueType& value, const JacobianType& jacobian)
195 : 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 inline const ValueType& value() const { return value_; }
201 inline const JacobianType& jacobian() const { return jacobian_; }
202 void computeValue(const ConfigurationIn_t arg) {
203 if (vValid_) return;
204 static_cast<T*>(this)->impl_value(arg);
205 vValid_ = true;
206 }
207 void computeJacobian(const ConfigurationIn_t arg) {
208 if (jValid_) return;
209 static_cast<T*>(this)->impl_jacobian(arg);
210 jValid_ = true;
211 }
212 void invalidate() {
213 vValid_ = false;
214 jValid_ = false;
215 cValid_ = false;
216 }
217 inline const CrossType& cross() const { return cross_; }
218 void computeCrossValue(const ConfigurationIn_t arg) {
219 if (cValid_) return;
220 computeValue(arg);
221 computeCrossMatrix(value_, cross_);
222 cValid_ = true;
223 }
224
225 protected:
226 ValueType value_;
227 JacobianType jacobian_;
228 CrossType cross_;
229
230 bool vValid_, jValid_, cValid_;
231
232 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 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(operator-, Difference)
242 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(operator+, Sum)
243 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(operator*, ScalarProduct)
244 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(operator^, CrossProduct)
245
246 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(operator*, value_type, ScalarMultiply)
247 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(operator*, JointPtr_t, RotationMultiply)
248 HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(operator*, JointTranspose, RotationMultiply)
249
250
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 static Ptr_t create(const typename Traits<LhsValue>::Ptr_t& lhs,
264 const typename Traits<RhsValue>::Ptr_t& rhs) {
265 Ptr_t p(new Expression(lhs, rhs));
266 p->init(p);
267 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 Expression(const typename Traits<LhsValue>::Ptr_t& lhs,
279 const typename Traits<RhsValue>::Ptr_t& rhs)
280 : rhs_(rhs), lhs_(lhs) {}
281
282 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
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 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 CrossProduct(const typename Traits<LhsValue>::Ptr_t& lhs,
308 const typename Traits<RhsValue>::Ptr_t& rhs)
309 : e_(Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
310
311 void impl_value(const ConfigurationIn_t arg) {
312 e_->lhs_->computeCrossValue(arg);
313 e_->rhs_->computeValue(arg);
314 this->value_ = e_->lhs_->cross() * e_->rhs_->value();
315 }
316 void impl_jacobian(const ConfigurationIn_t arg) {
317 e_->lhs_->computeCrossValue(arg);
318 e_->rhs_->computeCrossValue(arg);
319 e_->lhs_->computeJacobian(arg);
320 e_->rhs_->computeJacobian(arg);
321 this->jacobian_ = e_->lhs_->cross() * e_->rhs_->jacobian() -
322 e_->rhs_->cross() * e_->lhs_->jacobian();
323 }
324 void invalidate() {
325 Parent_t::invalidate();
326 e_->rhs_->invalidate();
327 e_->lhs_->invalidate();
328 }
329
330 protected:
331 typename Expression<LhsValue, RhsValue>::Ptr_t e_;
332
333 friend class Expression<LhsValue, RhsValue>;
334};
335
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 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 ScalarProduct(const typename Traits<LhsValue>::Ptr_t& lhs,
357 const typename Traits<RhsValue>::Ptr_t& rhs)
358 : e_(Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
359
360 void impl_value(const ConfigurationIn_t arg) {
361 e_->lhs_->computeValue(arg);
362 e_->rhs_->computeValue(arg);
363 this->value_[0] = e_->lhs_->value().dot(e_->rhs_->value());
364 }
365 void impl_jacobian(const ConfigurationIn_t arg) {
366 e_->lhs_->computeValue(arg);
367 e_->rhs_->computeValue(arg);
368 e_->lhs_->computeJacobian(arg);
369 e_->rhs_->computeJacobian(arg);
370 this->jacobian_ = e_->lhs_->value().transpose() * e_->rhs_->jacobian() +
371 e_->rhs_->value().transpose() * e_->lhs_->jacobian();
372 }
373 void invalidate() {
374 Parent_t::invalidate();
375 e_->rhs_->invalidate();
376 e_->lhs_->invalidate();
377 }
378
379 protected:
380 typename Expression<LhsValue, RhsValue>::Ptr_t e_;
381
382 friend class Expression<LhsValue, RhsValue>;
383};
384
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 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 Difference(const typename Traits<LhsValue>::Ptr_t& lhs,
402 const typename Traits<RhsValue>::Ptr_t& rhs)
403 : e_(Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
404
405 void impl_value(const ConfigurationIn_t arg) {
406 e_->lhs_->computeValue(arg);
407 e_->rhs_->computeValue(arg);
408 this->value_ = e_->lhs_->value() - e_->rhs_->value();
409 }
410 void impl_jacobian(const ConfigurationIn_t arg) {
411 e_->lhs_->computeJacobian(arg);
412 e_->rhs_->computeJacobian(arg);
413 this->jacobian_ = e_->lhs_->jacobian() - e_->rhs_->jacobian();
414 }
415 void invalidate() {
416 Parent_t::invalidate();
417 e_->rhs_->invalidate();
418 e_->lhs_->invalidate();
419 }
420
421 protected:
422 typename Expression<LhsValue, RhsValue>::Ptr_t e_;
423
424 friend class Expression<LhsValue, RhsValue>;
425};
426
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 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 Sum(const typename Traits<RhsValue>::Ptr_t& rhs,
442 const typename Traits<LhsValue>::Ptr_t& lhs)
443 : e_(Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
444
445 void impl_value(const ConfigurationIn_t arg) {
446 e_->lhs_->computeValue(arg);
447 e_->rhs_->computeValue(arg);
448 this->value_ = e_->lhs_->value() + e_->rhs_->value();
449 }
450 void impl_jacobian(const ConfigurationIn_t arg) {
451 e_->lhs_->computeJacobian(arg);
452 e_->rhs_->computeJacobian(arg);
453 this->jacobian_ = e_->lhs_->jacobian() + e_->rhs_->jacobian();
454 }
455 void invalidate() {
456 Parent_t::invalidate();
457 e_->rhs_->invalidate();
458 e_->lhs_->invalidate();
459 }
460
461 protected:
462 typename Expression<LhsValue, RhsValue>::Ptr_t e_;
463
464 friend class Expression<LhsValue, RhsValue>;
465};
466
468 template <typename RhsValue>
469 class ScalarMultiply : public CalculusBase<ScalarMultiply<RhsValue> > {
470 public:
471 typedef CalculusBase<ScalarMultiply<RhsValue> > Parent_t;
472
473 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 ScalarMultiply(const typename Traits<value_type>::Ptr_t& scalar,
484 const typename Traits<RhsValue>::Ptr_t& rhs)
485 : e_(Expression<value_type, RhsValue>::create(scalar, rhs)) {}
486
487 void impl_value(const ConfigurationIn_t arg) {
488 e_->rhs_->computeValue(arg);
489 this->value_ = e_->lhs_ * e_->rhs_->value();
490 }
491 void impl_jacobian(const ConfigurationIn_t arg) {
492 e_->rhs_->computeJacobian(arg);
493 this->jacobian_ = e_->lhs_ * e_->rhs_->jacobian();
494 }
495 void invalidate() {
496 Parent_t::invalidate();
497 e_->rhs_->invalidate();
498 }
499
500 protected:
501 typename Expression<value_type, RhsValue>::Ptr_t e_;
502
503 friend class Expression<value_type, RhsValue>;
504};
505
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 HPP_CONSTRAINTS_CB_CREATE2(RotationMultiply,
524 const typename Traits<JointTranspose>::Ptr_t&,
525 const typename Traits<RhsValue>::Ptr_t&)
526
527 RotationMultiply(const typename Traits<JointTranspose>::Ptr_t& joint,
528 const typename Traits<RhsValue>::Ptr_t& rhs)
529 : e_(Expression<pinocchio::Joint, RhsValue>::create(joint.j_, rhs)),
530 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 void impl_value(const ConfigurationIn_t arg) {
539 e_->rhs_->computeValue(arg);
540 const matrix3_t& R = e_->lhs_->currentTransformation().rotation();
541 if (transpose_)
542 this->value_ = R.transpose() * e_->rhs_->value();
543 else
544 this->value_ = R * e_->rhs_->value();
545 }
546 void impl_jacobian(const ConfigurationIn_t arg) {
547 e_->rhs_->computeJacobian(arg);
548 e_->rhs_->computeCrossValue(arg);
549 const JointJacobian_t& J = e_->lhs_->jacobian();
550 const matrix3_t& R = e_->lhs_->currentTransformation().rotation();
551 if (transpose_)
552 this->jacobian_ =
553 R.transpose() *
554 ((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 }
559 void invalidate() {
560 Parent_t::invalidate();
561 e_->rhs_->invalidate();
562 }
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
574class PointInJoint : public CalculusBase<PointInJoint> {
575 public:
576 typedef CalculusBase<PointInJoint> Parent_t;
577
578 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 PointInJoint(const JointPtr_t& joint, const vector3_t& pointInLocalFrame)
599 : joint_(joint),
600 local_(pointInLocalFrame),
601 center_(pointInLocalFrame.isZero()) {
602 assert(joint_ != NULL);
603 }
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 void impl_value(const ConfigurationIn_t) {
621 if (joint_ == NULL) return;
622 this->value_ = joint_->currentTransformation().act(local_);
623 }
624 void impl_jacobian(const ConfigurationIn_t) {
625 if (joint_ == NULL) return;
626 const JointJacobian_t& J(joint_->jacobian());
627 const matrix3_t& R = joint_->currentTransformation().rotation();
628 this->jacobian_.noalias() = R * J.topRows<3>();
629 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
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
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 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
735 FunctionExp(const FunctionTypePtr_t& func)
736 : Parent_t(vector_t::Zero(func->outputSize()),
737 matrix_t::Zero(func->outputDerivativeSize(),
738 func->inputDerivativeSize())),
739 f_(func),
740 lge_(func->outputSpace()) {}
741
742 void impl_value(const ConfigurationIn_t arg) {
743 f_->value(lge_, arg);
744 this->value_ = lge_.vector();
745 }
746 void impl_jacobian(const ConfigurationIn_t arg) {
747 f_->jacobian(this->jacobian_, arg);
748 }
749
750 private:
751 FunctionTypePtr_t f_;
752 LiegroupElement lge_;
753};
754
758 class Point : public CalculusBase<Point, vector3_t, JacobianMatrix> {
759 public:
760 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
774 Point(const vector3_t& point, size_t jacobianNbCols)
775 : CalculusBase<Point, vector3_t, JacobianMatrix>(
776 point, JacobianMatrix::Zero(3, jacobianNbCols)) {}
777
778 void impl_value(const ConfigurationIn_t) {}
779 void impl_jacobian(const ConfigurationIn_t) {}
780};
781
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 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 JointFrame(const JointPtr_t& joint) : joint_(joint) {
832 assert(joint_ != NULL);
833 this->jacobian_.resize(6,
834 joint->robot()->numberDof() -
835 joint->robot()->extraConfigSpace().dimension());
836 }
837
838 const JointPtr_t& joint() const { return joint_; }
839 void impl_value(const ConfigurationIn_t) {
840 const Transform3s& M = joint_->currentTransformation();
841 this->value_.head<3>() = M.translation();
842 logSO3(M.rotation(), theta_, this->value_.tail<3>());
843 }
844 void impl_jacobian(const ConfigurationIn_t arg) {
845 computeValue(arg);
846 const JointJacobian_t& J(joint_->jacobian());
847 const matrix3_t& R(joint_->currentTransformation().rotation());
848 // Compute vector r
849 eigen::matrix3_t Jlog;
850 assert(theta_ >= 0);
851 JlogSO3(theta_, this->value_.tail<3>(), Jlog);
852 this->jacobian_.topRows<3>().noalias() = R * J.topRows<3>();
853 this->jacobian_.bottomRows<3>().noalias() = Jlog * J.bottomRows<3>();
854 }
855
856 protected:
857 JointPtr_t joint_;
858 double theta_;
859
860 public:
861 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
862};
863
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 MatrixOfExpressions(const Eigen::Ref<const Value_t>& value,
888 const Eigen::Ref<const Jacobian_t>& jacobian)
889 : Parent_t(value, jacobian),
890 nRows_(0),
891 nCols_(0),
892 svd_(value.rows(), value.cols(),
893 Eigen::ComputeFullU | Eigen::ComputeFullV),
894 piValid_(false),
895 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 void setSize(std::size_t nRows, std::size_t nCols) {
916 nRows_ = nRows;
917 nCols_ = nCols;
918 elements_.resize(nRows_);
919 for (std::size_t i = 0; i < nRows; ++i) elements_[i].resize(nCols);
920 }
921
922 ElementPtr_t& operator()(std::size_t i, std::size_t j) {
923 return elements_[i][j];
924 }
925
926 void set(std::size_t i, std::size_t j, const ElementPtr_t ptr) {
927 elements_[i][j] = ptr;
928 }
929
930 void impl_value(const ConfigurationIn_t arg) {
931 size_type r = 0, c = 0, nr = 0, nc = 0;
932 for (std::size_t i = 0; i < nRows_; ++i) {
933 c = 0;
934 nr = elements_[i][0]->value().rows();
935 for (std::size_t j = 0; j < nCols_; ++j) {
936 elements_[i][j]->computeValue(arg);
937 assert(nr == elements_[i][j]->value().rows());
938 nc = elements_[i][j]->value().cols();
939 this->value_.block(r, c, nr, nc) = elements_[i][j]->value();
940 c += nc;
941 }
942 r += nr;
943 }
944 }
945 void impl_jacobian(const ConfigurationIn_t arg) {
946 size_type r = 0, c = 0, nr = 0, nc = 0;
947 for (std::size_t i = 0; i < nRows_; ++i) {
948 c = 0;
949 nr = elements_[i][0]->jacobian().rows();
950 for (std::size_t j = 0; j < nCols_; ++j) {
951 elements_[i][j]->computeJacobian(arg);
952 assert(nr == elements_[i][j]->jacobian().rows());
953 nc = elements_[i][j]->jacobian().cols();
954 this->jacobian_.block(r, c, nr, nc) = elements_[i][j]->jacobian();
955 c += nc;
956 }
957 r += nr;
958 }
959 }
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 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 size_type r = 0, c = 0, nr = 0, nc = 0;
1011 cache.setZero();
1012 for (std::size_t i = 0; i < nRows_; ++i) {
1013 c = 0;
1014 nr = elements_[i][0]->jacobian().rows();
1015 for (std::size_t j = 0; j < nCols_; ++j) {
1016 elements_[i][j]->computeJacobian(arg);
1017 assert(nr == elements_[i][j]->jacobian().rows());
1018 nc = elements_[i][j]->jacobian().cols();
1019 cache.middleRows(r, nr).noalias() +=
1020 this->jacobian_.block(r, c, nr, nc) * rhs[j];
1021 c += nc;
1022 }
1023 r += nr;
1024 }
1025 }
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 void invalidate() {
1052 Parent_t::invalidate();
1053 for (std::size_t i = 0; i < nRows_; ++i)
1054 for (std::size_t j = 0; j < nCols_; ++j) elements_[i][j]->invalidate();
1055 piValid_ = false;
1056 svdValid_ = false;
1057 }
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
1074} // namespace constraints
1075} // namespace hpp
1076
1077#endif // HPP_CONSTRAINTS_SYMBOLIC_CALCULUS_HH
center-of-mass-computation.hh
hpp::constraints::CalculusBaseAbstract
Definition symbolic-calculus.hh:165
hpp::constraints::CalculusBaseAbstract::jacobian
virtual const JacobianType & jacobian() const =0
hpp::constraints::CalculusBaseAbstract::computeValue
virtual void computeValue(const ConfigurationIn_t arg)=0
hpp::constraints::CalculusBaseAbstract::computeJacobian
virtual void computeJacobian(const ConfigurationIn_t arg)=0
hpp::constraints::CalculusBaseAbstract::invalidate
virtual void invalidate()=0
hpp::constraints::CalculusBaseAbstract::ValueType_t
ValueType ValueType_t
Definition symbolic-calculus.hh:167
hpp::constraints::CalculusBaseAbstract::value
virtual const ValueType & value() const =0
hpp::constraints::CalculusBaseAbstract::JacobianType_t
JacobianType JacobianType_t
Definition symbolic-calculus.hh:168
hpp::constraints::CalculusBase
Definition symbolic-calculus.hh:190
hpp::constraints::CalculusBase::value_
ValueType value_
Definition symbolic-calculus.hh:226
hpp::constraints::CalculusBase::cValid_
bool cValid_
Definition symbolic-calculus.hh:230
hpp::constraints::CalculusBase::vValid_
bool vValid_
Definition symbolic-calculus.hh:230
hpp::constraints::CalculusBase::computeValue
void computeValue(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:202
hpp::constraints::CalculusBase::computeCrossValue
void computeCrossValue(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:218
hpp::constraints::CalculusBase::jValid_
bool jValid_
Definition symbolic-calculus.hh:230
hpp::constraints::CalculusBase::cross
const CrossType & cross() const
Definition symbolic-calculus.hh:217
hpp::constraints::CalculusBase::CalculusBase
CalculusBase(const CalculusBase &o)
Definition symbolic-calculus.hh:197
hpp::constraints::CalculusBase::invalidate
void invalidate()
Definition symbolic-calculus.hh:212
hpp::constraints::CalculusBase::jacobian_
JacobianType jacobian_
Definition symbolic-calculus.hh:227
hpp::constraints::CalculusBase::CalculusBase
CalculusBase(const ValueType &value, const JacobianType &jacobian)
Definition symbolic-calculus.hh:194
hpp::constraints::CalculusBase::value
const ValueType & value() const
Definition symbolic-calculus.hh:200
hpp::constraints::CalculusBase::init
void init(const typename Traits< T >::Ptr_t &ptr)
Definition symbolic-calculus.hh:232
hpp::constraints::CalculusBase::jacobian
const JacobianType & jacobian() const
Definition symbolic-calculus.hh:201
hpp::constraints::CalculusBase::computeJacobian
void computeJacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:207
hpp::constraints::CalculusBase::CalculusBase
CalculusBase()
Definition symbolic-calculus.hh:192
hpp::constraints::CalculusBase::cross_
CrossType cross_
Definition symbolic-calculus.hh:228
hpp::constraints::CrossProduct
Cross product of two expressions.
Definition symbolic-calculus.hh:294
hpp::constraints::CrossProduct::impl_value
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:311
hpp::constraints::CrossProduct::Parent_t
CalculusBase< CrossProduct< LhsValue, RhsValue > > Parent_t
Definition symbolic-calculus.hh:296
hpp::constraints::CrossProduct::CrossProduct
CrossProduct(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition symbolic-calculus.hh:307
hpp::constraints::CrossProduct::CrossProduct
CrossProduct(const CalculusBase< CrossProduct > &other)
Definition symbolic-calculus.hh:304
hpp::constraints::CrossProduct::invalidate
void invalidate()
Definition symbolic-calculus.hh:324
hpp::constraints::CrossProduct::impl_jacobian
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:316
hpp::constraints::CrossProduct::e_
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition symbolic-calculus.hh:331
hpp::constraints::CrossProduct::HPP_CONSTRAINTS_CB_CREATE2
HPP_CONSTRAINTS_CB_CREATE2(CrossProduct, const typename Traits< LhsValue >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) CrossProduct()
Definition symbolic-calculus.hh:298
hpp::constraints::Difference
Difference of two expressions.
Definition symbolic-calculus.hh:387
hpp::constraints::Difference::Difference
Difference(const CalculusBase< Difference > &other)
Definition symbolic-calculus.hh:397
hpp::constraints::Difference::HPP_CONSTRAINTS_CB_CREATE2
HPP_CONSTRAINTS_CB_CREATE2(Difference, const typename Traits< LhsValue >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) Difference()
Definition symbolic-calculus.hh:391
hpp::constraints::Difference::invalidate
void invalidate()
Definition symbolic-calculus.hh:415
hpp::constraints::Difference::impl_jacobian
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:410
hpp::constraints::Difference::e_
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition symbolic-calculus.hh:422
hpp::constraints::Difference::Difference
Difference(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition symbolic-calculus.hh:401
hpp::constraints::Difference::impl_value
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:405
hpp::constraints::Difference::Parent_t
CalculusBase< Difference< LhsValue, RhsValue > > Parent_t
Definition symbolic-calculus.hh:389
hpp::constraints::Expression
Base class for classes representing an operation.
Definition symbolic-calculus.hh:252
hpp::constraints::Expression::self_
Traits< Expression >::WkPtr_t self_
Definition symbolic-calculus.hh:286
hpp::constraints::Expression::lhs_
Traits< LhsValue >::Ptr_t lhs_
Definition symbolic-calculus.hh:285
hpp::constraints::Expression::create
static Ptr_t create(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition symbolic-calculus.hh:263
hpp::constraints::Expression::rhs
const RhsValue & rhs() const
Definition symbolic-calculus.hh:272
hpp::constraints::Expression::Expression
Expression(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition symbolic-calculus.hh:278
hpp::constraints::Expression::Expression
Expression()
Definition symbolic-calculus.hh:274
hpp::constraints::Expression::WkPtr_t
weak_ptr< Expression< LhsValue, RhsValue > > WkPtr_t
Definition symbolic-calculus.hh:255
hpp::constraints::Expression::init
void init(Ptr_t self)
Definition symbolic-calculus.hh:282
hpp::constraints::Expression::Ptr_t
shared_ptr< Expression< LhsValue, RhsValue > > Ptr_t
Definition symbolic-calculus.hh:254
hpp::constraints::Expression::Expression
Expression(const Expression &other)
Definition symbolic-calculus.hh:276
hpp::constraints::Expression::create
static Ptr_t create()
Definition symbolic-calculus.hh:257
hpp::constraints::Expression::rhs_
Traits< RhsValue >::Ptr_t rhs_
Definition symbolic-calculus.hh:284
hpp::constraints::Expression::lhs
const LhsValue & lhs() const
Definition symbolic-calculus.hh:270
hpp::constraints::FunctionExp
Basic expression mapping a function as an expression.
Definition symbolic-calculus.hh:716
hpp::constraints::FunctionExp::impl_jacobian
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:746
hpp::constraints::FunctionExp::FunctionExp
FunctionExp(const Parent_t &other)
Definition symbolic-calculus.hh:726
hpp::constraints::FunctionExp::Parent_t
CalculusBase< FunctionExp< FunctionType >, vector_t, matrix_t > Parent_t
Definition symbolic-calculus.hh:718
hpp::constraints::FunctionExp::impl_value
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:742
hpp::constraints::FunctionExp::FunctionExp
FunctionExp(const FunctionExp &other)
Definition symbolic-calculus.hh:729
hpp::constraints::FunctionExp::FunctionTypePtr_t
shared_ptr< FunctionType > FunctionTypePtr_t
Definition symbolic-calculus.hh:719
hpp::constraints::FunctionExp::HPP_CONSTRAINTS_CB_CREATE1
HPP_CONSTRAINTS_CB_CREATE1(FunctionExp< FunctionType >, const FunctionTypePtr_t &) FunctionExp()
Definition symbolic-calculus.hh:721
hpp::constraints::FunctionExp::FunctionExp
FunctionExp(const FunctionTypePtr_t &func)
Definition symbolic-calculus.hh:735
hpp::constraints::JointFrame
Definition symbolic-calculus.hh:818
hpp::constraints::JointFrame::theta_
double theta_
Definition symbolic-calculus.hh:858
hpp::constraints::JointFrame::JointFrame
JointFrame(const Parent_t &other)
Definition symbolic-calculus.hh:826
hpp::constraints::JointFrame::impl_jacobian
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:844
hpp::constraints::JointFrame::joint_
JointPtr_t joint_
Definition symbolic-calculus.hh:857
hpp::constraints::JointFrame::JointFrame
JointFrame(const JointFrame &jf)
Definition symbolic-calculus.hh:829
hpp::constraints::JointFrame::Parent_t
CalculusBase< JointFrame, ValueType_t, JacobianType_t > Parent_t
Definition symbolic-calculus.hh:820
hpp::constraints::JointFrame::joint
const JointPtr_t & joint() const
Definition symbolic-calculus.hh:838
hpp::constraints::JointFrame::impl_value
void impl_value(const ConfigurationIn_t)
Definition symbolic-calculus.hh:839
hpp::constraints::JointFrame::JointFrame
JointFrame(const JointPtr_t &joint)
Definition symbolic-calculus.hh:831
hpp::constraints::MatrixOfExpressions
Matrix having Expression elements.
Definition symbolic-calculus.hh:871
hpp::constraints::MatrixOfExpressions::setSize
void setSize(std::size_t nRows, std::size_t nCols)
Definition symbolic-calculus.hh:915
hpp::constraints::MatrixOfExpressions::HPP_CONSTRAINTS_CB_CREATE2
HPP_CONSTRAINTS_CB_CREATE2(MatrixOfExpressions, const Eigen::Ref< const Value_t > &, const Eigen::Ref< const Jacobian_t > &) MatrixOfExpressions(const Eigen
Definition symbolic-calculus.hh:883
hpp::constraints::MatrixOfExpressions::Jacobian_t
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > Jacobian_t
Definition symbolic-calculus.hh:874
hpp::constraints::MatrixOfExpressions::jacobianTransposeTimes
void jacobianTransposeTimes(const ConfigurationIn_t arg, const Eigen::Ref< const Eigen::Matrix< value_type, Eigen::Dynamic, 1 > > &rhs, Eigen::Ref< Jacobian_t > cache) const
Definition symbolic-calculus.hh:1027
hpp::constraints::MatrixOfExpressions::nCols_
std::size_t nCols_
Definition symbolic-calculus.hh:1059
hpp::constraints::MatrixOfExpressions::PseudoInv_t
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > PseudoInv_t
Definition symbolic-calculus.hh:875
hpp::constraints::MatrixOfExpressions::ElementPtr_t
Traits< Element_t >::Ptr_t ElementPtr_t
Definition symbolic-calculus.hh:880
hpp::constraints::MatrixOfExpressions::MatrixOfExpressions
MatrixOfExpressions(const MatrixOfExpressions &matrix)
Definition symbolic-calculus.hh:906
hpp::constraints::MatrixOfExpressions::computePseudoInverseJacobian
void computePseudoInverseJacobian(const ConfigurationIn_t arg, const Eigen::Ref< const Eigen::Matrix< value_type, Eigen::Dynamic, 1 > > &rhs)
Definition symbolic-calculus.hh:978
hpp::constraints::MatrixOfExpressions::svd
Eigen::JacobiSVD< Value_t > & svd()
Definition symbolic-calculus.hh:1049
hpp::constraints::MatrixOfExpressions::operator()
ElementPtr_t & operator()(std::size_t i, std::size_t j)
Definition symbolic-calculus.hh:922
hpp::constraints::MatrixOfExpressions::pinvJacobian
const PseudoInvJacobian_t & pinvJacobian() const
Definition symbolic-calculus.hh:962
hpp::constraints::MatrixOfExpressions::pinv
const PseudoInv_t & pinv() const
Definition symbolic-calculus.hh:961
hpp::constraints::MatrixOfExpressions::jacobianTimes
void jacobianTimes(const ConfigurationIn_t arg, const Eigen::Ref< const Eigen::Matrix< value_type, Eigen::Dynamic, 1 > > &rhs, Eigen::Ref< Jacobian_t > cache) const
Definition symbolic-calculus.hh:1005
hpp::constraints::MatrixOfExpressions::computePseudoInverse
void computePseudoInverse(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:970
hpp::constraints::MatrixOfExpressions::Value_t
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > Value_t
Definition symbolic-calculus.hh:873
hpp::constraints::MatrixOfExpressions::elements_
std::vector< std::vector< ElementPtr_t > > elements_
Definition symbolic-calculus.hh:1060
hpp::constraints::MatrixOfExpressions::invalidate
void invalidate()
Definition symbolic-calculus.hh:1051
hpp::constraints::MatrixOfExpressions::PseudoInvJacobian_t
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > PseudoInvJacobian_t
Definition symbolic-calculus.hh:877
hpp::constraints::MatrixOfExpressions::MatrixOfExpressions
MatrixOfExpressions(const Parent_t &other)
Definition symbolic-calculus.hh:897
hpp::constraints::MatrixOfExpressions::SVD_t
Eigen::JacobiSVD< Value_t > SVD_t
Definition symbolic-calculus.hh:881
hpp::constraints::MatrixOfExpressions::impl_jacobian
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:945
hpp::constraints::MatrixOfExpressions::set
void set(std::size_t i, std::size_t j, const ElementPtr_t ptr)
Definition symbolic-calculus.hh:926
hpp::constraints::MatrixOfExpressions::impl_value
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:930
hpp::constraints::MatrixOfExpressions::Parent_t
CalculusBase< MatrixOfExpressions, Value_t, Jacobian_t > Parent_t
Definition symbolic-calculus.hh:878
hpp::constraints::MatrixOfExpressions::Element_t
CalculusBaseAbstract< ValueType, JacobianType > Element_t
Definition symbolic-calculus.hh:879
hpp::constraints::MatrixOfExpressions::computeSVD
void computeSVD(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:963
hpp::constraints::PointCom
Basic expression representing a COM.
Definition symbolic-calculus.hh:783
hpp::constraints::PointCom::jacobian
const ComJacobian_t & jacobian() const
Definition symbolic-calculus.hh:797
hpp::constraints::PointCom::impl_value
void impl_value(const ConfigurationIn_t)
Definition symbolic-calculus.hh:802
hpp::constraints::PointCom::centerOfMassComputation
const CenterOfMassComputationPtr_t & centerOfMassComputation() const
Definition symbolic-calculus.hh:799
hpp::constraints::PointCom::PointCom
PointCom(const CenterOfMassComputationPtr_t &comc)
Definition symbolic-calculus.hh:794
hpp::constraints::PointCom::value
const vector3_t & value() const
Definition symbolic-calculus.hh:796
hpp::constraints::PointCom::PointCom
PointCom(const Parent_t &other)
Definition symbolic-calculus.hh:790
hpp::constraints::PointCom::Parent_t
CalculusBase< PointCom, vector3_t, ComJacobian_t > Parent_t
Definition symbolic-calculus.hh:785
hpp::constraints::PointCom::comc_
CenterOfMassComputationPtr_t comc_
Definition symbolic-calculus.hh:813
hpp::constraints::PointCom::impl_jacobian
void impl_jacobian(const ConfigurationIn_t)
Definition symbolic-calculus.hh:805
hpp::constraints::PointInJoint
Basic expression representing a point in a joint frame.
Definition symbolic-calculus.hh:574
hpp::constraints::PointInJoint::impl_value
void impl_value(const ConfigurationIn_t)
Definition symbolic-calculus.hh:620
hpp::constraints::PointInJoint::joint_
JointPtr_t joint_
Definition symbolic-calculus.hh:645
hpp::constraints::PointInJoint::local_
vector3_t local_
Definition symbolic-calculus.hh:646
hpp::constraints::PointInJoint::PointInJoint
PointInJoint(const PointInJoint &pointInJoint)
Definition symbolic-calculus.hh:592
hpp::constraints::PointInJoint::joint
const JointPtr_t & joint() const
Definition symbolic-calculus.hh:618
hpp::constraints::PointInJoint::PointInJoint
PointInJoint(const JointPtr_t &joint, const vector3_t &pointInLocalFrame, size_type nbDof)
Definition symbolic-calculus.hh:605
hpp::constraints::PointInJoint::center_
bool center_
Definition symbolic-calculus.hh:647
hpp::constraints::PointInJoint::PointInJoint
PointInJoint(const CalculusBase< PointInJoint > &other)
Definition symbolic-calculus.hh:584
hpp::constraints::PointInJoint::PointInJoint
PointInJoint(const JointPtr_t &joint, const vector3_t &pointInLocalFrame)
Definition symbolic-calculus.hh:598
hpp::constraints::PointInJoint::local
const vector3_t & local() const
Definition symbolic-calculus.hh:619
hpp::constraints::PointInJoint::computeCrossRXl
void computeCrossRXl()
Definition symbolic-calculus.hh:634
hpp::constraints::PointInJoint::impl_jacobian
void impl_jacobian(const ConfigurationIn_t)
Definition symbolic-calculus.hh:624
hpp::constraints::PointInJoint::Parent_t
CalculusBase< PointInJoint > Parent_t
Definition symbolic-calculus.hh:576
hpp::constraints::Point
Definition symbolic-calculus.hh:758
hpp::constraints::Point::impl_value
void impl_value(const ConfigurationIn_t)
Definition symbolic-calculus.hh:778
hpp::constraints::Point::Point
Point(const vector3_t &point, size_t jacobianNbCols)
Definition symbolic-calculus.hh:774
hpp::constraints::Point::Point
Point(const CalculusBase< Point, vector3_t, JacobianMatrix > &other)
Definition symbolic-calculus.hh:764
hpp::constraints::Point::impl_jacobian
void impl_jacobian(const ConfigurationIn_t)
Definition symbolic-calculus.hh:779
hpp::constraints::Point::Point
Point(const Point &point)
Definition symbolic-calculus.hh:767
hpp::constraints::RotationMultiply
Multiplication of an expression by a rotation matrix.
Definition symbolic-calculus.hh:508
hpp::constraints::RotationMultiply::RotationMultiply
RotationMultiply(const CalculusBase< RotationMultiply > &other)
Definition symbolic-calculus.hh:514
hpp::constraints::RotationMultiply::e_
Expression< pinocchio::Joint, RhsValue >::Ptr_t e_
Definition symbolic-calculus.hh:565
hpp::constraints::RotationMultiply::HPP_CONSTRAINTS_CB_CREATE2
HPP_CONSTRAINTS_CB_CREATE2(RotationMultiply, const typename Traits< pinocchio::Joint >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) HPP_CONSTRAINTS_CB_CREATE2(RotationMultiply
hpp::constraints::RotationMultiply::impl_value
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:538
hpp::constraints::RotationMultiply::invalidate
void invalidate()
Definition symbolic-calculus.hh:559
hpp::constraints::RotationMultiply::impl_jacobian
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:546
hpp::constraints::RotationMultiply::RotationMultiply
RotationMultiply(const typename Traits< pinocchio::Joint >::Ptr_t &joint, const typename Traits< RhsValue >::Ptr_t &rhs, bool transpose=false)
Definition symbolic-calculus.hh:532
hpp::constraints::RotationMultiply::Parent_t
CalculusBase< RotationMultiply< RhsValue > > Parent_t
Definition symbolic-calculus.hh:510
hpp::constraints::RotationMultiply::RotationMultiply
RotationMultiply()
Definition symbolic-calculus.hh:512
hpp::constraints::ScalarMultiply
Multiplication of an expression by a scalar.
Definition symbolic-calculus.hh:469
hpp::constraints::ScalarMultiply::ScalarMultiply
ScalarMultiply(const typename Traits< value_type >::Ptr_t &scalar, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition symbolic-calculus.hh:483
hpp::constraints::ScalarMultiply::ScalarMultiply
ScalarMultiply(const CalculusBase< ScalarMultiply > &other)
Definition symbolic-calculus.hh:479
hpp::constraints::ScalarMultiply::impl_value
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:487
hpp::constraints::ScalarMultiply::Parent_t
CalculusBase< ScalarMultiply< RhsValue > > Parent_t
Definition symbolic-calculus.hh:471
hpp::constraints::ScalarMultiply::invalidate
void invalidate()
Definition symbolic-calculus.hh:495
hpp::constraints::ScalarMultiply::e_
Expression< value_type, RhsValue >::Ptr_t e_
Definition symbolic-calculus.hh:501
hpp::constraints::ScalarMultiply::HPP_CONSTRAINTS_CB_CREATE2
HPP_CONSTRAINTS_CB_CREATE2(ScalarMultiply, const typename Traits< value_type >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) ScalarMultiply()
Definition symbolic-calculus.hh:473
hpp::constraints::ScalarMultiply::impl_jacobian
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:491
hpp::constraints::ScalarProduct
Scalar product of two expressions.
Definition symbolic-calculus.hh:340
hpp::constraints::ScalarProduct::HPP_CONSTRAINTS_CB_CREATE2
HPP_CONSTRAINTS_CB_CREATE2(ScalarProduct, const typename Traits< LhsValue >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) ScalarProduct()
Definition symbolic-calculus.hh:346
hpp::constraints::ScalarProduct::invalidate
void invalidate()
Definition symbolic-calculus.hh:373
hpp::constraints::ScalarProduct::impl_jacobian
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:365
hpp::constraints::ScalarProduct::impl_value
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:360
hpp::constraints::ScalarProduct::Parent_t
CalculusBase< ScalarProduct< LhsValue, RhsValue >, Eigen::Matrix< value_type, 1, 1 >, RowJacobianMatrix > Parent_t
Definition symbolic-calculus.hh:344
hpp::constraints::ScalarProduct::e_
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition symbolic-calculus.hh:380
hpp::constraints::ScalarProduct::ScalarProduct
ScalarProduct(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition symbolic-calculus.hh:356
hpp::constraints::ScalarProduct::ScalarProduct
ScalarProduct(const CalculusBase< ScalarProduct > &other)
Definition symbolic-calculus.hh:352
hpp::constraints::Sum
Sum of two expressions.
Definition symbolic-calculus.hh:429
hpp::constraints::Sum::impl_jacobian
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:450
hpp::constraints::Sum::Sum
Sum(const typename Traits< RhsValue >::Ptr_t &rhs, const typename Traits< LhsValue >::Ptr_t &lhs)
Definition symbolic-calculus.hh:441
hpp::constraints::Sum::HPP_CONSTRAINTS_CB_CREATE2
HPP_CONSTRAINTS_CB_CREATE2(Sum, const typename Traits< LhsValue >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) Sum()
Definition symbolic-calculus.hh:433
hpp::constraints::Sum::invalidate
void invalidate()
Definition symbolic-calculus.hh:455
hpp::constraints::Sum::e_
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition symbolic-calculus.hh:462
hpp::constraints::Sum::Parent_t
CalculusBase< Sum< LhsValue, RhsValue > > Parent_t
Definition symbolic-calculus.hh:431
hpp::constraints::Sum::Sum
Sum(const CalculusBase< Sum > &other)
Definition symbolic-calculus.hh:438
hpp::constraints::Sum::impl_value
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:445
hpp::constraints::Traits
Definition symbolic-calculus.hh:126
hpp::constraints::Traits::Ptr_t
HPP_CONSTRAINTS_CB_REF< Class > Ptr_t
Definition symbolic-calculus.hh:127
hpp::constraints::Traits::WkPtr_t
HPP_CONSTRAINTS_CB_WKREF< Class > WkPtr_t
Definition symbolic-calculus.hh:128
hpp::constraints::VectorInJoint
Basic expression representing a vector in a joint frame.
Definition symbolic-calculus.hh:654
hpp::constraints::VectorInJoint::VectorInJoint
VectorInJoint(const VectorInJoint &vectorInJoint)
Definition symbolic-calculus.hh:667
hpp::constraints::VectorInJoint::VectorInJoint
VectorInJoint(const CalculusBase< VectorInJoint > &other)
Definition symbolic-calculus.hh:662
hpp::constraints::VectorInJoint::joint_
JointPtr_t joint_
Definition symbolic-calculus.hh:706
hpp::constraints::VectorInJoint::impl_value
void impl_value(const ConfigurationIn_t)
Definition symbolic-calculus.hh:688
hpp::constraints::VectorInJoint::impl_jacobian
void impl_jacobian(const ConfigurationIn_t)
Definition symbolic-calculus.hh:692
hpp::constraints::VectorInJoint::vector_
vector3_t vector_
Definition symbolic-calculus.hh:707
hpp::constraints::VectorInJoint::VectorInJoint
VectorInJoint(const JointPtr_t &joint, const vector3_t &vectorInLocalFrame, const size_type &nbDof)
Definition symbolic-calculus.hh:675
hpp::constraints::VectorInJoint::vector
const vector3_t & vector() const
Definition symbolic-calculus.hh:687
hpp::constraints::VectorInJoint::joint
const JointPtr_t & joint() const
Definition symbolic-calculus.hh:686
hpp::constraints::VectorInJoint::VectorInJoint
VectorInJoint(const JointPtr_t &joint, const vector3_t &vectorInLocalFrame)
Definition symbolic-calculus.hh:672
hpp::constraints::VectorInJoint::computeCrossRXl
void computeCrossRXl()
Definition symbolic-calculus.hh:699
hpp::pinocchio::Joint
hpp::pinocchio::LiegroupElementBase
fwd.hh
hpp::constraints::logSO3
void logSO3(const matrix3_t &R, value_type &theta, Eigen::MatrixBase< Derived > const &result)
Definition tools.hh:60
hpp::constraints::JlogSO3
void JlogSO3(const value_type &theta, const Eigen::MatrixBase< Derived > &log, matrix3_t &Jlog)
Definition tools.hh:129
hpp::constraints::JacobianMatrix
Eigen::Matrix< value_type, 3, Eigen::Dynamic, Eigen::RowMajor > JacobianMatrix
Definition symbolic-calculus.hh:123
hpp::constraints::CrossMatrix
eigen::matrix3_t CrossMatrix
Definition symbolic-calculus.hh:119
hpp::constraints::RowJacobianMatrix
Eigen::Matrix< value_type, 1, Eigen::Dynamic, Eigen::RowMajor > RowJacobianMatrix
Definition symbolic-calculus.hh:121
joint.hh
liegroup-element.hh
macros.hh
HPP_DEBUG_SVDCHECK
#define HPP_DEBUG_SVDCHECK(svd)
Definition macros.hh:59
Eigen
Definition fwd.hh:39
hpp::constraints::eigen::vector3_t
Eigen::Matrix< value_type, 3, 1 > vector3_t
Definition fwd.hh:74
hpp::constraints::eigen::matrix3_t
Eigen::Matrix< value_type, 3, 3 > matrix3_t
Definition fwd.hh:73
hpp::constraints::matrix3_t
pinocchio::matrix3_t matrix3_t
Definition fwd.hh:54
hpp::constraints::vector3_t
pinocchio::vector3_t vector3_t
Definition fwd.hh:53
hpp::constraints::JointJacobian_t
pinocchio::JointJacobian_t JointJacobian_t
Definition fwd.hh:64
hpp::constraints::Transform3s
pinocchio::Transform3s Transform3s
Definition fwd.hh:65
hpp::constraints::size_type
pinocchio::size_type size_type
Definition fwd.hh:48
hpp::constraints::ComJacobian_t
pinocchio::ComJacobian_t ComJacobian_t
Definition fwd.hh:63
hpp::constraints::value_type
pinocchio::value_type value_type
Definition fwd.hh:49
hpp::constraints::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::constraints::matrix_t
pinocchio::matrix_t matrix_t
Definition fwd.hh:57
hpp::constraints::CenterOfMassComputationPtr_t
pinocchio::CenterOfMassComputationPtr_t CenterOfMassComputationPtr_t
Definition fwd.hh:114
hpp::constraints::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:60
hpp::constraints::JointPtr_t
pinocchio::JointPtr_t JointPtr_t
Definition fwd.hh:50
hpp::pinocchio::COMPUTE_ALL
COMPUTE_ALL
hpp::pinocchio::COM
COM
hpp
pinocchio
hpp::constraints::JointTranspose
Definition symbolic-calculus.hh:141
hpp::constraints::JointTranspose::j_
const JointPtr_t j_
Definition symbolic-calculus.hh:143
hpp::constraints::JointTranspose::JointTranspose
JointTranspose(const JointPtr_t &joint)
Definition symbolic-calculus.hh:142
hpp::constraints::Traits< JointTranspose >::WkPtr_t
JointTranspose WkPtr_t
Definition symbolic-calculus.hh:148
hpp::constraints::Traits< JointTranspose >::Ptr_t
JointTranspose Ptr_t
Definition symbolic-calculus.hh:147
hpp::constraints::Traits< pinocchio::Joint >::WkPtr_t
JointPtr_t WkPtr_t
Definition symbolic-calculus.hh:138
hpp::constraints::Traits< pinocchio::Joint >::Ptr_t
JointPtr_t Ptr_t
Definition symbolic-calculus.hh:137
hpp::constraints::Traits< value_type >::WkPtr_t
value_type WkPtr_t
Definition symbolic-calculus.hh:133
hpp::constraints::Traits< value_type >::Ptr_t
value_type Ptr_t
Definition symbolic-calculus.hh:132
svd.hh
HPP_CONSTRAINTS_CB_CREATE1
#define HPP_CONSTRAINTS_CB_CREATE1(Class, Arg0Type)
Definition symbolic-calculus.hh:62
HPP_CONSTRAINTS_CB_CREATE2
#define HPP_CONSTRAINTS_CB_CREATE2(Class, Arg0Type, Arg1Type)
Definition symbolic-calculus.hh:69
HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1
#define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(op, InType, OutType)
Definition symbolic-calculus.hh:35
HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2
#define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(op, OutType)
Definition symbolic-calculus.hh:47
HPP_CONSTRAINTS_CB_CREATE3
#define HPP_CONSTRAINTS_CB_CREATE3(Class, Arg0Type, Arg1Type, Arg2Type)
Definition symbolic-calculus.hh:76
tools.hh