GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/friction-cone.hxx
Date: 2025-01-30 11:01:55
Exec Total Coverage
Lines: 93 149 62.4%
Branches: 61 170 35.9%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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 FrictionConeTpl<Scalar>::FrictionConeTpl()
17 3 : nf_(4),
18
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 A_(nf_ + 1, 3),
19
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 ub_(nf_ + 1),
20
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 lb_(nf_ + 1),
21
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 R_(Matrix3s::Identity()),
22 3 mu_(Scalar(0.7)),
23 3 inner_appr_(true),
24 3 min_nforce_(Scalar(0.)),
25 3 max_nforce_(std::numeric_limits<Scalar>::infinity()) {
26
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 A_.setZero();
27
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 ub_.setZero();
28
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 lb_.setZero();
29
30 // Update the inequality matrix and bounds
31
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 update();
32 3 }
33
34 template <typename Scalar>
35 922 FrictionConeTpl<Scalar>::FrictionConeTpl(const Matrix3s& R, const Scalar mu,
36 std::size_t nf, const bool inner_appr,
37 const Scalar min_nforce,
38 const Scalar max_nforce)
39 922 : nf_(nf),
40
1/2
✓ Branch 1 taken 922 times.
✗ Branch 2 not taken.
 922 R_(R),
41 922 mu_(mu),
42 922 inner_appr_(inner_appr),
43 922 min_nforce_(min_nforce),
44
2/4
✓ Branch 2 taken 922 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 922 times.
✗ Branch 6 not taken.
 922 max_nforce_(max_nforce) {
45
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 922 times.
 922 if (nf_ % 2 != 0) {
46 ✗ nf_ = 4;
47 ✗ std::cerr << "Warning: nf has to be an even number, set to 4" << std::endl;
48 }
49
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 922 times.
 922 if (mu < Scalar(0.)) {
50 ✗ mu_ = Scalar(1.);
51 ✗ std::cerr << "Warning: mu has to be a positive value, set to 1."
52 ✗ << std::endl;
53 }
54
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 922 times.
 922 if (min_nforce < Scalar(0.)) {
55 ✗ min_nforce_ = Scalar(0.);
56 ✗ std::cerr << "Warning: min_nforce has to be a positive value, set to 0"
57 ✗ << std::endl;
58 }
59
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 922 times.
 922 if (max_nforce < Scalar(0.)) {
60 ✗ max_nforce_ = std::numeric_limits<Scalar>::infinity();
61 ✗ std::cerr << "Warning: max_nforce has to be a positive value, set to "
62 "infinity value"
63 ✗ << std::endl;
64 }
65
2/4
✓ Branch 1 taken 922 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 922 times.
✗ Branch 5 not taken.
 922 A_ = MatrixX3s::Zero(nf_ + 1, 3);
66
2/4
✓ Branch 1 taken 922 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 922 times.
✗ Branch 5 not taken.
 922 ub_ = VectorXs::Zero(nf_ + 1);
67
2/4
✓ Branch 1 taken 922 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 922 times.
✗ Branch 5 not taken.
 922 lb_ = VectorXs::Zero(nf_ + 1);
68
69 // Update the inequality matrix and bounds
70
1/2
✓ Branch 1 taken 922 times.
✗ Branch 2 not taken.
 922 update();
71 922 }
72
73 template <typename Scalar>
74 FrictionConeTpl<Scalar>::FrictionConeTpl(const Vector3s& nsurf, const Scalar mu,
75 std::size_t nf, const bool inner_appr,
76 const Scalar min_nforce,
77 const Scalar max_nforce)
78 : nf_(nf),
79 R_(Quaternions::FromTwoVectors(nsurf, Vector3s::UnitZ())
80 .toRotationMatrix()),
81 mu_(mu),
82 inner_appr_(inner_appr),
83 min_nforce_(min_nforce),
84 max_nforce_(max_nforce) {
85 if (nf_ % 2 != 0) {
86 nf_ = 4;
87 std::cerr << "Warning: nf has to be an even number, set to 4" << std::endl;
88 }
89 Eigen::Vector3d normal = nsurf;
90 if (!nsurf.isUnitary()) {
91 normal /= nsurf.norm();
92 std::cerr
93 << "Warning: normal is not an unitary vector, then we normalized it"
94 << std::endl;
95 }
96 if (mu < Scalar(0.)) {
97 mu_ = Scalar(1.);
98 std::cerr << "Warning: mu has to be a positive value, set to 1."
99 << std::endl;
100 }
101 if (min_nforce < Scalar(0.)) {
102 min_nforce_ = Scalar(0.);
103 std::cerr << "Warning: min_nforce has to be a positive value, set to 0"
104 << std::endl;
105 }
106 if (max_nforce < Scalar(0.)) {
107 max_nforce_ = std::numeric_limits<Scalar>::infinity();
108 std::cerr << "Warning: max_nforce has to be a positive value, set to "
109 "infinity value"
110 << std::endl;
111 }
112 A_ = MatrixX3s::Zero(nf_ + 1, 3);
113 ub_ = VectorXs::Zero(nf_ + 1);
114 lb_ = VectorXs::Zero(nf_ + 1);
115 R_ =
116 Quaternions::FromTwoVectors(normal, Vector3s::UnitZ()).toRotationMatrix();
117
118 // Update the inequality matrix and bounds
119 update();
120 }
121
122 template <typename Scalar>
123 888 FrictionConeTpl<Scalar>::FrictionConeTpl(const FrictionConeTpl<Scalar>& cone)
124 888 : nf_(cone.get_nf()),
125 888 A_(cone.get_A()),
126
1/2
✓ Branch 2 taken 888 times.
✗ Branch 3 not taken.
 888 ub_(cone.get_ub()),
