GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/wrench-cone.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 122 185 65.9%
Branches: 187 402 46.5%
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 3 : nf_(4),
18
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 A_(nf_ + 13, 6),
19
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 ub_(nf_ + 13),
20
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 lb_(nf_ + 13),
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
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 box_(std::numeric_limits<Scalar>::infinity(),
23 3 std::numeric_limits<Scalar>::infinity()),
24 3 mu_(Scalar(0.7)),
25 3 inner_appr_(true),
26 3 min_nforce_(Scalar(0.)),
27 3 max_nforce_(std::numeric_limits<Scalar>::infinity()) {
28
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 A_.setZero();
29
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 ub_.setZero();
30
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 lb_.setZero();
31
32 // Update the inequality matrix and bounds
33
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 update();
34 3 }
35
36 template <typename Scalar>
37 872 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 872 : nf_(nf),
43
1/2
✓ Branch 1 taken 872 times.
✗ Branch 2 not taken.
 872 R_(R),
44
1/2
✓ Branch 1 taken 872 times.
✗ Branch 2 not taken.
 872 box_(box),
45 872 mu_(mu),
46 872 inner_appr_(inner_appr),
47 872 min_nforce_(min_nforce),
48
2/4
✓ Branch 2 taken 872 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 872 times.
✗ Branch 6 not taken.
 872 max_nforce_(max_nforce) {
49
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 872 times.
 872 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
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 872 times.
 872 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
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 872 times.
 872 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
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 872 times.
 872 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
2/4
✓ Branch 1 taken 872 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 872 times.
✗ Branch 5 not taken.
 872 A_ = MatrixX6s::Zero(nf_ + 13, 6);
70
2/4
✓ Branch 1 taken 872 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 872 times.
✗ Branch 5 not taken.
 872 ub_ = VectorXs::Zero(nf_ + 13);
71
2/4
✓ Branch 1 taken 872 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 872 times.
✗ Branch 5 not taken.
 872 lb_ = VectorXs::Zero(nf_ + 13);
72
73 // Update the inequality matrix and bounds
74
1/2
✓ Branch 1 taken 872 times.
✗ Branch 2 not taken.
 872 update();
75 872 }
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 182 WrenchConeTpl<Scalar>::WrenchConeTpl(const WrenchConeTpl<Scalar>& cone)
119 182 : nf_(cone.get_nf()),
120 182 A_(cone.get_A()),
121
1/2
✓ Branch 2 taken 182 times.
✗ Branch 3 not taken.
 182 ub_(cone.get_ub()),
122
1/2
✓ Branch 2 taken 182 times.
✗ Branch 3 not taken.
 182 lb_(cone.get_lb()),
123
1/2
✓ Branch 2 taken 182 times.
✗ Branch 3 not taken.
 182 R_(cone.get_R()),
124
1/2
✓ Branch 2 taken 182 times.
✗ Branch 3 not taken.
 182 box_(cone.get_box()),
