Loading...
Searching...
No Matches
QRRTStarImpl.h
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2020,
5 * Max Planck Institute for Intelligent Systems (MPI-IS).
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the MPI-IS nor the names
19 * of its contributors may be used to endorse or promote products
20 * derived from this software without specific prior written
21 * permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *********************************************************************/
36
37/* Author: Andreas Orthey, Sohaib Akbar */
38
39#ifndef OMPL_MULTILEVEL_PLANNERS_BundleSpace_QRRTSTARIMPL_
40#define OMPL_MULTILEVEL_PLANNERS_BundleSpace_QRRTSTARIMPL_
41#include <ompl/multilevel/datastructures/BundleSpaceGraph.h>
42#include <ompl/datastructures/PDF.h>
43
44namespace ompl
45{
46 namespace base
47 {
48 OMPL_CLASS_FORWARD(OptimizationObjective);
49 }
50 namespace multilevel
51 {
53 class QRRTStarImpl : public ompl::multilevel::BundleSpaceGraph
54 {
55 using BaseT = BundleSpaceGraph;
56
57 void setKNearest(bool useKNearest)
58 {
59 useKNearest_ = useKNearest;
60 }
61
62 bool getKNearest() const
63 {
64 return useKNearest_;
65 }
66 virtual bool getSolution(ompl::base::PathPtr &solution) override;
67 virtual void getPlannerData(ompl::base::PlannerData &data) const override;
68
69 virtual void clear() override;
70 virtual void setup() override;
71
72 void getNearestNeighbors(Configuration *x, std::vector<Configuration *> &nearest);
73 void removeFromParent(Configuration *q);
74 void calculateRewiringLowerBounds();
75
76 public:
77 QRRTStarImpl(const ompl::base::SpaceInformationPtr &si, BundleSpace *parent_);
78 virtual ~QRRTStarImpl() override;
79
81 virtual void grow() override;
82
83 void updateChildCosts(Configuration *q);
84
86 double k_rrt_Constant_{0};
87
89 double r_rrt_Constant_{0};
90
91 double rewireFactor_{1.1};
92
95
97 bool useKNearest_{true};
98
100 double d_{0};
101
102 // /** \brief list of configurations that satisfy the goal condition */
103 // std::vector<Configuration *> goalConfigurations_;
104 };
105 } // namespace multilevel
106} // namespace ompl
107
108#endif
Abstract definition of optimization objectives.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
A graph on a Bundle-space.
A single Bundle-space.
Definition BundleSpace.h:64
bool symmetric_
true if cost from a to b is same as b to a
double r_rrt_Constant_
a constant value to calculate radius
double k_rrt_Constant_
a constant value to calculate k
virtual void grow() override
One iteration of RRT with adjusted sampling function.
double d_
store dimension of bundle space to calc radius
bool useKNearest_
option to use k nn or radius
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains datastructures and planners to exploit multilevel abstractions,...
Main namespace. Contains everything in this library.