pinocchio  2.6.3
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
force-ref.hpp
1 //
2 // Copyright (c) 2017-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_force_ref_hpp__
6 #define __pinocchio_force_ref_hpp__
7 
8 namespace pinocchio
9 {
10 
11  template<typename Vector6ArgType>
12  struct traits< ForceRef<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;
29  typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
30  typedef DataRefType ToVectorReturnType;
31  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
32  typedef ConstDataRefType ToVectorConstReturnType;
34 
35  }; // traits ForceRef
36 
37  template<typename Vector6ArgType>
38  struct SE3GroupAction< ForceRef<Vector6ArgType> >
39  {
40  typedef typename traits< ForceRef<Vector6ArgType> >::ForcePlain ReturnType;
41  };
42 
43  template<typename Vector6ArgType, typename MotionDerived>
44  struct MotionAlgebraAction< ForceRef<Vector6ArgType>, MotionDerived >
45  {
46  typedef typename traits< ForceRef<Vector6ArgType> >::ForcePlain ReturnType;
47  };
48 
49  template<typename Vector6ArgType>
50  class ForceRef : public ForceDense< ForceRef<Vector6ArgType> >
51  {
52  public:
53  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
54  typedef ForceDense<ForceRef> Base;
55  typedef typename traits<ForceRef>::DataRefType DataRefType;
56  FORCE_TYPEDEF_TPL(ForceRef);
57 
58  using Base::operator=;
59  using Base::operator==;
60  using Base::operator!=;
61 
63  ForceRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) f_like)
64  : m_ref(f_like)
65  {
66  EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
67  YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
68  assert(f_like.size() == 6);
69  }
70 
72  ForceRef(const ForceRef & other)
73  : m_ref(other.m_ref)
74  {}
75 
76  ToVectorConstReturnType toVector_impl() const { return m_ref; }
77  ToVectorReturnType toVector_impl() { return m_ref; }
78 
79  // Getters
80  ConstAngularType angular_impl() const { return ConstAngularType(m_ref.derived(),ANGULAR); }
81  ConstLinearType linear_impl() const { return ConstLinearType(m_ref.derived(),LINEAR); }
82  AngularType angular_impl() { return m_ref.template segment<3> (ANGULAR); }
83  LinearType linear_impl() { return m_ref.template segment<3> (LINEAR); }
84 
85  template<typename V3>
86  void angular_impl(const Eigen::MatrixBase<V3> & w)
87  {
88  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
89  angular_impl()=w;
90  }
91 
92  template<typename V3>
93  void linear_impl(const Eigen::MatrixBase<V3> & v)
94  {
95  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
96  linear_impl()=v;
97  }
98 
99  ForceRef & ref() { return *this; }
100 
101  protected:
102  DataRefType m_ref;
103 
104  }; // class ForceRef<Vector6Like>
105 
106  template<typename Vector6ArgType>
107  struct traits< ForceRef<const Vector6ArgType> >
108  {
109  typedef typename Vector6ArgType::Scalar Scalar;
110  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
111  enum {
112  LINEAR = 0,
113  ANGULAR = 3,
114  Options = Vector6::Options
115  };
116  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
117  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
118  typedef Matrix6 ActionMatrixType;
119  typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
120  typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
121  typedef ConstLinearType LinearType;
122  typedef ConstAngularType AngularType;
124  typedef ForcePlain PlainReturnType;
125  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
126  typedef ConstDataRefType ToVectorConstReturnType;
127  typedef ConstDataRefType DataRefType;
128  typedef DataRefType ToVectorReturnType;
130 
131  }; // traits ForceRef<const Vector6ArgType>
132 
133  template<typename Vector6ArgType>
134  class ForceRef<const Vector6ArgType>
135  : public ForceDense< ForceRef<const Vector6ArgType> >
136  {
137  public:
138  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
139  typedef ForceDense<ForceRef> Base;
140  typedef typename traits<ForceRef>::DataRefType DataRefType;
141  FORCE_TYPEDEF_TPL(ForceRef);
142 
143  ForceRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) f_like)
144  : m_ref(f_like)
145  {
146  EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
147  YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
148  assert(f_like.size() == 6);
149  }
150 
151  ToVectorConstReturnType toVector_impl() const { return m_ref; }
152 
153  // Getters
154  ConstAngularType angular_impl() const { return ConstAngularType(m_ref.derived(),ANGULAR); }
155  ConstLinearType linear_impl() const { return ConstLinearType(m_ref.derived(),LINEAR); }
156 
157  const ForceRef & ref() const { return *this; }
158 
159  protected:
160  DataRefType m_ref;
161 
162  }; // class ForceRef<Vector6Like>
163 
164 } // namespace pinocchio
165 
166 #endif // ifndef __pinocchio_force_ref_hpp__
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::ForceRef::ForceRef
ForceRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) f_like)
Default constructor from a 6 dimensional vector.
Definition: force-ref.hpp:63
pinocchio::ForceTpl< Scalar, Options >
pinocchio::ForceRef::ForceRef
ForceRef(const ForceRef &other)
Copy constructor from another ForceRef.
Definition: force-ref.hpp:72
pinocchio::ForceRef
Definition: force-ref.hpp:50
pinocchio::ForceDense
Definition: force-dense.hpp:24
pinocchio::ForceRef< const Vector6ArgType >
Definition: force-ref.hpp:134
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24