hpp-constraints
4.10.1
Definition of basic geometric constraints for motion planning
|
Go to the documentation of this file.
17 #ifndef HPP_CONSTRAINTS_SYMBOLIC_CALCULUS_HH
18 #define HPP_CONSTRAINTS_SYMBOLIC_CALCULUS_HH
20 #define HPP_CONSTRAINTS_CB_REF boost::shared_ptr
21 #define HPP_CONSTRAINTS_CB_WKREF boost::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 {
94 template <
typename LhsValue,
typename RhsValue>
class Expression;
95 template <
typename LhsValue,
typename RhsValue>
class CrossProduct;
97 template <
typename LhsValue,
typename RhsValue>
class Difference;
98 template <
typename LhsValue,
typename RhsValue>
class Sum;
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;
149 virtual const JacobianType&
jacobian ()
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>
248 typedef boost::shared_ptr <
249 Expression < LhsValue, RhsValue >
251 typedef boost::weak_ptr <
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);
331 this->jacobian_ =
e_->lhs_->cross () *
e_->rhs_->jacobian ()
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);
558 this->value_ = R.transpose() *
e_->
rhs_->value ();
560 this->value_ = R *
e_->
rhs_->value ();
563 e_->
rhs_->computeJacobian (arg);
564 e_->
rhs_->computeCrossValue (arg);
568 this->jacobian_ = R.transpose()
569 * ((
e_->
rhs_->cross () * R) * J.bottomRows<3>() +
e_->
rhs_->jacobian ());
572 * ((
e_->
rhs_->cross () * R) * J.bottomRows<3>() +
e_->
rhs_->jacobian ());
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];
630 this->jacobian_.resize (3, nbDof);
631 this->jacobian_.setZero ();
632 this->cross_.setZero ();
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>();
654 this->jacobian_.noalias() -= (this->cross_ * R) * J.bottomRows<3>();
658 if (joint_ == NULL)
return;
660 this->cross_.setZero ();
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];
711 this->jacobian_.resize (3, nbDof);
712 this->jacobian_.setZero ();
713 this->cross_.setZero ();
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 ();
732 this->jacobian_.noalias() = (- this->cross_ * R ) * J.bottomRows<3>();
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 ());
906 JlogSO3 (theta_, this->value_.tail<3>(), Jlog);
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),
946 svd_ (value.rows(), value.cols(),
Eigen::ComputeFullU |
Eigen::ComputeFullV),
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);
994 assert (nr == elements_[i][j]->value().rows());
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);
1010 assert (nr == elements_[i][j]->jacobian().rows());
1011 nc = elements_[i][j]->jacobian().cols();
1012 this->jacobian_.block (r, c, nr, nc)
1013 = elements_[i][j]->jacobian();
1027 if (svdValid_)
return;
1028 this->computeValue (arg);
1029 svd_.compute (this->value_);
1034 if (piValid_)
return;
1035 this->computeValue (arg);
1036 this->computeSVD(arg);
1037 pi_.resize (this->value_.cols(), this->value_.rows());
1038 pseudoInverse <SVD_t> (svd_, pi_);
1042 this->computeJacobian (arg);
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);
1049 Jacobian_t cache (this->jacobian_.rows(), nbDof);
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);
1073 assert (nr == elements_[i][j]->jacobian().rows());
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);
1091 assert (nr == elements_[i][j]->jacobian().rows());
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
1129 #endif // HPP_CONSTRAINTS_SYMBOLIC_CALCULUS_HH
#define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR1(op, InType, OutType)
Definition: symbolic-calculus.hh:23
static Ptr_t create()
Definition: symbolic-calculus.hh:255
PointCom(const Parent_t &other)
Definition: symbolic-calculus.hh:833
void computeJacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:196
#define HPP_CONSTRAINTS_CB_DEFINE_OPERATOR2(op, OutType)
Definition: symbolic-calculus.hh:37
static Ptr_t create(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:261
pinocchio::CenterOfMassComputationPtr_t CenterOfMassComputationPtr_t
Definition: fwd.hh:99
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > PseudoInv_t
Definition: symbolic-calculus.hh:932
MatrixOfExpressions(const Parent_t &other)
Definition: symbolic-calculus.hh:950
const JacobianType & jacobian() const
Definition: symbolic-calculus.hh:188
Cross product of two expressions.
Definition: symbolic-calculus.hh:95
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
value_type Ptr_t
Definition: symbolic-calculus.hh:111
const CrossType & cross() const
Definition: symbolic-calculus.hh:206
JointFrame(const Parent_t &other)
Definition: symbolic-calculus.hh:875
pinocchio::vector_t vector_t
Definition: fwd.hh:47
void init(Ptr_t self)
Definition: symbolic-calculus.hh:287
void impl_jacobian(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:821
const vector3_t & vector() const
Definition: symbolic-calculus.hh:720
const JointPtr_t j_
Definition: symbolic-calculus.hh:122
#define HPP_DEBUG_SVDCHECK(svd)
Definition: macros.hh:46
CalculusBase< ScalarProduct< LhsValue, RhsValue >, Eigen::Matrix< value_type, 1, 1 >, RowJacobianMatrix > Parent_t
Definition: symbolic-calculus.hh:353
Multiplication of an expression by a scalar.
Definition: symbolic-calculus.hh:99
Basic expression representing a point in a joint frame.
Definition: symbolic-calculus.hh:589
Multiplication of an expression by a rotation matrix.
Definition: symbolic-calculus.hh:100
Eigen::JacobiSVD< Value_t > SVD_t
Definition: symbolic-calculus.hh:938
JointFrame(const JointPtr_t &joint)
Definition: symbolic-calculus.hh:884
virtual void invalidate()=0
ScalarMultiply(const typename Traits< value_type >::Ptr_t &scalar, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:499
void invalidate()
Definition: symbolic-calculus.hh:334
Definition: symbolic-calculus.hh:119
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:415
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:432
const JointPtr_t & joint() const
Definition: symbolic-calculus.hh:637
eigen::matrix3_t CrossMatrix
Definition: symbolic-calculus.hh:100
Basic expression representing a COM.
Definition: symbolic-calculus.hh:825
FunctionExp(const Parent_t &other)
Definition: symbolic-calculus.hh:761
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:899
virtual const JacobianType & jacobian() const =0
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:341
void invalidate()
Definition: symbolic-calculus.hh:425
Base class for classes representing an operation.
Definition: symbolic-calculus.hh:94
Basic expression mapping a function as an expression.
Definition: symbolic-calculus.hh:751
pinocchio::matrix3_t matrix3_t
Definition: fwd.hh:41
JointPtr_t Ptr_t
Definition: symbolic-calculus.hh:115
void init(const typename Traits< T >::Ptr_t &ptr)
Definition: symbolic-calculus.hh:223
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:852
void computeCrossValue(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:209
JointPtr_t joint_
Definition: symbolic-calculus.hh:742
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:373
pinocchio::ComJacobian_t ComJacobian_t
Definition: fwd.hh:50
const vector3_t &const void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:643
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > PseudoInvJacobian_t
Definition: symbolic-calculus.hh:934
std::vector< std::vector< ElementPtr_t > > elements_
Definition: symbolic-calculus.hh:1112
Expression< value_type, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:517
JointTranspose(const JointPtr_t &joint)
Definition: symbolic-calculus.hh:120
Scalar product of two expressions.
Definition: symbolic-calculus.hh:96
Eigen::Matrix< value_type, 1, Eigen::Dynamic, Eigen::RowMajor > RowJacobianMatrix
Definition: symbolic-calculus.hh:102
virtual const ValueType & value() const =0
CalculusBase< PointInJoint > Parent_t
Definition: symbolic-calculus.hh:592
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
Sum of two expressions.
Definition: symbolic-calculus.hh:98
Point(const Point &point)
Definition: symbolic-calculus.hh:805
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:723
VectorInJoint(const JointPtr_t &joint, const vector3_t &vectorInLocalFrame)
Definition: symbolic-calculus.hh:699
PointInJoint(const CalculusBase< PointInJoint > &other)
Definition: symbolic-calculus.hh:599
const CenterOfMassComputationPtr_t & centerOfMassComputation() const
Definition: symbolic-calculus.hh:849
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:326
const LhsValue & lhs() const
Definition: symbolic-calculus.hh:268
Definition: symbolic-calculus.hh:170
Definition: symbolic-calculus.hh:91
pinocchio::vector3_t vector3_t
Definition: fwd.hh:40
Difference(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:411
assert(d.lhs()._blocks()==d.rhs()._blocks())
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:1003
CalculusBase< FunctionExp< FunctionType >, vector_t, matrix_t > Parent_t
Definition: symbolic-calculus.hh:754
VectorInJoint(const VectorInJoint &vectorInJoint)
Definition: symbolic-calculus.hh:693
JacobianType jacobian_
Definition: symbolic-calculus.hh:218
void impl_jacobian(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:727
void set(std::size_t i, std::size_t j, const ElementPtr_t ptr)
Definition: symbolic-calculus.hh:983
bool jValid_
Definition: symbolic-calculus.hh:221
boost::shared_ptr< Expression< LhsValue, RhsValue > > Ptr_t
Definition: symbolic-calculus.hh:250
JointPtr_t WkPtr_t
Definition: symbolic-calculus.hh:116
CalculusBase< CrossProduct< LhsValue, RhsValue > > Parent_t
Definition: symbolic-calculus.hh:306
CalculusBase(const CalculusBase &o)
Definition: symbolic-calculus.hh:179
const PseudoInvJacobian_t & pinvJacobian() const
Definition: symbolic-calculus.hh:1023
const RhsValue & rhs() const
Definition: symbolic-calculus.hh:272
Definition: symbolic-calculus.hh:793
ScalarMultiply(const CalculusBase< ScalarMultiply > &other)
Definition: symbolic-calculus.hh:494
vector3_t local_
Definition: symbolic-calculus.hh:670
FunctionExp(const FunctionTypePtr_t &func)
Definition: symbolic-calculus.hh:772
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:778
Point(const vector3_t &point, size_t jacobianNbCols)
Definition: symbolic-calculus.hh:814
virtual void computeValue(const ConfigurationIn_t arg)=0
JointPtr_t joint_
Definition: symbolic-calculus.hh:912
vector3_t vector_
Definition: symbolic-calculus.hh:743
Traits< RhsValue >::Ptr_t rhs_
Definition: symbolic-calculus.hh:291
Difference of two expressions.
Definition: symbolic-calculus.hh:97
VectorInJoint(const JointPtr_t &joint, const vector3_t &vectorInLocalFrame, const size_type &nbDof)
Definition: symbolic-calculus.hh:704
CalculusBase< JointFrame, ValueType_t, JacobianType_t > Parent_t
Definition: symbolic-calculus.hh:869
CrossProduct(const CalculusBase< CrossProduct > &other)
Definition: symbolic-calculus.hh:312
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
const vector3_t & value() const
Definition: symbolic-calculus.hh:842
void computeCrossRXl()
Definition: symbolic-calculus.hh:657
void invalidate()
Definition: symbolic-calculus.hh:511
const ValueType & value() const
Definition: symbolic-calculus.hh:185
CalculusBase< ScalarMultiply< RhsValue > > Parent_t
Definition: symbolic-calculus.hh:488
Definition: active-set-differentiable-function.hh:24
void computeCrossRXl()
Definition: symbolic-calculus.hh:734
CalculusBase< Difference< LhsValue, RhsValue > > Parent_t
Definition: symbolic-calculus.hh:400
void invalidate()
Definition: symbolic-calculus.hh:469
Traits< LhsValue >::Ptr_t lhs_
Definition: symbolic-calculus.hh:292
void computePseudoInverse(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:1033
bool center_
Definition: symbolic-calculus.hh:671
CrossType cross_
Definition: symbolic-calculus.hh:219
Traits< Element_t >::Ptr_t ElementPtr_t
Definition: symbolic-calculus.hh:937
PointInJoint(const JointPtr_t &joint, const vector3_t &pointInLocalFrame, size_type nbDof)
Definition: symbolic-calculus.hh:622
const JointPtr_t & joint() const
Definition: symbolic-calculus.hh:891
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:503
void computeValue(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:191
void invalidate()
Definition: symbolic-calculus.hh:201
Expression(const Expression &other)
Definition: symbolic-calculus.hh:278
Definition: symbolic-calculus.hh:92
RotationMultiply()
Definition: symbolic-calculus.hh:531
pinocchio::size_type size_type
Definition: fwd.hh:36
const PseudoInv_t & pinv() const
Definition: symbolic-calculus.hh:1020
virtual void computeJacobian(const ConfigurationIn_t arg)=0
ValueType value_
Definition: symbolic-calculus.hh:217
JointPtr_t joint_
Definition: symbolic-calculus.hh:669
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:894
Expression< pinocchio::Joint, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:580
bool vValid_
Definition: symbolic-calculus.hh:221
void invalidate()
Definition: symbolic-calculus.hh:1102
PointInJoint(const PointInJoint &pointInJoint)
Definition: symbolic-calculus.hh:608
std::size_t nRows_
Definition: symbolic-calculus.hh:1111
JointTranspose Ptr_t
Definition: symbolic-calculus.hh:125
CrossProduct(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:317
boost::weak_ptr< Expression< LhsValue, RhsValue > > WkPtr_t
Definition: symbolic-calculus.hh:253
CalculusBase(const ValueType &value, const JacobianType &jacobian)
Definition: symbolic-calculus.hh:175
CalculusBase< RotationMultiply< RhsValue > > Parent_t
Definition: symbolic-calculus.hh:529
CalculusBase< Sum< LhsValue, RhsValue > > Parent_t
Definition: symbolic-calculus.hh:444
void invalidate()
Definition: symbolic-calculus.hh:381
double theta_
Definition: symbolic-calculus.hh:913
Matrix having Expression elements.
Definition: symbolic-calculus.hh:921
CalculusBase()
Definition: symbolic-calculus.hh:173
Point(const CalculusBase< Point, vector3_t, JacobianMatrix > &other)
Definition: symbolic-calculus.hh:800
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:464
pinocchio::value_type value_type
Definition: fwd.hh:37
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:783
#define HPP_CONSTRAINTS_CB_CREATE2(Class, Arg0Type, Arg1Type)
Definition: symbolic-calculus.hh:59
JointTranspose WkPtr_t
Definition: symbolic-calculus.hh:126
void computeSVD(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:1026
FunctionExp(const FunctionExp &other)
Definition: symbolic-calculus.hh:765
void invalidate()
Definition: symbolic-calculus.hh:574
void setSize(std::size_t nRows, std::size_t nCols)
Definition: symbolic-calculus.hh:971
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:554
HPP_CONSTRAINTS_CB_WKREF< Class > WkPtr_t
Definition: symbolic-calculus.hh:108
pinocchio::Transform3f Transform3f
Definition: fwd.hh:52
Eigen::Matrix< value_type, 3, 3 > matrix3_t
Definition: fwd.hh:60
bool cValid_
Definition: symbolic-calculus.hh:221
PointCom(const CenterOfMassComputationPtr_t &comc)
Definition: symbolic-calculus.hh:839
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:38
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:987
boost::shared_ptr< FunctionType > FunctionTypePtr_t
Definition: symbolic-calculus.hh:755
Sum(const typename Traits< RhsValue >::Ptr_t &rhs, const typename Traits< LhsValue >::Ptr_t &lhs)
Definition: symbolic-calculus.hh:455
ScalarProduct(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:364
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:507
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > Value_t
Definition: symbolic-calculus.hh:928
JointFrame(const JointFrame &jf)
Definition: symbolic-calculus.hh:880
Sum(const CalculusBase< Sum > &other)
Definition: symbolic-calculus.hh:450
CalculusBase< PointCom, vector3_t, ComJacobian_t > Parent_t
Definition: symbolic-calculus.hh:828
void impl_jacobian(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:855
const ComJacobian_t & jacobian() const
Definition: symbolic-calculus.hh:845
Basic expression representing a vector in a joint frame.
Definition: symbolic-calculus.hh:678
Difference(const CalculusBase< Difference > &other)
Definition: symbolic-calculus.hh:406
Expression()
Definition: symbolic-calculus.hh:276
Traits< Expression >::WkPtr_t self_
Definition: symbolic-calculus.hh:293
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:388
void impl_jacobian(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:647
Definition: symbolic-calculus.hh:866
const JointPtr_t & joint() const
Definition: symbolic-calculus.hh:717
Expression< LhsValue, RhsValue >::Ptr_t e_
Definition: symbolic-calculus.hh:476
ValueType ValueType_t
Definition: symbolic-calculus.hh:145
CalculusBaseAbstract< ValueType, JacobianType > Element_t
Definition: symbolic-calculus.hh:936
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:321
MatrixOfExpressions(const MatrixOfExpressions &matrix)
Definition: symbolic-calculus.hh:961
RotationMultiply(const CalculusBase< RotationMultiply > &other)
Definition: symbolic-calculus.hh:533
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:368
void impl_value(const ConfigurationIn_t)
Definition: symbolic-calculus.hh:820
value_type WkPtr_t
Definition: symbolic-calculus.hh:112
JacobianType JacobianType_t
Definition: symbolic-calculus.hh:146
CalculusBase< MatrixOfExpressions, Value_t, Jacobian_t > Parent_t
Definition: symbolic-calculus.hh:935
Expression(const typename Traits< LhsValue >::Ptr_t &lhs, const typename Traits< RhsValue >::Ptr_t &rhs)
Definition: symbolic-calculus.hh:282
#define HPP_CONSTRAINTS_CB_CREATE3(Class, Arg0Type, Arg1Type, Arg2Type)
Definition: symbolic-calculus.hh:66
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:420
pinocchio::JointJacobian_t JointJacobian_t
Definition: fwd.hh:51
VectorInJoint(const CalculusBase< VectorInJoint > &other)
Definition: symbolic-calculus.hh:687
void impl_value(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:459
PointInJoint(const JointPtr_t &joint, const vector3_t &pointInLocalFrame)
Definition: symbolic-calculus.hh:614
ScalarProduct(const CalculusBase< ScalarProduct > &other)
Definition: symbolic-calculus.hh:359
Eigen::Matrix< value_type, 3, 1 > vector3_t
Definition: fwd.hh:61
CenterOfMassComputationPtr_t comc_
Definition: symbolic-calculus.hh:863
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
Eigen::Matrix< value_type, 3, Eigen::Dynamic, Eigen::RowMajor > JacobianMatrix
Definition: symbolic-calculus.hh:103
void impl_jacobian(const ConfigurationIn_t arg)
Definition: symbolic-calculus.hh:562
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > Jacobian_t
Definition: symbolic-calculus.hh:930
Eigen::JacobiSVD< Value_t > & svd()
Definition: symbolic-calculus.hh:1100
HPP_CONSTRAINTS_CB_REF< Class > Ptr_t
Definition: symbolic-calculus.hh:107
void computePseudoInverseJacobian(const ConfigurationIn_t arg, const Eigen::Ref< const Eigen::Matrix< value_type, Eigen::Dynamic, 1 > > &rhs)
Definition: symbolic-calculus.hh:1041
#define HPP_CONSTRAINTS_CB_CREATE1(Class, Arg0Type)
Definition: symbolic-calculus.hh:52
RotationMultiply(const typename Traits< pinocchio::Joint >::Ptr_t &joint, const typename Traits< RhsValue >::Ptr_t &rhs, bool transpose=false)
Definition: symbolic-calculus.hh:548