1 #ifndef HPP_RBPRM_REACHABILITY_HH 2 #define HPP_RBPRM_REACHABILITY_HH 4 #include <hpp/centroidal-dynamics/centroidal_dynamics.hh> 5 #include <hpp/core/path.hh> 16 namespace reachability {
33 x(fcl::Vec3f::Zero()),
41 x(fcl::Vec3f::Zero()),
62 if (
path_->length() > 0) {
80 const std::pair<MatrixXX, VectorX>& Ab,
81 const std::pair<MatrixXX, VectorX>& Cd);
84 const fcl::Vec3f& c, fcl::Vec3f& c_out);
87 const centroidal_dynamics::Equilibrium& contactPhase,
88 const fcl::Vec3f& int_point = fcl::Vec3f(0, 0, 0),
89 const fcl::Vec3f& acc = fcl::Vec3f(0, 0, 0));
92 const RbPrmFullBodyPtr_t& fullbody,
State& state,
bool&
success,
93 const fcl::Vec3f& acc);
96 const RbPrmFullBodyPtr_t& fullbody,
State& state,
bool&
success);
114 State& next,
const fcl::Vec3f& acc = fcl::Vec3f::Zero(),
115 bool useIntermediateState =
false);
118 State& next,
bool tryQuasiStatic =
true,
119 std::vector<double> timings = std::vector<double>(),
120 int numPointsPerPhases = 0);
125 #endif // REACHABILITY_HH Result isReachableDynamic(const RbPrmFullBodyPtr_t &fullbody, State &previous, State &next, bool tryQuasiStatic=true, std::vector< double > timings=std::vector< double >(), int numPointsPerPhases=0)
fcl::Vec3f xBreak_
Definition: reachability.hh:71
std::pair< MatrixXX, VectorX > constraints_
Definition: reachability.hh:73
Definition: algorithm.hh:26
core::PathPtr_t path_
Definition: reachability.hh:74
Definition: reachability.hh:25
VectorX timings_
Definition: reachability.hh:75
Definition: reachability.hh:22
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, 1 > VectorX
Definition: rbprm-limb.hh:48
Result isReachable(const RbPrmFullBodyPtr_t &fullbody, State &previous, State &next, const fcl::Vec3f &acc=fcl::Vec3f::Zero(), bool useIntermediateState=false)
isReachable Compute the feasibility of the contact transition between the two state, with the quasiStatic formulation of 2-PAC (https://hal.archives-ouvertes.fr/hal-01609055)
Definition: reachability.hh:23
Definition: reachability.hh:24
std::pair< MatrixXX, VectorX > computeConstraintsForState(const RbPrmFullBodyPtr_t &fullbody, State &state, bool &success)
Definition: reachability.hh:21
HPP_PREDEF_CLASS(RbPrmFullBody)
Definition: reachability.hh:30
Definition: reachability.hh:20
fcl::Vec3f xCreate_
Definition: reachability.hh:72
Result(Status status)
Definition: reachability.hh:39
Definition: reachability.hh:26
Definition: reachability.hh:19
Status status
Definition: reachability.hh:69
fcl::Vec3f x
Definition: reachability.hh:70
std::pair< MatrixXX, VectorX > computeStabilityConstraints(const centroidal_dynamics::Equilibrium &contactPhase, const fcl::Vec3f &int_point=fcl::Vec3f(0, 0, 0), const fcl::Vec3f &acc=fcl::Vec3f(0, 0, 0))
bool pathExist()
Definition: reachability.hh:60
Result()
Definition: reachability.hh:31
std::pair< MatrixXX, VectorX > computeStabilityConstraintsForState(const RbPrmFullBodyPtr_t &fullbody, State &state, bool &success, const fcl::Vec3f &acc)
Definition: rbprm-state.hh:40
bool success()
Definition: reachability.hh:55
Definition: reachability.hh:27
std::pair< MatrixXX, VectorX > stackConstraints(const std::pair< MatrixXX, VectorX > &Ab, const std::pair< MatrixXX, VectorX > &Cd)
bool intersectionExist(const std::pair< MatrixXX, VectorX > &Ab, const fcl::Vec3f &c, fcl::Vec3f &c_out)
Status
Definition: reachability.hh:18
Result(Status status, fcl::Vec3f x)
Definition: reachability.hh:47
std::vector< core::PathPtr_t > pathPerPhases_
Definition: reachability.hh:76
shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:11