17 #ifndef HPP_CONSTRAINTS_SYMBOLIC_CALCULUS_HH
18 #define HPP_CONSTRAINTS_SYMBOLIC_CALCULUS_HH
20 #define HPP_CONSTRAINTS_CB_REF shared_ptr
21 #define HPP_CONSTRAINTS_CB_WKREF shared_ptr
23 #define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(op, InType, OutType) \
24 template < typename RhsType > \
25 typename Traits <OutType < RhsType > >::Ptr_t op ( \
27 const HPP_CONSTRAINTS_CB_REF <RhsType>& rhs) { \
28 typedef OutType < RhsType> Op_t; \
29 return Op_t::create (lhs, rhs); \
31 #define HPP_CONSTRAINTS_CB_FRIEND_OPERATOR1(op, InType, OutType) \
32 template < typename RhsType > \
33 friend typename Traits <OutType < RhsType > >::Ptr_t op ( \
35 const HPP_CONSTRAINTS_CB_REF <RhsType>& rhs);
37 #define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(op, OutType) \
38 template < typename LhsType, typename RhsType > \
39 typename Traits <OutType < LhsType, RhsType > >::Ptr_t op ( \
40 const HPP_CONSTRAINTS_CB_REF <LhsType>& lhs, \
41 const HPP_CONSTRAINTS_CB_REF <RhsType>& rhs) { \
42 typedef OutType < LhsType, RhsType> Op_t; \
43 return Op_t::create (lhs, rhs); \
46 #define HPP_CONSTRAINTS_CB_FRIEND_OPERATOR2(op, OutType) \
47 template < typename LhsType, typename RhsType > \
48 friend typename Traits <OutType < LhsType, RhsType > >::Ptr_t op ( \
49 const HPP_CONSTRAINTS_CB_REF <LhsType>& lhs, \
50 const HPP_CONSTRAINTS_CB_REF <RhsType>& rhs);
52 #define HPP_CONSTRAINTS_CB_CREATE1(Class, Arg0Type) \
53 static typename Traits <Class>::Ptr_t create (Arg0Type arg0) { \
54 typename Traits <Class>::Ptr_t ptr (new Class (arg0)); \
59 #define HPP_CONSTRAINTS_CB_CREATE2(Class, Arg0Type, Arg1Type) \
60 static typename Traits <Class>::Ptr_t create (Arg0Type arg0, Arg1Type arg1) { \
61 typename Traits <Class>::Ptr_t ptr (new Class (arg0, arg1)); \
66 #define HPP_CONSTRAINTS_CB_CREATE3(Class, Arg0Type, Arg1Type, Arg2Type) \
67 static typename Traits <Class>::Ptr_t create (Arg0Type arg0, Arg1Type arg1, Arg2Type arg2) { \
68 typename Traits <Class>::Ptr_t ptr (new Class (arg0, arg1, arg2)); \
75 #include <hpp/pinocchio/joint.hh>
76 #include <hpp/pinocchio/center-of-mass-computation.hh>
77 #include <hpp/pinocchio/liegroup-element.hh>
85 namespace constraints {
91 template <
typename ValueType,
typename JacobianType>
class CalculusBaseAbstract;
92 template <
typename T>
class Traits;
94 template <
typename LhsValue,
typename RhsValue>
class Expression;
95 template <
typename LhsValue,
typename RhsValue>
class CrossProduct;
96 template <
typename LhsValue,
typename RhsValue>
class ScalarProduct;
97 template <
typename LhsValue,
typename RhsValue>
class Difference;
98 template <
typename LhsValue,
typename RhsValue>
class Sum;
99 template <
typename RhsValue>
class ScalarMultiply;
100 template <
typename RhsValue>
class RotationMultiply;
103 typedef Eigen::Matrix <value_type, 3, Eigen::Dynamic, Eigen::RowMajor>
JacobianMatrix;
105 template <
typename Class>
107 typedef HPP_CONSTRAINTS_CB_REF <Class>
Ptr_t;
108 typedef HPP_CONSTRAINTS_CB_WKREF <Class>
WkPtr_t;
114 template <>
struct Traits<pinocchio::Joint> {
148 virtual const ValueType&
value ()
const = 0;
185 inline const ValueType&
value ()
const {
193 static_cast<T*
>(
this)->impl_value (arg);
198 static_cast<T*
>(
this)->impl_jacobian (arg);
206 inline const CrossType&
cross ()
const {
231 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
244 template <
typename LhsValue,
typename RhsValue>
268 const LhsValue&
lhs ()
const {
272 const RhsValue&
rhs ()
const {
296 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
300 template <
typename LhsValue,
typename RhsValue>
302 public CalculusBase < CrossProduct < LhsValue, RhsValue > >
318 e_ (
Expression < LhsValue, RhsValue >::create (lhs, rhs))
322 e_->lhs_->computeCrossValue (arg);
323 e_->rhs_->computeValue (arg);
324 this->
value_ =
e_->lhs_->cross () *
e_->rhs_->value ();
327 e_->lhs_->computeCrossValue (arg);
328 e_->rhs_->computeCrossValue (arg);
329 e_->lhs_->computeJacobian (arg);
330 e_->rhs_->computeJacobian (arg);
332 -
e_->rhs_->cross () *
e_->lhs_->jacobian ();
336 e_->rhs_->invalidate ();
337 e_->lhs_->invalidate ();
347 template <
typename LhsValue,
typename RhsValue>
349 public CalculusBase < ScalarProduct < LhsValue, RhsValue >, Eigen::Matrix<value_type,1,1>, RowJacobianMatrix >
365 e_ (
Expression < LhsValue, RhsValue >::create (lhs, rhs))
369 e_->lhs_->computeValue (arg);
370 e_->rhs_->computeValue (arg);
371 this->
value_[0] =
e_->lhs_->value ().dot (
e_->rhs_->value ());
374 e_->lhs_->computeValue (arg);
375 e_->rhs_->computeValue (arg);
376 e_->lhs_->computeJacobian (arg);
377 e_->rhs_->computeJacobian (arg);
378 this->
jacobian_ =
e_->lhs_->value ().transpose () *
e_->rhs_->jacobian ()
379 +
e_->rhs_->value ().transpose () *
e_->lhs_->jacobian ();
383 e_->rhs_->invalidate ();
384 e_->lhs_->invalidate ();
394 template <
typename LhsValue,
typename RhsValue>
396 public CalculusBase < Difference < LhsValue, RhsValue > >
412 e_ (
Expression < LhsValue, RhsValue >::create (lhs, rhs))
416 e_->lhs_->computeValue (arg);
417 e_->rhs_->computeValue (arg);
418 this->
value_ =
e_->lhs_->value () -
e_->rhs_->value ();
421 e_->lhs_->computeJacobian (arg);
422 e_->rhs_->computeJacobian (arg);
423 this->
jacobian_ =
e_->lhs_->jacobian () -
e_->rhs_->jacobian ();
427 e_->rhs_->invalidate ();
428 e_->lhs_->invalidate ();
438 template <
typename LhsValue,
typename RhsValue>
452 e_ (static_cast <const
Sum&>(other).
e_)
456 e_ (
Expression < LhsValue, RhsValue >::create (lhs, rhs))
460 e_->lhs_->computeValue (arg);
461 e_->rhs_->computeValue (arg);
462 this->
value_ =
e_->lhs_->value () +
e_->rhs_->value ();
465 e_->lhs_->computeJacobian (arg);
466 e_->rhs_->computeJacobian (arg);
467 this->
jacobian_ =
e_->lhs_->jacobian () +
e_->rhs_->jacobian ();
471 e_->rhs_->invalidate ();
472 e_->lhs_->invalidate ();
482 template <
typename RhsValue>
504 e_->
rhs_->computeValue (arg);
508 e_->
rhs_->computeJacobian (arg);
523 template <
typename RhsValue>
536 transpose_ (other.transpose_)
544 e_ (
Expression < pinocchio::Joint, RhsValue >::create (joint.j_, rhs)),
549 bool transpose =
false):
550 e_ (
Expression < pinocchio::Joint, RhsValue >::create (joint, rhs)),
551 transpose_ (transpose)
555 e_->
rhs_->computeValue (arg);
563 e_->
rhs_->computeJacobian (arg);
564 e_->
rhs_->computeCrossValue (arg);
569 * ((
e_->
rhs_->cross () * R) * J.bottomRows<3>() +
e_->
rhs_->jacobian ());
572 * ((
e_->
rhs_->cross () * R) * J.bottomRows<3>() +
e_->
rhs_->jacobian ());
582 friend class Expression <pinocchio::Joint, RhsValue>;
605 center_= local_.isZero ();
610 joint_ (pointInJoint.joint ()), local_ (pointInJoint.local ()),
611 center_ (local_.isZero ())
616 joint_ (joint), local_ (pointInLocalFrame),
617 center_ (pointInLocalFrame.isZero ())
625 joint_ (joint), local_ (pointInLocalFrame),
626 center_ (pointInLocalFrame.isZero ())
628 if (joint_ == NULL) {
629 for (
int i = 0; i < 3; ++i) this->
value_[i] = local_[i];
644 if (joint_ == NULL)
return;
645 this->
value_ = joint_->currentTransformation ().act (local_);
648 if (joint_ == NULL)
return;
650 const matrix3_t& R = joint_->currentTransformation ().rotation ();
651 this->
jacobian_.noalias() = R * J.topRows<3>();
658 if (joint_ == NULL)
return;
664 joint_->currentTransformation ().rotation () * local_,
674 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
695 joint_ (vectorInJoint.joint ()),
696 vector_ (vectorInJoint.vector())
701 joint_ (joint), vector_ (vectorInLocalFrame)
707 joint_ (joint), vector_ (vectorInLocalFrame)
709 if (joint_ == NULL) {
710 for (
int i = 0; i < 3; ++i) this->
value_[i] = vector_[i];
724 if (joint_ == NULL)
return;
725 this->
value_ = joint_->currentTransformation ().rotation () * vector_;
728 if (joint_ == NULL)
return;
730 const matrix3_t& R = joint_->currentTransformation ().rotation ();
735 if (joint_ == NULL)
return;
737 joint_->currentTransformation ().rotation () * vector_,
746 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
750 template <
typename FunctionType>
762 Parent_t (other), f_ (other.f_), lge_ (other.lge_)
766 Parent_t (other), f_ (other.f_), lge_ (other.lge_)
774 matrix_t::Zero(func->outputDerivativeSize(), func->inputDerivativeSize())),
775 f_ (func), lge_ (func->outputSpace())
780 f_->value (lge_, arg);
781 this->
value_ = lge_.vector();
786 FunctionTypePtr_t f_;
835 comc_ (static_cast <const
PointCom&>(other).centerOfMassComputation ())
846 return comc_->jacobian();
853 comc_->compute (hpp::pinocchio::COM);
856 comc_->compute (hpp::pinocchio::COMPUTE_ALL);
866 class JointFrame :
public CalculusBase <JointFrame, Eigen::Matrix<value_type, 6, 1>, Eigen::Matrix<value_type, 6, Eigen::Dynamic> >
877 joint_ (static_cast <const
JointFrame&>(other).joint_)
888 this->
jacobian_.resize(6,joint->robot()->numberDof()-joint->robot()->extraConfigSpace().dimension());
895 const Transform3f& M = joint_->currentTransformation ();
896 this->
value_.head<3>() = M.translation ();
897 logSO3 (M.rotation(), theta_, this->value_.tail<3>());
902 const matrix3_t& R (joint_->currentTransformation ().rotation ());
907 this->
jacobian_.topRows<3>().noalias() = R * J.topRows<3>();
908 this->
jacobian_.bottomRows<3>().noalias() = Jlog * J.bottomRows<3>();
916 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
920 template <
typename ValueType = eigen::vector3_t,
typename JacobianType = JacobianMatrix>
922 public CalculusBase <MatrixOfExpressions <ValueType, JacobianType > ,
923 Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic >,
924 Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic > >
927 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic >
929 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic >
931 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic >
933 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic >
938 typedef Eigen::JacobiSVD <Value_t>
SVD_t;
945 nRows_ (0), nCols_ (0),
947 piValid_ (false), svdValid_ (false)
963 nRows_ (matrix.nRows_), nCols_ (matrix.nCols_),
964 elements_ (matrix.elements_),
966 piValid_ (matrix.piValid_),
967 svdValid_ (matrix.svdValid_)
971 void setSize (std::size_t nRows, std::size_t nCols) {
974 elements_.resize (nRows_);
975 for (std::size_t i = 0; i < nRows; ++i)
976 elements_[i].resize(nCols);
980 return elements_[i][j];
984 elements_[i][j] = ptr;
989 for (std::size_t i = 0; i < nRows_; ++i) {
991 nr = elements_[i][0]->value().rows();
992 for (std::size_t j = 0; j < nCols_; ++j) {
993 elements_[i][j]->computeValue (arg);
995 nc = elements_[i][j]->value().cols();
996 this->
value_.block (r, c, nr, nc)
997 = elements_[i][j]->value();
1005 for (std::size_t i = 0; i < nRows_; ++i) {
1007 nr = elements_[i][0]->jacobian().rows();
1008 for (std::size_t j = 0; j < nCols_; ++j) {
1009 elements_[i][j]->computeJacobian (arg);
1011 nc = elements_[i][j]->jacobian().cols();
1013 = elements_[i][j]->jacobian();
1027 if (svdValid_)
return;
1029 svd_.compute (this->
value_);
1034 if (piValid_)
return;
1036 this->computeSVD(arg);
1037 pi_.resize (this->
value_.cols(), this->value_.rows());
1038 pseudoInverse <SVD_t> (svd_, pi_);
1043 computePseudoInverse (arg);
1044 const std::size_t nbDof = elements_[0][0]->jacobian().cols();
1045 const std::size_t inSize = this->
value_.cols();
1046 const vector_t piTrhs = svd_.solve (rhs);
1050 jacobianTimes (arg, piTrhs, cache);
1051 pij_.noalias() = - pi_ * cache;
1052 cache.resize (inSize, nbDof);
1054 pkInv_.resize (pi_.cols(), pi_.cols());
1055 projectorOnKernelOfInv <SVD_t> (svd_, pkInv_,
true);
1056 jacobianTransposeTimes (arg, pkInv_ * rhs, cache);
1057 pij_.noalias() += (pi_ * pi_.transpose()) * cache;
1059 jacobianTransposeTimes (arg, pi_.transpose() * piTrhs , cache);
1060 pk_.resize (inSize, inSize);
1061 projectorOnKernel <SVD_t> (svd_, pk_,
true);
1062 pij_.noalias() += pk_ * cache;
1068 for (std::size_t i = 0; i < nRows_; ++i) {
1070 nr = elements_[i][0]->jacobian().rows();
1071 for (std::size_t j = 0; j < nCols_; ++j) {
1072 elements_[i][j]->computeJacobian (arg);
1074 nc = elements_[i][j]->jacobian().cols();
1075 cache.middleRows (r,nr).noalias() +=
1076 this->
jacobian_.block (r, c, nr, nc) * rhs[j];
1086 for (std::size_t i = 0; i < nRows_; ++i) {
1088 nr = elements_[i][0]->jacobian().rows();
1089 for (std::size_t j = 0; j < nCols_; ++j) {
1090 elements_[i][j]->computeJacobian (arg);
1092 nc = elements_[i][j]->jacobian().cols();
1093 cache.row (j) += rhs.segment (r, nr).transpose() * this->
jacobian_.block (r, c, nr, nc);
1100 Eigen::JacobiSVD <Value_t>&
svd () {
return svd_; }
1104 for (std::size_t i = 0; i < nRows_; ++i)
1105 for (std::size_t j = 0; j < nCols_; ++j)
1119 bool piValid_, svdValid_;
1122 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: symbolic-calculus.hh:143
virtual void computeValue(const ConfigurationIn_t arg)=0
virtual void computeJacobian(const ConfigurationIn_t arg)=0
virtual const JacobianType & jacobian() const =0
virtual const ValueType & value() const =0
virtual void invalidate()=0
ValueType ValueType_t
Definition: symbolic-calculus.hh:145
JacobianType JacobianType_t
Definition: symbolic-calculus.hh:146
Definition: symbolic-calculus.hh:171
ValueType value_
Definition: symbolic-calculus.hh:217
bool cValid_
Definition: symbolic-calculus.hh:221
bool vValid_
Definition: symbolic-calculus.hh:221
void computeValue(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:191
void computeCrossValue(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:209
bool jValid_
Definition: symbolic-calculus.hh:221
CalculusBase(const CalculusBase &o)
Definition: symbolic-calculus.hh:179
void invalidate()
Definition: symbolic-calculus.hh:201
JacobianType jacobian_
Definition: symbolic-calculus.hh:218
CalculusBase(const ValueType &value, const JacobianType &jacobian)
Definition: symbolic-calculus.hh:175
void init(const typename Traits< T >::Ptr_t &ptr)
Definition: symbolic-calculus.hh:223
void computeJacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:196
CalculusBase()
Definition: symbolic-calculus.hh:173
CrossType cross_
Definition: symbolic-calculus.hh:219
const JacobianType & jacobian() const
Definition: symbolic-calculus.hh:188
const CrossType & cross() const
Definition: symbolic-calculus.hh:206
const ValueType & value() const
Definition: symbolic-calculus.hh:185
Cross product of two expressions.
Definition: symbolic-calculus.hh:303
CalculusBase< CrossProduct< LhsValue, RhsValue > > Parent_t
Definition: symbolic-calculus.hh:306
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:321
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:341
CrossProduct(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:317
CrossProduct(const CalculusBase< CrossProduct > &other)
Definition: symbolic-calculus.hh:312
void invalidate()
Definition: symbolic-calculus.hh:334
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:326
Difference of two expressions.
Definition: symbolic-calculus.hh:397
Difference(const CalculusBase< Difference > &other)
Definition: symbolic-calculus.hh:406
CalculusBase< Difference< LhsValue, RhsValue > > Parent_t
Definition: symbolic-calculus.hh:400
void invalidate()
Definition: symbolic-calculus.hh:425
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:420
Difference(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:411
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:415
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:432
Base class for classes representing an operation.
Definition: symbolic-calculus.hh:246
weak_ptr< Expression< LhsValue, RhsValue > > WkPtr_t
Definition: symbolic-calculus.hh:253
Traits< Expression >::WkPtr_t self_
Definition: symbolic-calculus.hh:293
Traits< LhsValue >::Ptr_t lhs_
Definition: symbolic-calculus.hh:292
static Ptr_t create(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:261
Expression(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:282
Expression()
Definition: symbolic-calculus.hh:276
const RhsValue & rhs() const
Definition: symbolic-calculus.hh:272
shared_ptr< Expression< LhsValue, RhsValue > > Ptr_t
Definition: symbolic-calculus.hh:250
void init(Ptr_t self)
Definition: symbolic-calculus.hh:287
const LhsValue & lhs() const
Definition: symbolic-calculus.hh:268
Expression(const Expression &other)
Definition: symbolic-calculus.hh:278
static Ptr_t create()
Definition: symbolic-calculus.hh:255
Traits< RhsValue >::Ptr_t rhs_
Definition: symbolic-calculus.hh:291
Basic expression mapping a function as an expression.
Definition: symbolic-calculus.hh:752
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:783
FunctionExp(const Parent_t &other)
Definition: symbolic-calculus.hh:761
CalculusBase< FunctionExp< FunctionType >, vector_t, matrix_t > Parent_t
Definition: symbolic-calculus.hh:754
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:778
FunctionExp(const FunctionExp &other)
Definition: symbolic-calculus.hh:765
shared_ptr< FunctionType > FunctionTypePtr_t
Definition: symbolic-calculus.hh:755
FunctionExp(const FunctionTypePtr_t &func)
Definition: symbolic-calculus.hh:772
Definition: symbolic-calculus.hh:867
double theta_
Definition: symbolic-calculus.hh:913
JointFrame(const Parent_t &other)
Definition: symbolic-calculus.hh:875
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:899
JointPtr_t joint_
Definition: symbolic-calculus.hh:912
JointFrame(const JointFrame &jf)
Definition: symbolic-calculus.hh:880
CalculusBase< JointFrame, ValueType_t, JacobianType_t > Parent_t
Definition: symbolic-calculus.hh:869
const JointPtr_t & joint() const
Definition: symbolic-calculus.hh:891
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:894
JointFrame(const JointPtr_t &joint)
Definition: symbolic-calculus.hh:884
Matrix having Expression elements.
Definition: symbolic-calculus.hh:925
void setSize(std::size_t nRows, std::size_t nCols)
Definition: symbolic-calculus.hh:971
const PseudoInv_t & pinv() const
Definition: symbolic-calculus.hh:1020
Eigen::JacobiSVD< Value_t > & svd()
Definition: symbolic-calculus.hh:1100
CalculusBase< MatrixOfExpressions, Value_t, Jacobian_t > Parent_t
Definition: symbolic-calculus.hh:935
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > Jacobian_t
Definition: symbolic-calculus.hh:930
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:1083
std::size_t nCols_
Definition: symbolic-calculus.hh:1111
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > PseudoInv_t
Definition: symbolic-calculus.hh:932
const PseudoInvJacobian_t & pinvJacobian() const
Definition: symbolic-calculus.hh:1023
std::vector< std::vector< ElementPtr_t > > elements_
Definition: symbolic-calculus.hh:1112
Traits< Element_t >::Ptr_t ElementPtr_t
Definition: symbolic-calculus.hh:937
MatrixOfExpressions(const MatrixOfExpressions &matrix)
Definition: symbolic-calculus.hh:961
void computePseudoInverseJacobian(const ConfigurationIn_t arg, const Eigen::Ref< const Eigen::Matrix< value_type, Eigen::Dynamic, 1 > > &rhs)
Definition: symbolic-calculus.hh:1041
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > PseudoInvJacobian_t
Definition: symbolic-calculus.hh:934
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:1065
void computePseudoInverse(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:1033
void invalidate()
Definition: symbolic-calculus.hh:1102
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > Value_t
Definition: symbolic-calculus.hh:928
MatrixOfExpressions(const Parent_t &other)
Definition: symbolic-calculus.hh:950
Eigen::JacobiSVD< Value_t > SVD_t
Definition: symbolic-calculus.hh:938
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:1003
void set(std::size_t i, std::size_t j, const ElementPtr_t ptr)
Definition: symbolic-calculus.hh:983
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:987
CalculusBaseAbstract< ValueType, JacobianType > Element_t
Definition: symbolic-calculus.hh:936
void computeSVD(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:1026
Basic expression representing a COM.
Definition: symbolic-calculus.hh:826
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:852
const CenterOfMassComputationPtr_t & centerOfMassComputation() const
Definition: symbolic-calculus.hh:849
PointCom(const CenterOfMassComputationPtr_t &comc)
Definition: symbolic-calculus.hh:839
PointCom(const Parent_t &other)
Definition: symbolic-calculus.hh:833
CalculusBase< PointCom, vector3_t, ComJacobian_t > Parent_t
Definition: symbolic-calculus.hh:828
CenterOfMassComputationPtr_t comc_
Definition: symbolic-calculus.hh:863
void impl_jacobian(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:855
const ComJacobian_t & jacobian() const
Definition: symbolic-calculus.hh:845
const vector3_t & value() const
Definition: symbolic-calculus.hh:842
Basic expression representing a point in a joint frame.
Definition: symbolic-calculus.hh:590
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:643
JointPtr_t joint_
Definition: symbolic-calculus.hh:669
vector3_t local_
Definition: symbolic-calculus.hh:670
PointInJoint(const PointInJoint &pointInJoint)
Definition: symbolic-calculus.hh:608
PointInJoint(const JointPtr_t &joint, const vector3_t &pointInLocalFrame, size_type nbDof)
Definition: symbolic-calculus.hh:622
bool center_
Definition: symbolic-calculus.hh:671
PointInJoint(const CalculusBase< PointInJoint > &other)
Definition: symbolic-calculus.hh:599
PointInJoint(const JointPtr_t &joint, const vector3_t &pointInLocalFrame)
Definition: symbolic-calculus.hh:614
const JointPtr_t & joint() const
Definition: symbolic-calculus.hh:637
void computeCrossRXl()
Definition: symbolic-calculus.hh:657
void impl_jacobian(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:647
CalculusBase< PointInJoint > Parent_t
Definition: symbolic-calculus.hh:592
Definition: symbolic-calculus.hh:794
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:820
Point(const vector3_t &point, size_t jacobianNbCols)
Definition: symbolic-calculus.hh:814
Point(const CalculusBase< Point, vector3_t, JacobianMatrix > &other)
Definition: symbolic-calculus.hh:800
void impl_jacobian(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:821
Point(const Point &point)
Definition: symbolic-calculus.hh:805
Multiplication of an expression by a rotation matrix.
Definition: symbolic-calculus.hh:526
CalculusBase< RotationMultiply< RhsValue > > Parent_t
Definition: symbolic-calculus.hh:529
Expression< pinocchio::Joint, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:580
RotationMultiply(const CalculusBase< RotationMultiply > &other)
Definition: symbolic-calculus.hh:533
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:554
void invalidate()
Definition: symbolic-calculus.hh:574
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:562
RotationMultiply(const typename Traits< pinocchio::Joint >::Ptr_t &joint, const typename Traits< RhsValue >::Ptr_t &rhs, bool transpose=false)
Definition: symbolic-calculus.hh:548
RotationMultiply()
Definition: symbolic-calculus.hh:531
Multiplication of an expression by a scalar.
Definition: symbolic-calculus.hh:485
CalculusBase< ScalarMultiply< RhsValue > > Parent_t
Definition: symbolic-calculus.hh:488
ScalarMultiply(const typename Traits< value_type >::Ptr_t &scalar, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:499
ScalarMultiply(const CalculusBase< ScalarMultiply > &other)
Definition: symbolic-calculus.hh:494
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:503
void invalidate()
Definition: symbolic-calculus.hh:511
Expression< value_type, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:517
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:507
Scalar product of two expressions.
Definition: symbolic-calculus.hh:350
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:388
void invalidate()
Definition: symbolic-calculus.hh:381
CalculusBase< ScalarProduct< LhsValue, RhsValue >, Eigen::Matrix< value_type, 1, 1 >, RowJacobianMatrix > Parent_t
Definition: symbolic-calculus.hh:353
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:373
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:368
ScalarProduct(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:364
ScalarProduct(const CalculusBase< ScalarProduct > &other)
Definition: symbolic-calculus.hh:359
Sum of two expressions.
Definition: symbolic-calculus.hh:441
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:476
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:464
Sum(const typename Traits< RhsValue >::Ptr_t &rhs, const typename Traits< LhsValue >::Ptr_t &lhs)
Definition: symbolic-calculus.hh:455
void invalidate()
Definition: symbolic-calculus.hh:469
CalculusBase< Sum< LhsValue, RhsValue > > Parent_t
Definition: symbolic-calculus.hh:444
Sum(const CalculusBase< Sum > &other)
Definition: symbolic-calculus.hh:450
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:459
Definition: symbolic-calculus.hh:106
HPP_CONSTRAINTS_CB_REF< Class > Ptr_t
Definition: symbolic-calculus.hh:107
HPP_CONSTRAINTS_CB_WKREF< Class > WkPtr_t
Definition: symbolic-calculus.hh:108
Basic expression representing a vector in a joint frame.
Definition: symbolic-calculus.hh:679
VectorInJoint(const VectorInJoint &vectorInJoint)
Definition: symbolic-calculus.hh:693
const vector3_t & vector() const
Definition: symbolic-calculus.hh:720
VectorInJoint(const CalculusBase< VectorInJoint > &other)
Definition: symbolic-calculus.hh:687
JointPtr_t joint_
Definition: symbolic-calculus.hh:742
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:723
void impl_jacobian(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:727
vector3_t vector_
Definition: symbolic-calculus.hh:743
VectorInJoint(const JointPtr_t &joint, const vector3_t &vectorInLocalFrame, const size_type &nbDof)
Definition: symbolic-calculus.hh:704
VectorInJoint(const JointPtr_t &joint, const vector3_t &vectorInLocalFrame)
Definition: symbolic-calculus.hh:699
void computeCrossRXl()
Definition: symbolic-calculus.hh:734
const JointPtr_t & joint() const
Definition: symbolic-calculus.hh:717
Eigen::Matrix< value_type, 3, Eigen::Dynamic, Eigen::RowMajor > JacobianMatrix
Definition: symbolic-calculus.hh:103
eigen::matrix3_t CrossMatrix
Definition: symbolic-calculus.hh:100
Eigen::Matrix< value_type, 1, Eigen::Dynamic, Eigen::RowMajor > RowJacobianMatrix
Definition: symbolic-calculus.hh:102
#define HPP_DEBUG_SVDCHECK(svd)
Definition: macros.hh:46
assert(d.lhs()._blocks()==d.rhs()._blocks())
Eigen::Matrix< value_type, 3, 1 > vector3_t
Definition: fwd.hh:61
Eigen::Matrix< value_type, 3, 3 > matrix3_t
Definition: fwd.hh:60
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
pinocchio::matrix3_t matrix3_t
Definition: fwd.hh:41
pinocchio::vector3_t vector3_t
Definition: fwd.hh:40
pinocchio::JointJacobian_t JointJacobian_t
Definition: fwd.hh:51
pinocchio::Transform3f Transform3f
Definition: fwd.hh:52
pinocchio::size_type size_type
Definition: fwd.hh:36
pinocchio::ComJacobian_t ComJacobian_t
Definition: fwd.hh:50
pinocchio::value_type value_type
Definition: fwd.hh:37
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
pinocchio::CenterOfMassComputationPtr_t CenterOfMassComputationPtr_t
Definition: fwd.hh:99
pinocchio::vector_t vector_t
Definition: fwd.hh:47
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:38
Definition: active-set-differentiable-function.hh:24
Definition: symbolic-calculus.hh:119
const JointPtr_t j_
Definition: symbolic-calculus.hh:122
JointTranspose(const JointPtr_t &joint)
Definition: symbolic-calculus.hh:120
JointTranspose WkPtr_t
Definition: symbolic-calculus.hh:126
JointTranspose Ptr_t
Definition: symbolic-calculus.hh:125
JointPtr_t WkPtr_t
Definition: symbolic-calculus.hh:116
JointPtr_t Ptr_t
Definition: symbolic-calculus.hh:115
value_type WkPtr_t
Definition: symbolic-calculus.hh:112
value_type Ptr_t
Definition: symbolic-calculus.hh:111
#define HPP_CONSTRAINTS_CB_CREATE1(Class, Arg0Type)
Definition: symbolic-calculus.hh:52
#define HPP_CONSTRAINTS_CB_CREATE2(Class, Arg0Type, Arg1Type)
Definition: symbolic-calculus.hh:59
#define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(op, InType, OutType)
Definition: symbolic-calculus.hh:23
#define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(op, OutType)
Definition: symbolic-calculus.hh:37
#define HPP_CONSTRAINTS_CB_CREATE3(Class, Arg0Type, Arg1Type, Arg2Type)
Definition: symbolic-calculus.hh:66