hpp-rbprm  4.13.0
Implementation of RB-PRM planner using hpp.
rbprm-steering-kinodynamic.hh
Go to the documentation of this file.
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>
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 
36  : public core::steeringMethod::Kinodynamic {
37  public:
38  core::PathPtr_t operator()(core::ConfigurationIn_t q1,
39  const core::NodePtr_t x) {
40  try {
41  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  core::PathPtr_t operator()(const core::NodePtr_t x,
49  core::ConfigurationIn_t q2) {
50  try {
51  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  }
58  static SteeringMethodKinodynamicPtr_t create(
59  core::ProblemConstPtr_t problem) {
61  SteeringMethodKinodynamicPtr_t shPtr(ptr);
62  ptr->init(shPtr);
63  return shPtr;
64  }
65 
67  static SteeringMethodKinodynamicPtr_t createCopy(
68  const SteeringMethodKinodynamicPtr_t& other) {
70  SteeringMethodKinodynamicPtr_t shPtr(ptr);
71  ptr->init(shPtr);
72  return shPtr;
73  }
74 
76  virtual core::SteeringMethodPtr_t copy() const {
77  return createCopy(weak_.lock());
78  }
79 
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 
90  int dirValid_;
91  int dirTotal_;
93  const double maxLength_;
94 
95  protected:
97  SteeringMethodKinodynamic(core::ProblemConstPtr_t problem);
98 
101 
103  void init(SteeringMethodKinodynamicWkPtr_t weak) {
104  core::SteeringMethod::init(weak);
105  weak_ = weak;
106  }
107 
115  core::PathPtr_t computeDirection(const core::ConfigurationIn_t from,
116  const core::ConfigurationIn_t to,
117  bool reverse);
118 
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
Eigen::Matrix< pinocchio::value_type, 3, 1 > Vector3
Definition: rbprm-limb.hh:49
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
Definition: algorithm.hh:26
double totalTimeComputed_
Definition: rbprm-steering-kinodynamic.hh:88
virtual core::SteeringMethodPtr_t copy() const
Copy instance and return shared pointer.
Definition: rbprm-steering-kinodynamic.hh:76
shared_ptr< SteeringMethodKinodynamic > SteeringMethodKinodynamicPtr_t
Definition: rbprm-steering-kinodynamic.hh:33
Definition: rbprm-node.hh:22
Definition: rbprm-steering-kinodynamic.hh:35
core::PathPtr_t operator()(const core::NodePtr_t x, core::ConfigurationIn_t q2)
Definition: rbprm-steering-kinodynamic.hh:48
int dirValid_
Definition: rbprm-steering-kinodynamic.hh:90
HPP_PREDEF_CLASS(RbPrmFullBody)
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
double totalTimeValidated_
Definition: rbprm-steering-kinodynamic.hh:89
core::PathPtr_t operator()(core::ConfigurationIn_t q1, const core::NodePtr_t x)
Definition: rbprm-steering-kinodynamic.hh:38
int rejectedPath_
Definition: rbprm-steering-kinodynamic.hh:92
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
void init(SteeringMethodKinodynamicWkPtr_t weak)
Store weak pointer to itself.
Definition: rbprm-steering-kinodynamic.hh:103