pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
constraint-base.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS, INRIA
3 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_multibody_constraint_base_hpp__
7 #define __pinocchio_multibody_constraint_base_hpp__
8 
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/spatial/fwd.hpp"
11 #include "pinocchio/spatial/motion.hpp"
12 #include "pinocchio/spatial/act-on-set.hpp"
13 
14 // S : v \in M^6 -> v_J \in lie(Q) ~= R^nv
15 // S^T : f_J \in lie(Q)^* ~= R^nv -> f \in F^6
16 
17 #define PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED,TYPENAME) \
18  typedef TYPENAME traits<DERIVED>::Scalar Scalar; \
19  typedef TYPENAME traits<DERIVED>::JointMotion JointMotion; \
20  typedef TYPENAME traits<DERIVED>::JointForce JointForce; \
21  typedef TYPENAME traits<DERIVED>::DenseBase DenseBase; \
22  typedef TYPENAME traits<DERIVED>::MatrixReturnType MatrixReturnType; \
23  typedef TYPENAME traits<DERIVED>::ConstMatrixReturnType ConstMatrixReturnType; \
24  enum { LINEAR = traits<DERIVED>::LINEAR, ANGULAR = traits<DERIVED>::ANGULAR }; \
25  enum { Options = traits<DERIVED>::Options };
26 
27 #define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED) PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED,typename)
28 #define PINOCCHIO_CONSTRAINT_TYPEDEF(DERIVED) PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED,PINOCCHIO_EMPTY_ARG)
29 
30 namespace pinocchio
31 {
32 
34  template<class ConstraintDerived, typename Force>
36  {
37  typedef ReturnTypeNotDefined ReturnType;
38  };
39 
41  template<class ConstraintDerived, typename ForceSet>
43  {
44  typedef ReturnTypeNotDefined ReturnType;
45  };
46 
47  template<class Derived>
49  {
50  protected:
51  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(Derived)
52 
53  public:
54  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
55 
56  Derived & derived() { return *static_cast<Derived*>(this); }
57  const Derived & derived() const { return *static_cast<const Derived*>(this); }
58 
59  template<typename VectorLike>
60  JointMotion operator*(const Eigen::MatrixBase<VectorLike> & vj) const
61  { return derived().__mult__(vj); }
62 
63  MatrixReturnType matrix() { return derived().matrix_impl(); }
64  ConstMatrixReturnType matrix() const { return derived().matrix_impl(); }
65 
66  int nv() const { return derived().nv_impl(); }
67 
68  static int rows() { return 6; }
69  int cols() const { return nv(); }
70 
71  template<class OtherDerived>
72  bool isApprox(const ConstraintBase<OtherDerived> & other,
73  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
74  { return matrix().isApprox(other.matrix(),prec); }
75 
76  void disp(std::ostream & os) const { derived().disp_impl(os); }
77  friend std::ostream & operator << (std::ostream & os,const ConstraintBase<Derived> & X)
78  {
79  X.disp(os);
80  return os;
81  }
82 
83  typename SE3GroupAction<Derived>::ReturnType
84  se3Action(const SE3Tpl<Scalar,Options> & m) const
85  {
86  return derived().se3Action(m);
87  }
88 
89  typename SE3GroupAction<Derived>::ReturnType
90  se3ActionInverse(const SE3Tpl<Scalar,Options> & m) const
91  {
92  return derived().se3ActionInverse(m);
93  }
94 
95  template<typename MotionDerived>
96  typename MotionAlgebraAction<Derived,MotionDerived>::ReturnType
97  motionAction(const MotionDense<MotionDerived> & v) const
98  {
99  return derived().motionAction(v);
100  }
101 
102  bool operator==(const ConstraintBase<Derived> & other) const
103  {
104  return derived().isEqual(other.derived());
105  }
106 
107  }; // class ConstraintBase
108 
110  template<typename Scalar, int Options, typename ConstraintDerived>
111  typename MultiplicationOp<InertiaTpl<Scalar,Options>,ConstraintDerived>::ReturnType
113  const ConstraintBase<ConstraintDerived> & constraint)
114  {
115  return impl::LhsMultiplicationOp<InertiaTpl<Scalar,Options>,ConstraintDerived>::run(Y,
116  constraint.derived());
117  }
118 
120  template<typename MatrixDerived, typename ConstraintDerived>
121  typename MultiplicationOp<Eigen::MatrixBase<MatrixDerived>,ConstraintDerived>::ReturnType
122  operator*(const Eigen::MatrixBase<MatrixDerived> & Y,
123  const ConstraintBase<ConstraintDerived> & constraint)
124  {
125  return impl::LhsMultiplicationOp<Eigen::MatrixBase<MatrixDerived>,ConstraintDerived>::run(Y.derived(),
126  constraint.derived());
127  }
128 
129 } // namespace pinocchio
130 
131 #endif // ifndef __pinocchio_multibody_constraint_base_hpp__
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
Forward declaration of the multiplication operation return type. Should be overloaded, otherwise it will procude a compilation error.
Definition: binary-op.hpp:15
Return type of the Constraint::Transpose * ForceSet operation.
Main pinocchio namespace.
Definition: treeview.dox:24
Return type of the Constraint::Transpose * Force operation.
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
 .