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 |