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" 14 #include "crocoddyl/core/utils/deprecate.hpp" 29 template <
typename _Scalar>
32 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
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>::max());
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,
const Scalar max_nforce = std::numeric_limits<Scalar>::max());)
88 void update(
const Matrix3s& R,
const Scalar mu,
const Vector2s& box,
const Scalar min_nforce = Scalar(0.),
89 const Scalar max_nforce = std::numeric_limits<Scalar>::max()));
94 const MatrixX6s&
get_A()
const;
99 const VectorXs&
get_lb()
const;
104 const VectorXs&
get_ub()
const;
109 std::size_t
get_nf()
const;
114 const Matrix3s&
get_R()
const;
119 const Vector2s&
get_box()
const;
124 const Scalar
get_mu()
const;
146 void set_R(
const Matrix3s& R);
153 void set_box(
const Vector2s& box);
160 void set_mu(
const Scalar mu);
183 WrenchConeTpl<Scalar>& operator=(
const WrenchConeTpl<Scalar>& other);
185 template <
class Scalar>
186 friend std::ostream& operator<<(std::ostream& os, const WrenchConeTpl<Scalar>& X);
203 #include "crocoddyl/multibody/wrench-cone.hxx" 205 #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 >::max());) 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.