GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/force-base.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 46 46 100.0%
Branches: 0 0 -%
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 2981762 Derived & derived()
29 {
30 2981762 return *static_cast<Derived *>(this);
31 }
32 2493081 const Derived & derived() const
33 {
34 2546521 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 671905 ConstAngularType angular() const
48 {
49 671905 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 749805 ConstLinearType linear() const
58 {
59 749805 return derived().linear_impl();
60 }
61
62 /// \copydoc ForceBase::angular
63 1225561 AngularType angular()
64 {
65 1225561 return derived().angular_impl();
66 }
67
68 /// \copydoc ForceBase::linear
69 1230396 LinearType linear()
70 {
71 1230396 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 1 void angular(const Eigen::MatrixBase<V3Like> & n)
83 {
84 1 derived().angular_impl(n.derived());
85 1 }
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 1 void linear(const Eigen::MatrixBase<V3Like> & f)
96 {
97 1 derived().linear_impl(f.derived());
98 1 }
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 575482 ToVectorConstReturnType toVector() const
109 {
110 575482 return derived().toVector_impl();
111 }
112
113 /// \copydoc ForceBase::toVector
114 154451 ToVectorReturnType toVector()
115 {
116 154451 return derived().toVector_impl();
117 }
118
119 /*
120 * @brief C-style cast operator
121 * \copydoc ForceBase::toVector
122 */
123 4 operator Vector6() const
124 {
125 4 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 1591 bool operator==(const ForceBase<F2> & other) const
134 {
135 1591 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 1 bool operator!=(const ForceBase<F2> & other) const
142 {
143 1 return !(derived() == other.derived());
144 }
145
146 /** \returns true if *this is approximately equal to other, within the precision given by prec.
147 */
148 2472 bool isApprox(
149 const Derived & other,
150 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
151 {
152 2472 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 2 bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
159 {
160 2 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 153 ForcePlain operator*(const OtherScalar & alpha) const
200 {
201 153 return derived().__mult__(alpha);
202 }
203
204 /** \return an expression of *this divided by the factor alpha
205 */
206 template<typename OtherScalar>
207 1 ForcePlain operator/(const OtherScalar & alpha) const
208 {
209 1 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 49537 typename SE3GroupAction<Derived>::ReturnType se3Action(const SE3Tpl<S2, O2> & m) const
248 {
249 49537 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 187 typename SE3GroupAction<Derived>::ReturnType se3ActionInverse(const SE3Tpl<S2, O2> & m) const
266 {
267 187 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 9 void disp(std::ostream & os) const
278 {
279 9 derived().disp_impl(os);
280 9 }
281 9 friend std::ostream & operator<<(std::ostream & os, const ForceBase<Derived> & X)
282 {
283 9 X.disp(os);
284 9 return os;
285 }
286
287 }; // class ForceBase
288
289 } // namespace pinocchio
290
291 #endif // ifndef __pinocchio_spatial_force_base_hpp__
292