friction-cone.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include <math.h>
10 #include <iostream>
11 
12 namespace crocoddyl {
13 
14 template <typename Scalar>
15 FrictionConeTpl<Scalar>::FrictionConeTpl() : nf_(4) {
16  A_.resize(nf_ + 1, 3);
17  lb_.resize(nf_ + 1);
18  ub_.resize(nf_ + 1);
19  // compute the matrix
20  update(Vector3s(0, 0, 1), 0.7, true, 0., std::numeric_limits<Scalar>::max());
21 }
22 
23 template <typename Scalar>
24 FrictionConeTpl<Scalar>::FrictionConeTpl(const Vector3s& normal, const Scalar& mu, std::size_t nf, bool inner_appr,
25  const Scalar& min_nforce, const Scalar& max_nforce)
26  : nf_(nf) {
27  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  A_.resize(nf_ + 1, 3);
32  lb_.resize(nf_ + 1);
33  ub_.resize(nf_ + 1);
34 
35  // compute the matrix
36  update(normal, mu, inner_appr, min_nforce, max_nforce);
37 }
38 
39 template <typename Scalar>
40 FrictionConeTpl<Scalar>::FrictionConeTpl(const FrictionConeTpl<Scalar>& cone)
41  : A_(cone.get_A()),
42  lb_(cone.get_lb()),
43  ub_(cone.get_ub()),
44  nsurf_(cone.get_nsurf()),
45  mu_(cone.get_mu()),
46  nf_(cone.get_nf()),
47  inner_appr_(cone.get_inner_appr()),
48  min_nforce_(cone.get_min_nforce()),
49  max_nforce_(cone.get_max_nforce()) {}
50 
51 template <typename Scalar>
52 FrictionConeTpl<Scalar>::~FrictionConeTpl() {}
53 
54 template <typename Scalar>
55 void FrictionConeTpl<Scalar>::update(const Vector3s& normal, const Scalar& mu, bool inner_appr,
56  const Scalar& min_nforce, const Scalar& max_nforce) {
57  nsurf_ = normal;
58  mu_ = mu;
59  inner_appr_ = inner_appr;
60  min_nforce_ = min_nforce;
61  max_nforce_ = max_nforce;
62 
63  // Sanity checks
64  if (!normal.isUnitary()) {
65  nsurf_ /= normal.norm();
66  std::cerr << "Warning: normal is not an unitary vector, then we normalized it" << std::endl;
67  }
68  if (min_nforce < 0.) {
69  min_nforce_ = 0.;
70  std::cerr << "Warning: min_nforce has to be a positive value, set to 0" << std::endl;
71  }
72  if (max_nforce < 0.) {
73  max_nforce_ = std::numeric_limits<Scalar>::max();
74  std::cerr << "Warning: max_nforce has to be a positive value, set to maximun value" << std::endl;
75  }
76 
77  Scalar theta = 2 * M_PI / static_cast<Scalar>(nf_);
78  if (inner_appr_) {
79  mu_ *= cos(theta / 2.);
80  }
81 
82  Matrix3s c_R_o = Quaternions::FromTwoVectors(nsurf_, Vector3s::UnitZ()).toRotationMatrix();
83  for (std::size_t i = 0; i < nf_ / 2; ++i) {
84  Scalar theta_i = theta * static_cast<Scalar>(i);
85  Vector3s tsurf_i = Vector3s(cos(theta_i), sin(theta_i), 0.);
86  A_.row(2 * i) = (-mu_ * Vector3s::UnitZ() + tsurf_i).transpose() * c_R_o;
87  A_.row(2 * i + 1) = (-mu_ * Vector3s::UnitZ() - tsurf_i).transpose() * c_R_o;
88  lb_(2 * i) = -std::numeric_limits<Scalar>::max();
89  lb_(2 * i + 1) = -std::numeric_limits<Scalar>::max();
90  ub_(2 * i) = 0.;
91  ub_(2 * i + 1) = 0.;
92  }
93  A_.row(nf_) = nsurf_.transpose();
94  lb_(nf_) = min_nforce_;
95  ub_(nf_) = max_nforce_;
96 }
97 
98 template <typename Scalar>
99 const typename MathBaseTpl<Scalar>::MatrixX3s& FrictionConeTpl<Scalar>::get_A() const {
100  return A_;
101 }
102 
103 template <typename Scalar>
104 const typename MathBaseTpl<Scalar>::VectorXs& FrictionConeTpl<Scalar>::get_lb() const {
105  return lb_;
106 }
107 
108 template <typename Scalar>
109 const typename MathBaseTpl<Scalar>::VectorXs& FrictionConeTpl<Scalar>::get_ub() const {
110  return ub_;
111 }
112 
113 template <typename Scalar>
114 const typename MathBaseTpl<Scalar>::Vector3s& FrictionConeTpl<Scalar>::get_nsurf() const {
115  return nsurf_;
116 }
117 
118 template <typename Scalar>
119 const Scalar& FrictionConeTpl<Scalar>::get_mu() const {
120  return mu_;
121 }
122 
123 template <typename Scalar>
124 const std::size_t& FrictionConeTpl<Scalar>::get_nf() const {
125  return nf_;
126 }
127 
128 template <typename Scalar>
129 const bool& FrictionConeTpl<Scalar>::get_inner_appr() const {
130  return inner_appr_;
131 }
132 
133 template <typename Scalar>
134 const Scalar& FrictionConeTpl<Scalar>::get_min_nforce() const {
135  return min_nforce_;
136 }
137 
138 template <typename Scalar>
139 const Scalar& FrictionConeTpl<Scalar>::get_max_nforce() const {
140  return max_nforce_;
141 }
142 
143 template <typename Scalar>
144 std::ostream& operator<<(std::ostream& os, const FrictionConeTpl<Scalar>& X) {
145  os << " normal: " << X.get_nsurf().transpose() << std::endl;
146  os << " mu: " << X.get_mu() << std::endl;
147  os << " nf: " << X.get_nf() << std::endl;
148  os << "inner_appr: " << X.get_inner_appr() << std::endl;
149  os << " min_force: " << X.get_min_nforce() << std::endl;
150  os << " max_force: " << X.get_max_nforce() << std::endl;
151  return os;
152 }
153 
154 } // namespace crocoddyl
Definition: action-base.hxx:11