crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
wrench-cone.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020, University of Edinburgh
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 
15 namespace crocoddyl {
16 
25 template <typename _Scalar>
26 class WrenchConeTpl {
27  public:
28  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
29 
30  typedef _Scalar Scalar;
31  typedef MathBaseTpl<Scalar> MathBase;
32  typedef typename MathBase::Vector2s Vector2s;
33  typedef typename MathBase::Vector3s Vector3s;
34  typedef typename MathBase::Matrix3s Matrix3s;
35  typedef typename MathBase::Matrix6s Matrix6s;
36  typedef typename MathBase::VectorXs VectorXs;
37  typedef typename MathBase::MatrixXs MatrixXs;
38  typedef typename MathBase::MatrixX3s MatrixX3s;
39  typedef typename MathBase::MatrixX6s MatrixX6s;
40  typedef typename MathBase::Quaternions Quaternions;
41 
45  explicit WrenchConeTpl();
46 
57  WrenchConeTpl(const Matrix3s& R, const Scalar& mu, const Vector2s& box_size, std::size_t nf = 16,
58  const Scalar& min_nforce = Scalar(0.), const Scalar& max_nforce = std::numeric_limits<Scalar>::max());
59 
65  WrenchConeTpl(const WrenchConeTpl<Scalar>& cone);
66  ~WrenchConeTpl();
67 
85  void update(const Matrix3s& R, const Scalar& mu, const Vector2s& box_size, const Scalar& min_nforce = Scalar(0.),
86  const Scalar& max_nforce = std::numeric_limits<Scalar>::max());
87 
91  const MatrixX6s& get_A() const;
92 
96  const VectorXs& get_lb() const;
97 
101  const VectorXs& get_ub() const;
102 
106  const Matrix3s& get_R() const;
107 
111  const Vector2s& get_box() const;
112 
116  const Scalar& get_mu() const;
117 
121  const std::size_t& get_nf() const;
122 
126  const Scalar& get_min_nforce() const;
127 
131  const Scalar& get_max_nforce() const;
132 
136  void set_R(Matrix3s R);
137 
141  void set_box(Vector2s box);
142 
146  void set_mu(Scalar mu);
147 
151  void set_min_nforce(Scalar min_nforce);
152 
156  void set_max_nforce(Scalar max_nforce);
157 
161  template <class Scalar>
162  friend std::ostream& operator<<(std::ostream& os, const WrenchConeTpl<Scalar>& X);
163 
164  private:
165  MatrixX6s A_;
166  VectorXs ub_;
167  VectorXs lb_;
168  Matrix3s R_;
169  Vector2s box_;
170  Scalar mu_;
171  std::size_t nf_;
172  Scalar min_nforce_;
173  Scalar max_nforce_;
174 };
175 
176 } // namespace crocoddyl
177 
178 #include "crocoddyl/multibody/wrench-cone.hxx"
179 
180 #endif // CROCODDYL_MULTIBODY_WRENCH_CONE_HPP_
const Matrix3s & get_R() const
Return the rotation matrix that defines the cone orientation.
const VectorXs & get_lb() const
Return the lower bound of inequalities.
const MatrixX6s & get_A() const
Return the matrix of wrench cone.
void set_mu(Scalar mu)
Modify friction coefficient.
const Scalar & get_min_nforce() const
Return the minimum normal force.
WrenchConeTpl()
Initialize the wrench cone.
const Vector2s & get_box() const
Return dimension of the foot surface dim = (length, width)
void set_R(Matrix3s R)
Modify the rotation matrix that defines the cone orientation.
const std::size_t & get_nf() const
Return the number of facets.
void set_min_nforce(Scalar min_nforce)
Modify the minium normal force.
void set_max_nforce(Scalar max_nforce)
Modify the maximum normal force.
const Scalar & get_mu() const
Return friction coefficient.
void update(const Matrix3s &R, const Scalar &mu, const Vector2s &box_size, const Scalar &min_nforce=Scalar(0.), const Scalar &max_nforce=std::numeric_limits< Scalar >::max())
Update the matrix of wrench cone inequaliteis in the world frame.
const Scalar & get_max_nforce() const
Return the maximum normal force.
const VectorXs & get_ub() const
Return the upper bound of inequalities.
void set_box(Vector2s box)
Modify dimension of the foot surface dim = (length, width)