9 #ifndef CROCODDYL_MULTIBODY_WRENCH_CONE_HPP_ 10 #define CROCODDYL_MULTIBODY_WRENCH_CONE_HPP_ 12 #include "crocoddyl/multibody/fwd.hpp" 13 #include "crocoddyl/core/mathbase.hpp" 25 template <
typename _Scalar>
28 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
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());
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());
91 const MatrixX6s&
get_A()
const;
96 const VectorXs&
get_lb()
const;
101 const VectorXs&
get_ub()
const;
106 const Matrix3s&
get_R()
const;
111 const Vector2s&
get_box()
const;
116 const Scalar&
get_mu()
const;
121 const std::size_t&
get_nf()
const;
136 void set_R(Matrix3s R);
161 template <
class Scalar>
162 friend std::ostream& operator<<(std::ostream& os, const WrenchConeTpl<Scalar>& X);
178 #include "crocoddyl/multibody/wrench-cone.hxx" 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)