1 |
|
|
// Copyright (c) 2016, LAAS-CNRS |
2 |
|
|
// Authors: Pierre Fernbach (pierre.fernbach@laas.fr) |
3 |
|
|
// |
4 |
|
|
// This file is part of hpp-core |
5 |
|
|
// hpp-core is free software: you can redistribute it |
6 |
|
|
// and/or modify it under the terms of the GNU Lesser General Public |
7 |
|
|
// License as published by the Free Software Foundation, either version |
8 |
|
|
// 3 of the License, or (at your option) any later version. |
9 |
|
|
// |
10 |
|
|
// hpp-core is distributed in the hope that it will be |
11 |
|
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
12 |
|
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
13 |
|
|
// General Lesser Public License for more details. You should have |
14 |
|
|
// received a copy of the GNU Lesser General Public License along with |
15 |
|
|
// hpp-core If not, see |
16 |
|
|
// <http://www.gnu.org/licenses/>. |
17 |
|
|
|
18 |
|
|
#ifndef HPP_RBPRM_STEERING_METHOD_KINODYNAMIC_HH |
19 |
|
|
#define HPP_RBPRM_STEERING_METHOD_KINODYNAMIC_HH |
20 |
|
|
|
21 |
|
|
#include <hpp/core/steering-method/steering-kinodynamic.hh> |
22 |
|
|
#include <hpp/rbprm/config.hh> |
23 |
|
|
#include <hpp/rbprm/planner/rbprm-node.hh> |
24 |
|
|
|
25 |
|
|
namespace hpp { |
26 |
|
|
namespace rbprm { |
27 |
|
|
|
28 |
|
|
using core::ConfigurationIn_t; |
29 |
|
|
using core::Path; |
30 |
|
|
using core::Problem; |
31 |
|
|
|
32 |
|
|
HPP_PREDEF_CLASS(SteeringMethodKinodynamic); |
33 |
|
|
typedef shared_ptr<SteeringMethodKinodynamic> SteeringMethodKinodynamicPtr_t; |
34 |
|
|
|
35 |
|
|
class HPP_RBPRM_DLLAPI SteeringMethodKinodynamic |
36 |
|
|
: public core::steeringMethod::Kinodynamic { |
37 |
|
|
public: |
38 |
|
431 |
core::PathPtr_t operator()(core::ConfigurationIn_t q1, |
39 |
|
|
const core::NodePtr_t x) { |
40 |
|
|
try { |
41 |
✓✗✓✗
|
431 |
return impl_compute(q1, x); |
42 |
|
|
} catch (const core::projection_error& e) { |
43 |
|
|
hppDout(info, "Could not build path: " << e.what()); |
44 |
|
|
} |
45 |
|
|
return core::PathPtr_t(); |
46 |
|
|
} |
47 |
|
|
|
48 |
|
13579 |
core::PathPtr_t operator()(const core::NodePtr_t x, |
49 |
|
|
core::ConfigurationIn_t q2) { |
50 |
|
|
try { |
51 |
✓✗✓✗
|
13579 |
return impl_compute(x, q2); |
52 |
|
|
} catch (const core::projection_error& e) { |
53 |
|
|
hppDout(info, "Could not build path: " << e.what()); |
54 |
|
|
} |
55 |
|
|
return core::PathPtr_t(); |
56 |
|
|
} |
57 |
|
|
/// Create an instance |
58 |
|
37 |
static SteeringMethodKinodynamicPtr_t create( |
59 |
|
|
core::ProblemConstPtr_t problem) { |
60 |
✓✗✓✗
|
37 |
SteeringMethodKinodynamic* ptr = new SteeringMethodKinodynamic(problem); |
61 |
|
37 |
SteeringMethodKinodynamicPtr_t shPtr(ptr); |
62 |
|
37 |
ptr->init(shPtr); |
63 |
|
37 |
return shPtr; |
64 |
|
|
} |
65 |
|
|
|
66 |
|
|
/// Copy instance and return shared pointer |
67 |
|
|
static SteeringMethodKinodynamicPtr_t createCopy( |
68 |
|
|
const SteeringMethodKinodynamicPtr_t& other) { |
69 |
|
|
SteeringMethodKinodynamic* ptr = new SteeringMethodKinodynamic(*other); |
70 |
|
|
SteeringMethodKinodynamicPtr_t shPtr(ptr); |
71 |
|
|
ptr->init(shPtr); |
72 |
|
|
return shPtr; |
73 |
|
|
} |
74 |
|
|
|
75 |
|
|
/// Copy instance and return shared pointer |
76 |
|
|
virtual core::SteeringMethodPtr_t copy() const { |
77 |
|
|
return createCopy(weak_.lock()); |
78 |
|
|
} |
79 |
|
|
|
80 |
|
|
/// create a path between two configurations |
81 |
|
|
virtual core::PathPtr_t impl_compute(core::ConfigurationIn_t q1, |
82 |
|
|
core::ConfigurationIn_t q2) const; |
83 |
|
|
|
84 |
|
|
core::PathPtr_t impl_compute(core::NodePtr_t x, core::ConfigurationIn_t q2); |
85 |
|
|
|
86 |
|
|
core::PathPtr_t impl_compute(core::ConfigurationIn_t q1, core::NodePtr_t x); |
87 |
|
|
|
88 |
|
|
double totalTimeComputed_; |
89 |
|
|
double totalTimeValidated_; |
90 |
|
|
int dirValid_; |
91 |
|
|
int dirTotal_; |
92 |
|
|
int rejectedPath_; |
93 |
|
|
const double maxLength_; |
94 |
|
|
|
95 |
|
|
protected: |
96 |
|
|
/// Constructor |
97 |
|
|
SteeringMethodKinodynamic(core::ProblemConstPtr_t problem); |
98 |
|
|
|
99 |
|
|
/// Copy constructor |
100 |
|
|
SteeringMethodKinodynamic(const SteeringMethodKinodynamic& other); |
101 |
|
|
|
102 |
|
|
/// Store weak pointer to itself |
103 |
|
37 |
void init(SteeringMethodKinodynamicWkPtr_t weak) { |
104 |
|
37 |
core::SteeringMethod::init(weak); |
105 |
|
37 |
weak_ = weak; |
106 |
|
37 |
} |
107 |
|
|
|
108 |
|
|
/** |
109 |
|
|
* @brief computeDirection compute the direction that the steering method will |
110 |
|
|
* choose in order to connect from to to |
111 |
|
|
* @param from |
112 |
|
|
* @param to |
113 |
|
|
* @return |
114 |
|
|
*/ |
115 |
|
|
core::PathPtr_t computeDirection(const core::ConfigurationIn_t from, |
116 |
|
|
const core::ConfigurationIn_t to, |
117 |
|
|
bool reverse); |
118 |
|
|
|
119 |
|
|
/** |
120 |
|
|
* @brief setSteeringMethodBounds Compute the maximal acceleration on a |
121 |
|
|
* direction from near to target, and send it to the steering method |
122 |
|
|
* @param near the node from where we take the the information about contact |
123 |
|
|
* and position |
124 |
|
|
* @param target the target configuration |
125 |
|
|
* @param reverse if true, we compute the acceleration from target to near, |
126 |
|
|
* with the information from near |
127 |
|
|
* @return the node casted from near |
128 |
|
|
*/ |
129 |
|
|
core::PathPtr_t setSteeringMethodBounds(const core::RbprmNodePtr_t& near, |
130 |
|
|
const core::ConfigurationIn_t target, |
131 |
|
|
bool reverse); |
132 |
|
|
|
133 |
|
|
private: |
134 |
|
|
core::DeviceWkPtr_t device_; |
135 |
|
|
centroidal_dynamics::Vector3 lastDirection_; |
136 |
|
|
centroidal_dynamics::Equilibrium* sEq_; |
137 |
|
|
bool boundsUpToDate_; |
138 |
|
|
SteeringMethodKinodynamicWkPtr_t weak_; |
139 |
|
|
|
140 |
|
|
}; // class rbprm-kinodynamic |
141 |
|
|
} // namespace rbprm |
142 |
|
|
} // namespace hpp |
143 |
|
|
|
144 |
|
|
#endif // HPP_RBPRM_STEERING_METHOD_KINODYNAMIC_HH |