GCC Code Coverage Report

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