127
1/2
✓ Branch 2 taken 888 times.
✗ Branch 3 not taken.
 888 lb_(cone.get_lb()),
128
1/2
✓ Branch 2 taken 888 times.
✗ Branch 3 not taken.
 888 R_(cone.get_R()),
129 888 mu_(cone.get_mu()),
130 888 inner_appr_(cone.get_inner_appr()),
131 888 min_nforce_(cone.get_min_nforce()),
132 888 max_nforce_(cone.get_max_nforce()) {}
133
134 template <typename Scalar>
135 1813 FrictionConeTpl<Scalar>::~FrictionConeTpl() {}
136
137 template <typename Scalar>
138 925 void FrictionConeTpl<Scalar>::update() {
139 // Initialize the matrix and bounds
140
1/2
✓ Branch 1 taken 925 times.
✗ Branch 2 not taken.
 925 A_.setZero();
141
1/2
✓ Branch 1 taken 925 times.
✗ Branch 2 not taken.
 925 ub_.setZero();
142
1/2
✓ Branch 1 taken 925 times.
✗ Branch 2 not taken.
 925 lb_.setOnes();
143
1/2
✓ Branch 2 taken 925 times.
✗ Branch 3 not taken.
 925 lb_ *= -std::numeric_limits<Scalar>::infinity();
144
145 // Compute the mu given the type of friction cone approximation
146 925 Scalar mu = mu_;
147 925 const Scalar theta = Scalar(2) * M_PI / static_cast<Scalar>(nf_);
148
2/2
✓ Branch 0 taken 102 times.
✓ Branch 1 taken 823 times.
 925 if (inner_appr_) {
149 102 mu *= cos(theta / Scalar(2.));
150 }
151
152 // Update the inequality matrix and the bounds
153 // This matrix is defined as
154 // [ 1 0 -mu;
155 // -1 0 -mu;
156 // 0 1 -mu;
157 // 0 -1 -mu;
158 // 0 0 1]
159 Scalar theta_i;
160
1/2
✓ Branch 1 taken 925 times.
✗ Branch 2 not taken.
 925 Vector3s tsurf_i;
161
2/2
✓ Branch 0 taken 1942 times.
✓ Branch 1 taken 925 times.
 2867 for (std::size_t i = 0; i < nf_ / 2; ++i) {
162 1942 theta_i = theta * static_cast<Scalar>(i);
163
3/6
✓ Branch 1 taken 1942 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1942 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1942 times.
✗ Branch 8 not taken.
 1942 tsurf_i << cos(theta_i), sin(theta_i), Scalar(0.);
164
7/14
✓ Branch 1 taken 1942 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1942 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1942 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1942 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1942 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1942 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1942 times.
✗ Branch 20 not taken.
 1942 A_.row(2 * i) =
165
1/2
✓ Branch 1 taken 1942 times.
✗ Branch 2 not taken.
 1942 (-mu * Vector3s::UnitZ() + tsurf_i).transpose() * R_.transpose();
166
7/14
✓ Branch 1 taken 1942 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1942 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1942 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1942 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1942 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1942 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1942 times.
✗ Branch 20 not taken.
 1942 A_.row(2 * i + 1) =
167
1/2
✓ Branch 1 taken 1942 times.
✗ Branch 2 not taken.
 3884 (-mu * Vector3s::UnitZ() - tsurf_i).transpose() * R_.transpose();
168 }
169
4/8
✓ Branch 1 taken 925 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 925 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 925 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 925 times.
✗ Branch 11 not taken.
 925 A_.row(nf_) = R_.col(2).transpose();
170
1/2
✓ Branch 1 taken 925 times.
✗ Branch 2 not taken.
 925 lb_(nf_) = min_nforce_;
171
1/2
✓ Branch 1 taken 925 times.
✗ Branch 2 not taken.
 925 ub_(nf_) = max_nforce_;
172 925 }
173
174 template <typename Scalar>
175 ✗ void FrictionConeTpl<Scalar>::update(const Vector3s& normal, const Scalar mu,
176 const bool inner_appr,
177 const Scalar min_nforce,
178 const Scalar max_nforce) {
179 ✗ Eigen::Vector3d nsurf = normal;
180 // Sanity checks
181 ✗ if (!nsurf.isUnitary()) {
182 ✗ nsurf /= normal.norm();
183 std::cerr
184 ✗ << "Warning: normal is not an unitary vector, then we normalized it"
185 ✗ << std::endl;
186 }
187 ✗ R_ = Quaternions::FromTwoVectors(nsurf, Vector3s::UnitZ()).toRotationMatrix();
188 ✗ set_mu(mu);
189 ✗ set_inner_appr(inner_appr);
190 ✗ set_min_nforce(min_nforce);
191 ✗ set_max_nforce(max_nforce);
192
193 // Update the inequality matrix and bounds
194 ✗ update();
195 }
196
197 template <typename Scalar>
198 96813 const typename MathBaseTpl<Scalar>::MatrixX3s& FrictionConeTpl<Scalar>::get_A()
199 const {
200 96813 return A_;
201 }
202
203 template <typename Scalar>
204 1914 const typename MathBaseTpl<Scalar>::VectorXs& FrictionConeTpl<Scalar>::get_ub()
205 const {
206 1914 return ub_;
207 }
208
209 template <typename Scalar>
210 1850 const typename MathBaseTpl<Scalar>::VectorXs& FrictionConeTpl<Scalar>::get_lb()
211 const {
212 1850 return lb_;
213 }
214
215 template <typename Scalar>
216 897 const typename MathBaseTpl<Scalar>::Matrix3s& FrictionConeTpl<Scalar>::get_R()
217 const {
218 897 return R_;
219 }
220
221 template <typename Scalar>
222 typename MathBaseTpl<Scalar>::Vector3s FrictionConeTpl<Scalar>::get_nsurf() {
223 return R_.row(2).transpose();
224 }
225
226 template <typename Scalar>
227 897 const Scalar FrictionConeTpl<Scalar>::get_mu() const {
228 897 return mu_;
229 }
230
231 template <typename Scalar>
232 1779 std::size_t FrictionConeTpl<Scalar>::get_nf() const {
233 1779 return nf_;
234 }
235
236 template <typename Scalar>
237 896 bool FrictionConeTpl<Scalar>::get_inner_appr() const {
238 896 return inner_appr_;
239 }
240
241 template <typename Scalar>
242 894 const Scalar FrictionConeTpl<Scalar>::get_min_nforce() const {
243 894 return min_nforce_;
244 }
245
246 template <typename Scalar>
247 894 const Scalar FrictionConeTpl<Scalar>::get_max_nforce() const {
248 894 return max_nforce_;
249 }
250
251 template <typename Scalar>
252 ✗ void FrictionConeTpl<Scalar>::set_R(const Matrix3s& R) {
253 ✗ R_ = R;
254 }
255
256 template <typename Scalar>
257 void FrictionConeTpl<Scalar>::set_nsurf(const Vector3s& nsurf) {
258 Eigen::Vector3d normal = nsurf;
259 // Sanity checks
260 if (!nsurf.isUnitary()) {
261 normal /= nsurf.norm();
262 std::cerr
263 << "Warning: normal is not an unitary vector, then we normalized it"
264 << std::endl;
265 }
266 R_ =
267 Quaternions::FromTwoVectors(normal, Vector3s::UnitZ()).toRotationMatrix();
268 }
269
270 template <typename Scalar>
271 ✗ void FrictionConeTpl<Scalar>::set_mu(const Scalar mu) {
272 ✗ if (mu < Scalar(0.)) {
273 ✗ mu_ = Scalar(1.);
274 ✗ std::cerr << "Warning: mu has to be a positive value, set to 1."
275 ✗ << std::endl;
276 }
277 ✗ mu_ = mu;
278 }
279
280 template <typename Scalar>
281 ✗ void FrictionConeTpl<Scalar>::set_inner_appr(const bool inner_appr) {
282 ✗ inner_appr_ = inner_appr;
283 }
284
285 template <typename Scalar>
286 ✗ void FrictionConeTpl<Scalar>::set_min_nforce(const Scalar min_nforce) {
287 ✗ if (min_nforce < Scalar(0.)) {
288 ✗ min_nforce_ = Scalar(0.);
289 ✗ std::cerr << "Warning: min_nforce has to be a positive value, set to 0"
290 ✗ << std::endl;
291 }
292 ✗ min_nforce_ = min_nforce;
293 }
294
295 template <typename Scalar>
296 ✗ void FrictionConeTpl<Scalar>::set_max_nforce(const Scalar max_nforce) {
297 ✗ if (max_nforce < Scalar(0.)) {
298 ✗ max_nforce_ = std::numeric_limits<Scalar>::infinity();
299 ✗ std::cerr << "Warning: max_nforce has to be a positive value, set to "
300 "infinity value"
301 ✗ << std::endl;
302 }
303 ✗ max_nforce_ = max_nforce;
304 }
305
306 template <typename Scalar>
307 2 FrictionConeTpl<Scalar>& FrictionConeTpl<Scalar>::operator=(
308 const FrictionConeTpl<Scalar>& other) {
309
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 if (this != &other) {
310 2 nf_ = other.get_nf();
311 2 A_ = other.get_A();
312 2 ub_ = other.get_ub();
313 2 lb_ = other.get_lb();
314 2 R_ = other.get_R();
315 2 mu_ = other.get_mu();
316 2 inner_appr_ = other.get_inner_appr();
317 2 min_nforce_ = other.get_min_nforce();
318 2 max_nforce_ = other.get_max_nforce();
319 }
320 2 return *this;
321 }
322
323 template <typename Scalar>
324 ✗ std::ostream& operator<<(std::ostream& os, const FrictionConeTpl<Scalar>& X) {
325 ✗ os << " R: " << X.get_R() << std::endl;
326 ✗ os << " mu: " << X.get_mu() << std::endl;
327 ✗ os << " nf: " << X.get_nf() << std::endl;
328 ✗ os << "inner_appr: ";
329 ✗ if (X.get_inner_appr()) {
330 ✗ os << "true" << std::endl;
331 } else {
332 ✗ os << "false" << std::endl;
333 }
334 ✗ os << " min_force: " << X.get_min_nforce() << std::endl;
335 ✗ os << " max_force: " << X.get_max_nforce() << std::endl;
336 ✗ return os;
337 }
338
339 } // namespace crocoddyl
340