hpp-rbprm 4.14.0
Implementation of RB-PRM planner using hpp.
|
Typedefs | |
typedef Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > | MatrixXX |
typedef Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, 1 > | VectorX |
Functions | |
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) |
std::pair< MatrixXX, VectorX > | ComputeCentroidalCone (const RbPrmFullBodyPtr_t fullbody, State &state, const core::value_type friction=0.5) |
centroidal_dynamics::Equilibrium | initLibrary (const RbPrmFullBodyPtr_t fullbody) |
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) |
bool | Contains (const Eigen::Matrix< double, Eigen::Dynamic, 1 > support, const Eigen::Vector3d &aPoint, const Eigen::VectorXd &xs, const Eigen::VectorXd &ys) |
typedef Eigen::Matrix<pinocchio::value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> hpp::rbprm::stability::MatrixXX |
typedef Eigen::Matrix<pinocchio::value_type, Eigen::Dynamic, 1> hpp::rbprm::stability::VectorX |
std::pair< MatrixXX, VectorX > hpp::rbprm::stability::ComputeCentroidalCone | ( | const RbPrmFullBodyPtr_t | fullbody, |
State & | state, | ||
const core::value_type | friction = 0.5 |
||
) |
Using the polytope computation of the gravito inertial wrench cone, returns the CWC of the robot at a given state
fullbody | The considered robot for static equilibrium |
state | The current State of the robots, in terms of contact creation |
bool hpp::rbprm::stability::Contains | ( | const Eigen::Matrix< double, Eigen::Dynamic, 1 > | support, |
const Eigen::Vector3d & | aPoint, | ||
const Eigen::VectorXd & | xs, | ||
const Eigen::VectorXd & | ys | ||
) |
Implementation of the gift wrapping algorithm to determine the 2D projection of the convex hull of a 3D set of rectangles and whether a point belongs to it or not.
support | a Vector containing all the points at the center of the rectangles used to determine the convex hull |
aPoint | The point for which to test belonging the the convex hull |
xs | Vector of width offsets for the rectangle in support |
ys | Vector of heights offsets for the rectangle in support |
aPoint | The point for which to test belonging the the convex hull return whether aPoint belongs to the convex polygon determined as the convex hull of the rectangle indicated |
centroidal_dynamics::Equilibrium hpp::rbprm::stability::initLibrary | ( | const RbPrmFullBodyPtr_t | fullbody | ) |
double hpp::rbprm::stability::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 |
||
) |
Using the polytope computation of the gravito inertial wrench cone, performs a static equilibrium test on the robot.
fullbody | The considered robot for static equilibrium |
state | The current State of the robots, in terms of contact creation |
acc | acceleration of the COM of the robot |
centroidal_dynamics::Vector3 hpp::rbprm::stability::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 |
||
) |