pinocchio  2.6.0
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;
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;
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 
86  MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
87  : m_ref(v_like)
88  {
89  EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
90  YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
91  assert(v_like.size() == 6);
92  }
93 
95  MotionRef(const MotionRef & other)
96  : m_ref(other.m_ref)
97  {}
98 
99  ToVectorConstReturnType toVector_impl() const { return m_ref; }
100  ToVectorReturnType toVector_impl() { return m_ref; }
101 
102  // Getters
103  ConstAngularType angular_impl() const { return ConstAngularType(m_ref.derived(),ANGULAR); }
104  ConstLinearType linear_impl() const { return ConstLinearType(m_ref.derived(),LINEAR); }
105  AngularType angular_impl() { return m_ref.template segment<3> (ANGULAR); }
106  LinearType linear_impl() { return m_ref.template segment<3> (LINEAR); }
107 
108  template<typename V3>
109  void angular_impl(const Eigen::MatrixBase<V3> & w)
110  {
111  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
112  angular_impl()=w;
113  }
114 
115  template<typename V3>
116  void linear_impl(const Eigen::MatrixBase<V3> & v)
117  {
118  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
119  linear_impl()=v;
120  }
121 
122  // Specific operators for MotionTpl and MotionRef
123  template<typename S1, int O1>
124  MotionPlain __plus__(const MotionTpl<S1,O1> & v) const
125  { return MotionPlain(m_ref+v.toVector()); }
126 
127  template<typename Vector6Like>
128  MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
129  { return MotionPlain(m_ref+v.toVector()); }
130 
131  template<typename S1, int O1>
132  MotionPlain __minus__(const MotionTpl<S1,O1> & v) const
133  { return MotionPlain(m_ref-v.toVector()); }
134 
135  template<typename Vector6Like>
136  MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
137  { return MotionPlain(m_ref-v.toVector()); }
138 
139  template<typename S1, int O1>
140  MotionRef & __pequ__(const MotionTpl<S1,O1> & v)
141  { m_ref += v.toVector(); return *this; }
142 
143  template<typename Vector6Like>
144  MotionRef & __pequ__(const MotionRef<Vector6ArgType> & v)
145  { m_ref += v.toVector(); return *this; }
146 
147  template<typename S1, int O1>
148  MotionRef & __mequ__(const MotionTpl<S1,O1> & v)
149  { m_ref -= v.toVector(); return *this; }
150 
151  template<typename Vector6Like>
152  MotionRef & __mequ__(const MotionRef<Vector6ArgType> & v)
153  { m_ref -= v.toVector(); return *this; }
154 
155  template<typename OtherScalar>
156  MotionPlain __mult__(const OtherScalar & alpha) const
157  { return MotionPlain(alpha*m_ref); }
158 
159  MotionRef & ref() { return *this; }
160 
161  inline PlainReturnType plain() const { return PlainReturnType(m_ref); }
162 
163  protected:
164  DataRefType m_ref;
165 
166  }; // class MotionRef<Vector6Like>
167 
168  template<typename Vector6ArgType>
169  struct traits< MotionRef<const Vector6ArgType> >
170  {
171  typedef typename Vector6ArgType::Scalar Scalar;
172  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
173  enum {
174  LINEAR = 0,
175  ANGULAR = 3,
176  Options = Vector6::Options
177  };
178  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
179  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
180  typedef Matrix6 ActionMatrixType;
181  typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
182  typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
183  typedef ConstLinearType LinearType;
184  typedef ConstAngularType AngularType;
187  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
188  typedef ConstDataRefType ToVectorConstReturnType;
189  typedef ConstDataRefType DataRefType;
190  typedef DataRefType ToVectorReturnType;
192 
193  }; // traits MotionRef<const Vector6ArgType>
194 
195  template<typename Vector6ArgType>
196  class MotionRef<const Vector6ArgType>
197  : public MotionDense< MotionRef<const Vector6ArgType> >
198  {
199  public:
200  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
202  typedef typename traits<MotionRef>::DataRefType DataRefType;
203  MOTION_TYPEDEF_TPL(MotionRef);
204 
205  using Base::operator=;
206  using Base::linear;
207  using Base::angular;
208 
209  using Base::__plus__;
210  using Base::__opposite__;
211  using Base::__minus__;
212  using Base::__mult__;
213 
214  MotionRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) v_like)
215  : m_ref(v_like)
216  {
217  EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
218  YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
219  assert(v_like.size() == 6);
220  }
221 
222  ToVectorConstReturnType toVector_impl() const { return m_ref; }
223 
224  // Getters
225  ConstAngularType angular_impl() const { return ConstAngularType(m_ref.derived(),ANGULAR); }
226  ConstLinearType linear_impl() const { return ConstLinearType(m_ref.derived(),LINEAR); }
227 
228  // Specific operators for MotionTpl and MotionRef
229  template<typename S1, int O1>
230  MotionPlain __plus__(const MotionTpl<S1,O1> & v) const
231  { return MotionPlain(m_ref+v.toVector()); }
232 
233  template<typename Vector6Like>
234  MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
235  { return MotionPlain(m_ref+v.toVector()); }
236 
237  template<typename S1, int O1>
238  MotionPlain __minus__(const MotionTpl<S1,O1> & v) const
239  { return MotionPlain(m_ref-v.toVector()); }
240 
241  template<typename Vector6Like>
242  MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
243  { return MotionPlain(m_ref-v.toVector()); }
244 
245  template<typename OtherScalar>
246  MotionPlain __mult__(const OtherScalar & alpha) const
247  { return MotionPlain(alpha*m_ref); }
248 
249  const MotionRef & ref() const { return *this; }
250 
251  inline PlainReturnType plain() const { return PlainReturnType(m_ref); }
252 
253  protected:
254  DataRefType m_ref;
255 
256  }; // class MotionRef<const Vector6Like>
257 
258 } // namespace pinocchio
259 
260 #endif // ifndef __pinocchio_motion_ref_hpp__
pinocchio::MotionRef::MotionRef
MotionRef(const MotionRef &other)
Copy constructor from another MotionRef.
Definition: motion-ref.hpp:95
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:44
pinocchio::SE3GroupAction
Definition: se3.hpp:39
pinocchio::MotionRef< const Vector6ArgType >
Definition: motion-ref.hpp:196
pinocchio::MotionRef::MotionRef
MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
Default constructor from a 6 dimensional vector.
Definition: motion-ref.hpp:86
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:24