hpp-rbprm 4.14.0
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
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>
9namespace hpp {
10namespace rbprm {
11
12HPP_PREDEF_CLASS(RbPrmFullBody);
13class RbPrmFullBody;
14typedef shared_ptr<RbPrmFullBody> RbPrmFullBodyPtr_t;
15
16namespace reachability {
17
18enum Status {
28};
29
30struct Result {
33 x(fcl::Vec3f::Zero()),
34 xBreak_(fcl::Vec3f::Zero()),
35 xCreate_(fcl::Vec3f::Zero()),
37 path_() {}
38
40 : status(status),
41 x(fcl::Vec3f::Zero()),
42 xBreak_(fcl::Vec3f::Zero()),
43 xCreate_(fcl::Vec3f::Zero()),
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()),
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
79std::pair<MatrixXX, VectorX> stackConstraints(
80 const std::pair<MatrixXX, VectorX>& Ab,
81 const std::pair<MatrixXX, VectorX>& Cd);
82
83bool intersectionExist(const std::pair<MatrixXX, VectorX>& Ab,
84 const fcl::Vec3f& c, fcl::Vec3f& c_out);
85
86std::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
91std::pair<MatrixXX, VectorX> computeStabilityConstraintsForState(
92 const RbPrmFullBodyPtr_t& fullbody, State& state, bool& success,
93 const fcl::Vec3f& acc);
94
95std::pair<MatrixXX, VectorX> computeConstraintsForState(
96 const RbPrmFullBodyPtr_t& fullbody, State& state, bool& success);
97
113Result isReachable(const RbPrmFullBodyPtr_t& fullbody, State& previous,
114 State& next, const fcl::Vec3f& acc = fcl::Vec3f::Zero(),
115 bool useIntermediateState = false);
116
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)
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,...
bool intersectionExist(const std::pair< MatrixXX, VectorX > &Ab, const fcl::Vec3f &c, fcl::Vec3f &c_out)
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))
Status
Definition: reachability.hh:18
@ NO_CONTACT_VARIATION
Definition: reachability.hh:23
@ SAME_ROOT_POSITION
Definition: reachability.hh:21
@ QUASI_STATIC
Definition: reachability.hh:26
@ TOO_MANY_CONTACTS_VARIATION
Definition: reachability.hh:22
@ UNREACHABLE
Definition: reachability.hh:19
@ REACHABLE
Definition: reachability.hh:27
@ CONTACT_BREAK_FAILED
Definition: reachability.hh:24
@ UNABLE_TO_COMPUTE
Definition: reachability.hh:20
@ CONTACT_CREATION_FAILED
Definition: reachability.hh:25
std::pair< MatrixXX, VectorX > computeStabilityConstraintsForState(const RbPrmFullBodyPtr_t &fullbody, State &state, bool &success, const fcl::Vec3f &acc)
std::pair< MatrixXX, VectorX > computeConstraintsForState(const RbPrmFullBodyPtr_t &fullbody, State &state, bool &success)
std::pair< MatrixXX, VectorX > stackConstraints(const std::pair< MatrixXX, VectorX > &Ab, const std::pair< MatrixXX, VectorX > &Cd)
HPP_PREDEF_CLASS(RbPrmFullBody)
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, 1 > VectorX
Definition: rbprm-limb.hh:48
shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
Definition: algorithm.hh:26
Definition: rbprm-state.hh:40
Definition: reachability.hh:30
Status status
Definition: reachability.hh:69
fcl::Vec3f xCreate_
Definition: reachability.hh:72
fcl::Vec3f xBreak_
Definition: reachability.hh:71
std::pair< MatrixXX, VectorX > constraints_
Definition: reachability.hh:73
bool success()
Definition: reachability.hh:55
fcl::Vec3f x
Definition: reachability.hh:70
Result()
Definition: reachability.hh:31
Result(Status status, fcl::Vec3f x)
Definition: reachability.hh:47
std::vector< core::PathPtr_t > pathPerPhases_
Definition: reachability.hh:76
core::PathPtr_t path_
Definition: reachability.hh:74
bool pathExist()
Definition: reachability.hh:60
Result(Status status)
Definition: reachability.hh:39
VectorX timings_
Definition: reachability.hh:75