hpp-rbprm 4.14.0
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
parabola-planner.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2016 CNRS
3// Authors: Fernbach Pierre
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_PARABOLA_PLANNER_HH
20#define HPP_RBPRM_PARABOLA_PLANNER_HH
21
22#include <boost/tuple/tuple.hpp>
23#include <hpp/core/path-planner.hh>
24#include <hpp/core/steering-method.hh>
27
28namespace hpp {
29namespace rbprm {
32
33// forward declaration of class Planner
35// Planner objects are manipulated only via shared pointers
36typedef shared_ptr<ParabolaPlanner> ParabolaPlannerPtr_t;
37typedef boost::tuple<core::NodePtr_t, core::ConfigurationPtr_t, core::PathPtr_t>
39typedef std::vector<DelayedEdge_t> DelayedEdges_t;
40
42class ParabolaPlanner : public core::PathPlanner {
43 public:
46 const core::Problem& problem, const core::RoadmapPtr_t& roadmap);
48 static ParabolaPlannerPtr_t create(const core::Problem& problem);
50 virtual void oneStep();
51
52 // for debugging
53 // virtual core::PathVectorPtr_t solve ();
54
55 virtual void startSolve();
56
57 // disabled during testing
58 virtual void tryDirectPath();
59
60 // This method call SteeringMethodParabola, but we don't try to connect two
61 // confuration, instead we shoot a random alpha0 and V0 valide for the
62 // initiale configuration and then compute the final point. Then we check for
63 // collision (for the trunk) and we check if the final point is in a valide
64 // configuration (trunk not in collision but limbs in accessible contact
65 // zone). (Not anymore ) If this is true we do a reverse collision check until
66 // we find the first valide configuration, then we check for the friction cone
67 // and impact velocity constraint.(Not anymore : can't find normal after this)
68
77 core::PathPtr_t computeRandomParabola(core::NodePtr_t x_start,
78 core::ConfigurationPtr_t q_target,
79 DelayedEdges_t& delayedEdges);
80
82 void configurationShooter(const core::ConfigurationShooterPtr_t& shooter);
83
84 // we need both method, because smart_pointer inheritance is not implemented
85 // (compiler don't know that rbprmRoadmapPtr_t derive from RoadmapPtr_t).
86 virtual const core::RoadmapPtr_t& roadmap() const { return roadmap_; }
87
88 const core::RbprmRoadmapPtr_t& rbprmRoadmap() const { return rbRoadmap_; }
89
90 protected:
92 ParabolaPlanner(const core::Problem& problem,
93 const core::RoadmapPtr_t& roadmap);
95 ParabolaPlanner(const core::Problem& problem);
97 void init(const ParabolaPlannerWkPtr_t& weak);
101 virtual core::PathPtr_t extend(const core::NodePtr_t& near,
102 const core::ConfigurationPtr_t& target);
103 virtual core::PathPtr_t extendParabola(
104 const core::NodePtr_t& near, const core::ConfigurationPtr_t& target);
105
106 private:
113 void computeGIWC(const core::RbprmNodePtr_t x,
114 core::ValidationReportPtr_t report);
115
121 void computeGIWC(const core::RbprmNodePtr_t x);
122
123 void computeGIWC(const core::NodePtr_t x) {
124 computeGIWC(static_cast<core::RbprmNodePtr_t>(x));
125 }
126
127 void computeGIWC(const core::NodePtr_t node,
128 core::ValidationReportPtr_t report) {
129 computeGIWC(static_cast<core::RbprmNodePtr_t>(node), report);
130 }
131
132 core::ConfigurationShooterPtr_t configurationShooter_;
133 mutable core::Configuration_t qProj_;
134 ParabolaPlannerWkPtr_t weakPtr_;
135 SteeringMethodParabolaPtr_t smParabola_;
136 const core::RbprmRoadmapPtr_t rbRoadmap_;
137 const core::RoadmapPtr_t roadmap_;
138};
140} // namespace rbprm
141} // namespace hpp
142#endif // HPP_CORE_DIFFUSING_PLANNER_HH
Definition: rbprm-node.hh:22
Generic implementation of RRT algorithm.
Definition: parabola-planner.hh:42
void configurationShooter(const core::ConfigurationShooterPtr_t &shooter)
Set configuration shooter.
core::PathPtr_t computeRandomParabola(core::NodePtr_t x_start, core::ConfigurationPtr_t q_target, DelayedEdges_t &delayedEdges)
computeRandomParabola
ParabolaPlanner(const core::Problem &problem, const core::RoadmapPtr_t &roadmap)
Constructor.
void init(const ParabolaPlannerWkPtr_t &weak)
Store weak pointer to itself.
ParabolaPlanner(const core::Problem &problem)
Constructor with roadmap.
virtual core::PathPtr_t extend(const core::NodePtr_t &near, const core::ConfigurationPtr_t &target)
virtual void oneStep()
One step of extension.
static ParabolaPlannerPtr_t create(const core::Problem &problem)
Return shared pointer to new object.
virtual void tryDirectPath()
static ParabolaPlannerPtr_t createWithRoadmap(const core::Problem &problem, const core::RoadmapPtr_t &roadmap)
Return shared pointer to new object.
const core::RbprmRoadmapPtr_t & rbprmRoadmap() const
Definition: parabola-planner.hh:88
virtual core::PathPtr_t extendParabola(const core::NodePtr_t &near, const core::ConfigurationPtr_t &target)
virtual const core::RoadmapPtr_t & roadmap() const
Definition: parabola-planner.hh:86
std::vector< DelayedEdge_t > DelayedEdges_t
Definition: parabola-planner.hh:39
shared_ptr< ParabolaPlanner > ParabolaPlannerPtr_t
Definition: parabola-planner.hh:36
boost::tuple< core::NodePtr_t, core::ConfigurationPtr_t, core::PathPtr_t > DelayedEdge_t
Definition: parabola-planner.hh:38
shared_ptr< SteeringMethodParabola > SteeringMethodParabolaPtr_t
Definition: steering-method-parabola.hh:37
shared_ptr< RbprmRoadmap > RbprmRoadmapPtr_t
Definition: rbprm-roadmap.hh:13
HPP_PREDEF_CLASS(RbPrmFullBody)
Definition: algorithm.hh:26