29 #ifndef HPP_CONSTRAINTS_SYMBOLIC_CALCULUS_HH 30 #define HPP_CONSTRAINTS_SYMBOLIC_CALCULUS_HH 32 #define HPP_CONSTRAINTS_CB_REF shared_ptr 33 #define HPP_CONSTRAINTS_CB_WKREF shared_ptr 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); \ 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); 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); \ 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); 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)); \ 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)); \ 76 #define HPP_CONSTRAINTS_CB_CREATE3(Class, Arg0Type, Arg1Type, Arg2Type) \ 77 static typename Traits<Class>::Ptr_t create(Arg0Type arg0, Arg1Type arg1, \ 79 typename Traits<Class>::Ptr_t ptr(new Class(arg0, arg1, arg2)); \ 89 #include <hpp/pinocchio/center-of-mass-computation.hh> 90 #include <hpp/pinocchio/joint.hh> 91 #include <hpp/pinocchio/liegroup-element.hh> 94 namespace constraints {
100 template <
typename ValueType,
typename JacobianType>
102 template <
typename T>
105 template <
typename LhsValue,
typename RhsValue>
107 template <
typename LhsValue,
typename RhsValue>
109 template <
typename LhsValue,
typename RhsValue>
111 template <
typename LhsValue,
typename RhsValue>
113 template <
typename LhsValue,
typename RhsValue>
115 template <
typename RhsValue>
117 template <
typename RhsValue>
120 typedef Eigen::Matrix<value_type, 1, Eigen::Dynamic, Eigen::RowMajor>
122 typedef Eigen::Matrix<value_type, 3, Eigen::Dynamic, Eigen::RowMajor>
125 template <
typename Class>
127 typedef HPP_CONSTRAINTS_CB_REF<Class>
Ptr_t;
128 typedef HPP_CONSTRAINTS_CB_WKREF<Class>
WkPtr_t;
170 virtual const ValueType& value()
const = 0;
171 virtual const JacobianType& jacobian()
const = 0;
174 virtual void invalidate() = 0;
188 template <
class T,
class ValueType =
vector3_t,
189 class JacobianType =
JacobianMatrix,
class CrossType = CrossMatrix>
195 : value_(value), jacobian_(jacobian), cross_(CrossMatrix::Zero()) {}
198 : value_(o.value()), jacobian_(o.jacobian_), cross_(o.cross()) {}
200 inline const ValueType&
value()
const {
return value_; }
201 inline const JacobianType&
jacobian()
const {
return jacobian_; }
204 static_cast<T*
>(
this)->impl_value(arg);
209 static_cast<T*
>(
this)->impl_jacobian(arg);
217 inline const CrossType&
cross()
const {
return cross_; }
221 computeCrossMatrix(value_, cross_);
238 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
250 template <
typename LhsValue,
typename RhsValue>
254 typedef shared_ptr<Expression<LhsValue, RhsValue> >
Ptr_t;
255 typedef weak_ptr<Expression<LhsValue, RhsValue> >
WkPtr_t;
270 const LhsValue&
lhs()
const {
return lhs_; }
272 const RhsValue&
rhs()
const {
return rhs_; }
280 : rhs_(rhs), lhs_(lhs) {}
282 inline void init(Ptr_t
self) { self_ =
self; }
289 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
293 template <
typename LhsValue,
typename RhsValue>
305 : Parent_t(other), e_(static_cast<const
CrossProduct&>(other).e_) {}
309 : e_(
Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
312 e_->lhs_->computeCrossValue(arg);
313 e_->rhs_->computeValue(arg);
314 this->value_ = e_->lhs_->cross() * e_->rhs_->value();
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();
325 Parent_t::invalidate();
326 e_->rhs_->invalidate();
327 e_->lhs_->invalidate();
337 template <
typename LhsValue,
typename RhsValue>
339 :
public CalculusBase<ScalarProduct<LhsValue, RhsValue>,
340 Eigen::Matrix<value_type, 1, 1>, RowJacobianMatrix> {
358 : e_(
Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
361 e_->lhs_->computeValue(arg);
362 e_->rhs_->computeValue(arg);
363 this->value_[0] = e_->lhs_->value().dot(e_->rhs_->value());
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();
374 Parent_t::invalidate();
375 e_->rhs_->invalidate();
376 e_->lhs_->invalidate();
386 template <
typename LhsValue,
typename RhsValue>
399 e_(static_cast<const
Difference&>(other).e_) {}
403 : e_(
Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
406 e_->lhs_->computeValue(arg);
407 e_->rhs_->computeValue(arg);
408 this->value_ = e_->lhs_->value() - e_->rhs_->value();
411 e_->lhs_->computeJacobian(arg);
412 e_->rhs_->computeJacobian(arg);
413 this->jacobian_ = e_->lhs_->jacobian() - e_->rhs_->jacobian();
416 Parent_t::invalidate();
417 e_->rhs_->invalidate();
418 e_->lhs_->invalidate();
428 template <
typename LhsValue,
typename RhsValue>
443 : e_(
Expression<LhsValue, RhsValue>::create(lhs, rhs)) {}
446 e_->lhs_->computeValue(arg);
447 e_->rhs_->computeValue(arg);
448 this->value_ = e_->lhs_->value() + e_->rhs_->value();
451 e_->lhs_->computeJacobian(arg);
452 e_->rhs_->computeJacobian(arg);
453 this->jacobian_ = e_->lhs_->jacobian() + e_->rhs_->jacobian();
456 Parent_t::invalidate();
457 e_->rhs_->invalidate();
458 e_->lhs_->invalidate();
468 template <
typename RhsValue>
488 e_->rhs_->computeValue(arg);
489 this->value_ = e_->lhs_ * e_->rhs_->value();
492 e_->rhs_->computeJacobian(arg);
493 this->jacobian_ = e_->lhs_ * e_->rhs_->jacobian();
496 Parent_t::invalidate();
497 e_->rhs_->invalidate();
507 template <
typename RhsValue>
517 transpose_(other.transpose_) {}
529 : e_(
Expression<pinocchio::Joint, RhsValue>::create(joint.j_, rhs)),
534 bool transpose =
false)
535 : e_(
Expression<pinocchio::Joint, RhsValue>::create(joint, rhs)),
536 transpose_(transpose) {}
539 e_->rhs_->computeValue(arg);
540 const matrix3_t& R = e_->lhs_->currentTransformation().rotation();
542 this->value_ = R.transpose() * e_->rhs_->value();
544 this->value_ = R * e_->rhs_->value();
547 e_->rhs_->computeJacobian(arg);
548 e_->rhs_->computeCrossValue(arg);
550 const matrix3_t& R = e_->lhs_->currentTransformation().rotation();
554 ((e_->rhs_->cross() * R) * J.bottomRows<3>() + e_->rhs_->jacobian());
556 this->jacobian_ = R * ((e_->rhs_->cross() * R) * J.bottomRows<3>() +
557 e_->rhs_->jacobian());
560 Parent_t::invalidate();
561 e_->rhs_->invalidate();
589 center_ = local_.isZero();
594 joint_(pointInJoint.joint()),
595 local_(pointInJoint.local()),
596 center_(local_.isZero()) {}
600 local_(pointInLocalFrame),
601 center_(pointInLocalFrame.isZero()) {
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();
621 if (joint_ == NULL)
return;
622 this->value_ = joint_->currentTransformation().act(local_);
625 if (joint_ == NULL)
return;
627 const matrix3_t& R = joint_->currentTransformation().rotation();
628 this->jacobian_.noalias() = R * J.topRows<3>();
631 this->jacobian_.noalias() -= (this->cross_ * R) * J.bottomRows<3>();
635 if (joint_ == NULL)
return;
637 this->cross_.setZero();
640 computeCrossMatrix(joint_->currentTransformation().rotation() * local_,
650 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
665 vector_(static_cast<const
VectorInJoint&>(other).vector()) {}
669 joint_(vectorInJoint.joint()),
670 vector_(vectorInJoint.vector()) {}
673 : joint_(joint), vector_(vectorInLocalFrame) {}
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();
689 if (joint_ == NULL)
return;
690 this->value_ = joint_->currentTransformation().rotation() * vector_;
693 if (joint_ == NULL)
return;
695 const matrix3_t& R = joint_->currentTransformation().rotation();
697 this->jacobian_.noalias() = (-this->cross_ * R) * J.bottomRows<3>();
700 if (joint_ == NULL)
return;
701 computeCrossMatrix(joint_->currentTransformation().rotation() * vector_,
710 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
714 template <
typename FunctionType>
716 :
public CalculusBase<FunctionExp<FunctionType>, vector_t, matrix_t> {
722 const FunctionTypePtr_t&)
727 : Parent_t(other), f_(other.f_), lge_(other.lge_) {}
730 : Parent_t(other), f_(other.f_), lge_(other.lge_) {}
736 : Parent_t(vector_t::Zero(func->outputSize()),
737 matrix_t::Zero(func->outputDerivativeSize(),
738 func->inputDerivativeSize())),
740 lge_(func->outputSpace()) {}
743 f_->value(lge_, arg);
744 this->value_ = lge_.vector();
747 f_->jacobian(this->jacobian_, arg);
751 FunctionTypePtr_t f_;
792 comc_(static_cast<const
PointCom&>(other).centerOfMassComputation()) {}
803 comc_->compute(hpp::pinocchio::COM);
806 comc_->compute(hpp::pinocchio::COMPUTE_ALL);
817 :
public CalculusBase<JointFrame, Eigen::Matrix<value_type, 6, 1>,
818 Eigen::Matrix<value_type, 6, Eigen::Dynamic> > {
827 : Parent_t(other), joint_(static_cast<const
JointFrame&>(other).joint_) {}
833 this->jacobian_.resize(6,
834 joint->robot()->numberDof() -
835 joint->robot()->extraConfigSpace().dimension());
840 const Transform3f& M = joint_->currentTransformation();
841 this->value_.head<3>() = M.translation();
842 logSO3(M.rotation(), theta_, this->value_.tail<3>());
847 const matrix3_t& R(joint_->currentTransformation().rotation());
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>();
861 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
869 MatrixOfExpressions<ValueType, JacobianType>,
870 Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic>,
871 Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic> > {
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;
884 const Eigen::Ref<const Value_t>&,
885 const Eigen::Ref<const Jacobian_t>&)
888 const Eigen::Ref<const Jacobian_t>& jacobian)
889 : Parent_t(value, jacobian),
892 svd_(value.rows(), value.cols(),
908 nRows_(matrix.nRows_),
909 nCols_(matrix.nCols_),
910 elements_(matrix.elements_),
912 piValid_(matrix.piValid_),
913 svdValid_(matrix.svdValid_) {}
915 void setSize(std::size_t nRows, std::size_t nCols) {
918 elements_.resize(nRows_);
919 for (std::size_t i = 0; i < nRows; ++i) elements_[i].resize(nCols);
923 return elements_[i][j];
926 void set(std::size_t i, std::size_t j,
const ElementPtr_t ptr) {
927 elements_[i][j] = ptr;
932 for (std::size_t i = 0; i < nRows_; ++i) {
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();
947 for (std::size_t i = 0; i < nRows_; ++i) {
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();
961 inline const PseudoInv_t&
pinv()
const {
return pi_; }
964 if (svdValid_)
return;
965 this->computeValue(arg);
966 svd_.compute(this->value_);
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_);
980 const Eigen::Ref<
const Eigen::Matrix<value_type, Eigen::Dynamic, 1> >&
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);
989 Jacobian_t cache(this->jacobian_.rows(), nbDof);
990 jacobianTimes(arg, piTrhs, cache);
991 pij_.noalias() = -pi_ * cache;
992 cache.resize(inSize, nbDof);
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;
999 jacobianTransposeTimes(arg, pi_.transpose() * piTrhs, cache);
1000 pk_.resize(inSize, inSize);
1001 projectorOnKernel<SVD_t>(svd_, pk_,
true);
1002 pij_.noalias() += pk_ * cache;
1007 const Eigen::Ref<
const Eigen::Matrix<value_type, Eigen::Dynamic, 1> >&
1009 Eigen::Ref<Jacobian_t> cache)
const {
1012 for (std::size_t i = 0; i < nRows_; ++i) {
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];
1029 const Eigen::Ref<
const Eigen::Matrix<value_type, Eigen::Dynamic, 1> >&
1031 Eigen::Ref<Jacobian_t> cache)
const {
1034 for (std::size_t i = 0; i < nRows_; ++i) {
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);
1049 Eigen::JacobiSVD<Value_t>&
svd() {
return svd_; }
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();
1067 bool piValid_, svdValid_;
1070 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1077 #endif // HPP_CONSTRAINTS_SYMBOLIC_CALCULUS_HH 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
PointCom(const CenterOfMassComputationPtr_t &comc)
Definition: symbolic-calculus.hh:794
CalculusBase< JointFrame, ValueType_t, JacobianType_t > Parent_t
Definition: symbolic-calculus.hh:820
const vector3_t & vector() const
Definition: symbolic-calculus.hh:687
CalculusBaseAbstract< ValueType, JacobianType > Element_t
Definition: symbolic-calculus.hh:879
pinocchio::vector_t vector_t
Definition: fwd.hh:58
PointInJoint(const JointPtr_t &joint, const vector3_t &pointInLocalFrame, size_type nbDof)
Definition: symbolic-calculus.hh:605
CalculusBase< RotationMultiply< RhsValue > > Parent_t
Definition: symbolic-calculus.hh:510
Difference of two expressions.
Definition: symbolic-calculus.hh:112
void invalidate()
Definition: symbolic-calculus.hh:212
#define HPP_DEBUG_SVDCHECK(svd)
Definition: macros.hh:59
Eigen::Matrix< value_type, 3, 3 > matrix3_t
Definition: fwd.hh:71
Sum(const typename Traits< RhsValue >::Ptr_t &rhs, const typename Traits< LhsValue >::Ptr_t &lhs)
Definition: symbolic-calculus.hh:441
Multiplication of an expression by a scalar.
Definition: symbolic-calculus.hh:116
shared_ptr< FunctionType > FunctionTypePtr_t
Definition: symbolic-calculus.hh:719
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > Value_t
Definition: symbolic-calculus.hh:873
JacobianType jacobian_
Definition: symbolic-calculus.hh:227
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > Jacobian_t
Definition: symbolic-calculus.hh:874
bool vValid_
Definition: symbolic-calculus.hh:230
pinocchio::JointJacobian_t JointJacobian_t
Definition: fwd.hh:62
void impl_jacobian(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:624
MatrixOfExpressions(const Parent_t &other)
Definition: symbolic-calculus.hh:897
PointInJoint(const CalculusBase< PointInJoint > &other)
Definition: symbolic-calculus.hh:584
value_type Ptr_t
Definition: symbolic-calculus.hh:132
#define HPP_CONSTRAINTS_CB_CREATE2(Class, Arg0Type, Arg1Type)
Definition: symbolic-calculus.hh:69
Eigen::JacobiSVD< Value_t > & svd()
Definition: symbolic-calculus.hh:1049
Traits< LhsValue >::Ptr_t lhs_
Definition: symbolic-calculus.hh:285
ValueType value_
Definition: symbolic-calculus.hh:226
void computePseudoInverseJacobian(const ConfigurationIn_t arg, const Eigen::Ref< const Eigen::Matrix< value_type, Eigen::Dynamic, 1 > > &rhs)
Definition: symbolic-calculus.hh:978
Definition: active-set-differentiable-function.hh:36
Basic expression representing a COM.
Definition: symbolic-calculus.hh:783
HPP_CONSTRAINTS_CB_CREATE2(Difference, const typename Traits< LhsValue >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) Difference()
Definition: symbolic-calculus.hh:391
Basic expression mapping a function as an expression.
Definition: symbolic-calculus.hh:715
Matrix having Expression elements.
Definition: symbolic-calculus.hh:867
Definition: symbolic-calculus.hh:103
Eigen::JacobiSVD< Value_t > SVD_t
Definition: symbolic-calculus.hh:881
Basic expression representing a point in a joint frame.
Definition: symbolic-calculus.hh:574
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:64
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > PseudoInv_t
Definition: symbolic-calculus.hh:875
Eigen::Matrix< value_type, 1, Eigen::Dynamic, Eigen::RowMajor > RowJacobianMatrix
Definition: symbolic-calculus.hh:121
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:365
void computeValue(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:202
Traits< Expression >::WkPtr_t self_
Definition: symbolic-calculus.hh:286
Definition: symbolic-calculus.hh:101
ElementPtr_t & operator()(std::size_t i, std::size_t j)
Definition: symbolic-calculus.hh:922
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
CalculusBase< FunctionExp< FunctionType >, vector_t, matrix_t > Parent_t
Definition: symbolic-calculus.hh:718
Point(const CalculusBase< Point, vector3_t, JacobianMatrix > &other)
Definition: symbolic-calculus.hh:764
void init(Ptr_t self)
Definition: symbolic-calculus.hh:282
pinocchio::matrix3_t matrix3_t
Definition: fwd.hh:52
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:945
void computeCrossRXl()
Definition: symbolic-calculus.hh:634
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:802
void impl_jacobian(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:805
void invalidate()
Definition: symbolic-calculus.hh:415
Eigen::Matrix< value_type, 3, Eigen::Dynamic, Eigen::RowMajor > JacobianMatrix
Definition: symbolic-calculus.hh:123
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:462
ValueType ValueType_t
Definition: symbolic-calculus.hh:167
CalculusBase(const ValueType &value, const JacobianType &jacobian)
Definition: symbolic-calculus.hh:194
VectorInJoint(const JointPtr_t &joint, const vector3_t &vectorInLocalFrame)
Definition: symbolic-calculus.hh:672
pinocchio::matrix_t matrix_t
Definition: fwd.hh:55
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:311
CalculusBase()
Definition: symbolic-calculus.hh:192
ScalarMultiply(const typename Traits< value_type >::Ptr_t &scalar, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:483
CalculusBase< MatrixOfExpressions, Value_t, Jacobian_t > Parent_t
Definition: symbolic-calculus.hh:878
HPP_CONSTRAINTS_CB_CREATE1(FunctionExp< FunctionType >, const FunctionTypePtr_t &) FunctionExp()
Definition: symbolic-calculus.hh:721
void invalidate()
Definition: symbolic-calculus.hh:455
JointPtr_t joint_
Definition: symbolic-calculus.hh:706
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:742
pinocchio::ComJacobian_t ComJacobian_t
Definition: fwd.hh:61
Point(const Point &point)
Definition: symbolic-calculus.hh:767
VectorInJoint(const VectorInJoint &vectorInJoint)
Definition: symbolic-calculus.hh:667
shared_ptr< Expression< LhsValue, RhsValue > > Ptr_t
Definition: symbolic-calculus.hh:254
const LhsValue & lhs() const
Definition: symbolic-calculus.hh:270
Point(const vector3_t &point, size_t jacobianNbCols)
Definition: symbolic-calculus.hh:774
ScalarMultiply(const CalculusBase< ScalarMultiply > &other)
Definition: symbolic-calculus.hh:479
pinocchio::vector3_t vector3_t
Definition: fwd.hh:51
CalculusBase< CrossProduct< LhsValue, RhsValue > > Parent_t
Definition: symbolic-calculus.hh:296
Expression()
Definition: symbolic-calculus.hh:274
std::vector< std::vector< ElementPtr_t > > elements_
Definition: symbolic-calculus.hh:1060
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:746
#define HPP_CONSTRAINTS_CB_CREATE3(Class, Arg0Type, Arg1Type, Arg2Type)
Definition: symbolic-calculus.hh:76
PointCom(const Parent_t &other)
Definition: symbolic-calculus.hh:790
const PseudoInvJacobian_t & pinvJacobian() const
Definition: symbolic-calculus.hh:962
void computePseudoInverse(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:970
JacobianType JacobianType_t
Definition: symbolic-calculus.hh:168
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:688
void invalidate()
Definition: symbolic-calculus.hh:324
Expression< pinocchio::Joint, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:565
CenterOfMassComputationPtr_t comc_
Definition: symbolic-calculus.hh:813
Eigen::Matrix< value_type, 3, 1 > vector3_t
Definition: fwd.hh:72
VectorInJoint(const JointPtr_t &joint, const vector3_t &vectorInLocalFrame, const size_type &nbDof)
Definition: symbolic-calculus.hh:675
value_type WkPtr_t
Definition: symbolic-calculus.hh:133
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:105
void impl_jacobian(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:779
const PseudoInv_t & pinv() const
Definition: symbolic-calculus.hh:961
JointPtr_t joint_
Definition: symbolic-calculus.hh:645
assert(d.lhs()._blocks()==d.rhs()._blocks())
Scalar product of two expressions.
Definition: symbolic-calculus.hh:110
void computeCrossRXl()
Definition: symbolic-calculus.hh:699
Sum of two expressions.
Definition: symbolic-calculus.hh:114
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:49
#define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(op, InType, OutType)
Definition: symbolic-calculus.hh:35
void invalidate()
Definition: symbolic-calculus.hh:495
std::size_t nRows_
Definition: symbolic-calculus.hh:1059
Expression(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:278
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:405
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:422
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_CB_CREATE2(ScalarProduct, const typename Traits< LhsValue >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) ScalarProduct()
Definition: symbolic-calculus.hh:346
const JacobianType & jacobian() const
Definition: symbolic-calculus.hh:201
void impl_jacobian(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:692
Traits< Element_t >::Ptr_t ElementPtr_t
Definition: symbolic-calculus.hh:880
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:620
FunctionExp(const Parent_t &other)
Definition: symbolic-calculus.hh:726
const JointPtr_t j_
Definition: symbolic-calculus.hh:143
CalculusBase< PointCom, vector3_t, ComJacobian_t > Parent_t
Definition: symbolic-calculus.hh:785
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:410
Cross product of two expressions.
Definition: symbolic-calculus.hh:108
vector3_t vector_
Definition: symbolic-calculus.hh:707
#define HPP_CONSTRAINTS_CB_CREATE1(Class, Arg0Type)
Definition: symbolic-calculus.hh:62
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:487
Definition: symbolic-calculus.hh:141
CalculusBase< Sum< LhsValue, RhsValue > > Parent_t
Definition: symbolic-calculus.hh:431
CalculusBase< PointInJoint > Parent_t
Definition: symbolic-calculus.hh:576
CalculusBase< ScalarMultiply< RhsValue > > Parent_t
Definition: symbolic-calculus.hh:471
void computeJacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:207
#define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(op, OutType)
Definition: symbolic-calculus.hh:47
void invalidate()
Definition: symbolic-calculus.hh:373
const ComJacobian_t & jacobian() const
Definition: symbolic-calculus.hh:797
FunctionExp(const FunctionExp &other)
Definition: symbolic-calculus.hh:729
void computeCrossValue(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:218
HPP_CONSTRAINTS_CB_CREATE2(Sum, const typename Traits< LhsValue >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) Sum()
Definition: symbolic-calculus.hh:433
Definition: symbolic-calculus.hh:758
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:380
static Ptr_t create()
Definition: symbolic-calculus.hh:257
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > PseudoInvJacobian_t
Definition: symbolic-calculus.hh:877
vector3_t local_
Definition: symbolic-calculus.hh:646
void setSize(std::size_t nRows, std::size_t nCols)
Definition: symbolic-calculus.hh:915
double theta_
Definition: symbolic-calculus.hh:858
const vector3_t & local() const
Definition: symbolic-calculus.hh:619
FunctionExp(const FunctionTypePtr_t &func)
Definition: symbolic-calculus.hh:735
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:331
Sum(const CalculusBase< Sum > &other)
Definition: symbolic-calculus.hh:438
void invalidate()
Definition: symbolic-calculus.hh:1051
RotationMultiply()
Definition: symbolic-calculus.hh:512
PointInJoint(const JointPtr_t &joint, const vector3_t &pointInLocalFrame)
Definition: symbolic-calculus.hh:598
CrossProduct(const CalculusBase< CrossProduct > &other)
Definition: symbolic-calculus.hh:304
Multiplication of an expression by a rotation matrix.
Definition: symbolic-calculus.hh:118
void invalidate()
Definition: symbolic-calculus.hh:559
Definition: symbolic-calculus.hh:816
const CenterOfMassComputationPtr_t & centerOfMassComputation() const
Definition: symbolic-calculus.hh:799
const JointPtr_t & joint() const
Definition: symbolic-calculus.hh:838
const CrossType & cross() const
Definition: symbolic-calculus.hh:217
bool center_
Definition: symbolic-calculus.hh:647
JointFrame(const Parent_t &other)
Definition: symbolic-calculus.hh:826
const vector3_t & value() const
Definition: symbolic-calculus.hh:796
HPP_CONSTRAINTS_CB_REF< Class > Ptr_t
Definition: symbolic-calculus.hh:127
Base class for classes representing an operation.
Definition: symbolic-calculus.hh:106
const JointPtr_t & joint() const
Definition: symbolic-calculus.hh:686
void computeSVD(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:963
JointPtr_t Ptr_t
Definition: symbolic-calculus.hh:137
Difference(const CalculusBase< Difference > &other)
Definition: symbolic-calculus.hh:397
pinocchio::size_type size_type
Definition: fwd.hh:47
RotationMultiply(const typename Traits< pinocchio::Joint >::Ptr_t &joint, const typename Traits< RhsValue >::Ptr_t &rhs, bool transpose=false)
Definition: symbolic-calculus.hh:532
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:538
JointTranspose Ptr_t
Definition: symbolic-calculus.hh:147
CalculusBase< Difference< LhsValue, RhsValue > > Parent_t
Definition: symbolic-calculus.hh:389
weak_ptr< Expression< LhsValue, RhsValue > > WkPtr_t
Definition: symbolic-calculus.hh:255
Expression< value_type, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:501
VectorInJoint(const CalculusBase< VectorInJoint > &other)
Definition: symbolic-calculus.hh:662
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:316
Definition: symbolic-calculus.hh:190
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:844
pinocchio::value_type value_type
Definition: fwd.hh:48
Expression(const Expression &other)
Definition: symbolic-calculus.hh:276
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:546
Traits< RhsValue >::Ptr_t rhs_
Definition: symbolic-calculus.hh:284
JointPtr_t joint_
Definition: symbolic-calculus.hh:857
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:778
HPP_CONSTRAINTS_CB_CREATE2(CrossProduct, const typename Traits< LhsValue >::Ptr_t &, const typename Traits< RhsValue >::Ptr_t &) CrossProduct()
Definition: symbolic-calculus.hh:298
eigen::matrix3_t CrossMatrix
Definition: symbolic-calculus.hh:118
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:839
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:491
JointFrame(const JointPtr_t &joint)
Definition: symbolic-calculus.hh:831
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:930
JointTranspose(const JointPtr_t &joint)
Definition: symbolic-calculus.hh:142
RotationMultiply(const CalculusBase< RotationMultiply > &other)
Definition: symbolic-calculus.hh:514
void init(const typename Traits< T >::Ptr_t &ptr)
Definition: symbolic-calculus.hh:232
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
ScalarProduct(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:356
Basic expression representing a vector in a joint frame.
Definition: symbolic-calculus.hh:654
const JointPtr_t & joint() const
Definition: symbolic-calculus.hh:618
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:360
MatrixOfExpressions(const MatrixOfExpressions &matrix)
Definition: symbolic-calculus.hh:906
CrossType cross_
Definition: symbolic-calculus.hh:228
JointFrame(const JointFrame &jf)
Definition: symbolic-calculus.hh:829
JointTranspose WkPtr_t
Definition: symbolic-calculus.hh:148
JointPtr_t WkPtr_t
Definition: symbolic-calculus.hh:138
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_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:445
const RhsValue & rhs() const
Definition: symbolic-calculus.hh:272
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:450
CrossProduct(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:307
pinocchio::CenterOfMassComputationPtr_t CenterOfMassComputationPtr_t
Definition: fwd.hh:110
pinocchio::Transform3f Transform3f
Definition: fwd.hh:63
const ValueType & value() const
Definition: symbolic-calculus.hh:200
CalculusBase< ScalarProduct< LhsValue, RhsValue >, Eigen::Matrix< value_type, 1, 1 >, RowJacobianMatrix > Parent_t
Definition: symbolic-calculus.hh:344
HPP_CONSTRAINTS_CB_WKREF< Class > WkPtr_t
Definition: symbolic-calculus.hh:128
CalculusBase(const CalculusBase &o)
Definition: symbolic-calculus.hh:197
PointInJoint(const PointInJoint &pointInJoint)
Definition: symbolic-calculus.hh:592
ScalarProduct(const CalculusBase< ScalarProduct > &other)
Definition: symbolic-calculus.hh:352