pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
motion-ref.hpp
1 //
2 // Copyright (c) 2017-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_motion_ref_hpp__
6 #define __pinocchio_motion_ref_hpp__
7 
8 namespace pinocchio
9 {
10 
11  template<typename Vector6ArgType>
12  struct traits< MotionRef<Vector6ArgType> >
13  {
14  typedef typename Vector6ArgType::Scalar Scalar;
15  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
16  enum {
17  LINEAR = 0,
18  ANGULAR = 3,
19  Options = Vector6::Options
20  };
21  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
22  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
23  typedef Matrix6 ActionMatrixType;
24  typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
25  typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
26  typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
27  typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
28  typedef MotionTpl<Scalar,Options> MotionPlain;
29  typedef MotionPlain PlainReturnType;
30  typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
31  typedef DataRefType ToVectorReturnType;
32  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
33  typedef ConstDataRefType ToVectorConstReturnType;
34  typedef MotionRef<Vector6ArgType> MotionRefType;
35 
36  }; // traits MotionRef
37 
38  template<typename Vector6ArgType>
39  struct SE3GroupAction< MotionRef<Vector6ArgType> >
40  {
41  typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
42  };
43 
44  template<typename Vector6ArgType, typename MotionDerived>
45  struct MotionAlgebraAction< MotionRef<Vector6ArgType>, MotionDerived >
46  {
47  typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
48  };
49 
50  namespace internal
51  {
52  template<typename Vector6ArgType, typename Scalar>
53  struct RHSScalarMultiplication< MotionRef<Vector6ArgType>, Scalar >
54  {
55  typedef typename pinocchio::traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
56  };
57 
58  template<typename Vector6ArgType, typename Scalar>
59  struct LHSScalarMultiplication< MotionRef<Vector6ArgType>, Scalar >
60  {
61  typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
62  };
63  }
64 
65  template<typename Vector6ArgType>
66  class MotionRef : public MotionDense< MotionRef<Vector6ArgType> >
67  {
68  public:
69  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70  typedef MotionDense<MotionRef> Base;
71  typedef typename traits<MotionRef>::DataRefType DataRefType;
72  MOTION_TYPEDEF_TPL(MotionRef);
73 
74  using Base::operator=;
75  using Base::linear;
76  using Base::angular;
77 
78  using Base::__plus__;
79  using Base::__opposite__;
80  using Base::__minus__;
81  using Base::__pequ__;
82  using Base::__mequ__;
83  using Base::__mult__;
84 
85  MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
86  : m_ref(v_like)
87  {
88  EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
89  YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
90  assert(v_like.size() == 6);
91  }
92 
93  ToVectorConstReturnType toVector_impl() const { return m_ref; }
94  ToVectorReturnType toVector_impl() { return m_ref; }
95 
96  // Getters
97  ConstAngularType angular_impl() const { return ConstAngularType(m_ref.derived(),ANGULAR); }
98  ConstLinearType linear_impl() const { return ConstLinearType(m_ref.derived(),LINEAR); }
99  AngularType angular_impl() { return m_ref.template segment<3> (ANGULAR); }
100  LinearType linear_impl() { return m_ref.template segment<3> (LINEAR); }
101 
102  template<typename V3>
103  void angular_impl(const Eigen::MatrixBase<V3> & w)
104  {
105  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
106  angular_impl()=w;
107  }
108 
109  template<typename V3>
110  void linear_impl(const Eigen::MatrixBase<V3> & v)
111  {
112  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
113  linear_impl()=v;
114  }
115 
116  // Specific operators for MotionTpl and MotionRef
117  template<typename S1, int O1>
118  MotionPlain __plus__(const MotionTpl<S1,O1> & v) const
119  { return MotionPlain(m_ref+v.toVector()); }
120 
121  template<typename Vector6Like>
122  MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
123  { return MotionPlain(m_ref+v.toVector()); }
124 
125  template<typename S1, int O1>
126  MotionPlain __minus__(const MotionTpl<S1,O1> & v) const
127  { return MotionPlain(m_ref-v.toVector()); }
128 
129  template<typename Vector6Like>
130  MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
131  { return MotionPlain(m_ref-v.toVector()); }
132 
133  template<typename S1, int O1>
134  MotionRef & __pequ__(const MotionTpl<S1,O1> & v)
135  { m_ref += v.toVector(); return *this; }
136 
137  template<typename Vector6Like>
138  MotionRef & __pequ__(const MotionRef<Vector6ArgType> & v)
139  { m_ref += v.toVector(); return *this; }
140 
141  template<typename S1, int O1>
142  MotionRef & __mequ__(const MotionTpl<S1,O1> & v)
143  { m_ref -= v.toVector(); return *this; }
144 
145  template<typename Vector6Like>
146  MotionRef & __mequ__(const MotionRef<Vector6ArgType> & v)
147  { m_ref -= v.toVector(); return *this; }
148 
149  template<typename OtherScalar>
150  MotionPlain __mult__(const OtherScalar & alpha) const
151  { return MotionPlain(alpha*m_ref); }
152 
153  MotionRef & ref() { return *this; }
154 
155  inline PlainReturnType plain() const { return PlainReturnType(m_ref); }
156 
157  protected:
158  DataRefType m_ref;
159 
160  }; // class MotionRef<Vector6Like>
161 
162  template<typename Vector6ArgType>
163  struct traits< MotionRef<const Vector6ArgType> >
164  {
165  typedef typename Vector6ArgType::Scalar Scalar;
166  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
167  enum {
168  LINEAR = 0,
169  ANGULAR = 3,
170  Options = Vector6::Options
171  };
172  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
173  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
174  typedef Matrix6 ActionMatrixType;
175  typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
176  typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
177  typedef ConstLinearType LinearType;
178  typedef ConstAngularType AngularType;
179  typedef MotionTpl<Scalar,Options> MotionPlain;
180  typedef MotionPlain PlainReturnType;
181  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
182  typedef ConstDataRefType ToVectorConstReturnType;
183  typedef ConstDataRefType DataRefType;
184  typedef DataRefType ToVectorReturnType;
185  typedef MotionRef<const Vector6ArgType> MotionRefType;
186 
187  }; // traits MotionRef<const Vector6ArgType>
188 
189  template<typename Vector6ArgType>
190  class MotionRef<const Vector6ArgType>
191  : public MotionDense< MotionRef<const Vector6ArgType> >
192  {
193  public:
194  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
195  typedef MotionDense<MotionRef> Base;
196  typedef typename traits<MotionRef>::DataRefType DataRefType;
197  MOTION_TYPEDEF_TPL(MotionRef);
198 
199  using Base::operator=;
200  using Base::linear;
201  using Base::angular;
202 
203  using Base::__plus__;
204  using Base::__opposite__;
205  using Base::__minus__;
206  using Base::__mult__;
207 
208  MotionRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) v_like)
209  : m_ref(v_like)
210  {
211  EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
212  YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
213  assert(v_like.size() == 6);
214  }
215 
216  ToVectorConstReturnType toVector_impl() const { return m_ref; }
217 
218  // Getters
219  ConstAngularType angular_impl() const { return ConstAngularType(m_ref.derived(),ANGULAR); }
220  ConstLinearType linear_impl() const { return ConstLinearType(m_ref.derived(),LINEAR); }
221 
222  // Specific operators for MotionTpl and MotionRef
223  template<typename S1, int O1>
224  MotionPlain __plus__(const MotionTpl<S1,O1> & v) const
225  { return MotionPlain(m_ref+v.toVector()); }
226 
227  template<typename Vector6Like>
228  MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
229  { return MotionPlain(m_ref+v.toVector()); }
230 
231  template<typename S1, int O1>
232  MotionPlain __minus__(const MotionTpl<S1,O1> & v) const
233  { return MotionPlain(m_ref-v.toVector()); }
234 
235  template<typename Vector6Like>
236  MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
237  { return MotionPlain(m_ref-v.toVector()); }
238 
239  template<typename OtherScalar>
240  MotionPlain __mult__(const OtherScalar & alpha) const
241  { return MotionPlain(alpha*m_ref); }
242 
243  const MotionRef & ref() const { return *this; }
244 
245  inline PlainReturnType plain() const { return PlainReturnType(m_ref); }
246 
247  protected:
248  DataRefType m_ref;
249 
250  }; // class MotionRef<const Vector6Like>
251 
252 } // namespace pinocchio
253 
254 #endif // ifndef __pinocchio_motion_ref_hpp__
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:44
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35