pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
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_spatial_force_tpl_hpp__
7#define __pinocchio_spatial_force_tpl_hpp__
8
9namespace pinocchio
10{
11 template<typename _Scalar, int _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 {
27 LINEAR = 0,
28 ANGULAR = 3,
29 Options = _Options
30 };
31
33 }; // traits ForceTpl
34
35 template<typename _Scalar, int _Options>
36 class ForceTpl : public ForceDense<ForceTpl<_Scalar, _Options>>
37 {
38 public:
41 FORCE_TYPEDEF_TPL(ForceTpl);
42 enum
43 {
44 Options = _Options
45 };
46
47 using Base::operator=;
48 using Base::operator!=;
49 using Base::angular;
50 using Base::linear;
51
52 // Constructors
53 ForceTpl()
54 {
55 }
56
57 template<typename V1, typename V2>
58 ForceTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w)
59 {
62 linear() = v;
63 angular() = w;
64 }
65
66 template<typename V6>
67 explicit ForceTpl(const Eigen::MatrixBase<V6> & v)
68 : m_data(v)
69 {
71 }
72
73 ForceTpl(const ForceTpl & clone) // Copy constructor
74 : m_data(clone.toVector())
75 {
76 }
77
78 template<typename S2, int O2>
79 explicit ForceTpl(const ForceTpl<S2, O2> & other)
80 {
81 *this = other.template cast<Scalar>();
82 }
83
84 template<typename F2>
85 explicit ForceTpl(const ForceBase<F2> & clone)
86 {
87 *this = clone;
88 }
89
90 ForceTpl & operator=(const ForceTpl & clone) // Copy assignment operator
91 {
92 m_data = clone.toVector();
93 return *this;
94 }
95
96 template<int O2>
97 explicit ForceTpl(const ForceTpl<Scalar, O2> & clone)
98 : m_data(clone.toVector())
99 {
100 }
101
102 template<typename M2>
103 explicit ForceTpl(const ForceDense<M2> & clone)
104 {
105 linear() = clone.linear();
106 angular() = clone.angular();
107 }
108
109 // initializers
110 static ForceTpl Zero()
111 {
112 return ForceTpl(Vector6::Zero());
113 }
114 static ForceTpl Random()
115 {
116 return ForceTpl(Vector6::Random());
117 }
118
119 ToVectorConstReturnType toVector_impl() const
120 {
121 return m_data;
122 }
123 ToVectorReturnType toVector_impl()
124 {
125 return m_data;
126 }
127
128 // Getters
129 ConstAngularType angular_impl() const
130 {
131 return m_data.template segment<3>(ANGULAR);
132 }
133 ConstLinearType linear_impl() const
134 {
135 return m_data.template segment<3>(LINEAR);
136 }
137 AngularType angular_impl()
138 {
139 return m_data.template segment<3>(ANGULAR);
140 }
141 LinearType linear_impl()
142 {
143 return m_data.template segment<3>(LINEAR);
144 }
145
146 template<typename V3>
147 void angular_impl(const Eigen::MatrixBase<V3> & w)
148 {
150 angular_impl() = w;
151 }
152 template<typename V3>
153 void linear_impl(const Eigen::MatrixBase<V3> & v)
154 {
156 linear_impl() = v;
157 }
158
160 {
161 return ForceRef<Vector6>(m_data);
162 }
163
165 template<typename NewScalar>
167 {
168 typedef ForceTpl<NewScalar, Options> ReturnType;
169 ReturnType res(linear().template cast<NewScalar>(), angular().template cast<NewScalar>());
170 return res;
171 }
172
173 protected:
174 Vector6 m_data;
175
176 }; // class ForceTpl
177
178} // namespace pinocchio
179
180#endif // ifndef __pinocchio_spatial_force_tpl_hpp__
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
ConstAngularType angular() const
Return the angular part of the force vector.
ConstLinearType linear() const
Return the linear part of the force vector.
ConstAngularType angular() const
Return the angular part of the force vector.
ConstLinearType linear() const
Return the linear part of the force vector.
ForceTpl< NewScalar, Options > cast() const
Main pinocchio namespace.
Definition treeview.dox:11
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72