hpp-rbprm  4.14.0
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 
4 #include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
5 #include <hpp/core/path.hh>
9 namespace hpp {
10 namespace rbprm {
11 
12 HPP_PREDEF_CLASS(RbPrmFullBody);
13 class RbPrmFullBody;
14 typedef 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),
49  x(x),
50  xBreak_(fcl::Vec3f::Zero()),
51  xCreate_(fcl::Vec3f::Zero()),
52  constraints_(),
53  path_() {}
54 
55  bool success() {
56  return (status == REACHABLE) || (status == NO_CONTACT_VARIATION) ||
58  }
59 
60  bool pathExist() {
61  if (path_) {
62  if (path_->length() > 0) {
63  return true;
64  }
65  }
66  return false;
67  }
68 
70  fcl::Vec3f x;
71  fcl::Vec3f xBreak_;
72  fcl::Vec3f xCreate_;
73  std::pair<MatrixXX, VectorX> constraints_;
74  core::PathPtr_t path_;
76  std::vector<core::PathPtr_t> pathPerPhases_;
77 };
78 
79 std::pair<MatrixXX, VectorX> stackConstraints(
80  const std::pair<MatrixXX, VectorX>& Ab,
81  const std::pair<MatrixXX, VectorX>& Cd);
82 
83 bool intersectionExist(const std::pair<MatrixXX, VectorX>& Ab,
84  const fcl::Vec3f& c, fcl::Vec3f& c_out);
85 
86 std::pair<MatrixXX, VectorX> computeStabilityConstraints(
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));
90 
91 std::pair<MatrixXX, VectorX> computeStabilityConstraintsForState(
92  const RbPrmFullBodyPtr_t& fullbody, State& state, bool& success,
93  const fcl::Vec3f& acc);
94 
95 std::pair<MatrixXX, VectorX> computeConstraintsForState(
96  const RbPrmFullBodyPtr_t& fullbody, State& state, bool& success);
97 
113 Result isReachable(const RbPrmFullBodyPtr_t& fullbody, State& previous,
114  State& next, const fcl::Vec3f& acc = fcl::Vec3f::Zero(),
115  bool useIntermediateState = false);
116 
117 Result isReachableDynamic(const RbPrmFullBodyPtr_t& fullbody, State& previous,
118  State& next, bool tryQuasiStatic = true,
119  std::vector<double> timings = std::vector<double>(),
120  int numPointsPerPhases = 0);
121 
122 } // namespace reachability
123 } // namespace rbprm
124 } // namespace hpp
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
VectorX timings_
Definition: reachability.hh:75
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)
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