9 #ifndef CROCODDYL_MULTIBODY_FRICTION_CONE_HPP_ 10 #define CROCODDYL_MULTIBODY_FRICTION_CONE_HPP_ 12 #include "crocoddyl/multibody/fwd.hpp" 13 #include "crocoddyl/core/mathbase.hpp" 14 #include "crocoddyl/core/utils/deprecate.hpp" 26 template <
typename _Scalar>
29 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
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.),
const Scalar max_nforce = std::numeric_limits<Scalar>::max());
57 DEPRECATED(
"Use constructor based on rotation matrix.",
58 FrictionConeTpl(
const Vector3s& normal,
const Scalar mu, std::size_t nf = 4,
const bool inner_appr =
true,
59 const Scalar min_nforce = Scalar(0.),
60 const Scalar max_nforce = std::numeric_limits<Scalar>::max());)
79 DEPRECATED(
"Use update()",
void update(
const Vector3s& normal,
const Scalar mu,
const bool inner_appr =
true,
80 const Scalar min_nforce = Scalar(0.),
81 const Scalar max_nforce = std::numeric_limits<Scalar>::max()));
86 const MatrixX3s&
get_A()
const;
91 const VectorXs&
get_ub()
const;
96 const VectorXs&
get_lb()
const;
101 std::size_t
get_nf()
const;
106 const Matrix3s&
get_R()
const;
108 DEPRECATED(
"Use get_R.", Vector3s get_nsurf();)
113 const Scalar
get_mu()
const;
135 void set_R(
const Matrix3s& R);
137 DEPRECATED(
"Use set_R.",
void set_nsurf(
const Vector3s& nsurf);)
144 void set_mu(
const Scalar mu);
169 template <
class Scalar>
170 friend std::ostream& operator<<(std::ostream& os, const FrictionConeTpl<Scalar>& X);
188 #include "crocoddyl/multibody/friction-cone.hxx" 190 #endif // CROCODDYL_MULTIBODY_FRICTION_CONE_HPP_ const Matrix3s & get_R() const
Return the surface normal vector.
bool get_inner_appr() const
Return the label that describes the type of friction cone approximation (inner/outer) ...
void set_mu(const Scalar mu)
Modify friction coefficient.
This class encapsulates a friction cone.
void update()
Update the matrix and bound of friction cone inequalities in the world frame.
void set_inner_appr(const bool inner_appr)
Modify the label that describes the type of friction cone approximation (inner/outer) ...
void set_min_nforce(const Scalar min_nforce)
Modify the maximum normal force.
void set_R(const Matrix3s &R)
Modify the rotation matrix that defines the cone orientation w.r.t. the inertial frame.
void set_max_nforce(const Scalar max_nforce)
Modify the maximum normal force.
const VectorXs & get_ub() const
Return the upper bound of the friction cone.
const Scalar get_max_nforce() const
Return the maximum normal force.
const MatrixX3s & get_A() const
Return the matrix of friction cone.
const Scalar get_min_nforce() const
Return the minimum normal force.
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 >::max());) FrictionConeTpl(const FrictionConeTpl< Scalar > &cone)
Initialize the wrench cone.
const Scalar get_mu() const
Return the friction coefficient.
const VectorXs & get_lb() const
Return the lower bound of the friction cone.
std::size_t get_nf() const
Return the number of facets.
FrictionConeTpl()
Initialize the friction cone.