125 182 mu_(cone.get_mu()),
126 182 inner_appr_(cone.get_inner_appr()),
127 182 min_nforce_(cone.get_min_nforce()),
128 182 max_nforce_(cone.get_max_nforce()) {}
129
130 template <typename Scalar>
131 1057 WrenchConeTpl<Scalar>::~WrenchConeTpl() {}
132
133 template <typename Scalar>
134 875 void WrenchConeTpl<Scalar>::update() {
135 // Initialize the matrix and bounds
136
1/2
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
 875 A_.setZero();
137
1/2
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
 875 ub_.setZero();
138
1/2
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
 875 lb_.setOnes();
139
1/2
✓ Branch 2 taken 875 times.
✗ Branch 3 not taken.
 875 lb_ *= -std::numeric_limits<Scalar>::infinity();
140
141 // Compute the mu given the type of friction cone approximation
142 875 Scalar mu = mu_;
143 875 Scalar theta = Scalar(2) * M_PI / static_cast<Scalar>(nf_);
144
2/2
✓ Branch 0 taken 57 times.
✓ Branch 1 taken 818 times.
 875 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
2/2
✓ Branch 0 taken 1780 times.
✓ Branch 1 taken 875 times.
 2655 for (std::size_t i = 0; i < nf_ / 2; ++i) {
156 1780 Scalar theta_i = theta * static_cast<Scalar>(i);
157
1/2
✓ Branch 1 taken 1780 times.
✗ Branch 2 not taken.
 1780 Vector3s tsurf_i = Vector3s(cos(theta_i), sin(theta_i), Scalar(0.));
158
3/6
✓ Branch 1 taken 1780 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1780 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1780 times.
✗ Branch 8 not taken.
 1780 Vector3s mu_nsurf = -mu * Vector3s::UnitZ();
159
7/14
✓ Branch 1 taken 1780 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1780 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1780 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1780 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1780 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1780 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1780 times.
✗ Branch 20 not taken.
 1780 A_.row(2 * i).head(3) = (mu_nsurf + tsurf_i).transpose() * R_.transpose();
160
6/12
✓ Branch 1 taken 1780 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1780 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1780 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1780 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1780 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1780 times.
✗ Branch 17 not taken.
 1780 A_.row(2 * i + 1).head(3) =
161
1/2
✓ Branch 1 taken 1780 times.
✗ Branch 2 not taken.
 3560 (mu_nsurf - tsurf_i).transpose() * R_.transpose();
162 }
163
5/10
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
 875 A_.row(nf_).head(3) = R_.col(2).transpose();
164
1/2
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
 875 lb_(nf_) = min_nforce_;
165
1/2
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
 875 ub_(nf_) = max_nforce_;
166
167 // CoP information
168
1/2
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
 875 const Scalar L = box_(0) / Scalar(2.);
169
1/2
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
 875 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
8/16
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 875 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 875 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 875 times.
✗ Branch 23 not taken.
 875 A_.row(nf_ + 1) << -W * R_.col(2).transpose(), R_.col(0).transpose();
176
9/18
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 875 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 875 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 875 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 875 times.
✗ Branch 26 not taken.
 875 A_.row(nf_ + 2) << -W * R_.col(2).transpose(), -R_.col(0).transpose();
177
8/16
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 875 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 875 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 875 times.
✗ Branch 23 not taken.
 875 A_.row(nf_ + 3) << -L * R_.col(2).transpose(), R_.col(1).transpose();
178
9/18
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 875 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 875 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 875 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 875 times.
✗ Branch 26 not taken.
 875 A_.row(nf_ + 4) << -L * R_.col(2).transpose(), -R_.col(1).transpose();
179
180 // Yaw-tau information
181 875 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
7/14
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 875 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 875 times.
✗ Branch 20 not taken.
 1750 A_.row(nf_ + 5) << Vector3s(W, L, mu_LW).transpose() * R_.transpose(),
188
4/8
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
 1750 Vector3s(-mu, -mu, Scalar(-1.)).transpose() * R_.transpose();
189
7/14
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 875 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 875 times.
✗ Branch 20 not taken.
 1750 A_.row(nf_ + 6) << Vector3s(W, -L, mu_LW).transpose() * R_.transpose(),
190
4/8
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
 1750 Vector3s(-mu, mu, Scalar(-1.)).transpose() * R_.transpose();
191
7/14
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 875 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 875 times.
✗ Branch 20 not taken.
 1750 A_.row(nf_ + 7) << Vector3s(-W, L, mu_LW).transpose() * R_.transpose(),
192
4/8
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
 1750 Vector3s(mu, -mu, Scalar(-1.)).transpose() * R_.transpose();
193
7/14
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 875 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 875 times.
✗ Branch 20 not taken.
 1750 A_.row(nf_ + 8) << Vector3s(-W, -L, mu_LW).transpose() * R_.transpose(),
194
4/8
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
 1750 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
7/14
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 875 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 875 times.
✗ Branch 20 not taken.
 1750 A_.row(nf_ + 9) << Vector3s(W, L, mu_LW).transpose() * R_.transpose(),
201
4/8
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
 1750 Vector3s(mu, mu, Scalar(1.)).transpose() * R_.transpose();
202
7/14
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 875 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 875 times.
✗ Branch 20 not taken.
 1750 A_.row(nf_ + 10) << Vector3s(W, -L, mu_LW).transpose() * R_.transpose(),
203
4/8
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
 1750 Vector3s(mu, -mu, Scalar(1.)).transpose() * R_.transpose();
204
7/14
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 875 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 875 times.
✗ Branch 20 not taken.
 1750 A_.row(nf_ + 11) << Vector3s(-W, L, mu_LW).transpose() * R_.transpose(),
205
4/8
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
 1750 Vector3s(-mu, mu, Scalar(1.)).transpose() * R_.transpose();
206
7/14
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 875 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 875 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 875 times.
✗ Branch 20 not taken.
 1750 A_.row(nf_ + 12) << Vector3s(-W, -L, mu_LW).transpose() * R_.transpose(),
207
4/8
✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
 1750 Vector3s(-mu, -mu, Scalar(1.)).transpose() * R_.transpose();
208 875 }
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 25751 const typename MathBaseTpl<Scalar>::MatrixX6s& WrenchConeTpl<Scalar>::get_A()
227 const {
228 25751 return A_;
229 }
230
231 template <typename Scalar>
232 1282 const typename MathBaseTpl<Scalar>::VectorXs& WrenchConeTpl<Scalar>::get_ub()
233 const {
234 1282 return ub_;
235 }
236
237 template <typename Scalar>
238 1194 const typename MathBaseTpl<Scalar>::VectorXs& WrenchConeTpl<Scalar>::get_lb()
239 const {
240 1194 return lb_;
241 }
242
243 template <typename Scalar>
244 372 std::size_t WrenchConeTpl<Scalar>::get_nf() const {
245 372 return nf_;
246 }
247
248 template <typename Scalar>
249 192 const typename MathBaseTpl<Scalar>::Matrix3s& WrenchConeTpl<Scalar>::get_R()
250 const {
251 192 return R_;
252 }
253
254 template <typename Scalar>
255 190 const typename MathBaseTpl<Scalar>::Vector2s& WrenchConeTpl<Scalar>::get_box()
256 const {
257 190 return box_;
258 }
259
260 template <typename Scalar>
261 191 const Scalar WrenchConeTpl<Scalar>::get_mu() const {
262 191 return mu_;
263 }
264
265 template <typename Scalar>
266 190 bool WrenchConeTpl<Scalar>::get_inner_appr() const {
267 190 return inner_appr_;
268 }
269
270 template <typename Scalar>
271 188 const Scalar WrenchConeTpl<Scalar>::get_min_nforce() const {
272 188 return min_nforce_;
273 }
274
275 template <typename Scalar>
276 188 const Scalar WrenchConeTpl<Scalar>::get_max_nforce() const {
277 188 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 ✗ mu_ = mu;
303 ✗ if (mu < Scalar(0.)) {
304 ✗ mu_ = Scalar(1.);
305 ✗ std::cerr << "Warning: mu has to be a positive value, set to 1."
306 ✗ << std::endl;
307 }
308 }
309
310 template <typename Scalar>
311 ✗ void WrenchConeTpl<Scalar>::set_inner_appr(const bool inner_appr) {
312 ✗ inner_appr_ = inner_appr;
313 }
314
315 template <typename Scalar>
316 ✗ void WrenchConeTpl<Scalar>::set_min_nforce(const Scalar min_nforce) {
317 ✗ min_nforce_ = min_nforce;
318 ✗ if (min_nforce < Scalar(0.)) {
319 ✗ min_nforce_ = Scalar(0.);
320 ✗ std::cerr << "Warning: min_nforce has to be a positive value, set to 0"
321 ✗ << std::endl;
322 }
323 }
324
325 template <typename Scalar>
326 ✗ void WrenchConeTpl<Scalar>::set_max_nforce(const Scalar max_nforce) {
327 ✗ max_nforce_ = max_nforce;
328 ✗ if (max_nforce < Scalar(0.)) {
329 ✗ max_nforce_ = std::numeric_limits<Scalar>::infinity();
330 ✗ std::cerr << "Warning: max_nforce has to be a positive value, set to "
331 "infinity value"
332 ✗ << std::endl;
333 }
334 }
335
336 template <typename Scalar>
337 2 WrenchConeTpl<Scalar>& WrenchConeTpl<Scalar>::operator=(
338 const WrenchConeTpl<Scalar>& other) {
339
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 if (this != &other) {
340 2 nf_ = other.get_nf();
341 2 A_ = other.get_A();
342 2 ub_ = other.get_ub();
343 2 lb_ = other.get_lb();
344 2 R_ = other.get_R();
345 2 box_ = other.get_box();
346 2 mu_ = other.get_mu();
347 2 inner_appr_ = other.get_inner_appr();
348 2 min_nforce_ = other.get_min_nforce();
349 2 max_nforce_ = other.get_max_nforce();
350 }
351 2 return *this;
352 }
353
354 template <typename Scalar>
355 ✗ std::ostream& operator<<(std::ostream& os, const WrenchConeTpl<Scalar>& X) {
356 ✗ os << " R: " << X.get_R() << std::endl;
357 ✗ os << " mu: " << X.get_mu() << std::endl;
358 ✗ os << " box: " << X.get_box().transpose() << std::endl;
359 ✗ os << " nf: " << X.get_nf() << std::endl;
360 ✗ os << "inner_appr: ";
361 ✗ if (X.get_inner_appr()) {
362 ✗ os << "true" << std::endl;
363 } else {
364 ✗ os << "false" << std::endl;
365 }
366 ✗ os << " min_force: " << X.get_min_nforce() << std::endl;
367 ✗ os << " max_force: " << X.get_max_nforce() << std::endl;
368 ✗ return os;
369 }
370
371 } // namespace crocoddyl
372