Commit fd0c32ad authored by Leopold Palomo's avatar Leopold Palomo

Imported Upstream version 0.13.0+git20130920.01d0ca4

parent 7d3efca0
repo: acc0a7de5b906703869dd1e58b000b426d18be78
node: 01d0ca40d1157295c959e4e01ec222ecb170fc2e
branch: default
latesttag: 0.13.0
latesttagdistance: 37
.registered
.DS_Store
~$
\.swp$
build/
bin/
lib/
\.pcd$
\.pyc$
\.so$
\.dylib$
CMakeModules/ompl.pc
doc/markdown/api_overview.md
doc/markdown/download.md
doc/markdown/mainpage.md
doc/html
py-bindings/ompl/bindings_generator.py
py-bindings/bindings
py-bindings/ompl/app
py-bindings/ompl/rrtstar
src/external/installPyPlusPlus.bat
src/external/installPyPlusPlus.sh
src/ompl/config.h
tests/BoostTestTeamCityReporter.h
tests/resources/config.h
_symlinks.sh$
aee178ba8590f58441747451fb2386e64e302d78 0.9.0
0b60e91782f8c385bf2ecc972e52f30ca690b635 0.9.1
b0c5d253d01c963726e23236dbee59154d9b1160 0.9.2
27fad3b02262f533e00b5c5171a5015c6ade2487 0.9.3
86398521479a6d522fd2dc4b48881ce86896f7cd ros-electric
0000000000000000000000000000000000000000 ros-electric
03b8a1abf309b1f21fd5060f917b328bf7573dce 0.9.4
2b2b5e4ba3f4004d0e882c79992d1b7c182fa3cc 0.9.5
0000000000000000000000000000000000000000 release 0.9.2
0000000000000000000000000000000000000000 release 0.9.3
0000000000000000000000000000000000000000 release 0.9.4
0000000000000000000000000000000000000000 release 0.9.5
1d3668ffd1b8083891dcfbd3140d97970ba7a786 0.10.0
1d3668ffd1b8083891dcfbd3140d97970ba7a786 0.10.0
75d8b0c7f0607084d1b7c3289b5482cfe7519edd 0.10.0
4dab2e897ce1e07974a8c721c797c82d5a3b21e2 0.10.1
3f4f9d78ccd54a1488344a98638d92b14dcd4f79 0.10.2
4926c9b62c3029cd86d76c0214e9d1b5b14e34a7 0.11.0
3dd7a9f95bace77eb5042c9aac34dce5fa37a087 0.11.1
98cef58fd24ff2bb8b11eb2618658fe936bfe033 0.12.0
d6a586a120bb699c8b7c6c80662d865f390f1f65 0.12.1
30eac1806535fcee8987ac611a502f7d6a89b508 0.12.2
c22bde1c6a18987e2eda5684a7f81d0404e60b4d 0.13.0
......@@ -4,13 +4,13 @@ project(ompl CXX C)
# set the default build type
if (NOT CMAKE_BUILD_TYPE)
# By default, use Release mode
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Type of build" FORCE)
# On 32bit architectures with gcc < 4.7, use RelWithDebInfo
if (CMAKE_COMPILER_IS_GNUCC AND CMAKE_SIZEOF_VOID_P EQUAL 4)
execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion OUTPUT_VARIABLE GCC_VERSION)
if (GCC_VERSION VERSION_LESS 4.7)
set(CMAKE_BUILD_TYPE RelWithDebInfo)
set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Type of build" FORCE)
endif()
endif()
endif()
......@@ -53,7 +53,21 @@ else(IS_ICPC)
add_definitions(-DBOOST_TEST_DYN_LINK)
endif(IS_ICPC)
find_package(Boost COMPONENTS date_time thread serialization filesystem system program_options unit_test_framework REQUIRED)
# Do one quiet find_package on Boost to see if it is recent enough to
# have the try_join_for call
find_package(Boost QUIET 1.50)
# try_join_for requires the chrono library, so if we will use
# try_join_for, we need to include the chrono component
# Must recheck the Boost version, since update_bindings will re-run CMake
# and this will pass for versions of Boost < 1.50
if (${Boost_FOUND} AND ${Boost_VERSION} GREATER 104900) # Boost version is at least 1.50
# we're using chrono
find_package(Boost COMPONENTS date_time thread serialization filesystem system program_options unit_test_framework chrono REQUIRED)
else()
# don't use chrono
find_package(Boost COMPONENTS date_time thread serialization filesystem system program_options unit_test_framework REQUIRED)
endif()
include_directories(SYSTEM ${Boost_INCLUDE_DIR})
if (NOT ${Boost_VERSION} LESS 104400)
......@@ -109,7 +123,7 @@ if (NOT MSVC)
install(FILES "${pkg_conf_file}" DESTINATION lib/pkgconfig/
COMPONENT pkgconfig RENAME "ompl${OMPL_INSTALL_SUFFIX}.pc")
install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/doc/dox/FindOMPL.cmake"
install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/doc/markdown/FindOMPL.cmake"
DESTINATION "share/ompl${OMPL_INSTALL_SUFFIX}"
RENAME ompl-config.cmake)
if (NOT ${CMAKE_VERSION} VERSION_LESS 2.8.6)
......
......@@ -16,6 +16,7 @@ set(CPACK_SOURCE_IGNORE_FILES
".so$"
".dylib$"
".orig$"
".log$"
".DS_Store"
".tm_properties"
"mkwebdocs.sh"
......@@ -48,7 +49,7 @@ if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
OUTPUT_VARIABLE UBUNTU_RELEASE
OUTPUT_STRIP_TRAILING_WHITESPACE)
set(CPACK_PACKAGE_FILE_NAME "omplapp_${OMPL_VERSION}_${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}-Ubuntu${UBUNTU_RELEASE}")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "python${PYTHON_VERSION}, libboost-all-dev, libode-dev")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "python${PYTHON_VERSION}, libboost-all-dev, libode-dev, libtriangle-dev")
endif()
if(WIN32)
......
......@@ -3,9 +3,13 @@ if(CMAKE_COMPILER_IS_GNUCXX)
-Wcast-qual -Wwrite-strings -Wunreachable-code -Wpointer-arith
-Winit-self -Wredundant-decls
-Wno-unused-parameter -Wno-unused-function)
# prepend optimizion flag (in case the default setting doesn't include one)
set(CMAKE_CXX_FLAGS_RELEASE "-O3 ${CMAKE_CXX_FLAGS_RELEASE}")
endif(CMAKE_COMPILER_IS_GNUCXX)
if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
add_definitions(-W -Wall -Wextra -Wno-missing-field-initializers -Wno-unused -Wno-unused-parameter -Wno-delete-non-virtual-dtor -Wno-overloaded-virtual -Wno-unknown-pragmas -Qunused-arguments)
# prepend optimizion flag (in case the default setting doesn't include one)
set(CMAKE_CXX_FLAGS_RELEASE "-O3 ${CMAKE_CXX_FLAGS_RELEASE}")
endif()
if(MSVC OR MSVC90 OR MSVC10)
......
......@@ -8,11 +8,15 @@ find_python_module(pygccxml QUIET)
find_package(GCCXML QUIET)
if(APPLE)
# The latest gccxml can be compiled with clang, but cannot be run by
# pretending to be clang. Instead, we have to explicitly tell it to
# pretend to be g++. Gccxml also mistakenly thinks that OS X is a 32-bit
# architecture.
set(PYOMPL_EXTRA_CFLAGS "--gccxml-compiler g++ -m64")
# The latest gccxml can be *compiled* with clang, but cannot *simulate*
# clang. If you compiled gccxml with clang, then you have to specify a
# g++ compiler by adding the following to PYOMPL_EXTRA_CFLAGS:
# --gccxml-compiler /opt/local/bin/g++-mp-4.8
# (You can use other versions of g++ as well.) Note that /usr/bin/g++
# is actually clang++ in Xcode 5.0, so that won't work.
#
# Gccxml mistakenly thinks that OS X is a 32-bit architecture.
set(PYOMPL_EXTRA_CFLAGS "-m64")
endif(APPLE)
if(PYTHON_FOUND AND Boost_PYTHON_LIBRARY)
......
......@@ -3,7 +3,7 @@
# For some boost::function's it generates code like this: boost::function<int()(int)>
# This script fixes that, albeit in a very fragile way. This could probably be improved
# by someone with better regexp writing skills.
file(GLOB fnames "${PATH}/*.cpp")
file(GLOB fnames "${PATH}/*.pypp.*")
foreach(fname ${fnames})
file(READ ${fname} _text)
string(REPLACE " ()(" " (" _new_text "${_text}")
......
* Start another 'concept' for ompl -- something that has algorithms for the analysis of spaces to plan in: Identify good lengths of path segments, good discretizations of control sets, automatic setting of parameters (projection cell sizes?), identification of good random projections
* Add meaningful RRT* tests
* Make PlannerTerminationCondition just sleep for a while (during the runtime of the algorithm), until condition should become true; this sleep should be interruptible from the parent thread. This may make some things slightly more efficient, but we need to check what the wait time is for the interruption to work.
* Better support for anytime planners? Need to think about this. Maybe it comes out from the previous point
- We need an event callback for anytime planners to let the user know that a solution has been found.
......@@ -27,6 +27,8 @@ if (OMPL_BUILD_DEMOS)
aux_source_directory(Koules Koules_SRC)
add_ompl_demo(demo_Koules ${Koules_SRC})
add_ompl_demo(demo_OptimalPlanning OptimalPlanning.cpp)
if (OMPL_EXTENSION_OPENDE)
add_ompl_demo(demo_OpenDERigidBodyPlanning OpenDERigidBodyPlanning.cpp)
endif()
......@@ -44,4 +46,4 @@ endif(OMPL_BUILD_DEMOS)
file(GLOB OMPL_DEMO_CXX_FILES "*.cpp")
file(GLOB OMPL_DEMO_PY_FILES "*.py")
install(FILES ${OMPL_DEMO_CXX_FILES} ${OMPL_DEMO_PY_FILES} DESTINATION "${OMPL_DEMO_INSTALL_DIR}")
install(DIRECTORY Koules DESTINATION "${OMPL_DEMO_INSTALL_DIR}")
\ No newline at end of file
install(DIRECTORY Koules DESTINATION "${OMPL_DEMO_INSTALL_DIR}")
......@@ -49,6 +49,7 @@
namespace ob = ompl::base;
namespace oc = ompl::control;
/// @cond IGNORE
class KoulesStateValidityChecker : public ompl::base::StateValidityChecker
{
public:
......@@ -61,6 +62,7 @@ public:
return si_->satisfiesBounds(state);
}
};
/// @endcond
ompl::control::DirectedControlSamplerPtr KoulesDirectedControlSamplerAllocator(
const ompl::control::SpaceInformation *si, const ompl::base::GoalPtr &goal, bool propagateMax)
......
......@@ -41,6 +41,7 @@
OMPL_CLASS_FORWARD(KoulesSetup);
/// @cond IGNORE
class KoulesSetup : public ompl::control::SimpleSetup
{
public:
......@@ -55,5 +56,6 @@ private:
void initialize(unsigned int numKoules, const std::string& plannerName,
const std::vector<double>& stateVec = std::vector<double>());
};
/// @endcond
#endif
......@@ -40,6 +40,7 @@
#include "KoulesConfig.h"
#include <ompl/base/spaces/RealVectorStateSpace.h>
/// @cond IGNORE
class KoulesStateSpace : public ompl::base::RealVectorStateSpace
{
public:
......@@ -61,5 +62,6 @@ protected:
std::vector<double> mass_;
std::vector<double> radius_;
};
/// @endcond
#endif
This diff is collapsed.
#!/usr/bin/env python
######################################################################
# Software License Agreement (BSD License)
#
# Copyright (c) 2010, Rice University
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of the Rice University nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
######################################################################
# Author: Luis G. Torres, Mark Moll
try:
from ompl import util as ou
from ompl import base as ob
from ompl import geometric as og
except:
# if the ompl module is not in the PYTHONPATH assume it is installed in a
# subdirectory of the parent directory called "py-bindings."
from os.path import basename, abspath, dirname, join
import sys
sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
from ompl import util as ou
from ompl import base as ob
from ompl import geometric as og
from math import sqrt
from sys import argv
## @cond IGNORE
# Our "collision checker". For this demo, our robot's state space
# lies in [0,1]x[0,1], with a circular obstacle of radius 0.25
# centered at (0.5,0.5). Any states lying in this circular region are
# considered "in collision".
class ValidityChecker(ob.StateValidityChecker):
def __init__(self, si):
super(ValidityChecker, self).__init__(si)
# Returns whether the given state's position overlaps the
# circular obstacle
def isValid(self, state):
return self.clearance(state) > 0.0
# Returns the distance from the given state's position to the
# boundary of the circular obstacle.
def clearance(self, state):
# Extract the robot's (x,y) position from its state
x = state[0]
y = state[1]
# Distance formula between two points, offset by the circle's
# radius
return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25
## Returns a structure representing the optimization objective to use
# for optimal motion planning. This method returns an objective
# which attempts to minimize the length in configuration space of
# computed paths.
def getPathLengthObjective(si):
return ob.PathLengthOptimizationObjective(si)
## Returns an optimization objective which attempts to minimize path
# length that is satisfied when a path of length shorter than 1.51
# is found.
def getThresholdPathLengthObj(si):
obj = ob.PathLengthOptimizationObjective(si)
obj.setCostThreshold(ob.Cost(1.51))
return obj
## Defines an optimization objective which attempts to steer the
# robot away from obstacles. To formulate this objective as a
# minimization of path cost, we can define the cost of a path as a
# summation of the costs of each of the states along the path, where
# each state cost is a function of that state's clearance from
# obstacles.
#
# The class StateCostIntegralObjective represents objectives as
# summations of state costs, just like we require. All we need to do
# then is inherit from that base class and define our specific state
# cost function by overriding the stateCost() method.
#
class ClearanceObjective(ob.StateCostIntegralObjective):
def __init__(self, si):
super(ClearanceObjective, self).__init__(si, True)
self.si_ = si
# Our requirement is to maximize path clearance from obstacles,
# but we want to represent the objective as a path cost
# minimization. Therefore, we set each state's cost to be the
# reciprocal of its clearance, so that as state clearance
# increases, the state cost decreases.
def stateCost(self, s):
return ob.Cost(1 / self.si_.getStateValidityChecker().clearance(s))
## Return an optimization objective which attempts to steer the robot
# away from obstacles.
def getClearanceObjective(si):
return ClearanceObjective(si)
## Create an optimization objective which attempts to optimize both
# path length and clearance. We do this by defining our individual
# objectives, then adding them to a MultiOptimizationObjective
# object. This results in an optimization objective where path cost
# is equivalent to adding up each of the individual objectives' path
# costs.
#
# When adding objectives, we can also optionally specify each
# objective's weighting factor to signify how important it is in
# optimal planning. If no weight is specified, the weight defaults to
# 1.0.
def getBalancedObjective1(si):
lengthObj = ob.PathLengthOptimizationObjective(si)
clearObj = ClearanceObjective(si)
opt = ob.MultiOptimizationObjective(si)
opt.addObjective(lengthObj, 5.0)
opt.addObjective(clearObj, 1.0)
return opt
## Create an optimization objective equivalent to the one returned by
# getBalancedObjective1(), but use an alternate syntax.
# THIS DOESN'T WORK YET. THE OPERATORS SOMEHOW AREN'T EXPORTED BY Py++.
# def getBalancedObjective2(si):
# lengthObj = ob.PathLengthOptimizationObjective(si)
# clearObj = ClearanceObjective(si)
#
# return 5.0*lengthObj + clearObj
## Create an optimization objective for minimizing path length, and
# specify a cost-to-go heuristic suitable for this optimal planning
# problem.
def getPathLengthObjWithCostToGo(si):
obj = ob.PathLengthOptimizationObjective(si)
obj.setCostToGoHeuristic(ob.CostToGoHeuristic(ob.goalRegionCostToGo))
return obj
def plan(fname = None):
# Construct the robot state space in which we're planning. We're
# planning in [0,1]x[0,1], a subset of R^2.
space = ob.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,1].
space.setBounds(0.0, 1.0)
# Construct a space information instance for this state space
si = ob.SpaceInformation(space)
# Set the object used to check which states in the space are valid
validityChecker = ValidityChecker(si)
si.setStateValidityChecker(validityChecker)
si.setup()
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = 0.0
start[1] = 0.0
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = 1.0
goal[1] = 1.0
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Since we want to find an optimal plan, we need to define what
# is optimal with an OptimizationObjective structure. Un-comment
# exactly one of the following 6 lines to see some examples of
# optimization objectives.
pdef.setOptimizationObjective(getPathLengthObjective(si))
# pdef.setOptimizationObjective(getThresholdPathLengthObj(si))
# pdef.setOptimizationObjective(getClearanceObjective(si))
# pdef.setOptimizationObjective(getBalancedObjective1(si))
# pdef.setOptimizationObjective(getBalancedObjective2(si))
# pdef.setOptimizationObjective(getPathLengthObjWithCostToGo(si))
# Construct our optimal planner using the RRTstar algorithm.
optimizingPlanner = og.RRTstar(si)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
# attempt to solve the planning problem within one second of
# planning time
solved = optimizingPlanner.solve(10.0)
if solved:
# Output the length of the path found
print("Found solution of path length %g" % pdef.getSolutionPath().length())
# If a filename was specified, output the path as a matrix to
# that file for visualization
if fname:
with open(fname,'w') as outFile:
outFile.write(pdef.getSolutionPath().printAsMatrix())
else:
print("No solution found.")
if __name__ == "__main__":
fname = None if len(argv)<2 else argv[1]
plan(fname)
## @endcond
......@@ -10,13 +10,13 @@ if (OMPL_BUILD_DOC)
OUTPUT_VARIABLE OMPL_RELEASE_DATE
OUTPUT_STRIP_TRAILING_WHITESPACE)
endif()
configure_file("${CMAKE_CURRENT_SOURCE_DIR}/dox/download.md.in"
"${CMAKE_CURRENT_SOURCE_DIR}/dox/download.md")
configure_file("${CMAKE_CURRENT_SOURCE_DIR}/dox/mainpage.md.in"
"${CMAKE_CURRENT_SOURCE_DIR}/dox/mainpage.md")
configure_file("${CMAKE_CURRENT_SOURCE_DIR}/markdown/download.md.in"
"${CMAKE_CURRENT_SOURCE_DIR}/markdown/download.md")
configure_file("${CMAKE_CURRENT_SOURCE_DIR}/markdown/mainpage.md.in"
"${CMAKE_CURRENT_SOURCE_DIR}/markdown/mainpage.md")
file(READ "${CMAKE_CURRENT_SOURCE_DIR}/images/ompl.svg" OMPLSVG)
configure_file("${CMAKE_CURRENT_SOURCE_DIR}/dox/api_overview.md.in"
"${CMAKE_CURRENT_SOURCE_DIR}/dox/api_overview.md")
configure_file("${CMAKE_CURRENT_SOURCE_DIR}/markdown/api_overview.md.in"
"${CMAKE_CURRENT_SOURCE_DIR}/markdown/api_overview.md")
# add "doc" target if Doxygen is detected
add_custom_target(doc
......
......@@ -654,7 +654,7 @@ WARN_LOGFILE =
# directories like "/usr/src/myproject". Separate the files or directories
# with spaces.
INPUT = ./dox/ \
INPUT = ./markdown/ \
../src/ompl \
../demos
......@@ -722,7 +722,7 @@ EXCLUDE_SYMBOLS =
# the \include command).
EXAMPLE_PATH = ../demos \
dox/code
markdown/code
# If the value of the EXAMPLE_PATH tag contains directories, you can use the
# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp
......
This diff is collapsed.
......@@ -22,5 +22,6 @@
- [Rigid body planning with an Inverse Kinematics solver generating goal states in a separate thread.](RigidBodyPlanningWithIK_8cpp_source.html) This demo shows off two neat features of OMPL: a genetic algorithm-based Inverse Kinematics solver and a lazy goal state sampler. In a separate thread goal states are computed by the IK solver. While solving a motion planning problem, the planning algorithms select a random goal state from the ones computed so far.
- [Rigid body planning using the Open Dynamics Engine (OpenDE).](OpenDERigidBodyPlanning_8cpp_source.html) When OpenDE is installed, OMPL will compile an extension that makes is easier to use OpenDE for forward propagation of models of motion. In this example, a box is pushed around in the plane from a start position to a goal position.
- [Planning for Dubins and Reeds-Shepp cars.](GeometricCarPlanning_8cpp_source.html) This demo illustrates the use of the ompl::base::DubinsStateSpace and ompl::base::ReedsSheppStateSpace. The demo can solve two simple planning problems, print trajectories from the origin to a user-specified state, or print a discretized distance field.
- [Optimal planning for a 2D point robot.](OptimalPlanning_8cpp_source.html) [[Python version]](OptimalPlanning_8py_source.html). This demo illustrates the use of `ompl::base::OptimizationObjective` to construct optimization objectives for optimal motion planning.
@}
......@@ -8,11 +8,13 @@ OMPL is developed and maintained by the [Physical and Biological Computing Group
- [Ioan Șucan](http://ioan.sucan.ro), Willow Garage (formerly Rice University)
- [Ryan Luna](http://www.ryanluna.com), Rice University
- [Matt Maly](http://kavrakilab.org/profiles/mmaly), Rice University
- [Luis Torres](http://luis.web.unc.edu), Ron Alterovitz' [Computational Robotics Group](http://robotics.cs.unc.edu), University of North Carolina at Chapel Hill
# Contributors:
- [Oren Salzman](http://acg.cs.tau.ac.il/people/oren-salzman/oren-salzman), Dan Halperin's [Computational Geometry Lab](http://acg.cs.tau.ac.il), Tel Aviv University
- [Dave Coleman](http://davetcoleman.com/), Nikolaus Correll's [group](http://correll.cs.colorado.edu/), University of Colorado Boulder
- [Luis Torres](http://luis.web.unc.edu), Ron Alterovitz' [Computational Robotics Group](http://robotics.cs.unc.edu), University of North Carolina at Chapel Hill
- [Andrew Dobson](http://www.pracsyslab.org/dobson), Kostas Bekris' [Physics-aware Research for Autonomous Computational SYStems group](http://www.pracsyslab.org), Rutgers University
- [James Marble](http://www.cse.unr.edu/robotics/pracsys/marble), Kostas Bekris' [Physics-aware Research for Autonomous Computational SYStems group](http://www.cse.unr.edu/robotics/pracsys), University of Nevada, Reno
- [Alejandro Perez](http://people.csail.mit.edu/aperez/www), Seth Teller's [Robotics, Vision, and Sensor Networks Group](http://rvsn.csail.mit.edu), MIT
......
# Optimal Planning
In some motion planning problems, you might not want just _any_ valid path between your start and states goal. You might be interested in the shortest path, or perhaps the path that steers the farthest away from obstacles. In these cases you're looking for an _optimal_ path: a path which satisfies your constraints (connects start and goal states without collisions) and also optimizes some path quality metric. Path length and path clearance are two examples of path quality metrics. Motion planners which attempt to optimize path quality metrics are known as _optimizing planners_.
In order to perform optimal planning, you need two things:
1. A path quality metric, or _optimization objective_.
2. An optimizing motion planner
You can specify a path quality metric using the `ompl::base::OptimizationObjective` class. As for the optimizing planner, OMPL currently provides two optimizing planners that guarantee _asymptotic optimality_ of returned solutions:
- `ompl::geometric::PRMstar`
- `ompl::geometric::RRTstar`
You can find out more about asymptotic optimality in motion planning by checking out [this paper](http://sertac.scripts.mit.edu/web/wp-content/papercite-data/pdf/karaman.frazzoli-ijrr11.pdf) by Karaman and Frazzoli. The following planners also support `ompl::base::OptimizationObjective`, but do not provide theoretical guarantees on solution optimality:
- `ompl::geometric::PRM`
- `ompl::geometric::TRRT`
Lastly, the following two planners provide a theoretical guarantee of _asymptotic near optimality_, but currently do not support user-specified objectives:
- `ompl::geometric::SPARS`
- `ompl::geometric::SPARStwo`
## Optimization Objectives
OMPL comes with several predefined optimization objectives:
- path length
- minimum path clearance
- general state cost integral
- mechanical work
Each of these optimization objectives defines a notion of the _cost_ of a path. In OMPL we define cost as a value accrued over a motion in the robot's configuration space. In geometric planning, a motion is fully defined by a start state and an end state. By default, these objectives attempt to minimize the path cost, but this behavior can be customized. We also assume that the cost of an entire path can be broken down into an accumulation of the costs of the smaller motions that make up the path; the method of accumulation (e.g. summation, multiplication, etc.) can be customized.
OMPL also provides users with the ability to combine objectives for multi-objective optimal planning problems by using the `ompl::base::MultiOptimizationObjective` class.
## Limitations
To maximize computational efficiency, OMPL assumes that the cost of a path can be represented with one `double` value. In many problems one value will suffice to fully define the optimization objective, even if we're working with a multi-objective problem. In these cases, the cost of a path can be represented as a weighted sum of the costs of the path under each of the individual objectives which make up the multi-objective.
However, there are problems that cannot be exactly represented with this assumption. For instance, OMPL cannot represent a multi-objective which combines the following two objectives:
- minimizing path length
- maximizing minimum clearance
The reason why these two objectives cannot be combined is because we need more than one value to perform the accumulation of the path cost. We need one value to hold the accumulation of length along the path, and another value to hold the the minimum clearance value encountered so far in the path. Therefore, _the multi-objective problems that cannot be represented in OMPL are those where the individual objectives do not share a cost accumulation function_. We note that the above objective can be approximated by combining a path length objective with a state cost integral objective where state cost is a function of clearance.
OMPL currently does not support control planning with general optimization objectives.
## Wanna learn more?
Check out these tutorials:
- [Optimal planning](optimalPlanningTutorial.html)
- [Defining optimization objectives for optimal planning](optimizationObjectivesTutorial.html)
and the [optimal planning demo](OptimalPlanning_8cpp_source.html).
# Optimal Planning Tutorial
Defining an optimal motion planning problem is almost exactly the same as defining a regular motion planning problem, with two main differences:
1. You need to specify an optimization objective to `ompl::base::ProblemDefinition`.
2. You need to use an optimizing planner for the actual motion planning.
## Finding the shortest path
We'll demonstrate OMPL's optimal planning framework with an example. In this example, our robot is represented as a (x,y) coordinate on a square, where (0,0) is the square's bottom-left corner and (1,1) is the square's top-right corner. There is also an obstacle in this square that the robot cannot pass through; this obstacle is a circle of radius 0.25 centered at (0.5,0.5). To reflect this environment, we use a two-dimensional `ompl::base::RealVectorStateSpace` and define our state validity checker as follows:
~~~{.cpp}
// Our collision checker. For this demo, our robot's state space
// lies in [0,1]x[0,1], with a circular obstacle of radius 0.25
// centered at (0.5,0.5). Any states lying in this circular region are
// considered "in collision".
class ValidityChecker : public ob::StateValidityChecker
{
public:
ValidityChecker(const ob::SpaceInformationPtr& si) :
ob::StateValidityChecker(si) {}
// Returns whether the given state's position overlaps the
// circular obstacle
bool isValid(const ob::State* state) const
{
return this->clearance(state) > 0.0;
}
// Returns the distance from the given state's position to the
// boundary of the circular obstacle.
double clearance(const ob::State* state) const
{
// We know we're working with a RealVectorStateSpace in this
// example, so we downcast state into the specific type.
const ob::RealVectorStateSpace::StateType* state2D =
state->as<ob::RealVectorStateSpace::StateType>();
// Extract the robot's (x,y) position from its state
double x = state2D->values[0];