| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2020-2021, University of Edinburgh, University of Oxford |
||
5 |
// Copyright note valid unless otherwise stated in individual files. |
||
6 |
// All rights reserved. |
||
7 |
/////////////////////////////////////////////////////////////////////////////// |
||
8 |
|||
9 |
#include <math.h> |
||
10 |
|||
11 |
#include <iostream> |
||
12 |
|||
13 |
namespace crocoddyl { |
||
14 |
|||
15 |
template <typename Scalar> |
||
16 |
3 |
WrenchConeTpl<Scalar>::WrenchConeTpl() |
|
17 |
: nf_(4), |
||
18 |
A_(nf_ + 13, 6), |
||
19 |
ub_(nf_ + 13), |
||
20 |
lb_(nf_ + 13), |
||
21 |
R_(Matrix3s::Identity()), |
||
22 |
box_(std::numeric_limits<Scalar>::infinity(), |
||
23 |
3 |
std::numeric_limits<Scalar>::infinity()), |
|
24 |
mu_(Scalar(0.7)), |
||
25 |
inner_appr_(true), |
||
26 |
min_nforce_(Scalar(0.)), |
||
27 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
6 |
max_nforce_(std::numeric_limits<Scalar>::infinity()) { |
28 |
✓✗ | 3 |
A_.setZero(); |
29 |
✓✗ | 3 |
ub_.setZero(); |
30 |
✓✗ | 3 |
lb_.setZero(); |
31 |
|||
32 |
// Update the inequality matrix and bounds |
||
33 |
✓✗ | 3 |
update(); |
34 |
3 |
} |
|
35 |
|||
36 |
template <typename Scalar> |
||
37 |
868 |
WrenchConeTpl<Scalar>::WrenchConeTpl(const Matrix3s& R, const Scalar mu, |
|
38 |
const Vector2s& box, const std::size_t nf, |
||
39 |
const bool inner_appr, |
||
40 |
const Scalar min_nforce, |
||
41 |
const Scalar max_nforce) |
||
42 |
: nf_(nf), |
||
43 |
R_(R), |
||
44 |
box_(box), |
||
45 |
mu_(mu), |
||
46 |
inner_appr_(inner_appr), |
||
47 |
min_nforce_(min_nforce), |
||
48 |
✓✗✓✗ ✓✗✓✗ |
868 |
max_nforce_(max_nforce) { |
49 |
✗✓ | 868 |
if (nf_ % 2 != 0) { |
50 |
nf_ = 4; |
||
51 |
std::cerr << "Warning: nf has to be an even number, set to 4" << std::endl; |
||
52 |
} |
||
53 |
✗✓ | 868 |
if (mu < Scalar(0.)) { |
54 |
mu_ = Scalar(1.); |
||
55 |
std::cerr << "Warning: mu has to be a positive value, set to 1." |
||
56 |
<< std::endl; |
||
57 |
} |
||
58 |
✗✓ | 868 |
if (min_nforce < Scalar(0.)) { |
59 |
min_nforce_ = Scalar(0.); |
||
60 |
std::cerr << "Warning: min_nforce has to be a positive value, set to 0" |
||
61 |
<< std::endl; |
||
62 |
} |
||
63 |
✗✓ | 868 |
if (max_nforce < Scalar(0.)) { |
64 |
max_nforce_ = std::numeric_limits<Scalar>::infinity(); |
||
65 |
std::cerr << "Warning: max_nforce has to be a positive value, set to " |
||
66 |
"infinity value" |
||
67 |
<< std::endl; |
||
68 |
} |
||
69 |
✓✗✓✗ |
868 |
A_ = MatrixX6s::Zero(nf_ + 13, 6); |
70 |
✓✗✓✗ |
868 |
ub_ = VectorXs::Zero(nf_ + 13); |
71 |
✓✗✓✗ |
868 |
lb_ = VectorXs::Zero(nf_ + 13); |
72 |
|||
73 |
// Update the inequality matrix and bounds |
||
74 |
✓✗ | 868 |
update(); |
75 |
868 |
} |
|
76 |
|||
77 |
template <typename Scalar> |
||
78 |
WrenchConeTpl<Scalar>::WrenchConeTpl(const Matrix3s& R, const Scalar mu, |
||
79 |
const Vector2s& box, std::size_t nf, |
||
80 |
const Scalar min_nforce, |
||
81 |
const Scalar max_nforce) |
||
82 |
: nf_(nf), |
||
83 |
R_(R), |
||
84 |
box_(box), |
||
85 |
mu_(mu), |
||
86 |
inner_appr_(true), |
||
87 |
min_nforce_(min_nforce), |
||
88 |
max_nforce_(max_nforce) { |
||
89 |
if (nf_ % 2 != 0) { |
||
90 |
nf_ = 4; |
||
91 |
std::cerr << "Warning: nf has to be an even number, set to 4" << std::endl; |
||
92 |
} |
||
93 |
if (mu < Scalar(0.)) { |
||
94 |
mu_ = Scalar(1.); |
||
95 |
std::cerr << "Warning: mu has to be a positive value, set to 1." |
||
96 |
<< std::endl; |
||
97 |
} |
||
98 |
if (min_nforce < Scalar(0.)) { |
||
99 |
min_nforce_ = Scalar(0.); |
||
100 |
std::cerr << "Warning: min_nforce has to be a positive value, set to 0" |
||
101 |
<< std::endl; |
||
102 |
} |
||
103 |
if (max_nforce < Scalar(0.)) { |
||
104 |
max_nforce_ = std::numeric_limits<Scalar>::infinity(); |
||
105 |
std::cerr << "Warning: max_nforce has to be a positive value, set to " |
||
106 |
"infinity value" |
||
107 |
<< std::endl; |
||
108 |
} |
||
109 |
A_ = MatrixX3s::Zero(nf_ + 13, 6); |
||
110 |
ub_ = VectorXs::Zero(nf_ + 13); |
||
111 |
lb_ = VectorXs::Zero(nf_ + 13); |
||
112 |
|||
113 |
// Update the inequality matrix and bounds |
||
114 |
update(); |
||
115 |
} |
||
116 |
|||
117 |
template <typename Scalar> |
||
118 |
180 |
WrenchConeTpl<Scalar>::WrenchConeTpl(const WrenchConeTpl<Scalar>& cone) |
|
119 |
: nf_(cone.get_nf()), |
||
120 |
A_(cone.get_A()), |
||
121 |
ub_(cone.get_ub()), |
||
122 |
lb_(cone.get_lb()), |
||
123 |
R_(cone.get_R()), |
||
124 |
box_(cone.get_box()), |
||
125 |
mu_(cone.get_mu()), |
||
126 |
inner_appr_(cone.get_inner_appr()), |
||
127 |
min_nforce_(cone.get_min_nforce()), |
||
128 |
✓✗✓✗ ✓✗✓✗ |
180 |
max_nforce_(cone.get_max_nforce()) {} |
129 |
|||
130 |
template <typename Scalar> |
||
131 |
1051 |
WrenchConeTpl<Scalar>::~WrenchConeTpl() {} |
|
132 |
|||
133 |
template <typename Scalar> |
||
134 |
871 |
void WrenchConeTpl<Scalar>::update() { |
|
135 |
// Initialize the matrix and bounds |
||
136 |
✓✗ | 871 |
A_.setZero(); |
137 |
✓✗ | 871 |
ub_.setZero(); |
138 |
✓✗ | 871 |
lb_.setOnes(); |
139 |
✓✗ | 871 |
lb_ *= -std::numeric_limits<Scalar>::infinity(); |
140 |
|||
141 |
// Compute the mu given the type of friction cone approximation |
||
142 |
871 |
Scalar mu = mu_; |
|
143 |
871 |
Scalar theta = Scalar(2) * M_PI / static_cast<Scalar>(nf_); |
|
144 |
✓✓ | 871 |
if (inner_appr_) { |
145 |
57 |
mu *= cos(theta / Scalar(2.)); |
|
146 |
} |
||
147 |
|||
148 |
// Friction cone information |
||
149 |
// This segment of matrix is defined as |
||
150 |
// [ 1 0 -mu 0 0 0; |
||
151 |
// -1 0 -mu 0 0 0; |
||
152 |
// 0 1 -mu 0 0 0; |
||
153 |
// 0 -1 -mu 0 0 0; |
||
154 |
// 0 0 1 0 0 0] |
||
155 |
✓✓ | 2643 |
for (std::size_t i = 0; i < nf_ / 2; ++i) { |
156 |
1772 |
Scalar theta_i = theta * static_cast<Scalar>(i); |
|
157 |
✓✗ | 1772 |
Vector3s tsurf_i = Vector3s(cos(theta_i), sin(theta_i), Scalar(0.)); |
158 |
✓✗✓✗ ✓✗ |
1772 |
Vector3s mu_nsurf = -mu * Vector3s::UnitZ(); |
159 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1772 |
A_.row(2 * i).head(3) = (mu_nsurf + tsurf_i).transpose() * R_.transpose(); |
160 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1772 |
A_.row(2 * i + 1).head(3) = |
161 |
✓✗ | 3544 |
(mu_nsurf - tsurf_i).transpose() * R_.transpose(); |
162 |
} |
||
163 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
871 |
A_.row(nf_).head(3) = R_.col(2).transpose(); |
164 |
✓✗ | 871 |
lb_(nf_) = min_nforce_; |
165 |
✓✗ | 871 |
ub_(nf_) = max_nforce_; |
166 |
|||
167 |
// CoP information |
||
168 |
✓✗ | 871 |
const Scalar L = box_(0) / Scalar(2.); |
169 |
✓✗ | 871 |
const Scalar W = box_(1) / Scalar(2.); |
170 |
// This segment of matrix is defined as |
||
171 |
// [0 0 -W 1 0 0; |
||
172 |
// 0 0 -W -1 0 0; |
||
173 |
// 0 0 -L 0 1 0; |
||
174 |
// 0 0 -L 0 -1 0] |
||
175 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
871 |
A_.row(nf_ + 1) << -W * R_.col(2).transpose(), R_.col(0).transpose(); |
176 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
871 |
A_.row(nf_ + 2) << -W * R_.col(2).transpose(), -R_.col(0).transpose(); |
177 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
871 |
A_.row(nf_ + 3) << -L * R_.col(2).transpose(), R_.col(1).transpose(); |
178 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
871 |
A_.row(nf_ + 4) << -L * R_.col(2).transpose(), -R_.col(1).transpose(); |
179 |
|||
180 |
// Yaw-tau information |
||
181 |
871 |
const Scalar mu_LW = -mu * (L + W); |
|
182 |
// The segment of the matrix that encodes the minimum torque is defined as |
||
183 |
// [ W L -mu*(L+W) -mu -mu -1; |
||
184 |
// W -L -mu*(L+W) -mu mu -1; |
||
185 |
// -W L -mu*(L+W) mu -mu -1; |
||
186 |
// -W -L -mu*(L+W) mu mu -1] |
||
187 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1742 |
A_.row(nf_ + 5) << Vector3s(W, L, mu_LW).transpose() * R_.transpose(), |
188 |
✓✗✓✗ ✓✗✓✗ |
1742 |
Vector3s(-mu, -mu, Scalar(-1.)).transpose() * R_.transpose(); |
189 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1742 |
A_.row(nf_ + 6) << Vector3s(W, -L, mu_LW).transpose() * R_.transpose(), |
190 |
✓✗✓✗ ✓✗✓✗ |
1742 |
Vector3s(-mu, mu, Scalar(-1.)).transpose() * R_.transpose(); |
191 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1742 |
A_.row(nf_ + 7) << Vector3s(-W, L, mu_LW).transpose() * R_.transpose(), |
192 |
✓✗✓✗ ✓✗✓✗ |
1742 |
Vector3s(mu, -mu, Scalar(-1.)).transpose() * R_.transpose(); |
193 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1742 |
A_.row(nf_ + 8) << Vector3s(-W, -L, mu_LW).transpose() * R_.transpose(), |
194 |
✓✗✓✗ ✓✗✓✗ |
1742 |
Vector3s(mu, mu, Scalar(-1.)).transpose() * R_.transpose(); |
195 |
// The segment of the matrix that encodes the infinity torque is defined as |
||
196 |
// [ W L -mu*(L+W) mu mu 1; |
||
197 |
// W -L -mu*(L+W) mu -mu 1; |
||
198 |
// -W L -mu*(L+W) -mu mu 1; |
||
199 |
// -W -L -mu*(L+W) -mu -mu 1] |
||
200 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1742 |
A_.row(nf_ + 9) << Vector3s(W, L, mu_LW).transpose() * R_.transpose(), |
201 |
✓✗✓✗ ✓✗✓✗ |
1742 |
Vector3s(mu, mu, Scalar(1.)).transpose() * R_.transpose(); |
202 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1742 |
A_.row(nf_ + 10) << Vector3s(W, -L, mu_LW).transpose() * R_.transpose(), |
203 |
✓✗✓✗ ✓✗✓✗ |
1742 |
Vector3s(mu, -mu, Scalar(1.)).transpose() * R_.transpose(); |
204 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1742 |
A_.row(nf_ + 11) << Vector3s(-W, L, mu_LW).transpose() * R_.transpose(), |
205 |
✓✗✓✗ ✓✗✓✗ |
1742 |
Vector3s(-mu, mu, Scalar(1.)).transpose() * R_.transpose(); |
206 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1742 |
A_.row(nf_ + 12) << Vector3s(-W, -L, mu_LW).transpose() * R_.transpose(), |
207 |
✓✗✓✗ ✓✗✓✗ |
1742 |
Vector3s(-mu, -mu, Scalar(1.)).transpose() * R_.transpose(); |
208 |
871 |
} |
|
209 |
|||
210 |
template <typename Scalar> |
||
211 |
void WrenchConeTpl<Scalar>::update(const Matrix3s& R, const Scalar mu, |
||
212 |
const Vector2s& box, const Scalar min_nforce, |
||
213 |
const Scalar max_nforce) { |
||
214 |
set_R(R); |
||
215 |
set_mu(mu); |
||
216 |
set_inner_appr(inner_appr_); |
||
217 |
set_box(box); |
||
218 |
set_min_nforce(min_nforce); |
||
219 |
set_max_nforce(max_nforce); |
||
220 |
|||
221 |
// Update the inequality matrix and bounds |
||
222 |
update(); |
||
223 |
} |
||
224 |
|||
225 |
template <typename Scalar> |
||
226 |
25749 |
const typename MathBaseTpl<Scalar>::MatrixX6s& WrenchConeTpl<Scalar>::get_A() |
|
227 |
const { |
||
228 |
25749 |
return A_; |
|
229 |
} |
||
230 |
|||
231 |
template <typename Scalar> |
||
232 |
1276 |
const typename MathBaseTpl<Scalar>::VectorXs& WrenchConeTpl<Scalar>::get_ub() |
|
233 |
const { |
||
234 |
1276 |
return ub_; |
|
235 |
} |
||
236 |
|||
237 |
template <typename Scalar> |
||
238 |
1188 |
const typename MathBaseTpl<Scalar>::VectorXs& WrenchConeTpl<Scalar>::get_lb() |
|
239 |
const { |
||
240 |
1188 |
return lb_; |
|
241 |
} |
||
242 |
|||
243 |
template <typename Scalar> |
||
244 |
368 |
std::size_t WrenchConeTpl<Scalar>::get_nf() const { |
|
245 |
368 |
return nf_; |
|
246 |
} |
||
247 |
|||
248 |
template <typename Scalar> |
||
249 |
190 |
const typename MathBaseTpl<Scalar>::Matrix3s& WrenchConeTpl<Scalar>::get_R() |
|
250 |
const { |
||
251 |
190 |
return R_; |
|
252 |
} |
||
253 |
|||
254 |
template <typename Scalar> |
||
255 |
188 |
const typename MathBaseTpl<Scalar>::Vector2s& WrenchConeTpl<Scalar>::get_box() |
|
256 |
const { |
||
257 |
188 |
return box_; |
|
258 |
} |
||
259 |
|||
260 |
template <typename Scalar> |
||
261 |
189 |
const Scalar WrenchConeTpl<Scalar>::get_mu() const { |
|
262 |
189 |
return mu_; |
|
263 |
} |
||
264 |
|||
265 |
template <typename Scalar> |
||
266 |
188 |
bool WrenchConeTpl<Scalar>::get_inner_appr() const { |
|
267 |
188 |
return inner_appr_; |
|
268 |
} |
||
269 |
|||
270 |
template <typename Scalar> |
||
271 |
186 |
const Scalar WrenchConeTpl<Scalar>::get_min_nforce() const { |
|
272 |
186 |
return min_nforce_; |
|
273 |
} |
||
274 |
|||
275 |
template <typename Scalar> |
||
276 |
186 |
const Scalar WrenchConeTpl<Scalar>::get_max_nforce() const { |
|
277 |
186 |
return max_nforce_; |
|
278 |
} |
||
279 |
|||
280 |
template <typename Scalar> |
||
281 |
void WrenchConeTpl<Scalar>::set_R(const Matrix3s& R) { |
||
282 |
R_ = R; |
||
283 |
} |
||
284 |
|||
285 |
template <typename Scalar> |
||
286 |
void WrenchConeTpl<Scalar>::set_box(const Vector2s& box) { |
||
287 |
box_ = box; |
||
288 |
if (box_(0) < Scalar(0.)) { |
||
289 |
box_(0) = std::numeric_limits<Scalar>::infinity(); |
||
290 |
std::cerr << "Warning: box(0) has to be a positive value, set to max. float" |
||
291 |
<< std::endl; |
||
292 |
} |
||
293 |
if (box_(1) < Scalar(0.)) { |
||
294 |
box_(1) = std::numeric_limits<Scalar>::infinity(); |
||
295 |
std::cerr << "Warning: box(0) has to be a positive value, set to max. float" |
||
296 |
<< std::endl; |
||
297 |
} |
||
298 |
} |
||
299 |
|||
300 |
template <typename Scalar> |
||
301 |
void WrenchConeTpl<Scalar>::set_mu(const Scalar mu) { |
||
302 |
if (mu < Scalar(0.)) { |
||
303 |
mu_ = Scalar(1.); |
||
304 |
std::cerr << "Warning: mu has to be a positive value, set to 1." |
||
305 |
<< std::endl; |
||
306 |
} |
||
307 |
} |
||
308 |
|||
309 |
template <typename Scalar> |
||
310 |
void WrenchConeTpl<Scalar>::set_inner_appr(const bool inner_appr) { |
||
311 |
inner_appr_ = inner_appr; |
||
312 |
} |
||
313 |
|||
314 |
template <typename Scalar> |
||
315 |
void WrenchConeTpl<Scalar>::set_min_nforce(const Scalar min_nforce) { |
||
316 |
if (min_nforce < Scalar(0.)) { |
||
317 |
min_nforce_ = Scalar(0.); |
||
318 |
std::cerr << "Warning: min_nforce has to be a positive value, set to 0" |
||
319 |
<< std::endl; |
||
320 |
} |
||
321 |
} |
||
322 |
|||
323 |
template <typename Scalar> |
||
324 |
void WrenchConeTpl<Scalar>::set_max_nforce(const Scalar max_nforce) { |
||
325 |
if (max_nforce < Scalar(0.)) { |
||
326 |
max_nforce_ = std::numeric_limits<Scalar>::infinity(); |
||
327 |
std::cerr << "Warning: max_nforce has to be a positive value, set to " |
||
328 |
"infinity value" |
||
329 |
<< std::endl; |
||
330 |
} |
||
331 |
} |
||
332 |
|||
333 |
template <typename Scalar> |
||
334 |
2 |
WrenchConeTpl<Scalar>& WrenchConeTpl<Scalar>::operator=( |
|
335 |
const WrenchConeTpl<Scalar>& other) { |
||
336 |
✓✗ | 2 |
if (this != &other) { |
337 |
2 |
nf_ = other.get_nf(); |
|
338 |
2 |
A_ = other.get_A(); |
|
339 |
2 |
ub_ = other.get_ub(); |
|
340 |
2 |
lb_ = other.get_lb(); |
|
341 |
2 |
R_ = other.get_R(); |
|
342 |
2 |
box_ = other.get_box(); |
|
343 |
2 |
mu_ = other.get_mu(); |
|
344 |
2 |
inner_appr_ = other.get_inner_appr(); |
|
345 |
2 |
min_nforce_ = other.get_min_nforce(); |
|
346 |
2 |
max_nforce_ = other.get_max_nforce(); |
|
347 |
} |
||
348 |
2 |
return *this; |
|
349 |
} |
||
350 |
|||
351 |
template <typename Scalar> |
||
352 |
std::ostream& operator<<(std::ostream& os, const WrenchConeTpl<Scalar>& X) { |
||
353 |
os << " R: " << X.get_R() << std::endl; |
||
354 |
os << " mu: " << X.get_mu() << std::endl; |
||
355 |
os << " box: " << X.get_box().transpose() << std::endl; |
||
356 |
os << " nf: " << X.get_nf() << std::endl; |
||
357 |
os << "inner_appr: "; |
||
358 |
if (X.get_inner_appr()) { |
||
359 |
os << "true" << std::endl; |
||
360 |
} else { |
||
361 |
os << "false" << std::endl; |
||
362 |
} |
||
363 |
os << " min_force: " << X.get_min_nforce() << std::endl; |
||
364 |
os << " max_force: " << X.get_max_nforce() << std::endl; |
||
365 |
return os; |
||
366 |
} |
||
367 |
|||
368 |
} // namespace crocoddyl |
| Generated by: GCOVR (Version 4.2) |