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