pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
constraint-generic.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_generic_hpp__
7 #define __pinocchio_multibody_constraint_generic_hpp__
8 
9 namespace pinocchio
10 {
11 
12  template<int _Dim, typename _Scalar, int _Options>
13  struct traits< ConstraintTpl<_Dim, _Scalar, _Options> >
14  {
15  typedef _Scalar Scalar;
16  enum
17  {
18  LINEAR = 0,
19  ANGULAR = 3,
20  Options = _Options,
21  Dim = _Dim
22  };
23 
25  typedef Eigen::Matrix<Scalar,Dim,1,Options> JointForce;
26  typedef Eigen::Matrix<Scalar,6,Dim,Options> DenseBase;
27 
28  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(DenseBase) ConstMatrixReturnType;
29  typedef typename PINOCCHIO_EIGEN_REF_TYPE(DenseBase) MatrixReturnType;
30 
31  }; // traits ConstraintTpl
32 
33  template<int Dim, typename Scalar, int Options>
34  struct SE3GroupAction< ConstraintTpl<Dim,Scalar,Options> >
35  { typedef Eigen::Matrix<Scalar,6,Dim> ReturnType; };
36 
37  template<int Dim, typename Scalar, int Options, typename MotionDerived>
38  struct MotionAlgebraAction< ConstraintTpl<Dim,Scalar,Options>, MotionDerived >
39  { typedef Eigen::Matrix<Scalar,6,Dim> ReturnType; };
40 
41  template<int _Dim, typename _Scalar, int _Options>
43  : public ConstraintBase< ConstraintTpl<_Dim,_Scalar,_Options> >
44  {
45  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46 
48 
49  friend class ConstraintBase<ConstraintTpl>;
50  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintTpl)
51 
52  enum { NV = _Dim };
53 
54  using Base::nv;
55 
56  template<typename D>
57  explicit ConstraintTpl(const Eigen::MatrixBase<D> & _S) : S(_S)
58  {
59  // There is currently a bug in Eigen/Core/util/StaticAssert.h in the use of the full namespace path
60  // TODO
61  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DenseBase, D);
62  }
63 
64  ConstraintTpl() : S()
65  {
66  EIGEN_STATIC_ASSERT(_Dim!=Eigen::Dynamic,
67  YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR)
68  }
69 
70  // It is only valid for dynamics size
71  explicit ConstraintTpl(const int dim) : S(6,dim)
72  {
73  EIGEN_STATIC_ASSERT(_Dim==Eigen::Dynamic,
74  YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR)
75  }
76 
77  static ConstraintTpl Zero(const int dim)
78  {
79  return ConstraintTpl(dim);
80  }
81 
82  template<typename VectorLike>
83  JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & vj) const
84  {
85  return JointMotion(S*vj);
86  }
87 
88  struct Transpose
89  {
90  const ConstraintTpl & ref;
91  Transpose( const ConstraintTpl & ref ) : ref(ref) {}
92 
93  template<typename Derived>
94  JointForce operator*(const ForceDense<Derived> & f) const
95  { return (ref.S.transpose()*f.toVector()).eval(); }
96 
97  template<typename D>
98  typename Eigen::Matrix<Scalar,NV,Eigen::Dynamic>
99  operator*(const Eigen::MatrixBase<D> & F)
100  {
101  return (ref.S.transpose()*F).eval();
102  }
103 
104  };
105 
106  Transpose transpose() const { return Transpose(*this); }
107 
108  MatrixReturnType matrix_impl() { return S; }
109  ConstMatrixReturnType matrix_impl() const { return S; }
110 
111  int nv_impl() const { return (int)S.cols(); }
112 
113  template<typename S2,int O2>
115  operator*(const InertiaTpl<S2,O2> & Y, const ConstraintTpl & S)
116  {
117  typedef typename ConstraintTpl::DenseBase ReturnType;
118  ReturnType res(6,S.nv());
119  motionSet::inertiaAction(Y,S.S,res);
120  return res;
121  }
122 
123  template<typename S2,int O2>
124  friend Eigen::Matrix<_Scalar,6,_Dim>
125  operator*(const Eigen::Matrix<S2,6,6,O2> & Ymatrix, const ConstraintTpl & S)
126  {
127  typedef Eigen::Matrix<_Scalar,6,_Dim> ReturnType;
128  return ReturnType(Ymatrix*S.matrix());
129 
130  }
131 
132  DenseBase se3Action(const SE3Tpl<Scalar,Options> & m) const
133  {
134  DenseBase res(6,nv());
135  motionSet::se3Action(m,S,res);
136  return res;
137  }
138 
139  DenseBase se3ActionInverse(const SE3Tpl<Scalar,Options> & m) const
140  {
141  DenseBase res(6,nv());
143  return res;
144  }
145 
146  template<typename MotionDerived>
147  DenseBase motionAction(const MotionDense<MotionDerived> & v) const
148  {
149  DenseBase res(6,nv());
150  motionSet::motionAction(v,S,res);
151  return res;
152  }
153 
154  void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;}
155 
156  bool isEqual(const ConstraintTpl & other) const
157  {
158  return S == other.S;
159  }
160 
161  protected:
162  DenseBase S;
163  }; // class ConstraintTpl
164 
165 
166 } // namespace pinocchio
167 
168 #endif // ifndef __pinocchio_multibody_constraint_generic_hpp__
169 
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:44
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Definition: force-base.hpp:81
static void motionAction(const MotionDense< MotionDerived > &v, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion on a set of motions, represented by a 6xN matrix whose columns represent a spatial...
static void se3ActionInverse(const SE3Tpl< Scalar, Options > &m, const Eigen::MatrixBase< Mat > &iV, Eigen::MatrixBase< MatRet > const &jV)
Inverse SE3 action on a set of motions, represented by a 6xN matrix whose column represent a spatial ...
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35
static void inertiaAction(const InertiaTpl< Scalar, Options > &I, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
Action of an Inertia matrix on a set of motions, represented by a 6xN matrix whose columns represent ...
static void se3Action(const SE3Tpl< Scalar, Options > &m, const Eigen::MatrixBase< Mat > &iV, Eigen::MatrixBase< MatRet > const &jV)
SE3 action on a set of motions, represented by a 6xN matrix whose column represent a spatial motion...
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
 .