force-tpl.hpp
Go to the documentation of this file.
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
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>
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 
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__
Vector6::template ConstFixedSegmentReturnType< 3 >::Type ConstLinearType
Definition: force-tpl.hpp:22
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
static ForceTpl Random()
Definition: force-tpl.hpp:88
ConstLinearType linear_impl() const
Definition: force-tpl.hpp:95
virtual CollisionGeometry * clone() const=0
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Definition: force-base.hpp:81
Vector6::template FixedSegmentReturnType< 3 >::Type LinearType
Definition: force-tpl.hpp:20
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef ForceDense< ForceTpl > Base
Definition: force-tpl.hpp:39
LinearType linear_impl()
Definition: force-tpl.hpp:97
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
Eigen::Matrix< Scalar, 6, 1, _Options > Vector6
Definition: force-tpl.hpp:16
ForceTpl(const ForceDense< M2 > &clone)
Definition: force-tpl.hpp:83
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
ForceTpl(const ForceTpl &clone)
Definition: force-tpl.hpp:67
ConstAngularType angular_impl() const
Definition: force-tpl.hpp:94
static ForceTpl Zero()
Definition: force-tpl.hpp:87
Eigen::Matrix< Scalar, 6, 6, _Options > Matrix6
Definition: force-tpl.hpp:17
#define FORCE_TYPEDEF_TPL(Derived)
Vector6::template ConstFixedSegmentReturnType< 3 >::Type ConstAngularType
Definition: force-tpl.hpp:23
ToVectorReturnType toVector_impl()
Definition: force-tpl.hpp:91
ForceTpl< NewScalar, Options > cast() const
Definition: force-tpl.hpp:116
ForceRef< Vector6 > ref()
Definition: force-tpl.hpp:112
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:42
#define PINOCCHIO_EIGEN_REF_TYPE(D)
Main pinocchio namespace.
Definition: timings.cpp:28
ToVectorConstReturnType toVector_impl() const
Definition: force-tpl.hpp:90
void linear_impl(const Eigen::MatrixBase< V3 > &v)
Definition: force-tpl.hpp:106
res
ForceTpl & operator=(const ForceTpl &clone)
Definition: force-tpl.hpp:71
ForceTpl(const Eigen::MatrixBase< V1 > &v, const Eigen::MatrixBase< V2 > &w)
Definition: force-tpl.hpp:52
ForceTpl(const ForceTpl< Scalar, O2 > &clone)
Definition: force-tpl.hpp:78
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
Vector6::template FixedSegmentReturnType< 3 >::Type AngularType
Definition: force-tpl.hpp:21
AngularType angular_impl()
Definition: force-tpl.hpp:96
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
Definition: force-tpl.hpp:15
void angular_impl(const Eigen::MatrixBase< V3 > &w)
Definition: force-tpl.hpp:100
w
Definition: ur5x4.py:45
ForceTpl(const Eigen::MatrixBase< V6 > &v)
Definition: force-tpl.hpp:60


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:30