crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (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/multibody/fwd.hpp"
13 #include "crocoddyl/core/mathbase.hpp"
14 #include "crocoddyl/core/utils/deprecate.hpp"
15 
16 namespace crocoddyl {
17 
26 template <typename _Scalar>
28  public:
29  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
30 
31  typedef _Scalar Scalar;
33  typedef typename MathBase::Vector3s Vector3s;
34  typedef typename MathBase::Matrix3s Matrix3s;
35  typedef typename MathBase::VectorXs VectorXs;
36  typedef typename MathBase::MatrixXs MatrixXs;
37  typedef typename MathBase::MatrixX3s MatrixX3s;
38  typedef typename MathBase::Quaternions Quaternions;
39 
43  explicit FrictionConeTpl();
44 
55  FrictionConeTpl(const Matrix3s& R, const Scalar mu, std::size_t nf = 4, const bool inner_appr = true,
56  const Scalar min_nforce = Scalar(0.),
57  const Scalar max_nforce = std::numeric_limits<Scalar>::infinity());
58  DEPRECATED("Use constructor based on rotation matrix.",
59  FrictionConeTpl(const Vector3s& normal, const Scalar mu, std::size_t nf = 4, const bool inner_appr = true,
60  const Scalar min_nforce = Scalar(0.),
61  const Scalar max_nforce = std::numeric_limits<Scalar>::infinity());)
62 
69  ~FrictionConeTpl();
70 
79  void update();
80  DEPRECATED("Use update()", void update(const Vector3s& normal, const Scalar mu, const bool inner_appr = true,
81  const Scalar min_nforce = Scalar(0.),
82  const Scalar max_nforce = std::numeric_limits<Scalar>::infinity()));
83 
87  const MatrixX3s& get_A() const;
88 
92  const VectorXs& get_ub() const;
93 
97  const VectorXs& get_lb() const;
98 
102  std::size_t get_nf() const;
103 
107  const Matrix3s& get_R() const;
108 
109  DEPRECATED("Use get_R.", Vector3s get_nsurf();)
110 
114  const Scalar get_mu() const;
115 
119  bool get_inner_appr() const;
120 
124  const Scalar get_min_nforce() const;
125 
129  const Scalar get_max_nforce() const;
130 
136  void set_R(const Matrix3s& R);
137 
138  DEPRECATED("Use set_R.", void set_nsurf(const Vector3s& nsurf);)
139 
145  void set_mu(const Scalar mu);
146 
152  void set_inner_appr(const bool inner_appr);
153 
159  void set_min_nforce(const Scalar min_nforce);
160 
166  void set_max_nforce(const Scalar max_nforce);
167 
168  FrictionConeTpl<Scalar>& operator=(const FrictionConeTpl<Scalar>& other);
169 
170  template <class Scalar>
171  friend std::ostream& operator<<(std::ostream& os, const FrictionConeTpl<Scalar>& X);
172 
173  private:
174  std::size_t nf_;
175  MatrixX3s A_;
176  VectorXs ub_;
177  VectorXs lb_;
178  Matrix3s R_;
179  Scalar mu_;
180  bool inner_appr_;
181  Scalar min_nforce_;
182  Scalar max_nforce_;
183 };
184 
185 } // namespace crocoddyl
186 /* --- Details -------------------------------------------------------------- */
187 /* --- Details -------------------------------------------------------------- */
188 /* --- Details -------------------------------------------------------------- */
189 #include "crocoddyl/multibody/friction-cone.hxx"
190 
191 #endif // CROCODDYL_MULTIBODY_FRICTION_CONE_HPP_
crocoddyl::FrictionConeTpl::get_ub
const VectorXs & get_ub() const
Return the upper bound of the friction cone.
crocoddyl::FrictionConeTpl::DEPRECATED
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.
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::FrictionConeTpl::get_lb
const VectorXs & get_lb() const
Return the lower bound of the friction cone.
crocoddyl::FrictionConeTpl::get_mu
const Scalar get_mu() const
Return the friction coefficient.
crocoddyl::FrictionConeTpl::get_nf
std::size_t get_nf() const
Return the number of facets.
crocoddyl::FrictionConeTpl::FrictionConeTpl
FrictionConeTpl()
Initialize the friction cone.
crocoddyl::FrictionConeTpl::get_inner_appr
bool get_inner_appr() const
Return the label that describes the type of friction cone approximation (inner/outer)
crocoddyl::FrictionConeTpl::set_mu
void set_mu(const Scalar mu)
Modify friction coefficient.
crocoddyl::FrictionConeTpl
This class encapsulates a friction cone.
Definition: friction-cone.hpp:27
crocoddyl::FrictionConeTpl::get_R
const Matrix3s & get_R() const
Return the surface normal vector.
crocoddyl::FrictionConeTpl::update
void update()
Update the matrix and bound of friction cone inequalities in the world frame.
crocoddyl::FrictionConeTpl::set_min_nforce
void set_min_nforce(const Scalar min_nforce)
Modify the maximum normal force.
crocoddyl::FrictionConeTpl::set_max_nforce
void set_max_nforce(const Scalar max_nforce)
Modify the maximum normal force.
crocoddyl::FrictionConeTpl::get_max_nforce
const Scalar get_max_nforce() const
Return the maximum normal force.
crocoddyl::FrictionConeTpl::set_inner_appr
void set_inner_appr(const bool inner_appr)
Modify the label that describes the type of friction cone approximation (inner/outer)
crocoddyl::FrictionConeTpl::get_A
const MatrixX3s & get_A() const
Return the matrix of friction cone.
crocoddyl::FrictionConeTpl::set_R
void set_R(const Matrix3s &R)
Modify the rotation matrix that defines the cone orientation w.r.t. the inertial frame.
crocoddyl::FrictionConeTpl::get_min_nforce
const Scalar get_min_nforce() const
Return the minimum normal force.