Loading...
Searching...
No Matches
BiRLRT.h
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ryan Luna */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_RLRT_BIRLRT_H_
38#define OMPL_GEOMETRIC_PLANNERS_RLRT_BIRLRT_H_
39
40#include "ompl/geometric/planners/PlannerIncludes.h"
41#include <vector>
42
43namespace ompl
44{
45 namespace geometric
46 {
61
63 class BiRLRT : public base::Planner
64 {
65 public:
66 BiRLRT(const base::SpaceInformationPtr &si);
67
68 virtual ~BiRLRT();
69
70 virtual void getPlannerData(base::PlannerData &data) const;
71
73
74 virtual void clear();
75
77 void setRange(double distance)
78 {
79 range_ = distance;
80 }
81
83 double getRange() const
84 {
85 return range_;
86 }
87
90 void setMaxDistanceNear(double dNear)
91 {
92 maxDistNear_ = dNear;
93 }
94
97 double getMaxDistanceNear() const
98 {
99 return maxDistNear_;
100 }
101
105 bool getKeepLast() const
106 {
107 return keepLast_;
108 }
109
114 void setKeepLast(bool keepLast)
115 {
116 keepLast_ = keepLast;
117 }
118
119 virtual void setup();
120
121 protected:
123 class Motion
124 {
125 public:
127 Motion(const base::SpaceInformationPtr &si) : state(si->allocState())
128 {
129 }
130
131 ~Motion() = default;
132
135
137 Motion *parent{nullptr};
138
140 const base::State *root{nullptr};
141 };
142
144 void freeMemory();
145
148 bool growTreeRangeLimited(std::vector<Motion *> &tree, Motion *xmotion);
149
152 bool growTreeKeepLast(std::vector<Motion *> &tree, Motion *xmotion,
153 std::pair<base::State *, double> &lastValid);
154
160 int connectToTree(const Motion *motion, std::vector<Motion *> &tree);
161
163 std::vector<Motion *> tStart_;
165 std::vector<Motion *> tGoal_;
166
168 base::StateSamplerPtr sampler_;
169
171 double range_{0.0};
172
175 double maxDistNear_{0.0};
176
179
182 std::pair<base::State *, base::State *> connectionPoint_{nullptr, nullptr};
183
184 bool keepLast_{false};
185 };
186
187 } // namespace geometric
188} // namespace ompl
189
190#endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
Definition of an abstract state.
Definition State.h:50
A motion (tree node) with parent pointer.
Definition BiRLRT.h:124
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition BiRLRT.h:127
const base::State * root
Pointer to the root of the tree this motion is connected to.
Definition BiRLRT.h:140
base::State * state
The state contained by the motion.
Definition BiRLRT.h:134
Motion * parent
The parent motion in the exploration tree.
Definition BiRLRT.h:137
void setMaxDistanceNear(double dNear)
Set the maximum distance (per dimension) when sampling near an existing state.
Definition BiRLRT.h:90
void setKeepLast(bool keepLast)
Set whether the planner will use the range or keep last heuristic. If keepLast = false,...
Definition BiRLRT.h:114
RNG rng_
The random number generator.
Definition BiRLRT.h:178
double range_
The maximum total length of a motion to be added to a tree.
Definition BiRLRT.h:171
double getMaxDistanceNear() const
Get the maximum distance (per dimension) when sampling near an existing state.
Definition BiRLRT.h:97
bool growTreeRangeLimited(std::vector< Motion * > &tree, Motion *xmotion)
Try to grow the tree randomly. Return true if a new state was added.
Definition BiRLRT.cpp:98
void setRange(double distance)
Set the maximum distance between states in the tree.
Definition BiRLRT.h:77
bool growTreeKeepLast(std::vector< Motion * > &tree, Motion *xmotion, std::pair< base::State *, double > &lastValid)
Try to grow the tree randomly. Return true if a new state was added.
Definition BiRLRT.cpp:126
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition BiRLRT.cpp:173
double maxDistNear_
The maximum distance (per dimension) when sampling near an existing configuration.
Definition BiRLRT.h:175
base::StateSamplerPtr sampler_
State sampler.
Definition BiRLRT.h:168
double getRange() const
Get the maximum distance between states in the tree.
Definition BiRLRT.h:83
bool getKeepLast() const
If true, the planner will not have the range limitation. Instead, if a collision is detected,...
Definition BiRLRT.h:105
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition BiRLRT.cpp:66
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition BiRLRT.h:182
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition BiRLRT.cpp:299
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition BiRLRT.cpp:58
void freeMemory()
Free the memory allocated by this planner.
Definition BiRLRT.cpp:78
std::vector< Motion * > tGoal_
Goal tree.
Definition BiRLRT.h:165
std::vector< Motion * > tStart_
Start tree.
Definition BiRLRT.h:163
int connectToTree(const Motion *motion, std::vector< Motion * > &tree)
Definition BiRLRT.cpp:154
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()