1 /********************************************************************* 2 * Software License Agreement (BSD License) 3 * 4 * Copyright (c) 2008, Willow Garage, Inc. 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 the Willow Garage 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: Ioan Sucan */ 36 37 #ifndef OMPL_GEOMETRIC_PATH_GEOMETRIC_ 38 #define OMPL_GEOMETRIC_PATH_GEOMETRIC_ 39 40 #include "ompl/base/SpaceInformation.h" 41 #include "ompl/base/Path.h" 42 #include <vector> 43 #include <utility> 44 45 namespace ompl 46 { 47 namespace base 48 { 49 /// @cond IGNORE 50 OMPL_CLASS_FORWARD(OptimizationObjective); 51 /// @endcond 52 } 53 54 /** \brief This namespace contains code that is specific to planning under geometric constraints */ 55 namespace geometric 56 { 57 /// @cond IGNORE 58 /** \brief Forward declaration of ompl::geometric::PathGeometric */ 59 OMPL_CLASS_FORWARD(PathGeometric); 60 /// @endcond 61 62 /** \brief Definition of a geometric path. 63 64 This is the type of path computed by geometric planners. */ 65 class PathGeometric : public base::Path 66 { 67 public: 68 /** \brief Construct a path instance for a given space information */ PathGeometric(const base::SpaceInformationPtr & si)69 PathGeometric(const base::SpaceInformationPtr &si) : base::Path(si) 70 { 71 } 72 73 /** \brief Copy constructor */ 74 PathGeometric(const PathGeometric &path); 75 76 /** \brief Construct a path instance from a single state */ 77 PathGeometric(const base::SpaceInformationPtr &si, const base::State *state); 78 79 /** \brief Construct a path instance from two states (thus making a segment) */ 80 PathGeometric(const base::SpaceInformationPtr &si, const base::State *state1, const base::State *state2); 81 ~PathGeometric()82 ~PathGeometric() override 83 { 84 freeMemory(); 85 } 86 87 /** \brief Assignment operator */ 88 PathGeometric &operator=(const PathGeometric &other); 89 90 /** \brief The sum of the costs for the sequence of segments that make up the path, computed using 91 OptimizationObjective::motionCost(). OptimizationObjective::initialCost() and 92 OptimizationObjective::terminalCost() 93 are also used in the computation for the first and last states, respectively. Empty paths have identity 94 cost. */ 95 base::Cost cost(const base::OptimizationObjectivePtr &obj) const override; 96 97 /** \brief Compute the length of a geometric path (sum of lengths of segments that make up the path) */ 98 double length() const override; 99 100 /** \brief Check if the path is valid */ 101 bool check() const override; 102 103 /** \brief Compute a notion of smoothness for this 104 path. The closer the value is to 0, the smoother the 105 path. Detailed formula follows. 106 107 The idea is to look at the triangles formed by consecutive path segments and compute the angle between 108 those segments using 109 Pythagora's theorem. Then, the outside angle for the computed angle is normalized by the path segments 110 and contributes to the path smoothness. 111 For a straight line path, the smoothness will be 0. 112 \f[ 113 \mbox{smoothness} = \sum\limits_{i=2}^{n-1}\left(\frac{2\left(\pi - 114 \arccos\left(\frac{a_i^2+b_i^2-c_i^2}{2 a_i b_i}\right)\right)}{a_i + b_i}\right)^2 115 \f] 116 where \f$a_i = \mbox{dist}(s_{i-2}, s_{i-1}), b_i = \mbox{dist}(s_{i-1}, s_{i}), c_i = 117 \mbox{dist}(s_{i-2}, s_i)\f$, \f$s_i\f$ is the i<sup>th</sup> 118 state along the path (see getState()) and \f$\mbox{dist}(s_i, s_j)\f$ gives the distance between two 119 states (see ompl::base::StateSpace::distance()). 120 */ 121 double smoothness() const; 122 123 /** \brief Compute the clearance of the way-points along 124 the path (no interpolation is performed). Detailed formula follows. 125 126 The formula used for computing clearance is: 127 \f[ 128 \mbox{clearance} = \frac{1}{n}\sum\limits_{i=0}^{n-1}cl(s_i) 129 \f] 130 \f$n\f$ is the number of states along the path (see getStateCount()) 131 \f$s_i\f$ is the i<sup>th</sup> state along the path (see getState()) 132 \f$cl()\f$ gives the distance to the nearest invalid state for a particular state (see 133 ompl::base::StateValidityChecker::clearance()) 134 */ 135 double clearance() const; 136 137 /** \brief Print the path to a stream */ 138 void print(std::ostream &out) const override; 139 140 /** \brief Print the path as a real-valued matrix where the 141 i-th row represents the i-th state along the path. Each 142 row contains the state components as returned by 143 ompl::base::StateSpace::copyToReals. */ 144 virtual void printAsMatrix(std::ostream &out) const; 145 146 /** @name Path operations 147 @{ */ 148 149 /** \brief Insert a number of states in a path so that the 150 path is made up of exactly \e count states. States are 151 inserted uniformly (more states on longer 152 segments). Changes are performed only if a path has 153 less than \e count states. */ 154 void interpolate(unsigned int count); 155 156 /** \brief Insert a number of states in a path so that the 157 path is made up of (approximately) the states checked 158 for validity when a discrete motion validator is 159 used. */ 160 void interpolate(); 161 162 /** \brief Add a state at the middle of each segment */ 163 void subdivide(); 164 165 /** \brief Reverse the path */ 166 void reverse(); 167 168 /** \brief Check if the path is valid. If it is not, 169 attempts are made to fix the path by sampling around 170 invalid states. Not more than \e attempts samples are 171 drawn. 172 \return A pair of boolean values is returned. The first 173 value represents the validity of the path before any 174 change was made. The second value represents the 175 validity of the path after changes were attempted. If 176 no changes are attempted, the both values are true. 177 178 \note If repairing a path fails, the path may still be altered */ 179 std::pair<bool, bool> checkAndRepair(unsigned int attempts); 180 181 /** \brief Overlay the path \e over on top of the current 182 path. States are added to the current path if needed 183 (by copying the last state). 184 185 If \e over consists of states form a different 186 state space than the existing path, the data from those 187 states is copied over, for the corresponding 188 components. If \e over is from the same state space as this path, 189 and \e startIndex is 0, this function's result will be the same 190 as with operator=() */ 191 void overlay(const PathGeometric &over, unsigned int startIndex = 0); 192 193 /** \brief Append \e state to the end of this path. The memory for \e state is copied. */ 194 void append(const base::State *state); 195 196 /** \brief Append \e path at the end of this path. States from \e path are copied. 197 198 Let the existing path consist of states [ \e s1, \e 199 s2, ..., \e sk ]. Let \e path consist of states [\e y1, ..., \e yp]. 200 201 If the existing path and \e path consist of states 202 from the same state space, [\e y1, ..., \e yp] are added after \e sk. 203 If they are not from the same state space, states [\e z1, ..., \e zp] 204 are added, where each \e zi is a copy of \e sk that 205 has components overwritten with ones in \e yi (if there are any common subspaces). 206 */ 207 void append(const PathGeometric &path); 208 209 /** \brief Prepend \e state to the start of this path. The memory for \e state is copied. */ 210 void prepend(const base::State *state); 211 212 /** \brief Keep the part of the path that is after \e state (getClosestIndex() is used to find out which 213 * way-point is closest to \e state) */ 214 void keepAfter(const base::State *state); 215 216 /** \brief Keep the part of the path that is before \e state (getClosestIndex() is used to find out which 217 * way-point is closest to \e state) */ 218 void keepBefore(const base::State *state); 219 220 /** \brief Set this path to a random segment */ 221 void random(); 222 223 /** \brief Set this path to a random valid segment. Sample \e attempts times for valid segments. Returns 224 * true on success.*/ 225 bool randomValid(unsigned int attempts); 226 /** @} */ 227 228 /** @name Functionality for accessing states 229 @{ */ 230 231 /** \brief Get the index of the way-point along the path that is closest to \e state. Returns -1 for an 232 * empty path. */ 233 int getClosestIndex(const base::State *state) const; 234 235 /** \brief Get the states that make up the path (as a reference, so it can be modified, hence the function 236 * is not const) */ getStates()237 std::vector<base::State *> &getStates() 238 { 239 return states_; 240 } 241 242 /** \brief Get the state located at \e index along the path */ getState(unsigned int index)243 base::State *getState(unsigned int index) 244 { 245 return states_[index]; 246 } 247 248 /** \brief Get the state located at \e index along the path */ getState(unsigned int index)249 const base::State *getState(unsigned int index) const 250 { 251 return states_[index]; 252 } 253 254 /** \brief Get the number of states (way-points) that make up this path */ getStateCount()255 std::size_t getStateCount() const 256 { 257 return states_.size(); 258 } 259 260 /** \brief Remove all states and clear memory */ 261 void clear(); 262 263 /** @} */ 264 265 protected: 266 /** \brief Free the memory corresponding to the states on this path */ 267 void freeMemory(); 268 269 /** \brief Copy data to this path from another path instance */ 270 void copyFrom(const PathGeometric &other); 271 272 /** \brief The list of states that make up the path */ 273 std::vector<base::State *> states_; 274 }; 275 } 276 } 277 278 #endif 279