pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
force-tpl.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_force_tpl_hpp__
7 #define __pinocchio_force_tpl_hpp__
8 
9 namespace pinocchio
10 {
11  template<typename _Scalar, int _Options>
12  struct traits< ForceTpl<_Scalar, _Options> >
13  {
14  typedef _Scalar Scalar;
15  typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
16  typedef Eigen::Matrix<Scalar,6,1,_Options> Vector6;
17  typedef Eigen::Matrix<Scalar,6,6,_Options> Matrix6;
18  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
19  typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
20  typedef typename Vector6::template FixedSegmentReturnType<3>::Type LinearType;
21  typedef typename Vector6::template FixedSegmentReturnType<3>::Type AngularType;
22  typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
23  typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
25  enum {
26  LINEAR = 0,
27  ANGULAR = 3,
28  Options = _Options
29  };
30 
32  }; // traits ForceTpl
33 
34  template<typename _Scalar, int _Options>
35  class ForceTpl : public ForceDense< ForceTpl<_Scalar, _Options> >
36  {
37  public:
38  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39  typedef ForceDense<ForceTpl> Base;
40  FORCE_TYPEDEF_TPL(ForceTpl);
41  enum { Options = _Options };
42 
43  using Base::operator=;
44  using Base::operator!=;
45  using Base::linear;
46  using Base::angular;
47 
48  // Constructors
49  ForceTpl() : m_data() {}
50 
51  template<typename V1,typename V2>
52  ForceTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w)
53  {
54  assert(v.size() == 3);
55  assert(w.size() == 3);
56  linear() = v; angular() = w;
57  }
58 
59  template<typename V6>
60  explicit ForceTpl(const Eigen::MatrixBase<V6> & v)
61  : m_data(v)
62  {
63  EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
64  assert(v.size() == 6);
65  }
66 
67  ForceTpl(const ForceTpl & clone) // Copy constructor
68  : m_data(clone.toVector())
69  {}
70 
71  ForceTpl& operator=(const ForceTpl & clone) // Copy assignment operator
72  {
73  m_data = clone.toVector();
74  return *this;
75  }
76 
77  template<int O2>
78  explicit ForceTpl(const ForceTpl<Scalar,O2> & clone)
79  : m_data(clone.toVector())
80  {}
81 
82  template<typename M2>
83  explicit ForceTpl(const ForceDense<M2> & clone)
84  { linear() = clone.linear(); angular() = clone.angular(); }
85 
86  // initializers
87  static ForceTpl Zero() { return ForceTpl(Vector6::Zero()); }
88  static ForceTpl Random() { return ForceTpl(Vector6::Random()); }
89 
90  ToVectorConstReturnType toVector_impl() const { return m_data; }
91  ToVectorReturnType toVector_impl() { return m_data; }
92 
93  // Getters
94  ConstAngularType angular_impl() const { return m_data.template segment<3> (ANGULAR); }
95  ConstLinearType linear_impl() const { return m_data.template segment<3> (LINEAR); }
96  AngularType angular_impl() { return m_data.template segment<3> (ANGULAR); }
97  LinearType linear_impl() { return m_data.template segment<3> (LINEAR); }
98 
99  template<typename V3>
100  void angular_impl(const Eigen::MatrixBase<V3> & w)
101  {
102  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
103  angular_impl()=w;
104  }
105  template<typename V3>
106  void linear_impl(const Eigen::MatrixBase<V3> & v)
107  {
108  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
109  linear_impl()=v;
110  }
111 
112  ForceRef<Vector6> ref() { return ForceRef<Vector6>(m_data); }
113 
115  template<typename NewScalar>
117  {
118  typedef ForceTpl<NewScalar,Options> ReturnType;
119  ReturnType res(linear().template cast<NewScalar>(),
120  angular().template cast<NewScalar>());
121  return res;
122  }
123 
124  protected:
125  Vector6 m_data;
126 
127  }; // class ForceTpl
128 
129 } // namespace pinocchio
130 
131 #endif // ifndef __pinocchio_force_tpl_hpp__
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Definition: force-base.hpp:81
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35
ForceTpl< NewScalar, Options > cast() const
Definition: force-tpl.hpp:116
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:42