pinocchio  UNKNOWN
motion-ref.hpp
1 
2 //
3 // Copyright (c) 2017-2018 CNRS
4 //
5 // This file is part of Pinocchio
6 // Pinocchio is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // Pinocchio is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // Pinocchio If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef __se3_motion_ref_hpp__
20 #define __se3_motion_ref_hpp__
21 
22 namespace se3
23 {
24 
25  template<typename Vector6ArgType>
26  struct traits< MotionRef<Vector6ArgType> >
27  {
28  typedef typename Vector6ArgType::Scalar Scalar;
29  typedef typename EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
30  enum {
31  LINEAR = 0,
32  ANGULAR = 3,
33  Options = Vector6::Options
34  };
35  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
36  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
37  typedef Matrix6 ActionMatrixType;
38  typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
39  typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
40  typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
41  typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
43  typedef typename EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
44  typedef DataRefType ToVectorReturnType;
45  typedef typename EIGEN_REF_CONSTTYPE(Vector6ArgType) ConstDataRefType;
46  typedef ConstDataRefType ToVectorConstReturnType;
48 
49  }; // traits MotionRef
50 
51  namespace internal
52  {
53  template<typename Vector6ArgType>
54  struct SE3GroupAction< MotionRef<Vector6ArgType> >
55  {
56  typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
57  };
58 
59  template<typename Vector6ArgType, typename MotionDerived>
60  struct MotionAlgebraAction< MotionRef<Vector6ArgType>, MotionDerived >
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(const Eigen::MatrixBase<Vector6ArgType> & v_like)
86  : m_ref(const_cast<Vector6ArgType &>(v_like.derived()))
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  protected:
156  DataRefType m_ref;
157 
158  }; // class MotionTpl
159 
160 } // namespace se3
161 
162 #endif // ifndef __se3_motion_ref_hpp__
EIGEN_PLAIN_TYPE(Matrix3x) cross(const Eigen
Applies the cross product onto the columns of M.
Definition: skew.hpp:227