GCC Code Coverage Report

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