hpp-rbprm  4.10.1
Implementation of RB-PRM planner using hpp.
reachability.hh
Go to the documentation of this file.
1 #ifndef HPP_RBPRM_REACHABILITY_HH
2 #define HPP_RBPRM_REACHABILITY_HH
3 
5 #include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
8 #include <hpp/core/path.hh>
9 namespace hpp {
10 namespace rbprm {
11 
12 HPP_PREDEF_CLASS(RbPrmFullBody);
13 class RbPrmFullBody;
14 typedef boost::shared_ptr<RbPrmFullBody> RbPrmFullBodyPtr_t;
15 
16 namespace reachability {
17 
18 enum Status {
28 };
29 
30 struct Result {
33  x(fcl::Vec3f::Zero()),
34  xBreak_(fcl::Vec3f::Zero()),
35  xCreate_(fcl::Vec3f::Zero()),
36  constraints_(),
37  path_() {}
38 
40  : status(status),
41  x(fcl::Vec3f::Zero()),
42  xBreak_(fcl::Vec3f::Zero()),
43  xCreate_(fcl::Vec3f::Zero()),
44  constraints_(),
45  path_() {}
46 
47  Result(Status status, fcl::Vec3f x)
48  : status(status), x(x), xBreak_(fcl::Vec3f::Zero()), xCreate_(fcl::Vec3f::Zero()), constraints_(), path_() {}
49 
50  bool success() {
53  }
54 
55  bool pathExist() {
56  if (path_) {
57  if (path_->length() > 0) {
58  return true;
59  }
60  }
61  return false;
62  }
63 
65  fcl::Vec3f x;
66  fcl::Vec3f xBreak_;
67  fcl::Vec3f xCreate_;
68  std::pair<MatrixXX, VectorX> constraints_;
69  core::PathPtr_t path_;
71  std::vector<core::PathPtr_t> pathPerPhases_;
72 };
73 
74 std::pair<MatrixXX, VectorX> stackConstraints(const std::pair<MatrixXX, VectorX>& Ab,
75  const std::pair<MatrixXX, VectorX>& Cd);
76 
77 bool intersectionExist(const std::pair<MatrixXX, VectorX>& Ab, const fcl::Vec3f& c, fcl::Vec3f& c_out);
78 
79 std::pair<MatrixXX, VectorX> computeStabilityConstraints(const centroidal_dynamics::Equilibrium& contactPhase,
80  const fcl::Vec3f& int_point = fcl::Vec3f(0, 0, 0),
81  const fcl::Vec3f& acc = fcl::Vec3f(0, 0, 0));
82 
83 std::pair<MatrixXX, VectorX> computeStabilityConstraintsForState(const RbPrmFullBodyPtr_t& fullbody, State& state,
84  bool& success, const fcl::Vec3f& acc);
85 
86 std::pair<MatrixXX, VectorX> computeConstraintsForState(const RbPrmFullBodyPtr_t& fullbody, State& state,
87  bool& success);
88 
101 Result isReachable(const RbPrmFullBodyPtr_t& fullbody, State& previous, State& next,
102  const fcl::Vec3f& acc = fcl::Vec3f::Zero(), bool useIntermediateState = false);
103 
104 Result isReachableDynamic(const RbPrmFullBodyPtr_t& fullbody, State& previous, State& next, bool tryQuasiStatic = true,
105  std::vector<double> timings = std::vector<double>(), int numPointsPerPhases = 0);
106 
107 } // namespace reachability
108 } // namespace rbprm
109 } // namespace hpp
110 #endif // REACHABILITY_HH
rbprm-limb.hh
hpp::rbprm::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(RbPrmFullBody)
hpp::rbprm::reachability::Result::timings_
VectorX timings_
Definition: reachability.hh:70
hpp::rbprm::reachability::CONTACT_CREATION_FAILED
@ CONTACT_CREATION_FAILED
Definition: reachability.hh:25
hpp::rbprm::reachability::Result::pathExist
bool pathExist()
Definition: reachability.hh:55
hpp::rbprm::reachability::computeStabilityConstraints
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))
hpp::rbprm::reachability::isReachableDynamic
Result isReachableDynamic(const RbPrmFullBodyPtr_t &fullbody, State &previous, State &next, bool tryQuasiStatic=true, std::vector< double > timings=std::vector< double >(), int numPointsPerPhases=0)
kinematics_constraints.hh
rbprm-state.hh
hpp::rbprm::reachability::NO_CONTACT_VARIATION
@ NO_CONTACT_VARIATION
Definition: reachability.hh:23
hpp::rbprm::reachability::TOO_MANY_CONTACTS_VARIATION
@ TOO_MANY_CONTACTS_VARIATION
Definition: reachability.hh:22
hpp::rbprm::reachability::Status
Status
Definition: reachability.hh:18
hpp::rbprm::reachability::stackConstraints
std::pair< MatrixXX, VectorX > stackConstraints(const std::pair< MatrixXX, VectorX > &Ab, const std::pair< MatrixXX, VectorX > &Cd)
hpp::rbprm::reachability::CONTACT_BREAK_FAILED
@ CONTACT_BREAK_FAILED
Definition: reachability.hh:24
hpp::rbprm::VectorX
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, 1 > VectorX
Definition: rbprm-limb.hh:47
hpp::rbprm::reachability::SAME_ROOT_POSITION
@ SAME_ROOT_POSITION
Definition: reachability.hh:21
hpp::rbprm::reachability::Result::xCreate_
fcl::Vec3f xCreate_
Definition: reachability.hh:67
hpp::rbprm::reachability::Result::Result
Result(Status status)
Definition: reachability.hh:39
hpp::rbprm::reachability::Result::constraints_
std::pair< MatrixXX, VectorX > constraints_
Definition: reachability.hh:68
hpp::rbprm::reachability::UNABLE_TO_COMPUTE
@ UNABLE_TO_COMPUTE
Definition: reachability.hh:20
hpp::rbprm::reachability::Result::Result
Result()
Definition: reachability.hh:31
hpp
Definition: algorithm.hh:27
hpp::rbprm::reachability::Result
Definition: reachability.hh:30
hpp::rbprm::reachability::intersectionExist
bool intersectionExist(const std::pair< MatrixXX, VectorX > &Ab, const fcl::Vec3f &c, fcl::Vec3f &c_out)
hpp::rbprm::reachability::Result::success
bool success()
Definition: reachability.hh:50
hpp::rbprm::reachability::Result::status
Status status
Definition: reachability.hh:64
hpp::rbprm::reachability::Result::x
fcl::Vec3f x
Definition: reachability.hh:65
hpp::rbprm::reachability::UNREACHABLE
@ UNREACHABLE
Definition: reachability.hh:19
hpp::rbprm::reachability::Result::Result
Result(Status status, fcl::Vec3f x)
Definition: reachability.hh:47
hpp::rbprm::reachability::QUASI_STATIC
@ QUASI_STATIC
Definition: reachability.hh:26
hpp::rbprm::reachability::isReachable
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,...
hpp::rbprm::reachability::computeStabilityConstraintsForState
std::pair< MatrixXX, VectorX > computeStabilityConstraintsForState(const RbPrmFullBodyPtr_t &fullbody, State &state, bool &success, const fcl::Vec3f &acc)
hpp::rbprm::State
Definition: rbprm-state.hh:40
hpp::rbprm::RbPrmFullBodyPtr_t
boost::shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
hpp::rbprm::reachability::Result::xBreak_
fcl::Vec3f xBreak_
Definition: reachability.hh:66
hpp::rbprm::reachability::Result::pathPerPhases_
std::vector< core::PathPtr_t > pathPerPhases_
Definition: reachability.hh:71
hpp::rbprm::reachability::Result::path_
core::PathPtr_t path_
Definition: reachability.hh:69
hpp::rbprm::reachability::computeConstraintsForState
std::pair< MatrixXX, VectorX > computeConstraintsForState(const RbPrmFullBodyPtr_t &fullbody, State &state, bool &success)
hpp::rbprm::reachability::REACHABLE
@ REACHABLE
Definition: reachability.hh:27