crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (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/multibody/fwd.hpp"
13 #include "crocoddyl/core/mathbase.hpp"
14 #include "crocoddyl/core/utils/deprecate.hpp"
15 
16 namespace crocoddyl {
17 
29 template <typename _Scalar>
30 class WrenchConeTpl {
31  public:
32  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
33 
34  typedef _Scalar Scalar;
35  typedef MathBaseTpl<Scalar> MathBase;
36  typedef typename MathBase::Vector2s Vector2s;
37  typedef typename MathBase::Vector3s Vector3s;
38  typedef typename MathBase::Matrix3s Matrix3s;
39  typedef typename MathBase::Matrix6s Matrix6s;
40  typedef typename MathBase::VectorXs VectorXs;
41  typedef typename MathBase::MatrixXs MatrixXs;
42  typedef typename MathBase::MatrixX3s MatrixX3s;
43  typedef typename MathBase::MatrixX6s MatrixX6s;
44  typedef typename MathBase::Quaternions Quaternions;
45 
49  explicit WrenchConeTpl();
50 
62  WrenchConeTpl(const Matrix3s& R, const Scalar mu, const Vector2s& box, const std::size_t nf = 4,
63  const bool inner_appr = true, const Scalar min_nforce = Scalar(0.),
64  const Scalar max_nforce = std::numeric_limits<Scalar>::infinity());
65  DEPRECATED("Use constructor that includes inner_appr",
66  WrenchConeTpl(const Matrix3s& R, const Scalar mu, const Vector2s& box, std::size_t nf,
67  const Scalar min_nforce,
68  const Scalar max_nforce = std::numeric_limits<Scalar>::infinity());)
69 
75  WrenchConeTpl(const WrenchConeTpl<Scalar>& cone);
76  ~WrenchConeTpl();
77 
87  void update();
88  DEPRECATED("Use update().",
89  void update(const Matrix3s& R, const Scalar mu, const Vector2s& box, const Scalar min_nforce = Scalar(0.),
90  const Scalar max_nforce = std::numeric_limits<Scalar>::infinity()));
91 
95  const MatrixX6s& get_A() const;
96 
100  const VectorXs& get_lb() const;
101 
105  const VectorXs& get_ub() const;
106 
110  std::size_t get_nf() const;
111 
115  const Matrix3s& get_R() const;
116 
120  const Vector2s& get_box() const;
121 
125  const Scalar get_mu() const;
126 
130  bool get_inner_appr() const;
131 
135  const Scalar get_min_nforce() const;
136 
140  const Scalar get_max_nforce() const;
141 
147  void set_R(const Matrix3s& R);
148 
154  void set_box(const Vector2s& box);
155 
161  void set_mu(const Scalar mu);
162 
168  void set_inner_appr(const bool inner_appr);
169 
175  void set_min_nforce(const Scalar min_nforce);
176 
182  void set_max_nforce(const Scalar max_nforce);
183 
184  WrenchConeTpl<Scalar>& operator=(const WrenchConeTpl<Scalar>& other);
185 
186  template <class Scalar>
187  friend std::ostream& operator<<(std::ostream& os, const WrenchConeTpl<Scalar>& X);
188 
189  private:
190  std::size_t nf_;
191  MatrixX6s A_;
192  VectorXs ub_;
193  VectorXs lb_;
194  Matrix3s R_;
195  Vector2s box_;
196  Scalar mu_;
197  bool inner_appr_;
198  Scalar min_nforce_;
199  Scalar max_nforce_;
200 };
201 
202 } // namespace crocoddyl
203 
204 #include "crocoddyl/multibody/wrench-cone.hxx"
205 
206 #endif // CROCODDYL_MULTIBODY_WRENCH_CONE_HPP_
const Matrix3s & get_R() const
Return the rotation matrix that defines the cone orientation w.r.t. the inertial frame.
const VectorXs & get_lb() const
Return the lower bound of inequalities.
const MatrixX6s & get_A() const
Return the matrix of wrench cone.
WrenchConeTpl()
Initialize the wrench cone.
const Vector2s & get_box() const
Return dimension of the foot surface dim = (length, width)
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.
const Scalar get_min_nforce() const
Return the minimum normal force.
void set_min_nforce(const Scalar min_nforce)
Modify the minium normal force.
void set_mu(const Scalar mu)
Modify friction coefficient.
std::size_t get_nf() const
Return the number of facets.
const VectorXs & get_ub() const
Return the upper bound of inequalities.
void update()
Update the matrix of wrench cone inequalities in the world frame.
void set_max_nforce(const Scalar max_nforce)
Modify the maximum normal force.
void set_inner_appr(const bool inner_appr)
Modify the label that describes the type of friction cone approximation (inner/outer) ...
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.
bool get_inner_appr() const
Return the label that describes the type of friction cone approximation (inner/outer) ...
void set_R(const Matrix3s &R)
Modify the rotation matrix that defines the cone orientation w.r.t. the inertial frame.
const Scalar get_mu() const
Return friction coefficient.