hpp-rbprm 4.15.1
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
dynamic-planner.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2014 CNRS
3// Authors: Florent Lamiraux
4//
5// This file is part of hpp-core
6// hpp-core is free software: you can redistribute it
7// and/or modify it under the terms of the GNU Lesser General Public
8// License as published by the Free Software Foundation, either version
9// 3 of the License, or (at your option) any later version.
10//
11// hpp-core is distributed in the hope that it will be
12// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14// General Lesser Public License for more details. You should have
15// received a copy of the GNU Lesser General Public License along with
16// hpp-core If not, see
17// <http://www.gnu.org/licenses/>.
18
19#ifndef HPP_RBPRM_DYNAMIC_PLANNER_HH
20#define HPP_RBPRM_DYNAMIC_PLANNER_HH
21
22#include <hpp/core/bi-rrt-planner.hh>
26
27namespace hpp {
28namespace rbprm {
31// forward declaration of class Planner
33// Planner objects are manipulated only via shared pointers
34typedef shared_ptr<DynamicPlanner> DynamicPlannerPtr_t;
35
36using core::Configuration_t;
37using core::Path;
38using core::PathPtr_t;
39using core::Problem;
40using core::Roadmap;
41using core::RoadmapPtr_t;
42
44class DynamicPlanner : public core::BiRRTPlanner {
45 public:
47 static DynamicPlannerPtr_t createWithRoadmap(core::ProblemConstPtr_t problem,
48 const RoadmapPtr_t& roadmap);
50 static DynamicPlannerPtr_t create(core::ProblemConstPtr_t problem);
52 virtual void oneStep();
55 virtual void tryConnectInitAndGoals();
56
57 // we need both method, because smart_pointer inheritance is not implemented
58 // (compiler don't know that rbprmRoadmapPtr_t derive from RoadmapPtr_t).
59 virtual const core::RoadmapPtr_t& roadmap() const { return roadmap_; }
60
61 virtual core::PathVectorPtr_t finishSolve(const core::PathVectorPtr_t& path);
62
63 protected:
65 DynamicPlanner(core::ProblemConstPtr_t problem, const RoadmapPtr_t& roadmap);
67 DynamicPlanner(core::ProblemConstPtr_t problem);
69 void init(const DynamicPlannerWkPtr_t& weak);
70
77 void computeGIWC(const core::NodePtr_t x, core::ValidationReportPtr_t report);
78
84 void computeGIWC(const core::NodePtr_t x, bool use_bestReport = true);
85
86 core::PathPtr_t extendInternal(core::ConfigurationPtr_t& qProj_,
87 const core::NodePtr_t& near,
88 const core::ConfigurationPtr_t& target,
89 bool reverse = false);
90
91 bool tryParabolaPath(const core::NodePtr_t& near,
92 core::ConfigurationPtr_t q_jump,
93 const core::ConfigurationPtr_t& target, bool reverse,
94 core::NodePtr_t& x_jump, core::NodePtr_t& nodeReached,
95 core::PathPtr_t& kinoPath, core::PathPtr_t& paraPath);
96
97 core::PathPtr_t extendParabola(const core::ConfigurationPtr_t& from,
98 const core::ConfigurationPtr_t& target,
99 bool reverse);
100
101 private:
102 core::ConfigurationPtr_t qProj_;
103 DynamicPlannerWkPtr_t weakPtr_;
104 const core::RoadmapPtr_t roadmap_;
106 const SteeringMethodParabolaPtr_t smParabola_;
107 const RbPrmPathValidationPtr_t rbprmPathValidation_;
108 double sizeFootX_, sizeFootY_;
109 bool rectangularContact_;
110 bool tryJump_;
111 double mu_;
112};
114} // namespace rbprm
115} // namespace hpp
116#endif // HPP_RBPRM_DYNAMIC_PLANNER_HH
Generic implementation of RRT algorithm.
Definition: dynamic-planner.hh:44
void computeGIWC(const core::NodePtr_t x, bool use_bestReport=true)
computeGIWC compute the GIWC for the node configuration and fill the node attribut,...
core::PathPtr_t extendInternal(core::ConfigurationPtr_t &qProj_, const core::NodePtr_t &near, const core::ConfigurationPtr_t &target, bool reverse=false)
DynamicPlanner(core::ProblemConstPtr_t problem, const RoadmapPtr_t &roadmap)
Constructor.
core::PathPtr_t extendParabola(const core::ConfigurationPtr_t &from, const core::ConfigurationPtr_t &target, bool reverse)
bool tryParabolaPath(const core::NodePtr_t &near, core::ConfigurationPtr_t q_jump, const core::ConfigurationPtr_t &target, bool reverse, core::NodePtr_t &x_jump, core::NodePtr_t &nodeReached, core::PathPtr_t &kinoPath, core::PathPtr_t &paraPath)
static DynamicPlannerPtr_t createWithRoadmap(core::ProblemConstPtr_t problem, const RoadmapPtr_t &roadmap)
Return shared pointer to new object.
virtual core::PathVectorPtr_t finishSolve(const core::PathVectorPtr_t &path)
virtual void oneStep()
One step of extension.
DynamicPlanner(core::ProblemConstPtr_t problem)
Constructor with roadmap.
void computeGIWC(const core::NodePtr_t x, core::ValidationReportPtr_t report)
computeGIWC compute the GIWC for the node configuration and fill the node attribut
virtual const core::RoadmapPtr_t & roadmap() const
Definition: dynamic-planner.hh:59
static DynamicPlannerPtr_t create(core::ProblemConstPtr_t problem)
Return shared pointer to new object.
virtual void tryConnectInitAndGoals()
void init(const DynamicPlannerWkPtr_t &weak)
Store weak pointer to itself.
shared_ptr< DynamicPlanner > DynamicPlannerPtr_t
Definition: dynamic-planner.hh:34
shared_ptr< SteeringMethodParabola > SteeringMethodParabolaPtr_t
Definition: steering-method-parabola.hh:37
HPP_PREDEF_CLASS(RbPrmFullBody)
shared_ptr< RbPrmPathValidation > RbPrmPathValidationPtr_t
Definition: rbprm-path-validation.hh:31
shared_ptr< SteeringMethodKinodynamic > SteeringMethodKinodynamicPtr_t
Definition: rbprm-steering-kinodynamic.hh:33
Definition: algorithm.hh:26