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 43 namespace ompl 44 { 45 namespace geometric 46 { 47 /** 48 \anchor gBiRLRT 49 50 \ref gBiRLRT "BiRLRT" is a basic bidirectional tree-based planner without any sophistic heuristics to guide the 51 exploration. It should be used as a baseline for comparison against other bidirectional tree-based planners. In 52 high-dimensional search spaces it can sometimes perform surprisingly well. 53 54 \par Associated publication: 55 R. Luna, M. Moll, J. Badger, and L. E. Kavraki, 56 A Scalable Motion Planner for High-Dimensional Kinematic Systems, 57 <em>Intl. J. of Robotics Research</em>, vol. 39, issue 4, pp. 361-388, Mar. 2020. 58 DOI: [10.1177/0278364919890408](http://dx.doi.org/10.1177/0278364919890408)<br> 59 [[PDF]](http://www.kavrakilab.org/publications/luna2020a-scalable-motion-planner-for-high-dimensional.pdf) 60 **/ 61 62 /// \brief Bi-directional Range-Limited Random Tree (Ryan Luna's Random Tree) 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 72 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 73 74 virtual void clear(); 75 76 /// \brief Set the maximum distance between states in the tree. setRange(double distance)77 void setRange(double distance) 78 { 79 range_ = distance; 80 } 81 82 /// \brief Get the maximum distance between states in the tree. getRange()83 double getRange() const 84 { 85 return range_; 86 } 87 88 /// \brief Set the maximum distance (per dimension) when sampling 89 /// near an existing state setMaxDistanceNear(double dNear)90 void setMaxDistanceNear(double dNear) 91 { 92 maxDistNear_ = dNear; 93 } 94 95 /// \brief Get the maximum distance (per dimension) when sampling 96 /// near an existing state getMaxDistanceNear()97 double getMaxDistanceNear() const 98 { 99 return maxDistNear_; 100 } 101 102 /// \brief If true, the planner will not have the range limitation. 103 /// Instead, if a collision is detected, the last valid state is 104 /// retained. getKeepLast()105 bool getKeepLast() const 106 { 107 return keepLast_; 108 } 109 110 /// \brief Set whether the planner will use the range or keep last 111 /// heuristic. If keepLast = false, motions are limited in 112 /// distance to range_, otherwise the last valid state is retained 113 /// when a collision is detected. setKeepLast(bool keepLast)114 void setKeepLast(bool keepLast) 115 { 116 keepLast_ = keepLast; 117 } 118 119 virtual void setup(); 120 121 protected: 122 /// A motion (tree node) with parent pointer 123 class Motion 124 { 125 public: 126 /// \brief Constructor that allocates memory for the state Motion(const base::SpaceInformationPtr & si)127 Motion(const base::SpaceInformationPtr &si) : state(si->allocState()) 128 { 129 } 130 131 ~Motion() = default; 132 133 /// \brief The state contained by the motion 134 base::State *state{nullptr}; 135 136 /// \brief The parent motion in the exploration tree 137 Motion *parent{nullptr}; 138 139 /// \brief Pointer to the root of the tree this motion is connected to 140 const base::State *root{nullptr}; 141 }; 142 143 /// \brief Free the memory allocated by this planner 144 void freeMemory(); 145 146 /// Try to grow the tree randomly. Return true if a new state was added 147 /// xmotion is scratch space for sampling, etc. 148 bool growTreeRangeLimited(std::vector<Motion *> &tree, Motion *xmotion); 149 150 /// Try to grow the tree randomly. Return true if a new state was added 151 /// xmotion is scratch space for sampling, etc. 152 bool growTreeKeepLast(std::vector<Motion *> &tree, Motion *xmotion, 153 std::pair<base::State *, double> &lastValid); 154 155 /// Attempt to connect the given motion (presumed to be in a tree) 156 /// to a state in another tree (presumed to be different from the 157 /// tree motion is in). If connection is successful, the index of 158 /// the motion in the other tree that the motion connects to is 159 /// returned. -1 for failed connection. 160 int connectToTree(const Motion *motion, std::vector<Motion *> &tree); 161 162 /// Start tree 163 std::vector<Motion *> tStart_; 164 /// Goal tree 165 std::vector<Motion *> tGoal_; 166 167 /// \brief State sampler. 168 base::StateSamplerPtr sampler_; 169 170 /// \brief The maximum total length of a motion to be added to a tree 171 double range_{0.0}; 172 173 /// \brief The maximum distance (per dimension) when sampling near 174 /// an existing configuration 175 double maxDistNear_{0.0}; 176 177 /// \brief The random number generator 178 RNG rng_; 179 180 /// \brief The pair of states in each tree connected during 181 /// planning. Used for PlannerData computation 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