hpp-rbprm  4.10.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/rbprm/config.hh>
22 #include <hpp/core/steering-method/steering-kinodynamic.hh>
24 
25 namespace hpp {
26  namespace rbprm {
27 
28  using core::Problem;
29  using core::ConfigurationIn_t;
30  using core::Path;
31 
32  HPP_PREDEF_CLASS (SteeringMethodKinodynamic);
33  typedef boost::shared_ptr <SteeringMethodKinodynamic> SteeringMethodKinodynamicPtr_t;
34 
35  class HPP_RBPRM_DLLAPI SteeringMethodKinodynamic : public core::steeringMethod::Kinodynamic{
36 
37  public:
38 
39 
40  core::PathPtr_t operator() (core::ConfigurationIn_t q1,
41  const core::NodePtr_t x)
42  {
43  try {
44  return impl_compute (q1, x);
45  } catch (const core::projection_error& e) {
46  hppDout (info, "Could not build path: " << e.what());
47  }
48  return core::PathPtr_t ();
49  }
50 
51  core::PathPtr_t operator() (const core::NodePtr_t x,
52  core::ConfigurationIn_t q2)
53  {
54  try {
55  return impl_compute (x, q2);
56  } catch (const core::projection_error& e) {
57  hppDout (info, "Could not build path: " << e.what());
58  }
59  return core::PathPtr_t ();
60  }
62  static SteeringMethodKinodynamicPtr_t create (const core::Problem& problem)
63  {
66  ptr->init (shPtr);
67  return shPtr;
68  }
69 
71  static SteeringMethodKinodynamicPtr_t createCopy
73  {
76  ptr->init (shPtr);
77  return shPtr;
78  }
79 
81  virtual core::SteeringMethodPtr_t copy () const
82  {
83  return createCopy (weak_.lock ());
84  }
85 
87  virtual core::PathPtr_t impl_compute (core::ConfigurationIn_t q1,
88  core::ConfigurationIn_t q2) const;
89 
90  core::PathPtr_t impl_compute (core::NodePtr_t x,
91  core::ConfigurationIn_t q2);
92 
93  core::PathPtr_t impl_compute (core::ConfigurationIn_t q1,core::NodePtr_t x);
94 
97  int dirValid_;
98  int dirTotal_;
100  const double maxLength_;
101 
102  protected:
103 
105  SteeringMethodKinodynamic (const core::Problem& problem);
106 
109 
111  void init (SteeringMethodKinodynamicWkPtr_t weak)
112  {
113  core::SteeringMethod::init (weak);
114  weak_ = weak;
115  }
116 
123  core::PathPtr_t computeDirection(const core::ConfigurationIn_t from, const core::ConfigurationIn_t to, bool reverse);
124 
133  core::PathPtr_t setSteeringMethodBounds(const core::RbprmNodePtr_t& near, const core::ConfigurationIn_t target,bool reverse);
134 
135 
136 
137 
138  private:
139  core::DeviceWkPtr_t device_;
140  centroidal_dynamics::Vector3 lastDirection_;
141  centroidal_dynamics::Equilibrium* sEq_;
142  bool boundsUpToDate_;
143  SteeringMethodKinodynamicWkPtr_t weak_;
144 
145  }; // class rbprm-kinodynamic
146  } // namespace hpp
147 } // namespace rbprm
148 
149 
150 #endif // HPP_RBPRM_STEERING_METHOD_KINODYNAMIC_HH
hpp::rbprm::SteeringMethodKinodynamic::dirTotal_
int dirTotal_
Definition: rbprm-steering-kinodynamic.hh:98
hpp::rbprm::SteeringMethodKinodynamic
Definition: rbprm-steering-kinodynamic.hh:35
hpp::rbprm::SteeringMethodKinodynamic::copy
virtual core::SteeringMethodPtr_t copy() const
Copy instance and return shared pointer.
Definition: rbprm-steering-kinodynamic.hh:81
hpp::rbprm::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(RbPrmFullBody)
hpp::core::RbprmNode
Definition: rbprm-node.hh:23
hpp::rbprm::SteeringMethodKinodynamic::rejectedPath_
int rejectedPath_
Definition: rbprm-steering-kinodynamic.hh:99
hpp::rbprm::Vector3
Eigen::Matrix< pinocchio::value_type, 3, 1 > Vector3
Definition: rbprm-limb.hh:50
hpp::rbprm::SteeringMethodKinodynamic::create
static SteeringMethodKinodynamicPtr_t create(const core::Problem &problem)
Create an instance.
Definition: rbprm-steering-kinodynamic.hh:62
hpp::rbprm::SteeringMethodKinodynamic::maxLength_
const double maxLength_
Definition: rbprm-steering-kinodynamic.hh:100
hpp::rbprm::SteeringMethodKinodynamicPtr_t
boost::shared_ptr< SteeringMethodKinodynamic > SteeringMethodKinodynamicPtr_t
Definition: rbprm-steering-kinodynamic.hh:33
hpp::rbprm::SteeringMethodKinodynamic::dirValid_
int dirValid_
Definition: rbprm-steering-kinodynamic.hh:97
hpp
Definition: algorithm.hh:27
rbprm-node.hh
hpp::rbprm::SteeringMethodKinodynamic::totalTimeValidated_
double totalTimeValidated_
Definition: rbprm-steering-kinodynamic.hh:96
hpp::rbprm::SteeringMethodKinodynamic::init
void init(SteeringMethodKinodynamicWkPtr_t weak)
Store weak pointer to itself.
Definition: rbprm-steering-kinodynamic.hh:111
hpp::rbprm::SteeringMethodKinodynamic::totalTimeComputed_
double totalTimeComputed_
Definition: rbprm-steering-kinodynamic.hh:95
config.hh
HPP_RBPRM_DLLAPI
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64