19 #ifndef HPP_RBPRM_STABILITY_HH
20 #define HPP_RBPRM_STABILITY_HH
22 #include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
23 #include <hpp/pinocchio/device.hh>
34 typedef Eigen::Matrix<pinocchio::value_type, Eigen::Dynamic, Eigen::Dynamic,
37 typedef Eigen::Matrix<pinocchio::value_type, Eigen::Dynamic, 1>
VectorX;
48 fcl::Vec3f acc = fcl::Vec3f(0, 0, 0),
49 fcl::Vec3f com = fcl::Vec3f(0, 0, 0),
50 const centroidal_dynamics::EquilibriumAlgorithm =
51 centroidal_dynamics::EQUILIBRIUM_ALGORITHM_DLP);
60 const core::value_type friction = 0.5);
66 centroidal_dynamics::Equilibrium& sEq,
67 centroidal_dynamics::EquilibriumAlgorithm& alg,
68 core::value_type friction = 0.6,
const double feetX = 0,
69 const double feetY = 0);
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixXX
Definition: stability.hh:36
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, 1 > VectorX
Definition: stability.hh:37
double IsStable(const RbPrmFullBodyPtr_t fullbody, State &state, fcl::Vec3f acc=fcl::Vec3f(0, 0, 0), fcl::Vec3f com=fcl::Vec3f(0, 0, 0), const centroidal_dynamics::EquilibriumAlgorithm=centroidal_dynamics::EQUILIBRIUM_ALGORITHM_DLP)
centroidal_dynamics::Vector3 setupLibrary(const RbPrmFullBodyPtr_t fullbody, State &state, centroidal_dynamics::Equilibrium &sEq, centroidal_dynamics::EquilibriumAlgorithm &alg, core::value_type friction=0.6, const double feetX=0, const double feetY=0)
centroidal_dynamics::Equilibrium initLibrary(const RbPrmFullBodyPtr_t fullbody)
std::pair< MatrixXX, VectorX > ComputeCentroidalCone(const RbPrmFullBodyPtr_t fullbody, State &state, const core::value_type friction=0.5)
shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:11
Eigen::Matrix< pinocchio::value_type, 3, 1 > Vector3
Definition: rbprm-limb.hh:49
Definition: algorithm.hh:26
Definition: rbprm-state.hh:40