pinocchio  2.2.1-dirty
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  template<typename VectorLike>
78  JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & vj) const
79  {
80  return JointMotion(S*vj);
81  }
82 
83  struct Transpose
84  {
85  const ConstraintTpl & ref;
86  Transpose( const ConstraintTpl & ref ) : ref(ref) {}
87 
88  template<typename Derived>
89  JointForce operator*(const ForceDense<Derived> & f) const
90  { return (ref.S.transpose()*f.toVector()).eval(); }
91 
92  template<typename D>
93  typename Eigen::Matrix<Scalar,NV,Eigen::Dynamic>
94  operator*(const Eigen::MatrixBase<D> & F)
95  {
96  return (ref.S.transpose()*F).eval();
97  }
98 
99  };
100 
101  Transpose transpose() const { return Transpose(*this); }
102 
103  MatrixReturnType matrix_impl() { return S; }
104  ConstMatrixReturnType matrix_impl() const { return S; }
105 
106  int nv_impl() const { return (int)S.cols(); }
107 
108  template<typename S2,int O2>
110  operator*(const InertiaTpl<S2,O2> & Y, const ConstraintTpl & S)
111  {
112  typedef typename ConstraintTpl::DenseBase ReturnType;
113  ReturnType res(6,S.nv());
114  motionSet::inertiaAction(Y,S.S,res);
115  return res;
116  }
117 
118  template<typename S2,int O2>
119  friend Eigen::Matrix<_Scalar,6,_Dim>
120  operator*(const Eigen::Matrix<S2,6,6,O2> & Ymatrix, const ConstraintTpl & S)
121  {
122  typedef Eigen::Matrix<_Scalar,6,_Dim> ReturnType;
123  return ReturnType(Ymatrix*S.matrix());
124 
125  }
126 
127  DenseBase se3Action(const SE3Tpl<Scalar,Options> & m) const
128  {
129  DenseBase res(6,nv());
130  motionSet::se3Action(m,S,res);
131  return res;
132  }
133 
134  DenseBase se3ActionInverse(const SE3Tpl<Scalar,Options> & m) const
135  {
136  DenseBase res(6,nv());
138  return res;
139  }
140 
141  template<typename MotionDerived>
142  DenseBase motionAction(const MotionDense<MotionDerived> & v) const
143  {
144  DenseBase res(6,nv());
145  motionSet::motionAction(v,S,res);
146  return res;
147  }
148 
149  void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;}
150 
151  protected:
152  DenseBase S;
153  }; // class ConstraintTpl
154 
155 
156 } // namespace pinocchio
157 
158 #endif // ifndef __pinocchio_multibody_constraint_generic_hpp__
159 
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.
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 ...
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Definition: force-base.hpp:86
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
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)
 .