GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/force-dense.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 63 91 69.2%
Branches: 80 238 33.6%
Line Branch Exec Source
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
8 namespace 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>
18 struct MotionAlgebraAction<ForceDense<Derived>, 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:
27 typedef ForceBase<Derived> Base;
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 2859 Derived & setZero()
39 {
40
1/2
✓ Branch 2 taken 2859 times.
✗ Branch 3 not taken.
 2859 linear().setZero();
41
1/2
✓ Branch 2 taken 2859 times.
✗ Branch 3 not taken.
 2859 angular().setZero();
42 2859 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 {
86 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
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 1188 ForcePlain operator-() const
94 {
95 1188 return derived().__opposite__();
96 }
97 template<typename F1>
98 50171 ForcePlain operator+(const ForceDense<F1> & f) const
99 {
100 50171 return derived().__plus__(f.derived());
101 }
102 template<typename F1>
103 189 ForcePlain operator-(const ForceDense<F1> & f) const
104 {
105 189 return derived().__minus__(f.derived());
106 }
107
108 template<typename F1>
109 137096 Derived & operator+=(const ForceDense<F1> & f)
110 {
111 137096 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 3963 Derived & operator-=(const ForceDense<M1> & v)
122 {
123 3963 return derived().__mequ__(v.derived());
124 }
125
126 1188 ForcePlain __opposite__() const
127 {
128
4/8
✓ Branch 2 taken 1188 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1188 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1188 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1188 times.
✗ Branch 12 not taken.
 1188 return ForcePlain(-linear(), -angular());
129 }
130
131 template<typename M1>
132 50171 ForcePlain __plus__(const ForceDense<M1> & v) const
133 {
134
6/12
✓ Branch 2 taken 50171 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50171 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50171 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50171 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50171 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 50171 times.
✗ Branch 18 not taken.
 50171 return ForcePlain(linear() + v.linear(), angular() + v.angular());
135 }
136
137 template<typename M1>
138 189 ForcePlain __minus__(const ForceDense<M1> & v) const
139 {
140
6/12
✓ Branch 2 taken 189 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 189 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 189 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 189 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 189 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 189 times.
✗ Branch 18 not taken.
 189 return ForcePlain(linear() - v.linear(), angular() - v.angular());
141 }
142
143 template<typename M1>
144 137096 Derived & __pequ__(const ForceDense<M1> & v)
145 {
146
2/4
✓ Branch 2 taken 109195 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 109195 times.
✗ Branch 6 not taken.
 137096 linear() += v.linear();
147
2/4
✓ Branch 2 taken 109195 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 109195 times.
✗ Branch 6 not taken.
 137096 angular() += v.angular();
148 137096 return derived();
149 }
150
151 template<typename M1>
152 3963 Derived & __mequ__(const ForceDense<M1> & v)
153 {
154
2/4
✓ Branch 2 taken 3963 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3963 times.
✗ Branch 6 not taken.
 3963 linear() -= v.linear();
155
2/4
✓ Branch 2 taken 3963 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3963 times.
✗ Branch 6 not taken.
 3963 angular() -= v.angular();
156 3963 return derived();
157 }
158
159 template<typename OtherScalar>
160 118 ForcePlain __mult__(const OtherScalar & alpha) const
161 {
162
4/8
✓ Branch 2 taken 118 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 118 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 118 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 118 times.
✗ Branch 12 not taken.
 118 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 81861 void motionAction(const MotionDense<M1> & v, ForceDense<M2> & fout) const
179 {
180
5/10
✓ Branch 2 taken 72643 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 72643 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 72643 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 72643 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 72643 times.
✗ Branch 15 not taken.
 81861 fout.linear().noalias() = v.angular().cross(linear());
181
9/18
✓ Branch 2 taken 72643 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 72643 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 72643 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 72643 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 72643 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 72643 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 72643 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 72643 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 72643 times.
✗ Branch 27 not taken.
 81861 fout.angular().noalias() = v.angular().cross(angular()) + v.linear().cross(linear());
182 81861 }
183
184 template<typename M1>
185 81535 ForcePlain motionAction(const MotionDense<M1> & v) const
186 {
187 81535 ForcePlain res;
188 81535 motionAction(v, res);
189 81535 return res;
190 }
191
192 template<typename M2>
193 14 bool isApprox(
194 const ForceDense<M2> & f,
195 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
196 {
197 14 return derived().isApprox_impl(f, prec);
198 }
199
200 template<typename D2>
201 35 bool isApprox_impl(
202 const ForceDense<D2> & f,
203 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
204 {
205
8/16
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 20 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 20 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 20 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 20 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 20 times.
✗ Branch 21 not taken.
 35 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 13915 void se3Action_impl(const SE3Tpl<S2, O2> & m, ForceDense<D2> & f) const
215 {
216
5/10
✓ Branch 2 taken 13844 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13844 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13844 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 13844 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 13844 times.
✗ Branch 15 not taken.
 13915 f.linear().noalias() = m.rotation() * linear();
217
5/10
✓ Branch 2 taken 13844 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13844 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13844 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 13844 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 13844 times.
✗ Branch 15 not taken.
 13915 f.angular().noalias() = m.rotation() * angular();
218
3/6
✓ Branch 3 taken 13844 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13844 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13844 times.
✗ Branch 10 not taken.
 13915 f.angular() += m.translation().cross(f.linear());
219 13915 }
220
221 template<typename S2, int O2>
222 13915 ForcePlain se3Action_impl(const SE3Tpl<S2, O2> & m) const
223 {
224 13915 ForcePlain res;
225 13915 se3Action_impl(m, res);
226 13915 return res;
227 }
228
229 template<typename S2, int O2, typename D2>
230 13 void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, ForceDense<D2> & f) const
231 {
232
6/12
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 13 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 13 times.
✗ Branch 18 not taken.
 13 f.linear().noalias() = m.rotation().transpose() * linear();
233
6/12
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
 39 f.angular().noalias() =
234
3/6
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
 39 m.rotation().transpose() * (angular() - m.translation().cross(linear()));
235 13 }
236
237 template<typename S2, int O2>
238 13 ForcePlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
239 {
240 13 ForcePlain res;
241 13 se3ActionInverse_impl(m, res);
242 13 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
251 /// \returns a ForceRef on this.
252 ForceRefType ref()
253 {
254 return derived().ref();
255 }
256
257 protected:
258 613149 ForceDense() {};
259
260 ForceDense(const ForceDense &) = delete;
261
262 }; // class ForceDense
263
264 /// Basic operations specialization
265 template<typename F1>
266 typename traits<F1>::ForcePlain
267 118 operator*(const typename traits<F1>::Scalar alpha, const ForceDense<F1> & f)
268 {
269 118 return f.derived() * alpha;
270 }
271
272 } // namespace pinocchio
273
274 #endif // ifndef __pinocchio_spatial_force_dense_hpp__
275