pinocchio  UNKNOWN
force-ref.hpp
1 //
2 // Copyright (c) 2017-2018 CNRS
3 //
4 // This file is part of Pinocchio
5 // Pinocchio is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // Pinocchio is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // Pinocchio If not, see
16 // <http://www.gnu.org/licenses/>.
17 
18 #ifndef __se3_force_ref_hpp__
19 #define __se3_force_ref_hpp__
20 
21 namespace se3
22 {
23 
24  template<typename Vector6ArgType>
25  struct traits< ForceRef<Vector6ArgType> >
26  {
27  typedef typename Vector6ArgType::Scalar Scalar;
28  typedef typename EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
29  enum {
30  LINEAR = 0,
31  ANGULAR = 3,
32  Options = Vector6::Options
33  };
34  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
35  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
36  typedef Matrix6 ActionMatrixType;
37  typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
38  typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
39  typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
40  typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
42  typedef typename EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
43  typedef DataRefType ToVectorReturnType;
44  typedef typename EIGEN_REF_CONSTTYPE(Vector6ArgType) ConstDataRefType;
45  typedef ConstDataRefType ToVectorConstReturnType;
47 
48  }; // traits ForceRef
49 
50  namespace internal
51  {
52  template<typename Vector6ArgType>
53  struct SE3GroupAction< ForceRef<Vector6ArgType> >
54  {
55  typedef typename traits< ForceRef<Vector6ArgType> >::ForcePlain ReturnType;
56  };
57 
58  template<typename Vector6ArgType, typename MotionDerived>
59  struct MotionAlgebraAction< ForceRef<Vector6ArgType>, MotionDerived >
60  {
61  typedef typename traits< ForceRef<Vector6ArgType> >::ForcePlain ReturnType;
62  };
63  }
64 
65  template<typename Vector6ArgType>
66  class ForceRef : public ForceDense< ForceRef<Vector6ArgType> >
67  {
68  public:
69  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70  typedef ForceDense<ForceRef> Base;
71  typedef typename traits<ForceRef>::DataRefType DataRefType;
72  FORCE_TYPEDEF_TPL(ForceRef);
73 
74  using Base::operator=;
75 
76  ForceRef(const Eigen::MatrixBase<Vector6ArgType> & f_like)
77  : m_ref(const_cast<Vector6ArgType &>(f_like.derived()))
78  {
79  EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
80  YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
81  assert(f_like.size() == 6);
82  }
83 
84  ToVectorConstReturnType toVector_impl() const { return m_ref; }
85  ToVectorReturnType toVector_impl() { return m_ref; }
86 
87  // Getters
88  ConstAngularType angular_impl() const { return ConstAngularType(m_ref.derived(),ANGULAR); }
89  ConstLinearType linear_impl() const { return ConstLinearType(m_ref.derived(),LINEAR); }
90  AngularType angular_impl() { return m_ref.template segment<3> (ANGULAR); }
91  LinearType linear_impl() { return m_ref.template segment<3> (LINEAR); }
92 
93  template<typename V3>
94  void angular_impl(const Eigen::MatrixBase<V3> & w)
95  {
96  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
97  angular_impl()=w;
98  }
99 
100  template<typename V3>
101  void linear_impl(const Eigen::MatrixBase<V3> & v)
102  {
103  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
104  linear_impl()=v;
105  }
106 
107  ForceRef & ref() { return *this; }
108 
109  protected:
110  DataRefType m_ref;
111 
112  }; // class MotionTpl
113 
114 } // namespace se3
115 
116 #endif // ifndef __se3_force_ref_hpp__
EIGEN_PLAIN_TYPE(Matrix3x) cross(const Eigen
Applies the cross product onto the columns of M.
Definition: skew.hpp:227