hpp-rbprm  4.10.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 
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 
17  namespace reachability{
18 
19 
20  enum Status{
30  };
31 
32  struct Result{
33 
34  Result():
36  x(fcl::Vec3f::Zero()),
37  xBreak_(fcl::Vec3f::Zero()),
38  xCreate_(fcl::Vec3f::Zero()),
39  constraints_(),
40  path_()
41  {}
42 
44  status(status),
45  x(fcl::Vec3f::Zero()),
46  xBreak_(fcl::Vec3f::Zero()),
47  xCreate_(fcl::Vec3f::Zero()),
48  constraints_(),
49  path_()
50  {}
51 
52  Result(Status status, fcl::Vec3f x):
53  status(status),
54  x(x),
55  xBreak_(fcl::Vec3f::Zero()),
56  xCreate_(fcl::Vec3f::Zero()),
57  constraints_(),
58  path_()
59  {}
60 
62 
63  bool pathExist(){
64  if(path_){
65  if (path_->length() > 0){
66  return true;
67  }
68  }
69  return false;
70  }
71 
73  fcl::Vec3f x;
74  fcl::Vec3f xBreak_;
75  fcl::Vec3f xCreate_;
76  std::pair<MatrixXX, VectorX> constraints_;
77  core::PathPtr_t path_;
79  std::vector<core::PathPtr_t> pathPerPhases_;
80  };
81 
82 
83 std::pair<MatrixXX, VectorX> stackConstraints(const std::pair<MatrixXX, VectorX> &Ab, const std::pair<MatrixXX, VectorX> &Cd);
84 
85 bool intersectionExist(const std::pair<MatrixXX, VectorX>& Ab, const fcl::Vec3f& c, fcl::Vec3f &c_out);
86 
87 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));
88 
89 std::pair<MatrixXX, VectorX> computeStabilityConstraintsForState(const RbPrmFullBodyPtr_t& fullbody, State &state, bool &success, const fcl::Vec3f &acc);
90 
91 std::pair<MatrixXX, VectorX> computeConstraintsForState(const RbPrmFullBodyPtr_t& fullbody, State &state,bool& success);
92 
103 Result isReachable(const RbPrmFullBodyPtr_t& fullbody,State &previous, State& next,const fcl::Vec3f& acc = fcl::Vec3f::Zero(), bool useIntermediateState = false);
104 
105 Result isReachableDynamic(const RbPrmFullBodyPtr_t& fullbody, State &previous, State& next, bool tryQuasiStatic = true, std::vector<double> timings = std::vector<double>(), int numPointsPerPhases = 0);
106 
107 
108 
109 }
110 }
111 }
112 #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:74
std::pair< MatrixXX, VectorX > constraints_
Definition: reachability.hh:76
Definition: algorithm.hh:27
core::PathPtr_t path_
Definition: reachability.hh:77
VectorX timings_
Definition: reachability.hh:78
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, 1 > VectorX
Definition: rbprm-limb.hh:49
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)
boost::shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
std::pair< MatrixXX, VectorX > computeConstraintsForState(const RbPrmFullBodyPtr_t &fullbody, State &state, bool &success)
Definition: reachability.hh:23
HPP_PREDEF_CLASS(RbPrmFullBody)
Definition: reachability.hh:32
Definition: reachability.hh:22
fcl::Vec3f xCreate_
Definition: reachability.hh:75
Result(Status status)
Definition: reachability.hh:43
Definition: reachability.hh:28
Definition: reachability.hh:21
Status status
Definition: reachability.hh:72
fcl::Vec3f x
Definition: reachability.hh:73
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:63
Result()
Definition: reachability.hh:34
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:61
Definition: reachability.hh:29
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:20
Result(Status status, fcl::Vec3f x)
Definition: reachability.hh:52
std::vector< core::PathPtr_t > pathPerPhases_
Definition: reachability.hh:79