GCC Code Coverage Report

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