crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
friction-cone.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, University of Edinburgh
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 
15 namespace crocoddyl {
16 
17 template <typename _Scalar>
19  public:
20  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
21 
22  typedef _Scalar Scalar;
24  typedef typename MathBase::Vector3s Vector3s;
25  typedef typename MathBase::Matrix3s Matrix3s;
26  typedef typename MathBase::VectorXs VectorXs;
27  typedef typename MathBase::MatrixXs MatrixXs;
28  typedef typename MathBase::MatrixX3s MatrixX3s;
29  typedef typename MathBase::Quaternions Quaternions;
30 
31  explicit FrictionConeTpl();
32  FrictionConeTpl(const Vector3s& normal, const Scalar& mu, std::size_t nf = 4, bool inner_appr = true,
33  const Scalar& min_nforce = Scalar(0.),
34  const Scalar& max_nforce = std::numeric_limits<Scalar>::max());
36  ~FrictionConeTpl();
37 
38  void update(const Vector3s& normal, const Scalar& mu, bool inner_appr = true, const Scalar& min_nforce = Scalar(0.),
39  const Scalar& max_nforce = std::numeric_limits<Scalar>::max());
40 
41  const MatrixX3s& get_A() const;
42  const VectorXs& get_lb() const;
43  const VectorXs& get_ub() const;
44  const Vector3s& get_nsurf() const;
45  const Scalar& get_mu() const;
46  const std::size_t& get_nf() const;
47  const bool& get_inner_appr() const;
48  const Scalar& get_min_nforce() const;
49  const Scalar& get_max_nforce() const;
50 
51  void set_nsurf(Vector3s nf);
52  void set_mu(Scalar mu);
53  void set_inner_appr(bool inner_appr);
54  void set_min_nforce(Scalar min_nforce);
55  void set_max_nforce(Scalar max_nforce);
56 
57  template <class Scalar>
58  friend std::ostream& operator<<(std::ostream& os, const FrictionConeTpl<Scalar>& X);
59 
60  private:
61  MatrixX3s A_;
62  VectorXs lb_;
63  VectorXs ub_;
64  Vector3s nsurf_;
65  Scalar mu_;
66  std::size_t nf_;
67  bool inner_appr_;
68  Scalar min_nforce_;
69  Scalar max_nforce_;
70 };
71 
72 } // namespace crocoddyl
73 /* --- Details -------------------------------------------------------------- */
74 /* --- Details -------------------------------------------------------------- */
75 /* --- Details -------------------------------------------------------------- */
76 #include "crocoddyl/multibody/friction-cone.hxx"
77 
78 #endif // CROCODDYL_MULTIBODY_FRICTION_CONE_HPP_