pinocchio  2.3.1-dirty
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  template<int O2>
68  explicit ForceTpl(const ForceTpl<Scalar,O2> & clone)
69  : m_data(clone.toVector())
70  {}
71 
72  template<typename M2>
73  explicit ForceTpl(const ForceDense<M2> & clone)
74  { linear() = clone.linear(); angular() = clone.angular(); }
75 
76  // initializers
77  static ForceTpl Zero() { return ForceTpl(Vector6::Zero()); }
78  static ForceTpl Random() { return ForceTpl(Vector6::Random()); }
79 
80  ToVectorConstReturnType toVector_impl() const { return m_data; }
81  ToVectorReturnType toVector_impl() { return m_data; }
82 
83  // Getters
84  ConstAngularType angular_impl() const { return m_data.template segment<3> (ANGULAR); }
85  ConstLinearType linear_impl() const { return m_data.template segment<3> (LINEAR); }
86  AngularType angular_impl() { return m_data.template segment<3> (ANGULAR); }
87  LinearType linear_impl() { return m_data.template segment<3> (LINEAR); }
88 
89  template<typename V3>
90  void angular_impl(const Eigen::MatrixBase<V3> & w)
91  {
92  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
93  angular_impl()=w;
94  }
95  template<typename V3>
96  void linear_impl(const Eigen::MatrixBase<V3> & v)
97  {
98  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
99  linear_impl()=v;
100  }
101 
102  ForceRef<Vector6> ref() { return ForceRef<Vector6>(m_data); }
103 
105  template<typename NewScalar>
107  {
108  typedef ForceTpl<NewScalar,Options> ReturnType;
109  ReturnType res(linear().template cast<NewScalar>(),
110  angular().template cast<NewScalar>());
111  return res;
112  }
113 
114  protected:
115  Vector6 m_data;
116 
117  }; // class ForceTpl
118 
119 } // namespace pinocchio
120 
121 #endif // ifndef __pinocchio_force_tpl_hpp__
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:40
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Definition: force-base.hpp:86
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:47
ForceTpl< NewScalar, Options > cast() const
Definition: force-tpl.hpp:106