18#ifndef HPP_RBPRM_STEERING_METHOD_KINODYNAMIC_HH
19#define HPP_RBPRM_STEERING_METHOD_KINODYNAMIC_HH
21#include <hpp/core/steering-method/steering-kinodynamic.hh>
28using core::ConfigurationIn_t;
36 :
public core::steeringMethod::Kinodynamic {
39 const core::NodePtr_t x) {
41 return impl_compute(q1, x);
42 }
catch (
const core::projection_error& e) {
43 hppDout(info,
"Could not build path: " << e.what());
45 return core::PathPtr_t();
49 core::ConfigurationIn_t q2) {
51 return impl_compute(x, q2);
52 }
catch (
const core::projection_error& e) {
53 hppDout(info,
"Could not build path: " << e.what());
55 return core::PathPtr_t();
59 core::ProblemConstPtr_t problem) {
76 virtual core::SteeringMethodPtr_t
copy()
const {
77 return createCopy(weak_.lock());
82 core::ConfigurationIn_t q2)
const;
84 core::PathPtr_t
impl_compute(core::NodePtr_t x, core::ConfigurationIn_t q2);
86 core::PathPtr_t
impl_compute(core::ConfigurationIn_t q1, core::NodePtr_t x);
103 void init(SteeringMethodKinodynamicWkPtr_t weak) {
104 core::SteeringMethod::init(weak);
116 const core::ConfigurationIn_t to,
130 const core::ConfigurationIn_t target,
134 core::DeviceWkPtr_t device_;
135 centroidal_dynamics::Vector3 lastDirection_;
136 centroidal_dynamics::Equilibrium* sEq_;
137 bool boundsUpToDate_;
138 SteeringMethodKinodynamicWkPtr_t weak_;
Definition: rbprm-node.hh:22
Definition: rbprm-steering-kinodynamic.hh:36
void init(SteeringMethodKinodynamicWkPtr_t weak)
Store weak pointer to itself.
Definition: rbprm-steering-kinodynamic.hh:103
int dirValid_
Definition: rbprm-steering-kinodynamic.hh:90
core::PathPtr_t operator()(const core::NodePtr_t x, core::ConfigurationIn_t q2)
Definition: rbprm-steering-kinodynamic.hh:48
virtual core::PathPtr_t impl_compute(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2) const
create a path between two configurations
virtual core::SteeringMethodPtr_t copy() const
Copy instance and return shared pointer.
Definition: rbprm-steering-kinodynamic.hh:76
static SteeringMethodKinodynamicPtr_t createCopy(const SteeringMethodKinodynamicPtr_t &other)
Copy instance and return shared pointer.
Definition: rbprm-steering-kinodynamic.hh:67
const double maxLength_
Definition: rbprm-steering-kinodynamic.hh:93
core::PathPtr_t computeDirection(const core::ConfigurationIn_t from, const core::ConfigurationIn_t to, bool reverse)
computeDirection compute the direction that the steering method will choose in order to connect from ...
int rejectedPath_
Definition: rbprm-steering-kinodynamic.hh:92
SteeringMethodKinodynamic(const SteeringMethodKinodynamic &other)
Copy constructor.
double totalTimeComputed_
Definition: rbprm-steering-kinodynamic.hh:88
core::PathPtr_t operator()(core::ConfigurationIn_t q1, const core::NodePtr_t x)
Definition: rbprm-steering-kinodynamic.hh:38
SteeringMethodKinodynamic(core::ProblemConstPtr_t problem)
Constructor.
core::PathPtr_t setSteeringMethodBounds(const core::RbprmNodePtr_t &near, const core::ConfigurationIn_t target, bool reverse)
setSteeringMethodBounds Compute the maximal acceleration on a direction from near to target,...
int dirTotal_
Definition: rbprm-steering-kinodynamic.hh:91
static SteeringMethodKinodynamicPtr_t create(core::ProblemConstPtr_t problem)
Create an instance.
Definition: rbprm-steering-kinodynamic.hh:58
core::PathPtr_t impl_compute(core::NodePtr_t x, core::ConfigurationIn_t q2)
core::PathPtr_t impl_compute(core::ConfigurationIn_t q1, core::NodePtr_t x)
double totalTimeValidated_
Definition: rbprm-steering-kinodynamic.hh:89
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
HPP_PREDEF_CLASS(RbPrmFullBody)
shared_ptr< SteeringMethodKinodynamic > SteeringMethodKinodynamicPtr_t
Definition: rbprm-steering-kinodynamic.hh:33
Definition: algorithm.hh:26