Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2020 CNRS INRIA |
3 |
|
|
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
4 |
|
|
// |
5 |
|
|
|
6 |
|
|
#ifndef __pinocchio_spatial_force_base_hpp__ |
7 |
|
|
#define __pinocchio_spatial_force_base_hpp__ |
8 |
|
|
|
9 |
|
|
namespace pinocchio |
10 |
|
|
{ |
11 |
|
|
/** |
12 |
|
|
* @brief Base interface for forces representation. |
13 |
|
|
* @details The Class implements all |
14 |
|
|
* \ingroup pinocchio_spatial |
15 |
|
|
* |
16 |
|
|
* This class hierarchy represents a spatial force, e.g. a spatial impulse or force associated to |
17 |
|
|
* a body. The spatial force is the mathematical representation of \f$ se^{*}(3) \f$, the dual of |
18 |
|
|
* \f$ se(3) \f$. |
19 |
|
|
* |
20 |
|
|
* @tparam Derived { description } |
21 |
|
|
*/ |
22 |
|
|
template<class Derived> |
23 |
|
|
class ForceBase : NumericalBase<Derived> |
24 |
|
|
{ |
25 |
|
|
public: |
26 |
|
|
FORCE_TYPEDEF_TPL(Derived); |
27 |
|
|
|
28 |
|
2239305 |
Derived & derived() |
29 |
|
|
{ |
30 |
|
2239305 |
return *static_cast<Derived *>(this); |
31 |
|
|
} |
32 |
|
644701 |
const Derived & derived() const |
33 |
|
|
{ |
34 |
|
1262343 |
return *static_cast<const Derived *>(this); |
35 |
|
|
} |
36 |
|
|
|
37 |
|
|
Derived & const_cast_derived() const |
38 |
|
|
{ |
39 |
|
|
return *const_cast<Derived *>(&derived()); |
40 |
|
|
} |
41 |
|
|
|
42 |
|
|
/** |
43 |
|
|
* @brief Return the angular part of the force vector |
44 |
|
|
* |
45 |
|
|
* @return The 3D vector associated to the angular part of the 6D force vector |
46 |
|
|
*/ |
47 |
|
390356 |
ConstAngularType angular() const |
48 |
|
|
{ |
49 |
|
390356 |
return derived().angular_impl(); |
50 |
|
|
} |
51 |
|
|
|
52 |
|
|
/** |
53 |
|
|
* @brief Return the linear part of the force vector |
54 |
|
|
* |
55 |
|
|
* @return The 3D vector associated to the linear part of the 6D force vector |
56 |
|
|
*/ |
57 |
|
475247 |
ConstLinearType linear() const |
58 |
|
|
{ |
59 |
|
475247 |
return derived().linear_impl(); |
60 |
|
|
} |
61 |
|
|
|
62 |
|
|
/// \copydoc ForceBase::angular |
63 |
|
946720 |
AngularType angular() |
64 |
|
|
{ |
65 |
|
946720 |
return derived().angular_impl(); |
66 |
|
|
} |
67 |
|
|
|
68 |
|
|
/// \copydoc ForceBase::linear |
69 |
|
943174 |
LinearType linear() |
70 |
|
|
{ |
71 |
|
943174 |
return derived().linear_impl(); |
72 |
|
|
} |
73 |
|
|
|
74 |
|
|
/** |
75 |
|
|
* @brief Set the angular part of the force vector |
76 |
|
|
* |
77 |
|
|
* @tparam V3Like A vector 3 like type. |
78 |
|
|
* |
79 |
|
|
* @param[in] n |
80 |
|
|
*/ |
81 |
|
|
template<typename V3Like> |
82 |
|
✗ |
void angular(const Eigen::MatrixBase<V3Like> & n) |
83 |
|
|
{ |
84 |
|
✗ |
derived().angular_impl(n.derived()); |
85 |
|
|
} |
86 |
|
|
|
87 |
|
|
/** |
88 |
|
|
* @brief Set the linear part of the force vector |
89 |
|
|
* |
90 |
|
|
* @tparam V3Like A vector 3 like type. |
91 |
|
|
* |
92 |
|
|
* @param[in] f |
93 |
|
|
*/ |
94 |
|
|
template<typename V3Like> |
95 |
|
✗ |
void linear(const Eigen::MatrixBase<V3Like> & f) |
96 |
|
|
{ |
97 |
|
✗ |
derived().linear_impl(f.derived()); |
98 |
|
|
} |
99 |
|
|
|
100 |
|
|
/** |
101 |
|
|
* @brief Return the force as an Eigen vector. |
102 |
|
|
* |
103 |
|
|
* @return The 6D vector \f$ \phi \f$ such that |
104 |
|
|
* \f{equation*} |
105 |
|
|
* {}^{A}\phi = \begin{bmatrix} {}^{A}f \\ {}^{A}\tau \end{bmatrix} |
106 |
|
|
* \f} |
107 |
|
|
*/ |
108 |
|
136516 |
ToVectorConstReturnType toVector() const |
109 |
|
|
{ |
110 |
|
136516 |
return derived().toVector_impl(); |
111 |
|
|
} |
112 |
|
|
|
113 |
|
|
/// \copydoc ForceBase::toVector |
114 |
|
40990 |
ToVectorReturnType toVector() |
115 |
|
|
{ |
116 |
|
40990 |
return derived().toVector_impl(); |
117 |
|
|
} |
118 |
|
|
|
119 |
|
|
/* |
120 |
|
|
* @brief C-style cast operator |
121 |
|
|
* \copydoc ForceBase::toVector |
122 |
|
|
*/ |
123 |
|
✗ |
operator Vector6() const |
124 |
|
|
{ |
125 |
|
✗ |
return toVector(); |
126 |
|
|
} |
127 |
|
|
|
128 |
|
|
/** \returns true if each coefficients of \c *this and \a other are all exactly equal. |
129 |
|
|
* \warning When using floating point scalar values you probably should rather use a |
130 |
|
|
* fuzzy comparison such as isApprox() |
131 |
|
|
*/ |
132 |
|
|
template<typename F2> |
133 |
|
✗ |
bool operator==(const ForceBase<F2> & other) const |
134 |
|
|
{ |
135 |
|
✗ |
return derived().isEqual_impl(other.derived()); |
136 |
|
|
} |
137 |
|
|
|
138 |
|
|
/** \returns true if at least one coefficient of \c *this and \a other does not match. |
139 |
|
|
*/ |
140 |
|
|
template<typename F2> |
141 |
|
✗ |
bool operator!=(const ForceBase<F2> & other) const |
142 |
|
|
{ |
143 |
|
✗ |
return !(derived() == other.derived()); |
144 |
|
|
} |
145 |
|
|
|
146 |
|
|
/** \returns true if *this is approximately equal to other, within the precision given by prec. |
147 |
|
|
*/ |
148 |
|
6 |
bool isApprox( |
149 |
|
|
const Derived & other, |
150 |
|
|
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
151 |
|
|
{ |
152 |
|
6 |
return derived().isApprox_impl(other, prec); |
153 |
|
|
} |
154 |
|
|
|
155 |
|
|
/** \returns true if the component of the linear and angular part of the Spatial Force are |
156 |
|
|
* approximately equal to zero, within the precision given by prec. |
157 |
|
|
*/ |
158 |
|
✗ |
bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
159 |
|
|
{ |
160 |
|
✗ |
return derived().isZero_impl(prec); |
161 |
|
|
} |
162 |
|
|
|
163 |
|
|
/** \brief Copies the Derived Force into *this |
164 |
|
|
* \return a reference to *this |
165 |
|
|
*/ |
166 |
|
|
Derived & operator=(const ForceBase<Derived> & other) |
167 |
|
|
{ |
168 |
|
|
return derived().setFrom(other.derived()); |
169 |
|
|
} |
170 |
|
|
|
171 |
|
|
/** |
172 |
|
|
* \brief Replaces *this by *this + other. |
173 |
|
|
* \return a reference to *this |
174 |
|
|
*/ |
175 |
|
|
Derived & operator+=(const ForceBase<Derived> & phi) |
176 |
|
|
{ |
177 |
|
|
return derived().__pequ__(phi.derived()); |
178 |
|
|
} |
179 |
|
|
|
180 |
|
|
/** |
181 |
|
|
* \brief Replaces *this by *this - other. |
182 |
|
|
* \return a reference to *this |
183 |
|
|
*/ |
184 |
|
|
Derived & operator-=(const ForceBase<Derived> & phi) |
185 |
|
|
{ |
186 |
|
|
return derived().__mequ__(phi.derived()); |
187 |
|
|
} |
188 |
|
|
|
189 |
|
|
/** \return an expression of the sum of *this and other |
190 |
|
|
*/ |
191 |
|
|
Derived operator+(const ForceBase<Derived> & phi) const |
192 |
|
|
{ |
193 |
|
|
return derived().__plus__(phi.derived()); |
194 |
|
|
} |
195 |
|
|
|
196 |
|
|
/** \return an expression of *this scaled by the factor alpha |
197 |
|
|
*/ |
198 |
|
|
template<typename OtherScalar> |
199 |
|
118 |
ForcePlain operator*(const OtherScalar & alpha) const |
200 |
|
|
{ |
201 |
|
118 |
return derived().__mult__(alpha); |
202 |
|
|
} |
203 |
|
|
|
204 |
|
|
/** \return an expression of *this divided by the factor alpha |
205 |
|
|
*/ |
206 |
|
|
template<typename OtherScalar> |
207 |
|
✗ |
ForcePlain operator/(const OtherScalar & alpha) const |
208 |
|
|
{ |
209 |
|
✗ |
return derived().__div__(alpha); |
210 |
|
|
} |
211 |
|
|
|
212 |
|
|
/** \return an expression of the opposite of *this |
213 |
|
|
*/ |
214 |
|
|
Derived operator-() const |
215 |
|
|
{ |
216 |
|
|
return derived().__opposite__(); |
217 |
|
|
} |
218 |
|
|
|
219 |
|
|
/** \return an expression of the difference of *this and phi |
220 |
|
|
*/ |
221 |
|
|
Derived operator-(const ForceBase<Derived> & phi) const |
222 |
|
|
{ |
223 |
|
|
return derived().__minus__(phi.derived()); |
224 |
|
|
} |
225 |
|
|
|
226 |
|
|
/** \return the dot product of *this with m * |
227 |
|
|
*/ |
228 |
|
|
template<typename MotionDerived> |
229 |
|
|
Scalar dot(const MotionDense<MotionDerived> & m) const |
230 |
|
|
{ |
231 |
|
|
return derived().dot(m.derived()); |
232 |
|
|
} |
233 |
|
|
|
234 |
|
|
/** |
235 |
|
|
* @brief Transform from A to B coordinates the Force represented by *this such that |
236 |
|
|
* \f{equation*} |
237 |
|
|
* {}^{B}f = {}^{B}X_A^* * {}^{A}f |
238 |
|
|
* \f} |
239 |
|
|
* |
240 |
|
|
* @param[in] m The rigid transformation \f$ {}^{B}m_A \f$ whose coordinates transform for |
241 |
|
|
* forces is |
242 |
|
|
* {}^{B}X_A^* |
243 |
|
|
* |
244 |
|
|
* @return an expression of the force expressed in the new coordinates |
245 |
|
|
*/ |
246 |
|
|
template<typename S2, int O2> |
247 |
|
13844 |
typename SE3GroupAction<Derived>::ReturnType se3Action(const SE3Tpl<S2, O2> & m) const |
248 |
|
|
{ |
249 |
|
13844 |
return derived().se3Action_impl(m); |
250 |
|
|
} |
251 |
|
|
|
252 |
|
|
/** |
253 |
|
|
* @brief Transform from B to A coordinates the Force represented by *this such that |
254 |
|
|
* \f{equation*} |
255 |
|
|
* {}^{A}f = {}^{A}X_B^* * {}^{A}f |
256 |
|
|
* \f} |
257 |
|
|
* |
258 |
|
|
* @param[in] m The rigid transformation \f$ {}^{B}m_A \f$ whose coordinates transform for |
259 |
|
|
* forces is |
260 |
|
|
* {}^{B}X_A^* |
261 |
|
|
* |
262 |
|
|
* @return an expression of the force expressed in the new coordinates |
263 |
|
|
*/ |
264 |
|
|
template<typename S2, int O2> |
265 |
|
13 |
typename SE3GroupAction<Derived>::ReturnType se3ActionInverse(const SE3Tpl<S2, O2> & m) const |
266 |
|
|
{ |
267 |
|
13 |
return derived().se3ActionInverse_impl(m); |
268 |
|
|
} |
269 |
|
|
|
270 |
|
|
template<typename M1> |
271 |
|
|
typename MotionAlgebraAction<Derived, M1>::ReturnType |
272 |
|
|
motionAction(const MotionDense<M1> & v) const |
273 |
|
|
{ |
274 |
|
|
return derived().motionAction(v.derived()); |
275 |
|
|
} |
276 |
|
|
|
277 |
|
✗ |
void disp(std::ostream & os) const |
278 |
|
|
{ |
279 |
|
✗ |
derived().disp_impl(os); |
280 |
|
|
} |
281 |
|
✗ |
friend std::ostream & operator<<(std::ostream & os, const ForceBase<Derived> & X) |
282 |
|
|
{ |
283 |
|
✗ |
X.disp(os); |
284 |
|
✗ |
return os; |
285 |
|
|
} |
286 |
|
|
|
287 |
|
|
}; // class ForceBase |
288 |
|
|
|
289 |
|
|
} // namespace pinocchio |
290 |
|
|
|
291 |
|
|
#endif // ifndef __pinocchio_spatial_force_base_hpp__ |
292 |
|
|
|