Crocoddyl
friction-cone.hpp
1 // 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.
8 
9 #ifndef CROCODDYL_MULTIBODY_FRICTION_CONE_HPP_
10 #define CROCODDYL_MULTIBODY_FRICTION_CONE_HPP_
11 
12 #include "crocoddyl/core/mathbase.hpp"
13 #include "crocoddyl/core/utils/deprecate.hpp"
14 #include "crocoddyl/multibody/fwd.hpp"
15 
16 namespace crocoddyl {
17 
27 template <typename _Scalar>
29  public:
30  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31 
32  typedef _Scalar Scalar;
34  typedef typename MathBase::Vector3s Vector3s;
35  typedef typename MathBase::Matrix3s Matrix3s;
36  typedef typename MathBase::VectorXs VectorXs;
37  typedef typename MathBase::MatrixXs MatrixXs;
38  typedef typename MathBase::MatrixX3s MatrixX3s;
39  typedef typename MathBase::Quaternions Quaternions;
40 
44  explicit FrictionConeTpl();
45 
59  const Matrix3s& R, const Scalar mu, std::size_t nf = 4,
60  const bool inner_appr = true, const Scalar min_nforce = Scalar(0.),
61  const Scalar max_nforce = std::numeric_limits<Scalar>::infinity());
62  DEPRECATED("Use constructor based on rotation matrix.",
63  FrictionConeTpl(const Vector3s& normal, const Scalar mu,
64  std::size_t nf = 4, const bool inner_appr = true,
65  const Scalar min_nforce = Scalar(0.),
66  const Scalar max_nforce =
67  std::numeric_limits<Scalar>::infinity());)
68 
75  ~FrictionConeTpl();
76 
86  void update();
87  DEPRECATED("Use update()",
88  void update(const Vector3s& normal, const Scalar mu,
89  const bool inner_appr = true,
90  const Scalar min_nforce = Scalar(0.),
91  const Scalar max_nforce =
92  std::numeric_limits<Scalar>::infinity()));
93 
97  const MatrixX3s& get_A() const;
98 
102  const VectorXs& get_ub() const;
103 
107  const VectorXs& get_lb() const;
108 
112  std::size_t get_nf() const;
113 
117  const Matrix3s& get_R() const;
118 
119  DEPRECATED("Use get_R.", Vector3s get_nsurf();)
120 
124  const Scalar get_mu() const;
125 
130  bool get_inner_appr() const;
131 
135  const Scalar get_min_nforce() const;
136 
140  const Scalar get_max_nforce() const;
141 
149  void set_R(const Matrix3s& R);
150 
151  DEPRECATED("Use set_R.", void set_nsurf(const Vector3s& nsurf);)
152 
159  void set_mu(const Scalar mu);
160 
168  void set_inner_appr(const bool inner_appr);
169 
176  void set_min_nforce(const Scalar min_nforce);
177 
184  void set_max_nforce(const Scalar max_nforce);
185 
186  FrictionConeTpl<Scalar>& operator=(const FrictionConeTpl<Scalar>& other);
187 
188  template <class Scalar>
189  friend std::ostream& operator<<(std::ostream& os,
190  const FrictionConeTpl<Scalar>& X);
191 
192  private:
193  std::size_t nf_;
194  MatrixX3s A_;
195  VectorXs ub_;
196  VectorXs lb_;
197  Matrix3s R_;
198  Scalar mu_;
199  bool inner_appr_;
201  Scalar min_nforce_;
202  Scalar max_nforce_;
203 };
204 
205 } // namespace crocoddyl
206 /* --- Details -------------------------------------------------------------- */
207 /* --- Details -------------------------------------------------------------- */
208 /* --- Details -------------------------------------------------------------- */
209 #include "crocoddyl/multibody/friction-cone.hxx"
210 
211 #endif // CROCODDYL_MULTIBODY_FRICTION_CONE_HPP_
This class encapsulates a friction cone.
void set_max_nforce(const Scalar max_nforce)
Modify the maximum normal force.
bool get_inner_appr() const
Return the label that describes the type of friction cone approximation (inner/outer)
const Scalar get_max_nforce() const
Return the maximum normal force.
const MatrixX3s & get_A() const
Return the matrix of friction cone.
void set_inner_appr(const bool inner_appr)
Modify the label that describes the type of friction cone approximation (inner/outer)
const VectorXs & get_lb() const
Return the lower bound of the friction cone.
const Scalar get_mu() const
Return the friction coefficient.
void set_min_nforce(const Scalar min_nforce)
Modify the maximum normal force.
std::size_t get_nf() const
Return the number of facets.
FrictionConeTpl()
Initialize the friction cone.
const VectorXs & get_ub() const
Return the upper bound of the friction cone.
void set_R(const Matrix3s &R)
Modify the rotation matrix that defines the cone orientation w.r.t. the inertial frame.
const Scalar get_min_nforce() const
Return the minimum normal force.
void set_mu(const Scalar mu)
Modify friction coefficient.
const Matrix3s & get_R() const
Return the surface normal vector.
void update()
Update the matrix and bound of friction cone inequalities in the world frame.
FrictionConeTpl(const Matrix3s &R, const Scalar mu, std::size_t nf=4, const bool inner_appr=true, const Scalar min_nforce=Scalar(0.), const Scalar max_nforce=std::numeric_limits< Scalar >::infinity())
Initialize the wrench cone.
DEPRECATED("Use constructor based on rotation matrix.", FrictionConeTpl(const Vector3s &normal, const Scalar mu, std::size_t nf=4, const bool inner_appr=true, const Scalar min_nforce=Scalar(0.), const Scalar max_nforce=std::numeric_limits< Scalar >::infinity());) FrictionConeTpl(const FrictionConeTpl< Scalar > &cone)
Initialize the wrench cone.