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> |
6 |
|
|
#include <hpp/rbprm/contact_generation/kinematics_constraints.hh> |
7 |
|
|
#include <hpp/rbprm/rbprm-limb.hh> |
8 |
|
|
#include <hpp/rbprm/rbprm-state.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 { |
19 |
|
|
UNREACHABLE, |
20 |
|
|
UNABLE_TO_COMPUTE, |
21 |
|
|
SAME_ROOT_POSITION, |
22 |
|
|
TOO_MANY_CONTACTS_VARIATION, |
23 |
|
|
NO_CONTACT_VARIATION, |
24 |
|
|
CONTACT_BREAK_FAILED, |
25 |
|
|
CONTACT_CREATION_FAILED, |
26 |
|
|
QUASI_STATIC, |
27 |
|
|
REACHABLE |
28 |
|
|
}; |
29 |
|
|
|
30 |
|
|
struct Result { |
31 |
|
16 |
Result() |
32 |
|
16 |
: status(UNABLE_TO_COMPUTE), |
33 |
|
|
x(fcl::Vec3f::Zero()), |
34 |
|
|
xBreak_(fcl::Vec3f::Zero()), |
35 |
|
|
xCreate_(fcl::Vec3f::Zero()), |
36 |
|
|
constraints_(), |
37 |
✓✗✓✗
✓✗✓✗
|
16 |
path_() {} |
38 |
|
|
|
39 |
|
|
Result(Status status) |
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 |
|
16 |
bool success() { |
56 |
✓✗ |
3 |
return (status == REACHABLE) || (status == NO_CONTACT_VARIATION) || |
57 |
✓✓✓✗
✗✓ |
19 |
(status == SAME_ROOT_POSITION) || status == QUASI_STATIC; |
58 |
|
|
} |
59 |
|
|
|
60 |
|
|
bool pathExist() { |
61 |
|
|
if (path_) { |
62 |
|
|
if (path_->length() > 0) { |
63 |
|
|
return true; |
64 |
|
|
} |
65 |
|
|
} |
66 |
|
|
return false; |
67 |
|
|
} |
68 |
|
|
|
69 |
|
|
Status status; |
70 |
|
|
fcl::Vec3f x; |
71 |
|
|
fcl::Vec3f xBreak_; |
72 |
|
|
fcl::Vec3f xCreate_; |
73 |
|
|
std::pair<MatrixXX, VectorX> constraints_; |
74 |
|
|
core::PathPtr_t path_; |
75 |
|
|
VectorX timings_; |
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 |
|
|
|
98 |
|
|
/** |
99 |
|
|
* @brief isReachable Compute the feasibility of the contact transition between |
100 |
|
|
* the two state, with the quasiStatic formulation of 2-PAC |
101 |
|
|
* (https://hal.archives-ouvertes.fr/hal-01609055) |
102 |
|
|
* @param fullbody |
103 |
|
|
* @param previous the first state of the transition |
104 |
|
|
* @param next the last state of the transition |
105 |
|
|
* @param acc the CoM acceleration |
106 |
|
|
* @param useIntermediateState boolean only relevant in the case of a contact |
107 |
|
|
* repositionning. If true, use an intermediate state such that there is only |
108 |
|
|
* one contact change between each state. (and thus compute two intersection |
109 |
|
|
* between 3 set of constrants). If false it only compute one intersection |
110 |
|
|
* between two set of constraints. |
111 |
|
|
* @return |
112 |
|
|
*/ |
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 |