motion-ref.hpp
Go to the documentation of this file.
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,
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;
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;
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  {
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
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>
143  { m_ref += v.toVector(); return *this; }
144 
145  template<typename Vector6Like>
147  { m_ref += v.toVector(); return *this; }
148 
149  template<typename S1, int O1>
151  { m_ref -= v.toVector(); return *this; }
152 
153  template<typename Vector6Like>
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,
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;
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;
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
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__
Vector6ArgType::template ConstFixedSegmentReturnType< 3 >::Type ConstLinearType
Definition: motion-ref.hpp:185
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
MotionPlain __mult__(const OtherScalar &alpha) const
Definition: motion-ref.hpp:250
Vector6ArgType::template ConstFixedSegmentReturnType< 3 >::Type ConstLinearType
Definition: motion-ref.hpp:28
Vector6ArgType::template ConstFixedSegmentReturnType< 3 >::Type ConstAngularType
Definition: motion-ref.hpp:186
traits< MotionRef< Vector6ArgType > >::MotionPlain ReturnType
Definition: motion-ref.hpp:49
MotionPlain __mult__(const OtherScalar &alpha) const
Definition: motion-ref.hpp:158
void linear_impl(const Eigen::MatrixBase< V3 > &v)
Definition: motion-ref.hpp:118
Return type of the ation of a Motion onto an object of type D.
#define MOTION_TYPEDEF_TPL(Derived)
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
Definition: motion-ref.hpp:23
MotionRef & ref()
Definition: motion-ref.hpp:161
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef MotionDense< MotionRef > Base
Definition: motion-ref.hpp:205
MotionRef & __mequ__(const MotionRef< Vector6ArgType > &v)
Definition: motion-ref.hpp:154
MotionPlain __minus__(const MotionRef< Vector6ArgType > &v) const
Definition: motion-ref.hpp:138
MotionRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) v_like)
Definition: motion-ref.hpp:218
MotionRef & __pequ__(const MotionRef< Vector6ArgType > &v)
Definition: motion-ref.hpp:146
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef MotionDense< MotionRef > Base
Definition: motion-ref.hpp:72
MotionRef(const MotionRef &other)
Copy constructor from another MotionRef.
Definition: motion-ref.hpp:97
MotionRef & __mequ__(const MotionTpl< S1, O1 > &v)
Definition: motion-ref.hpp:150
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: motion-ref.hpp:21
MotionRef< const Vector6ArgType > MotionRefType
Definition: motion-ref.hpp:195
SE3::Scalar Scalar
Definition: conversions.cpp:13
LinearType linear_impl()
Definition: motion-ref.hpp:108
traits< MotionRef< Vector6ArgType > >::MotionPlain ReturnType
Definition: motion-ref.hpp:43
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: motion-ref.hpp:180
ConstLinearType linear_impl() const
Definition: motion-ref.hpp:106
pinocchio::traits< MotionRef< Vector6ArgType > >::MotionPlain ReturnType
Definition: motion-ref.hpp:57
MotionRef & __pequ__(const MotionTpl< S1, O1 > &v)
Definition: motion-ref.hpp:142
ToVectorConstReturnType toVector_impl() const
Definition: motion-ref.hpp:101
Vector6ArgType::template ConstFixedSegmentReturnType< 3 >::Type ConstAngularType
Definition: motion-ref.hpp:29
MotionTpl< Scalar, Options > MotionPlain
Definition: motion-ref.hpp:30
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
Definition: motion-ref.hpp:181
MotionPlain __minus__(const MotionRef< Vector6ArgType > &v) const
Definition: motion-ref.hpp:246
#define PINOCCHIO_EIGEN_REF_TYPE(D)
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
Definition: motion-ref.hpp:22
Main pinocchio namespace.
Definition: timings.cpp:28
void angular_impl(const Eigen::MatrixBase< V3 > &w)
Definition: motion-ref.hpp:111
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
AngularType angular_impl()
Definition: motion-ref.hpp:107
MotionPlain __plus__(const MotionRef< Vector6ArgType > &v) const
Definition: motion-ref.hpp:130
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
Definition: motion-ref.hpp:182
MotionPlain __plus__(const MotionRef< Vector6ArgType > &v) const
Definition: motion-ref.hpp:238
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
traits< MotionRef >::DataRefType DataRefType
Definition: motion-ref.hpp:73
PlainReturnType plain() const
Definition: motion-ref.hpp:163
traits< MotionRef >::DataRefType DataRefType
Definition: motion-ref.hpp:206
MotionPlain __minus__(const MotionTpl< S1, O1 > &v) const
Definition: motion-ref.hpp:242
Vector6ArgType::template FixedSegmentReturnType< 3 >::Type LinearType
Definition: motion-ref.hpp:26
MotionPlain __plus__(const MotionTpl< S1, O1 > &v) const
Definition: motion-ref.hpp:234
ToVectorReturnType toVector_impl()
Definition: motion-ref.hpp:102
w
Definition: ur5x4.py:45
ToVectorConstReturnType toVector_impl() const
Definition: motion-ref.hpp:226
ConstAngularType angular_impl() const
Definition: motion-ref.hpp:105
MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
Default constructor from a 6 dimensional vector.
Definition: motion-ref.hpp:88
MotionPlain __plus__(const MotionTpl< S1, O1 > &v) const
Definition: motion-ref.hpp:126
Vector6ArgType::template FixedSegmentReturnType< 3 >::Type AngularType
Definition: motion-ref.hpp:27
MotionPlain __minus__(const MotionTpl< S1, O1 > &v) const
Definition: motion-ref.hpp:134


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32