Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/friction-cone.hxx	Lines:	72	131	55.0 %
Date:	2024-02-13 11:12:33	Branches:	61	170	35.9 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, University of Edinburgh, University of Oxford
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include <math.h>

#include <iostream>

namespace crocoddyl {

template <typename Scalar>
FrictionConeTpl<Scalar>::FrictionConeTpl()
    : nf_(4),
      A_(nf_ + 1, 3),
      ub_(nf_ + 1),
      lb_(nf_ + 1),
      R_(Matrix3s::Identity()),
      mu_(Scalar(0.7)),
      inner_appr_(true),
      min_nforce_(Scalar(0.)),
      max_nforce_(std::numeric_limits<Scalar>::infinity()) {
  A_.setZero();
  ub_.setZero();
  lb_.setZero();

  // Update the inequality matrix and bounds
  update();
}

template <typename Scalar>
FrictionConeTpl<Scalar>::FrictionConeTpl(const Matrix3s& R, const Scalar mu,
                                         std::size_t nf, const bool inner_appr,
                                         const Scalar min_nforce,
                                         const Scalar max_nforce)
    : nf_(nf),
      R_(R),
      mu_(mu),
      inner_appr_(inner_appr),
      min_nforce_(min_nforce),
      max_nforce_(max_nforce) {
  if (nf_ % 2 != 0) {
    nf_ = 4;
    std::cerr << "Warning: nf has to be an even number, set to 4" << std::endl;
  }
  if (mu < Scalar(0.)) {
    mu_ = Scalar(1.);
    std::cerr << "Warning: mu has to be a positive value, set to 1."
              << std::endl;
  }
  if (min_nforce < Scalar(0.)) {
    min_nforce_ = Scalar(0.);
    std::cerr << "Warning: min_nforce has to be a positive value, set to 0"
              << std::endl;
  }
  if (max_nforce < Scalar(0.)) {
    max_nforce_ = std::numeric_limits<Scalar>::infinity();
    std::cerr << "Warning: max_nforce has to be a positive value, set to "
                 "infinity value"
              << std::endl;
  }
  A_ = MatrixX3s::Zero(nf_ + 1, 3);
  ub_ = VectorXs::Zero(nf_ + 1);
  lb_ = VectorXs::Zero(nf_ + 1);

  // Update the inequality matrix and bounds
  update();
}

template <typename Scalar>
FrictionConeTpl<Scalar>::FrictionConeTpl(const Vector3s& nsurf, const Scalar mu,
                                         std::size_t nf, const bool inner_appr,
                                         const Scalar min_nforce,
                                         const Scalar max_nforce)
    : nf_(nf),
      R_(Quaternions::FromTwoVectors(nsurf, Vector3s::UnitZ())
             .toRotationMatrix()),
      mu_(mu),
      inner_appr_(inner_appr),
      min_nforce_(min_nforce),
      max_nforce_(max_nforce) {
  if (nf_ % 2 != 0) {
    nf_ = 4;
    std::cerr << "Warning: nf has to be an even number, set to 4" << std::endl;
  }
  Eigen::Vector3d normal = nsurf;
  if (!nsurf.isUnitary()) {
    normal /= nsurf.norm();
    std::cerr
        << "Warning: normal is not an unitary vector, then we normalized it"
        << std::endl;
  }
  if (mu < Scalar(0.)) {
    mu_ = Scalar(1.);
    std::cerr << "Warning: mu has to be a positive value, set to 1."
              << std::endl;
  }
  if (min_nforce < Scalar(0.)) {
    min_nforce_ = Scalar(0.);
    std::cerr << "Warning: min_nforce has to be a positive value, set to 0"
              << std::endl;
  }
  if (max_nforce < Scalar(0.)) {
    max_nforce_ = std::numeric_limits<Scalar>::infinity();
    std::cerr << "Warning: max_nforce has to be a positive value, set to "
                 "infinity value"
              << std::endl;
  }
  A_ = MatrixX3s::Zero(nf_ + 1, 3);
  ub_ = VectorXs::Zero(nf_ + 1);
  lb_ = VectorXs::Zero(nf_ + 1);
  R_ =
      Quaternions::FromTwoVectors(normal, Vector3s::UnitZ()).toRotationMatrix();

  // Update the inequality matrix and bounds
  update();
}

template <typename Scalar>
FrictionConeTpl<Scalar>::FrictionConeTpl(const FrictionConeTpl<Scalar>& cone)
    : nf_(cone.get_nf()),
      A_(cone.get_A()),
      ub_(cone.get_ub()),
      lb_(cone.get_lb()),
      R_(cone.get_R()),
      mu_(cone.get_mu()),
      inner_appr_(cone.get_inner_appr()),
      min_nforce_(cone.get_min_nforce()),
      max_nforce_(cone.get_max_nforce()) {}

template <typename Scalar>
FrictionConeTpl<Scalar>::~FrictionConeTpl() {}

template <typename Scalar>
void FrictionConeTpl<Scalar>::update() {
  // Initialize the matrix and bounds
  A_.setZero();
  ub_.setZero();
  lb_.setOnes();
  lb_ *= -std::numeric_limits<Scalar>::infinity();

  // Compute the mu given the type of friction cone approximation
  Scalar mu = mu_;
  const Scalar theta = Scalar(2) * M_PI / static_cast<Scalar>(nf_);
  if (inner_appr_) {
    mu *= cos(theta / Scalar(2.));
  }

  // Update the inequality matrix and the bounds
  // This matrix is defined as
  // [ 1  0 -mu;
  //  -1  0 -mu;
  //   0  1 -mu;
  //   0 -1 -mu;
  //   0  0   1]
  Scalar theta_i;
  Vector3s tsurf_i;
  for (std::size_t i = 0; i < nf_ / 2; ++i) {
    theta_i = theta * static_cast<Scalar>(i);
    tsurf_i << cos(theta_i), sin(theta_i), Scalar(0.);
    A_.row(2 * i) =
        (-mu * Vector3s::UnitZ() + tsurf_i).transpose() * R_.transpose();
    A_.row(2 * i + 1) =
        (-mu * Vector3s::UnitZ() - tsurf_i).transpose() * R_.transpose();
  }
  A_.row(nf_) = R_.col(2).transpose();
  lb_(nf_) = min_nforce_;
  ub_(nf_) = max_nforce_;
}

template <typename Scalar>
void FrictionConeTpl<Scalar>::update(const Vector3s& normal, const Scalar mu,
                                     const bool inner_appr,
                                     const Scalar min_nforce,
                                     const Scalar max_nforce) {
  Eigen::Vector3d nsurf = normal;
  // Sanity checks
  if (!nsurf.isUnitary()) {
    nsurf /= normal.norm();
    std::cerr
        << "Warning: normal is not an unitary vector, then we normalized it"
        << std::endl;
  }
  R_ = Quaternions::FromTwoVectors(nsurf, Vector3s::UnitZ()).toRotationMatrix();
  set_mu(mu);
  set_inner_appr(inner_appr);
  set_min_nforce(min_nforce);
  set_max_nforce(max_nforce);

  // Update the inequality matrix and bounds
  update();
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::MatrixX3s& FrictionConeTpl<Scalar>::get_A()
    const {
  return A_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs& FrictionConeTpl<Scalar>::get_ub()
    const {
  return ub_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs& FrictionConeTpl<Scalar>::get_lb()
    const {
  return lb_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::Matrix3s& FrictionConeTpl<Scalar>::get_R()
    const {
  return R_;
}

template <typename Scalar>
typename MathBaseTpl<Scalar>::Vector3s FrictionConeTpl<Scalar>::get_nsurf() {
  return R_.row(2).transpose();
}

template <typename Scalar>
const Scalar FrictionConeTpl<Scalar>::get_mu() const {
  return mu_;
}

template <typename Scalar>
std::size_t FrictionConeTpl<Scalar>::get_nf() const {
  return nf_;
}

template <typename Scalar>
bool FrictionConeTpl<Scalar>::get_inner_appr() const {
  return inner_appr_;
}

template <typename Scalar>
const Scalar FrictionConeTpl<Scalar>::get_min_nforce() const {
  return min_nforce_;
}

template <typename Scalar>
const Scalar FrictionConeTpl<Scalar>::get_max_nforce() const {
  return max_nforce_;
}

template <typename Scalar>
void FrictionConeTpl<Scalar>::set_R(const Matrix3s& R) {
  R_ = R;
}

template <typename Scalar>
void FrictionConeTpl<Scalar>::set_nsurf(const Vector3s& nsurf) {
  Eigen::Vector3d normal = nsurf;
  // Sanity checks
  if (!nsurf.isUnitary()) {
    normal /= nsurf.norm();
    std::cerr
        << "Warning: normal is not an unitary vector, then we normalized it"
        << std::endl;
  }
  R_ =
      Quaternions::FromTwoVectors(normal, Vector3s::UnitZ()).toRotationMatrix();
}

template <typename Scalar>
void FrictionConeTpl<Scalar>::set_mu(const Scalar mu) {
  if (mu < Scalar(0.)) {
    mu_ = Scalar(1.);
    std::cerr << "Warning: mu has to be a positive value, set to 1."
              << std::endl;
  }
  mu_ = mu;
}

template <typename Scalar>
void FrictionConeTpl<Scalar>::set_inner_appr(const bool inner_appr) {
  inner_appr_ = inner_appr;
}

template <typename Scalar>
void FrictionConeTpl<Scalar>::set_min_nforce(const Scalar min_nforce) {
  if (min_nforce < Scalar(0.)) {
    min_nforce_ = Scalar(0.);
    std::cerr << "Warning: min_nforce has to be a positive value, set to 0"
              << std::endl;
  }
  min_nforce_ = min_nforce;
}

template <typename Scalar>
void FrictionConeTpl<Scalar>::set_max_nforce(const Scalar max_nforce) {
  if (max_nforce < Scalar(0.)) {
    max_nforce_ = std::numeric_limits<Scalar>::infinity();
    std::cerr << "Warning: max_nforce has to be a positive value, set to "
                 "infinity value"
              << std::endl;
  }
  max_nforce_ = max_nforce;
}

template <typename Scalar>
FrictionConeTpl<Scalar>& FrictionConeTpl<Scalar>::operator=(
    const FrictionConeTpl<Scalar>& other) {
  if (this != &other) {
    nf_ = other.get_nf();
    A_ = other.get_A();
    ub_ = other.get_ub();
    lb_ = other.get_lb();
    R_ = other.get_R();
    mu_ = other.get_mu();
    inner_appr_ = other.get_inner_appr();
    min_nforce_ = other.get_min_nforce();
    max_nforce_ = other.get_max_nforce();
  }
  return *this;
}

template <typename Scalar>
std::ostream& operator<<(std::ostream& os, const FrictionConeTpl<Scalar>& X) {
  os << "         R: " << X.get_R() << std::endl;
  os << "        mu: " << X.get_mu() << std::endl;
  os << "        nf: " << X.get_nf() << std::endl;
  os << "inner_appr: ";
  if (X.get_inner_appr()) {
    os << "true" << std::endl;
  } else {
    os << "false" << std::endl;
  }
  os << " min_force: " << X.get_min_nforce() << std::endl;
  os << " max_force: " << X.get_max_nforce() << std::endl;
  return os;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

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