GCC Code Coverage Report

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