hpp-constraints 6.0.0
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>
89#include <hpp/pinocchio/center-of-mass-computation.hh>
90#include <hpp/pinocchio/joint.hh>
91#include <hpp/pinocchio/liegroup-element.hh>
92
93namespace hpp {
94namespace constraints {
96
99
100template <typename ValueType, typename JacobianType>
101class CalculusBaseAbstract;
102template <typename T>
103class Traits;
104
105template <typename LhsValue, typename RhsValue>
106class Expression;
107template <typename LhsValue, typename RhsValue>
108class CrossProduct;
109template <typename LhsValue, typename RhsValue>
110class ScalarProduct;
111template <typename LhsValue, typename RhsValue>
112class Difference;
113template <typename LhsValue, typename RhsValue>
114class Sum;
115template <typename RhsValue>
116class ScalarMultiply;
117template <typename RhsValue>
118class RotationMultiply;
120typedef Eigen::Matrix<value_type, 1, Eigen::Dynamic, Eigen::RowMajor>
122typedef Eigen::Matrix<value_type, 3, Eigen::Dynamic, Eigen::RowMajor>
124
125template <typename Class>
126struct Traits {
127 typedef HPP_CONSTRAINTS_CB_REF<Class> Ptr_t;
128 typedef HPP_CONSTRAINTS_CB_WKREF<Class> WkPtr_t;
129};
130template <>
134};
135template <>
136struct Traits<pinocchio::Joint> {
139};
140
142 JointTranspose(const JointPtr_t& joint) : j_(joint) {}
144};
145template <>
150
163template <class ValueType = eigen::vector3_t,
164 class JacobianType = JacobianMatrix>
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
188template <class T, class ValueType = vector3_t,
189 class JacobianType = JacobianMatrix, class CrossType = CrossMatrix>
190class CalculusBase : public CalculusBaseAbstract<ValueType, JacobianType> {
191 public:
193
194 CalculusBase(const ValueType& value, const JacobianType& jacobian)
196
199
200 inline const ValueType& value() const { return value_; }
201 inline const JacobianType& jacobian() const { return jacobian_; }
203 if (vValid_) return;
204 static_cast<T*>(this)->impl_value(arg);
205 vValid_ = true;
206 }
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_; }
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
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
241HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(operator-, Difference)
243HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(operator*, ScalarProduct)
244HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(operator^, CrossProduct)
245
246HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(operator*, value_type, ScalarMultiply)
247HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(operator*, JointPtr_t, RotationMultiply)
248HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(operator*, JointTranspose, RotationMultiply)
249
250
251template <typename LhsValue, typename RhsValue>
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
275
276 Expression(const Expression& other) : rhs_(other.rhs()), lhs_(other.lhs()) {}
277
279 const typename Traits<RhsValue>::Ptr_t& rhs)
280 : rhs_(rhs), lhs_(lhs) {}
281
282 inline void init(Ptr_t self) { self_ = self; }
283
287
288 public:
289 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
290};
291
293template <typename LhsValue, typename RhsValue>
294class CrossProduct : public CalculusBase<CrossProduct<LhsValue, RhsValue> > {
295 public:
297
303
305 : Parent_t(other), e_(static_cast<const CrossProduct&>(other).e_) {}
306
308 const typename Traits<RhsValue>::Ptr_t& rhs)
309 : e_(Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
310
312 e_->lhs_->computeCrossValue(arg);
313 e_->rhs_->computeValue(arg);
314 this->value_ = e_->lhs_->cross() * e_->rhs_->value();
315 }
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() {
326 e_->rhs_->invalidate();
327 e_->lhs_->invalidate();
328 }
329
330 protected:
332
333 friend class Expression<LhsValue, RhsValue>;
334};
335
337template <typename LhsValue, typename RhsValue>
339 : public CalculusBase<ScalarProduct<LhsValue, RhsValue>,
340 Eigen::Matrix<value_type, 1, 1>, RowJacobianMatrix> {
341 public:
343 Eigen::Matrix<value_type, 1, 1>, RowJacobianMatrix>
345
351
353 : CalculusBase<ScalarProduct>(other),
354 e_(static_cast<const ScalarProduct&>(other).e_) {}
355
357 const typename Traits<RhsValue>::Ptr_t& rhs)
358 : e_(Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
359
361 e_->lhs_->computeValue(arg);
362 e_->rhs_->computeValue(arg);
363 this->value_[0] = e_->lhs_->value().dot(e_->rhs_->value());
364 }
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() {
375 e_->rhs_->invalidate();
376 e_->lhs_->invalidate();
377 }
378
379 protected:
381
382 friend class Expression<LhsValue, RhsValue>;
383};
384
386template <typename LhsValue, typename RhsValue>
387class Difference : public CalculusBase<Difference<LhsValue, RhsValue> > {
388 public:
390
396
398 : CalculusBase<Difference>(other),
399 e_(static_cast<const Difference&>(other).e_) {}
400
402 const typename Traits<RhsValue>::Ptr_t& rhs)
403 : e_(Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
404
406 e_->lhs_->computeValue(arg);
407 e_->rhs_->computeValue(arg);
408 this->value_ = e_->lhs_->value() - e_->rhs_->value();
409 }
411 e_->lhs_->computeJacobian(arg);
412 e_->rhs_->computeJacobian(arg);
413 this->jacobian_ = e_->lhs_->jacobian() - e_->rhs_->jacobian();
414 }
415 void invalidate() {
417 e_->rhs_->invalidate();
418 e_->lhs_->invalidate();
419 }
420
421 protected:
423
424 friend class Expression<LhsValue, RhsValue>;
425};
426
428template <typename LhsValue, typename RhsValue>
429class Sum : public CalculusBase<Sum<LhsValue, RhsValue> > {
430 public:
432
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
446 e_->lhs_->computeValue(arg);
447 e_->rhs_->computeValue(arg);
448 this->value_ = e_->lhs_->value() + e_->rhs_->value();
449 }
451 e_->lhs_->computeJacobian(arg);
452 e_->rhs_->computeJacobian(arg);
453 this->jacobian_ = e_->lhs_->jacobian() + e_->rhs_->jacobian();
454 }
455 void invalidate() {
457 e_->rhs_->invalidate();
458 e_->lhs_->invalidate();
459 }
460
461 protected:
463
464 friend class Expression<LhsValue, RhsValue>;
465};
466
468template <typename RhsValue>
469class ScalarMultiply : public CalculusBase<ScalarMultiply<RhsValue> > {
470 public:
472
478
481 e_(static_cast<const ScalarMultiply&>(other).e_) {}
482
484 const typename Traits<RhsValue>::Ptr_t& rhs)
485 : e_(Expression<value_type, RhsValue>::create(scalar, rhs)) {}
486
488 e_->rhs_->computeValue(arg);
489 this->value_ = e_->lhs_ * e_->rhs_->value();
490 }
492 e_->rhs_->computeJacobian(arg);
493 this->jacobian_ = e_->lhs_ * e_->rhs_->jacobian();
494 }
495 void invalidate() {
497 e_->rhs_->invalidate();
498 }
499
500 protected:
502
503 friend class Expression<value_type, RhsValue>;
504};
505
507template <typename RhsValue>
508class RotationMultiply : public CalculusBase<RotationMultiply<RhsValue> > {
509 public:
511
513
516 e_(static_cast<const RotationMultiply&>(other).e_),
517 transpose_(other.transpose_) {}
518
520 const typename Traits<pinocchio::Joint>::Ptr_t&,
521 const typename Traits<RhsValue>::Ptr_t&)
522
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
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
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 }
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() {
561 e_->rhs_->invalidate();
562 }
563
564 protected:
566
567 friend class Expression<pinocchio::Joint, RhsValue>;
568
569 private:
570 bool transpose_;
571};
572
574class PointInJoint : public CalculusBase<PointInJoint> {
575 public:
577
583
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_; }
621 if (joint_ == NULL) return;
622 this->value_ = joint_->currentTransformation().act(local_);
623 }
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 }
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:
648
649 public:
650 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
651};
652
654class VectorInJoint : public CalculusBase<VectorInJoint> {
655 public:
661
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_; }
689 if (joint_ == NULL) return;
690 this->value_ = joint_->currentTransformation().rotation() * vector_;
691 }
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 }
700 if (joint_ == NULL) return;
701 computeCrossMatrix(joint_->currentTransformation().rotation() * vector_,
702 this->cross_);
703 }
704
705 protected:
708
709 public:
710 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
711};
712
714template <typename FunctionType>
716 : public CalculusBase<FunctionExp<FunctionType>, vector_t, matrix_t> {
717 public:
719 typedef shared_ptr<FunctionType> FunctionTypePtr_t;
720
725
726 FunctionExp(const Parent_t& other)
727 : Parent_t(other), f_(other.f_), lge_(other.lge_) {}
728
730 : Parent_t(other), f_(other.f_), lge_(other.lge_) {}
731
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
743 f_->value(lge_, arg);
744 this->value_ = lge_.vector();
745 }
747 f_->jacobian(this->jacobian_, arg);
748 }
749
750 private:
751 FunctionTypePtr_t f_;
752 LiegroupElement lge_;
753};
754
758class 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
766
767 Point(const Point& point)
769
774 Point(const vector3_t& point, size_t jacobianNbCols)
776 point, JacobianMatrix::Zero(3, jacobianNbCols)) {}
777
780};
781
783class PointCom : public CalculusBase<PointCom, vector3_t, ComJacobian_t> {
784 public:
787
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
800 return comc_;
801 }
803 comc_->compute(hpp::pinocchio::COM);
804 }
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:
814};
815
817 : public CalculusBase<JointFrame, Eigen::Matrix<value_type, 6, 1>,
818 Eigen::Matrix<value_type, 6, Eigen::Dynamic> > {
819 public:
821
823
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_; }
840 const Transform3s& M = joint_->currentTransformation();
841 this->value_.head<3>() = M.translation();
842 logSO3(M.rotation(), theta_, this->value_.tail<3>());
843 }
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:
858 double theta_;
859
860 public:
861 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
862};
863
865template <typename ValueType = eigen::vector3_t,
866 typename JacobianType = JacobianMatrix>
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>
881 typedef Eigen::JacobiSVD<Value_t> SVD_t;
882
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
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
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
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 }
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_; }
964 if (svdValid_) return;
965 this->computeValue(arg);
966 svd_.compute(this->value_);
967 HPP_DEBUG_SVDCHECK(svd_);
968 svdValid_ = true;
969 }
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 }
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
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
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_;
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
Definition symbolic-calculus.hh:165
virtual const JacobianType & jacobian() const =0
virtual void computeValue(const ConfigurationIn_t arg)=0
virtual void computeJacobian(const ConfigurationIn_t arg)=0
ValueType ValueType_t
Definition symbolic-calculus.hh:167
virtual const ValueType & value() const =0
JacobianType JacobianType_t
Definition symbolic-calculus.hh:168
Definition symbolic-calculus.hh:190
ValueType value_
Definition symbolic-calculus.hh:226
bool cValid_
Definition symbolic-calculus.hh:230
bool vValid_
Definition symbolic-calculus.hh:230
void computeValue(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:202
void computeCrossValue(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:218
bool jValid_
Definition symbolic-calculus.hh:230
const CrossType & cross() const
Definition symbolic-calculus.hh:217
CalculusBase(const CalculusBase &o)
Definition symbolic-calculus.hh:197
void invalidate()
Definition symbolic-calculus.hh:212
JacobianType jacobian_
Definition symbolic-calculus.hh:227
CalculusBase(const ValueType &value, const JacobianType &jacobian)
Definition symbolic-calculus.hh:194
const ValueType & value() const
Definition symbolic-calculus.hh:200
void init(const typename Traits< T >::Ptr_t &ptr)
Definition symbolic-calculus.hh:232
const JacobianType & jacobian() const
Definition symbolic-calculus.hh:201
void computeJacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:207
CalculusBase()
Definition symbolic-calculus.hh:192
CrossType cross_
Definition symbolic-calculus.hh:228
Cross product of two expressions.
Definition symbolic-calculus.hh:294
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:311
CalculusBase< CrossProduct< LhsValue, RhsValue > > Parent_t
Definition symbolic-calculus.hh:296
CrossProduct(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition symbolic-calculus.hh:307
CrossProduct(const CalculusBase< CrossProduct > &other)
Definition symbolic-calculus.hh:304
void invalidate()
Definition symbolic-calculus.hh:324
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:316
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition symbolic-calculus.hh:331
HPP_CONSTRAINTS_CB_CREATE2(CrossProduct, const typename Traits< LhsValue >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) CrossProduct()
Definition symbolic-calculus.hh:298
Difference of two expressions.
Definition symbolic-calculus.hh:387
Difference(const CalculusBase< Difference > &other)
Definition symbolic-calculus.hh:397
HPP_CONSTRAINTS_CB_CREATE2(Difference, const typename Traits< LhsValue >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) Difference()
Definition symbolic-calculus.hh:391
void invalidate()
Definition symbolic-calculus.hh:415
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:410
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition symbolic-calculus.hh:422
Difference(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition symbolic-calculus.hh:401
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:405
CalculusBase< Difference< LhsValue, RhsValue > > Parent_t
Definition symbolic-calculus.hh:389
Base class for classes representing an operation.
Definition symbolic-calculus.hh:252
Traits< Expression >::WkPtr_t self_
Definition symbolic-calculus.hh:286
Traits< LhsValue >::Ptr_t lhs_
Definition symbolic-calculus.hh:285
static Ptr_t create(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition symbolic-calculus.hh:263
const RhsValue & rhs() const
Definition symbolic-calculus.hh:272
Expression(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition symbolic-calculus.hh:278
Expression()
Definition symbolic-calculus.hh:274
weak_ptr< Expression< LhsValue, RhsValue > > WkPtr_t
Definition symbolic-calculus.hh:255
void init(Ptr_t self)
Definition symbolic-calculus.hh:282
shared_ptr< Expression< LhsValue, RhsValue > > Ptr_t
Definition symbolic-calculus.hh:254
Expression(const Expression &other)
Definition symbolic-calculus.hh:276
static Ptr_t create()
Definition symbolic-calculus.hh:257
Traits< RhsValue >::Ptr_t rhs_
Definition symbolic-calculus.hh:284
const LhsValue & lhs() const
Definition symbolic-calculus.hh:270
Basic expression mapping a function as an expression.
Definition symbolic-calculus.hh:716
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:746
FunctionExp(const Parent_t &other)
Definition symbolic-calculus.hh:726
CalculusBase< FunctionExp< FunctionType >, vector_t, matrix_t > Parent_t
Definition symbolic-calculus.hh:718
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:742
FunctionExp(const FunctionExp &other)
Definition symbolic-calculus.hh:729
shared_ptr< FunctionType > FunctionTypePtr_t
Definition symbolic-calculus.hh:719
HPP_CONSTRAINTS_CB_CREATE1(FunctionExp< FunctionType >, const FunctionTypePtr_t &) FunctionExp()
Definition symbolic-calculus.hh:721
FunctionExp(const FunctionTypePtr_t &func)
Definition symbolic-calculus.hh:735
Definition symbolic-calculus.hh:818
double theta_
Definition symbolic-calculus.hh:858
JointFrame(const Parent_t &other)
Definition symbolic-calculus.hh:826
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:844
JointPtr_t joint_
Definition symbolic-calculus.hh:857
JointFrame(const JointFrame &jf)
Definition symbolic-calculus.hh:829
CalculusBase< JointFrame, ValueType_t, JacobianType_t > Parent_t
Definition symbolic-calculus.hh:820
const JointPtr_t & joint() const
Definition symbolic-calculus.hh:838
void impl_value(const ConfigurationIn_t)
Definition symbolic-calculus.hh:839
JointFrame(const JointPtr_t &joint)
Definition symbolic-calculus.hh:831
Matrix having Expression elements.
Definition symbolic-calculus.hh:871
void setSize(std::size_t nRows, std::size_t nCols)
Definition symbolic-calculus.hh:915
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
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > Jacobian_t
Definition symbolic-calculus.hh:874
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
std::size_t nCols_
Definition symbolic-calculus.hh:1059
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > PseudoInv_t
Definition symbolic-calculus.hh:875
Traits< Element_t >::Ptr_t ElementPtr_t
Definition symbolic-calculus.hh:880
MatrixOfExpressions(const MatrixOfExpressions &matrix)
Definition symbolic-calculus.hh:906
void computePseudoInverseJacobian(const ConfigurationIn_t arg, const Eigen::Ref< const Eigen::Matrix< value_type, Eigen::Dynamic, 1 > > &rhs)
Definition symbolic-calculus.hh:978
Eigen::JacobiSVD< Value_t > & svd()
Definition symbolic-calculus.hh:1049
ElementPtr_t & operator()(std::size_t i, std::size_t j)
Definition symbolic-calculus.hh:922
const PseudoInvJacobian_t & pinvJacobian() const
Definition symbolic-calculus.hh:962
const PseudoInv_t & pinv() const
Definition symbolic-calculus.hh:961
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
void computePseudoInverse(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:970
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > Value_t
Definition symbolic-calculus.hh:873
std::vector< std::vector< ElementPtr_t > > elements_
Definition symbolic-calculus.hh:1060
void invalidate()
Definition symbolic-calculus.hh:1051
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > PseudoInvJacobian_t
Definition symbolic-calculus.hh:877
MatrixOfExpressions(const Parent_t &other)
Definition symbolic-calculus.hh:897
Eigen::JacobiSVD< Value_t > SVD_t
Definition symbolic-calculus.hh:881
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:945
void set(std::size_t i, std::size_t j, const ElementPtr_t ptr)
Definition symbolic-calculus.hh:926
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:930
CalculusBase< MatrixOfExpressions, Value_t, Jacobian_t > Parent_t
Definition symbolic-calculus.hh:878
CalculusBaseAbstract< ValueType, JacobianType > Element_t
Definition symbolic-calculus.hh:879
void computeSVD(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:963
Basic expression representing a COM.
Definition symbolic-calculus.hh:783
const ComJacobian_t & jacobian() const
Definition symbolic-calculus.hh:797
void impl_value(const ConfigurationIn_t)
Definition symbolic-calculus.hh:802
const CenterOfMassComputationPtr_t & centerOfMassComputation() const
Definition symbolic-calculus.hh:799
PointCom(const CenterOfMassComputationPtr_t &comc)
Definition symbolic-calculus.hh:794
const vector3_t & value() const
Definition symbolic-calculus.hh:796
PointCom(const Parent_t &other)
Definition symbolic-calculus.hh:790
CalculusBase< PointCom, vector3_t, ComJacobian_t > Parent_t
Definition symbolic-calculus.hh:785
CenterOfMassComputationPtr_t comc_
Definition symbolic-calculus.hh:813
void impl_jacobian(const ConfigurationIn_t)
Definition symbolic-calculus.hh:805
Basic expression representing a point in a joint frame.
Definition symbolic-calculus.hh:574
void impl_value(const ConfigurationIn_t)
Definition symbolic-calculus.hh:620
JointPtr_t joint_
Definition symbolic-calculus.hh:645
vector3_t local_
Definition symbolic-calculus.hh:646
PointInJoint(const PointInJoint &pointInJoint)
Definition symbolic-calculus.hh:592
const JointPtr_t & joint() const
Definition symbolic-calculus.hh:618
PointInJoint(const JointPtr_t &joint, const vector3_t &pointInLocalFrame, size_type nbDof)
Definition symbolic-calculus.hh:605
bool center_
Definition symbolic-calculus.hh:647
PointInJoint(const CalculusBase< PointInJoint > &other)
Definition symbolic-calculus.hh:584
PointInJoint(const JointPtr_t &joint, const vector3_t &pointInLocalFrame)
Definition symbolic-calculus.hh:598
const vector3_t & local() const
Definition symbolic-calculus.hh:619
void computeCrossRXl()
Definition symbolic-calculus.hh:634
void impl_jacobian(const ConfigurationIn_t)
Definition symbolic-calculus.hh:624
CalculusBase< PointInJoint > Parent_t
Definition symbolic-calculus.hh:576
Definition symbolic-calculus.hh:758
void impl_value(const ConfigurationIn_t)
Definition symbolic-calculus.hh:778
Point(const vector3_t &point, size_t jacobianNbCols)
Definition symbolic-calculus.hh:774
Point(const CalculusBase< Point, vector3_t, JacobianMatrix > &other)
Definition symbolic-calculus.hh:764
void impl_jacobian(const ConfigurationIn_t)
Definition symbolic-calculus.hh:779
Point(const Point &point)
Definition symbolic-calculus.hh:767
Multiplication of an expression by a rotation matrix.
Definition symbolic-calculus.hh:508
RotationMultiply(const CalculusBase< RotationMultiply > &other)
Definition symbolic-calculus.hh:514
Expression< pinocchio::Joint, RhsValue >::Ptr_t e_
Definition symbolic-calculus.hh:565
HPP_CONSTRAINTS_CB_CREATE2(RotationMultiply, const typename Traits< pinocchio::Joint >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) HPP_CONSTRAINTS_CB_CREATE2(RotationMultiply
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:538
void invalidate()
Definition symbolic-calculus.hh:559
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:546
RotationMultiply(const typename Traits< pinocchio::Joint >::Ptr_t &joint, const typename Traits< RhsValue >::Ptr_t &rhs, bool transpose=false)
Definition symbolic-calculus.hh:532
CalculusBase< RotationMultiply< RhsValue > > Parent_t
Definition symbolic-calculus.hh:510
RotationMultiply()
Definition symbolic-calculus.hh:512
Multiplication of an expression by a scalar.
Definition symbolic-calculus.hh:469
ScalarMultiply(const typename Traits< value_type >::Ptr_t &scalar, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition symbolic-calculus.hh:483
ScalarMultiply(const CalculusBase< ScalarMultiply > &other)
Definition symbolic-calculus.hh:479
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:487
CalculusBase< ScalarMultiply< RhsValue > > Parent_t
Definition symbolic-calculus.hh:471
void invalidate()
Definition symbolic-calculus.hh:495
Expression< value_type, RhsValue >::Ptr_t e_
Definition symbolic-calculus.hh:501
HPP_CONSTRAINTS_CB_CREATE2(ScalarMultiply, const typename Traits< value_type >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) ScalarMultiply()
Definition symbolic-calculus.hh:473
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:491
Scalar product of two expressions.
Definition symbolic-calculus.hh:340
HPP_CONSTRAINTS_CB_CREATE2(ScalarProduct, const typename Traits< LhsValue >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) ScalarProduct()
Definition symbolic-calculus.hh:346
void invalidate()
Definition symbolic-calculus.hh:373
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:365
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:360
CalculusBase< ScalarProduct< LhsValue, RhsValue >, Eigen::Matrix< value_type, 1, 1 >, RowJacobianMatrix > Parent_t
Definition symbolic-calculus.hh:344
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition symbolic-calculus.hh:380
ScalarProduct(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition symbolic-calculus.hh:356
ScalarProduct(const CalculusBase< ScalarProduct > &other)
Definition symbolic-calculus.hh:352
Sum of two expressions.
Definition symbolic-calculus.hh:429
void impl_jacobian(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:450
Sum(const typename Traits< RhsValue >::Ptr_t &rhs, const typename Traits< LhsValue >::Ptr_t &lhs)
Definition symbolic-calculus.hh:441
HPP_CONSTRAINTS_CB_CREATE2(Sum, const typename Traits< LhsValue >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) Sum()
Definition symbolic-calculus.hh:433
void invalidate()
Definition symbolic-calculus.hh:455
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition symbolic-calculus.hh:462
CalculusBase< Sum< LhsValue, RhsValue > > Parent_t
Definition symbolic-calculus.hh:431
Sum(const CalculusBase< Sum > &other)
Definition symbolic-calculus.hh:438
void impl_value(const ConfigurationIn_t arg)
Definition symbolic-calculus.hh:445
Definition symbolic-calculus.hh:126
HPP_CONSTRAINTS_CB_REF< Class > Ptr_t
Definition symbolic-calculus.hh:127
HPP_CONSTRAINTS_CB_WKREF< Class > WkPtr_t
Definition symbolic-calculus.hh:128
Basic expression representing a vector in a joint frame.
Definition symbolic-calculus.hh:654
VectorInJoint(const VectorInJoint &vectorInJoint)
Definition symbolic-calculus.hh:667
VectorInJoint(const CalculusBase< VectorInJoint > &other)
Definition symbolic-calculus.hh:662
JointPtr_t joint_
Definition symbolic-calculus.hh:706
void impl_value(const ConfigurationIn_t)
Definition symbolic-calculus.hh:688
void impl_jacobian(const ConfigurationIn_t)
Definition symbolic-calculus.hh:692
vector3_t vector_
Definition symbolic-calculus.hh:707
VectorInJoint(const JointPtr_t &joint, const vector3_t &vectorInLocalFrame, const size_type &nbDof)
Definition symbolic-calculus.hh:675
const vector3_t & vector() const
Definition symbolic-calculus.hh:687
const JointPtr_t & joint() const
Definition symbolic-calculus.hh:686
VectorInJoint(const JointPtr_t &joint, const vector3_t &vectorInLocalFrame)
Definition symbolic-calculus.hh:672
void computeCrossRXl()
Definition symbolic-calculus.hh:699
void logSO3(const matrix3_t &R, value_type &theta, Eigen::MatrixBase< Derived > const &result)
Definition tools.hh:60
void JlogSO3(const value_type &theta, const Eigen::MatrixBase< Derived > &log, matrix3_t &Jlog)
Definition tools.hh:129
Eigen::Matrix< value_type, 3, Eigen::Dynamic, Eigen::RowMajor > JacobianMatrix
Definition symbolic-calculus.hh:123
eigen::matrix3_t CrossMatrix
Definition symbolic-calculus.hh:119
Eigen::Matrix< value_type, 1, Eigen::Dynamic, Eigen::RowMajor > RowJacobianMatrix
Definition symbolic-calculus.hh:121
#define HPP_DEBUG_SVDCHECK(svd)
Definition macros.hh:59
Definition fwd.hh:38
Eigen::Matrix< value_type, 3, 1 > vector3_t
Definition fwd.hh:73
Eigen::Matrix< value_type, 3, 3 > matrix3_t
Definition fwd.hh:72
pinocchio::LiegroupElement LiegroupElement
Definition fwd.hh:65
pinocchio::matrix3_t matrix3_t
Definition fwd.hh:53
pinocchio::vector3_t vector3_t
Definition fwd.hh:52
pinocchio::JointJacobian_t JointJacobian_t
Definition fwd.hh:63
pinocchio::Transform3s Transform3s
Definition fwd.hh:64
pinocchio::Joint Joint
Definition fwd.hh:51
pinocchio::size_type size_type
Definition fwd.hh:47
pinocchio::ComJacobian_t ComJacobian_t
Definition fwd.hh:62
pinocchio::value_type value_type
Definition fwd.hh:48
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:106
pinocchio::matrix_t matrix_t
Definition fwd.hh:56
pinocchio::CenterOfMassComputationPtr_t CenterOfMassComputationPtr_t
Definition fwd.hh:112
pinocchio::vector_t vector_t
Definition fwd.hh:59
pinocchio::JointPtr_t JointPtr_t
Definition fwd.hh:49
Definition active-set-differentiable-function.hh:36
Definition symbolic-calculus.hh:141
const JointPtr_t j_
Definition symbolic-calculus.hh:143
JointTranspose(const JointPtr_t &joint)
Definition symbolic-calculus.hh:142
JointTranspose WkPtr_t
Definition symbolic-calculus.hh:148
JointTranspose Ptr_t
Definition symbolic-calculus.hh:147
JointPtr_t WkPtr_t
Definition symbolic-calculus.hh:138
JointPtr_t Ptr_t
Definition symbolic-calculus.hh:137
value_type WkPtr_t
Definition symbolic-calculus.hh:133
value_type Ptr_t
Definition symbolic-calculus.hh:132
#define HPP_CONSTRAINTS_CB_CREATE1(Class, Arg0Type)
Definition symbolic-calculus.hh:62
#define HPP_CONSTRAINTS_CB_CREATE2(Class, Arg0Type, Arg1Type)
Definition symbolic-calculus.hh:69
#define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(op, InType, OutType)
Definition symbolic-calculus.hh:35
#define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(op, OutType)
Definition symbolic-calculus.hh:47
#define HPP_CONSTRAINTS_CB_CREATE3(Class, Arg0Type, Arg1Type, Arg2Type)
Definition symbolic-calculus.hh:76