pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
force-dense.hpp
1//
2// Copyright (c) 2017-2020 CNRS INRIA
3//
4
5#ifndef __pinocchio_spatial_force_dense_hpp__
6#define __pinocchio_spatial_force_dense_hpp__
7
8namespace pinocchio
9{
10
11 template<typename Derived>
12 struct SE3GroupAction<ForceDense<Derived>>
13 {
14 typedef typename SE3GroupAction<Derived>::ReturnType ReturnType;
15 };
16
17 template<typename Derived, typename MotionDerived>
19 {
20 typedef typename MotionAlgebraAction<Derived, MotionDerived>::ReturnType ReturnType;
21 };
22
23 template<typename Derived>
24 class ForceDense : public ForceBase<Derived>
25 {
26 public:
28 FORCE_TYPEDEF_TPL(Derived);
29 typedef typename traits<Derived>::ForceRefType ForceRefType;
30
31 using Base::angular;
32 using Base::derived;
33 using Base::isApprox;
34 using Base::isZero;
35 using Base::linear;
36 using Base::operator=;
37
38 Derived & setZero()
39 {
40 linear().setZero();
41 angular().setZero();
42 return derived();
43 }
44 Derived & setRandom()
45 {
46 linear().setRandom();
47 angular().setRandom();
48 return derived();
49 }
50
51 template<typename D2>
52 bool isEqual_impl(const ForceDense<D2> & other) const
53 {
54 return linear() == other.linear() && angular() == other.angular();
55 }
56
57 template<typename D2>
58 bool isEqual_impl(const ForceBase<D2> & other) const
59 {
60 return other.derived() == derived();
61 }
62
63 // Arithmetic operators
64 template<typename D2>
65 Derived & operator=(const ForceDense<D2> & other)
66 {
67 return derived().set(other.derived());
68 }
69
70 Derived & operator=(const ForceDense & other)
71 {
72 return derived().set(other.derived());
73 }
74
75 template<typename D2>
76 Derived & set(const ForceDense<D2> & other)
77 {
78 linear() = other.linear();
79 angular() = other.angular();
80 return derived();
81 }
82
83 template<typename V6>
84 Derived & operator=(const Eigen::MatrixBase<V6> & v)
85 {
87 assert(v.size() == 6);
88 linear() = v.template segment<3>(LINEAR);
89 angular() = v.template segment<3>(ANGULAR);
90 return derived();
91 }
92
93 ForcePlain operator-() const
94 {
95 return derived().__opposite__();
96 }
97 template<typename F1>
98 ForcePlain operator+(const ForceDense<F1> & f) const
99 {
100 return derived().__plus__(f.derived());
101 }
102 template<typename F1>
103 ForcePlain operator-(const ForceDense<F1> & f) const
104 {
105 return derived().__minus__(f.derived());
106 }
107
108 template<typename F1>
109 Derived & operator+=(const ForceDense<F1> & f)
110 {
111 return derived().__pequ__(f.derived());
112 }
113 template<typename F1>
114 Derived & operator+=(const ForceBase<F1> & f)
115 {
116 f.derived().addTo(derived());
117 return derived();
118 }
119
120 template<typename M1>
121 Derived & operator-=(const ForceDense<M1> & v)
122 {
123 return derived().__mequ__(v.derived());
124 }
125
126 ForcePlain __opposite__() const
127 {
128 return ForcePlain(-linear(), -angular());
129 }
130
131 template<typename M1>
132 ForcePlain __plus__(const ForceDense<M1> & v) const
133 {
134 return ForcePlain(linear() + v.linear(), angular() + v.angular());
135 }
136
137 template<typename M1>
138 ForcePlain __minus__(const ForceDense<M1> & v) const
139 {
140 return ForcePlain(linear() - v.linear(), angular() - v.angular());
141 }
142
143 template<typename M1>
144 Derived & __pequ__(const ForceDense<M1> & v)
145 {
146 linear() += v.linear();
147 angular() += v.angular();
148 return derived();
149 }
150
151 template<typename M1>
152 Derived & __mequ__(const ForceDense<M1> & v)
153 {
154 linear() -= v.linear();
155 angular() -= v.angular();
156 return derived();
157 }
158
159 template<typename OtherScalar>
160 ForcePlain __mult__(const OtherScalar & alpha) const
161 {
162 return ForcePlain(alpha * linear(), alpha * angular());
163 }
164
165 template<typename OtherScalar>
166 ForcePlain __div__(const OtherScalar & alpha) const
167 {
168 return derived().__mult__((OtherScalar)(1) / alpha);
169 }
170
171 template<typename F1>
172 Scalar dot(const MotionDense<F1> & phi) const
173 {
174 return phi.linear().dot(linear()) + phi.angular().dot(angular());
175 }
176
177 template<typename M1, typename M2>
178 void motionAction(const MotionDense<M1> & v, ForceDense<M2> & fout) const
179 {
180 fout.linear().noalias() = v.angular().cross(linear());
181 fout.angular().noalias() = v.angular().cross(angular()) + v.linear().cross(linear());
182 }
183
184 template<typename M1>
185 ForcePlain motionAction(const MotionDense<M1> & v) const
186 {
187 ForcePlain res;
188 motionAction(v, res);
189 return res;
190 }
191
192 template<typename M2>
193 bool isApprox(
194 const ForceDense<M2> & f,
195 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
196 {
197 return derived().isApprox_impl(f, prec);
198 }
199
200 template<typename D2>
201 bool isApprox_impl(
202 const ForceDense<D2> & f,
203 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
204 {
205 return linear().isApprox(f.linear(), prec) && angular().isApprox(f.angular(), prec);
206 }
207
208 bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
209 {
210 return linear().isZero(prec) && angular().isZero(prec);
211 }
212
213 template<typename S2, int O2, typename D2>
214 void se3Action_impl(const SE3Tpl<S2, O2> & m, ForceDense<D2> & f) const
215 {
216 f.linear().noalias() = m.rotation() * linear();
217 f.angular().noalias() = m.rotation() * angular();
218 f.angular() += m.translation().cross(f.linear());
219 }
220
221 template<typename S2, int O2>
222 ForcePlain se3Action_impl(const SE3Tpl<S2, O2> & m) const
223 {
224 ForcePlain res;
225 se3Action_impl(m, res);
226 return res;
227 }
228
229 template<typename S2, int O2, typename D2>
230 void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, ForceDense<D2> & f) const
231 {
232 f.linear().noalias() = m.rotation().transpose() * linear();
233 f.angular().noalias() =
234 m.rotation().transpose() * (angular() - m.translation().cross(linear()));
235 }
236
237 template<typename S2, int O2>
238 ForcePlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
239 {
240 ForcePlain res;
241 se3ActionInverse_impl(m, res);
242 return res;
243 }
244
245 void disp_impl(std::ostream & os) const
246 {
247 os << " f = " << linear().transpose() << std::endl
248 << "tau = " << angular().transpose() << std::endl;
249 }
250
252 ForceRefType ref()
253 {
254 return derived().ref();
255 }
256
257 protected:
258 ForceDense() {};
259
260 ForceDense(const ForceDense &) = delete;
261
262 }; // class ForceDense
263
265 template<typename F1>
266 typename traits<F1>::ForcePlain
267 operator*(const typename traits<F1>::Scalar alpha, const ForceDense<F1> & f)
268 {
269 return f.derived() * alpha;
270 }
271
272} // namespace pinocchio
273
274#endif // ifndef __pinocchio_spatial_force_dense_hpp__
Base interface for forces representation.
bool isApprox(const Derived &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
ConstAngularType angular() const
Return the angular part of the force vector.
ConstLinearType linear() const
Return the linear part of the force vector.
bool isZero(const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
ConstAngularType angular() const
Return the angular part of the force vector.
ConstLinearType linear() const
Return the linear part of the force vector.
Main pinocchio namespace.
Definition treeview.dox:11
Return type of the ation of a Motion onto an object of type D.
Definition motion.hpp:46
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72