Crocoddyl
wrench-cone.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-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_WRENCH_CONE_HPP_
10 #define CROCODDYL_MULTIBODY_WRENCH_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 
31 template <typename _Scalar>
33  public:
34  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35 
36  typedef _Scalar Scalar;
38  typedef typename MathBase::Vector2s Vector2s;
39  typedef typename MathBase::Vector3s Vector3s;
40  typedef typename MathBase::Matrix3s Matrix3s;
41  typedef typename MathBase::Matrix6s Matrix6s;
42  typedef typename MathBase::VectorXs VectorXs;
43  typedef typename MathBase::MatrixXs MatrixXs;
44  typedef typename MathBase::MatrixX3s MatrixX3s;
45  typedef typename MathBase::MatrixX6s MatrixX6s;
46  typedef typename MathBase::Quaternions Quaternions;
47 
51  explicit WrenchConeTpl();
52 
67  const Matrix3s& R, const Scalar mu, const Vector2s& box,
68  const std::size_t nf = 4, const bool inner_appr = true,
69  const Scalar min_nforce = Scalar(0.),
70  const Scalar max_nforce = std::numeric_limits<Scalar>::infinity());
71  DEPRECATED("Use constructor that includes inner_appr",
72  WrenchConeTpl(const Matrix3s& R, const Scalar mu,
73  const Vector2s& box, std::size_t nf,
74  const Scalar min_nforce,
75  const Scalar max_nforce =
76  std::numeric_limits<Scalar>::infinity());)
77 
84  ~WrenchConeTpl();
85 
95  void update();
96  DEPRECATED("Use update().",
97  void update(const Matrix3s& R, const Scalar mu,
98  const Vector2s& box,
99  const Scalar min_nforce = Scalar(0.),
100  const Scalar max_nforce =
101  std::numeric_limits<Scalar>::infinity()));
102 
106  const MatrixX6s& get_A() const;
107 
111  const VectorXs& get_lb() const;
112 
116  const VectorXs& get_ub() const;
117 
121  std::size_t get_nf() const;
122 
127  const Matrix3s& get_R() const;
128 
132  const Vector2s& get_box() const;
133 
137  const Scalar get_mu() const;
138 
143  bool get_inner_appr() const;
144 
148  const Scalar get_min_nforce() const;
149 
153  const Scalar get_max_nforce() const;
154 
162  void set_R(const Matrix3s& R);
163 
170  void set_box(const Vector2s& box);
171 
178  void set_mu(const Scalar mu);
179 
187  void set_inner_appr(const bool inner_appr);
188 
195  void set_min_nforce(const Scalar min_nforce);
196 
203  void set_max_nforce(const Scalar max_nforce);
204 
205  WrenchConeTpl<Scalar>& operator=(const WrenchConeTpl<Scalar>& other);
206 
207  template <class Scalar>
208  friend std::ostream& operator<<(std::ostream& os,
209  const WrenchConeTpl<Scalar>& X);
210 
211  private:
212  std::size_t nf_;
213  MatrixX6s A_;
214  VectorXs ub_;
215  VectorXs lb_;
216  Matrix3s R_;
217  Vector2s box_;
218  Scalar mu_;
219  bool inner_appr_;
221  Scalar min_nforce_;
222  Scalar max_nforce_;
223 };
224 
225 } // namespace crocoddyl
226 
227 #include "crocoddyl/multibody/wrench-cone.hxx"
228 
229 #endif // CROCODDYL_MULTIBODY_WRENCH_CONE_HPP_
This class encapsulates a wrench cone.
Definition: wrench-cone.hpp:32
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)
WrenchConeTpl(const Matrix3s &R, const Scalar mu, const Vector2s &box, const 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.
void set_box(const Vector2s &box)
Modify dimension of the foot surface dim = (length, width)
const Scalar get_max_nforce() const
Return the maximum normal force.
WrenchConeTpl()
Initialize the wrench 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 inequalities.
const Scalar get_mu() const
Return friction coefficient.
void set_min_nforce(const Scalar min_nforce)
Modify the minium normal force.
std::size_t get_nf() const
Return the number of facets.
const MatrixX6s & get_A() const
Return the matrix of wrench cone.
const VectorXs & get_ub() const
Return the upper bound of inequalities.
void set_R(const Matrix3s &R)
Modify the rotation matrix that defines the cone orientation w.r.t. the inertial frame.
const Vector2s & get_box() const
Return dimension of the foot surface dim = (length, width)
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 rotation matrix that defines the cone orientation w.r.t. the inertial frame.
void update()
Update the matrix of wrench cone inequalities in the world frame.
DEPRECATED("Use constructor that includes inner_appr", WrenchConeTpl(const Matrix3s &R, const Scalar mu, const Vector2s &box, std::size_t nf, const Scalar min_nforce, const Scalar max_nforce=std::numeric_limits< Scalar >::infinity());) WrenchConeTpl(const WrenchConeTpl< Scalar > &cone)
Initialize the wrench cone.