Commit 0de28494 authored by Leopold Palomo's avatar Leopold Palomo

Imported Upstream version 0.14.0

parent 4311fa27
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
......@@ -81,12 +81,34 @@ else()
option(OMPL_ODESOLVER "Enable OMPL ODE solver classes" OFF)
endif()
# on OS X we need to check whether to use libc++ or libstdc++ with clang++
if(APPLE AND CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
include(GetPrerequisites)
get_prerequisites("${Boost_SYSTEM_LIBRARY}" _libs 0 0 "/" "")
set(CXXSTDLIB "")
foreach(_lib ${_libs})
if(_lib MATCHES "libc\\+\\+")
set(CXXSTDLIB "libc++")
elseif(_lib MATCHES "libstdc\\+\\+")
set(CXXSTDLIB "libstdc++")
endif()
endforeach()
if(CXXSTDLIB)
add_definitions(-stdlib=${CXXSTDLIB})
endif()
endif()
# pthread is sometimes needed, depending on OS / compiler
find_package(Threads QUIET)
enable_testing()
# ODE is only needed for Open Dynamics Engine bindings
# MORSE is only needed for Modular OpenRobots Simulation Engine bindings
find_package(MORSE QUIET)
set(OMPL_EXTENSION_MORSE ${MORSE_FOUND})
# OpenDE is only needed for Open Dynamics Engine bindings
find_package(OpenDE QUIET)
set(OMPL_EXTENSION_OPENDE ${OPENDE_FOUND})
if (OPENDE_FOUND)
......@@ -107,6 +129,15 @@ if (FLANN_FOUND)
include_directories(SYSTEM "${FLANN_INCLUDE_DIRS}")
endif()
option(OMPL_LOCAL_PYPLUSPLUS_INSTALL "Whether Py++ and dependencies should be installed in the build directory" OFF)
set(SUDO "sudo")
set(CMAKE_GCCXML_ARGS "")
if(OMPL_LOCAL_PYPLUSPLUS_INSTALL)
set(SUDO "")
set(CMAKE_GCCXML_ARGS "-DCMAKE_INSTALL_PREFIX=${PROJECT_BINARY_DIR}/pyplusplus")
set(DISTUTILS_ARGS "--prefix=${PROJECT_BINARY_DIR}/pyplusplus")
endif()
add_subdirectory(py-bindings)
add_subdirectory(src)
add_subdirectory(tests)
......@@ -120,11 +151,14 @@ if (NOT MSVC)
set(PKG_OMPL_LIBS "-lompl")
set(pkg_conf_file "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules/ompl.pc")
configure_file("${pkg_conf_file}.in" "${pkg_conf_file}" @ONLY)
install(FILES "${pkg_conf_file}" DESTINATION lib/pkgconfig/
COMPONENT pkgconfig RENAME "ompl${OMPL_INSTALL_SUFFIX}.pc")
install(FILES "${pkg_conf_file}"
DESTINATION lib/pkgconfig/
COMPONENT ompl
RENAME "ompl${OMPL_INSTALL_SUFFIX}.pc")
install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/doc/markdown/FindOMPL.cmake"
DESTINATION "share/ompl${OMPL_INSTALL_SUFFIX}"
COMPONENT ompl
RENAME ompl-config.cmake)
if (NOT ${CMAKE_VERSION} VERSION_LESS 2.8.6)
include(WriteBasicConfigVersionFile)
......@@ -132,7 +166,8 @@ if (NOT MSVC)
"${CMAKE_CURRENT_BINARY_DIR}/ompl-config-version.cmake"
VERSION ${OMPL_VERSION} COMPATIBILITY SameMajorVersion)
install(FILES "${CMAKE_CURRENT_BINARY_DIR}/ompl-config-version.cmake"
DESTINATION "share/ompl${OMPL_INSTALL_SUFFIX}")
DESTINATION "share/ompl${OMPL_INSTALL_SUFFIX}"
COMPONENT ompl)
endif()
endif()
......
......@@ -7,6 +7,20 @@ set(CPACK_PACKAGE_VERSION_MAJOR "${OMPL_MAJOR_VERSION}")
set(CPACK_PACKAGE_VERSION_MINOR "${OMPL_MINOR_VERSION}")
set(CPACK_PACKAGE_VERSION_PATCH "${OMPL_PATCH_VERSION}")
# component list
set(CPACK_COMPONENTS_ALL ompl python morse)
# display names for components
set(CPACK_COMPONENT_OMPL_DISPLAY_NAME "OMPL library, headers, and demos")
set(CPACK_COMPONENT_PYTHON_DISPLAY_NAME "Python bindings")
set(CPACK_COMPONENT_MORSE_DISPLAY_NAME "Blender/MORSE plugin")
# descriptions of components
set(CPACK_COMPONENT_MORSE_DESCRIPTION "The Blender/MORSE plugin allows one to plan paths using the MORSE robot simulator. MORSE is built on top of Blender and uses its built-in physics engine to compute physically realistic motions.")
# intercomponent dependencies
set(CPACK_COMPONENT_PYTHON_DEPENDS ompl)
set(CPACK_COMPONENT_MORSE_DEPENDS python)
# core library is required
set(CPACK_COMPONENT_OMPL_REQUIRED TRUE)
set(CPACK_SOURCE_IGNORE_FILES
"/.hg"
"/build/"
......@@ -52,8 +66,12 @@ if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "python${PYTHON_VERSION}, libboost-all-dev, libode-dev, libtriangle-dev")
endif()
if(APPLE)
set(CPACK_GENERATOR "PackageMaker;${CPACK_GENERATOR}")
endif()
if(WIN32)
set(CPACK_GENERATOR "ZIP;${CPACK_GENERATOR}")
set(CPACK_GENERATOR "ZIP;${CPACK_GENERATOR}")
endif()
include(CPack)
# - Find the GCC-XML front-end executable.
#
# This module will define the following variables:
# GCCXML - the GCC-XML front-end executable.
#=============================================================================
# Copyright 2001-2009 Kitware, Inc.
#
# Distributed under the OSI-approved BSD License (the "License");
# see accompanying file Copyright.txt for details.
#
# This software is distributed WITHOUT ANY WARRANTY; without even the
# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
# See the License for more information.
#=============================================================================
# (To distribute this file outside of CMake, substitute the full
# License text for the above reference.)
# added extra path, to check for gccxml installed in build directory
find_program(GCCXML NAMES gccxml
PATHS "${PROJECT_BINARY_DIR}/pyplusplus/bin"
[HKEY_CURRENT_USER\\Software\\Kitware\\GCC_XML;loc]
"$ENV{ProgramFiles}/GCC_XML"
"C:/Program Files/GCC_XML")
mark_as_advanced(GCCXML)
# - Find MORSE
# This module finds if the Modular OpenRobots Simulation Engine (MORSE)
# is installed and determines where the include files and libraries
# are. The MORSE_PATH variable (or environment variable) can be set
# to specify where to look for MORSE.
#
# The following variables are set:
# MORSE_VERSION = the version of MORSE that was found; empty if morse was not found
include(FindPackageHandleStandardArgs)
# Uncomment the following line to enable debug output
#set(_MORSE_DEBUG_OUTPUT true)
if (NOT MORSE_PATH)
set(MORSE_PATH $ENV{MORSE_PATH})
endif()
find_program(MORSE_EXEC NAMES morse PATHS ${MORSE_PATH} PATH_SUFFIXES bin)
if (MORSE_EXEC)
# MORSE's version info is written to stderr, not stdout
execute_process(COMMAND ${MORSE_EXEC} --version ERROR_VARIABLE MORSE_VERSION)
if (MORSE_VERSION)
# remove newlines and 'morse '
string(REGEX REPLACE "[\r\n]+" "" MORSE_VERSION ${MORSE_VERSION})
string(REPLACE "morse " "" MORSE_VERSION ${MORSE_VERSION})
endif()
endif()
if (_MORSE_DEBUG_OUTPUT)
message(STATUS "------- FindMORSE.cmake Debug -------")
message(STATUS "MORSE = '${MORSE_EXEC}'")
message(STATUS "MORSE_VERSION = '${MORSE_VERSION}'")
message(STATUS "-------------------------------------")
endif()
find_package_handle_standard_args(MORSE DEFAULT_MSG MORSE_EXEC)
mark_as_advanced(MORSE_EXEC MORSE_VERSION)
# set the version in a way CMake can use
set(OMPL_MAJOR_VERSION 0)
set(OMPL_MINOR_VERSION 13)
set(OMPL_MINOR_VERSION 14)
set(OMPL_PATCH_VERSION 0)
set(OMPL_VERSION "${OMPL_MAJOR_VERSION}.${OMPL_MINOR_VERSION}.${OMPL_PATCH_VERSION}")
# increment this when we have ABI changes
set(OMPL_ABI_VERSION 7)
set(OMPL_ABI_VERSION 8)
......@@ -3,6 +3,7 @@ find_package(Boost COMPONENTS python)
# You can optionally specify the desired version like so:
# find_package(Python 2.6)
find_package(Python QUIET)
set(ENV{PYTHONPATH} "${PROJECT_BINARY_DIR}/pyplusplus/lib/python${PYTHON_VERSION}/site-packages:$ENV{PYTHONPATH}")
find_python_module(pyplusplus QUIET)
find_python_module(pygccxml QUIET)
find_package(GCCXML QUIET)
......@@ -33,6 +34,7 @@ if(PYTHON_FOUND AND Boost_PYTHON_LIBRARY)
set(OMPL_PYTHON_INSTALL_DIR "${PYTHON_SITE_MODULES}" CACHE STRING
"Path to directory where OMPL python modules will be installed")
endif()
if(PYTHON_FOUND AND Boost_PYTHON_LIBRARY AND PY_PYPLUSPLUS
AND PY_PYGCCXML AND GCCXML)
# make sure targets are defined only once
......@@ -70,7 +72,9 @@ function(create_module_code_generation_target module dir)
# target for regenerating code. Cmake is run so that the list of
# sources for the py_ompl_${module} target (see below) is updated.
add_custom_target(update_${module}_bindings
COMMAND ${PYTHON_EXEC}
COMMAND env
PYTHONPATH="${PROJECT_BINARY_DIR}/pyplusplus/lib/python${PYTHON_VERSION}/site-packages:$ENV{PYTHONPATH}"
${PYTHON_EXEC}
"${CMAKE_CURRENT_SOURCE_DIR}/generate_bindings.py" "${module}"
"1>${CMAKE_BINARY_DIR}/pyplusplus_${module}.log" "2>&1"
COMMAND ${CMAKE_COMMAND} -D "PATH=${dir}/bindings/${module}"
......@@ -131,7 +135,17 @@ function(create_module_target module dir)
WORKING_DIRECTORY ${LIBRARY_OUTPUT_PATH}
COMMENT "Copying python module ${module} into place")
endif(WIN32)
install(TARGETS py_ompl_${module} DESTINATION "${OMPL_PYTHON_INSTALL_DIR}/ompl/${module}/")
# put omplapp and MORSE bindings in separate components
if(${module} STREQUAL "app")
set(_component "omplapp")
elseif(${module} STREQUAL "morse")
set(_component "morse")
else()
set(_component "python")
endif()
install(TARGETS py_ompl_${module}
DESTINATION "${OMPL_PYTHON_INSTALL_DIR}/ompl/${module}/"
COMPONENT ${_component})
include_directories("${dir}/bindings/${module}" "${dir}")
else(NUM_SOURCE_FILES GREATER 0)
if(PY_OMPL_GENERATE)
......
* 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.
......@@ -24,10 +24,13 @@ if (OMPL_BUILD_DEMOS)
add_ompl_demo(demo_StateSampling StateSampling.cpp)
add_ompl_demo(demo_GeometricCarPlanning GeometricCarPlanning.cpp)
add_ompl_demo(demo_HybridSystemPlanning HybridSystemPlanning.cpp)
add_ompl_demo(demo_KinematicChainBenchmark KinematicChainBenchmark.cpp)
add_ompl_demo(demo_HypercubeBenchmark HypercubeBenchmark.cpp)
aux_source_directory(Koules Koules_SRC)
add_ompl_demo(demo_Koules ${Koules_SRC})
add_ompl_demo(demo_OptimalPlanning OptimalPlanning.cpp)
add_ompl_demo(demo_PlannerProgressProperties PlannerProgressProperties.cpp)
if (OMPL_EXTENSION_OPENDE)
add_ompl_demo(demo_OpenDERigidBodyPlanning OpenDERigidBodyPlanning.cpp)
......@@ -41,9 +44,14 @@ if (OMPL_BUILD_DEMOS)
if(NOT "${Boost_VERSION}" LESS 104400)
add_ompl_demo(demo_PlannerData PlannerData.cpp)
endif(NOT "${Boost_VERSION}" LESS 104400)
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}")
install(FILES ${OMPL_DEMO_CXX_FILES} ${OMPL_DEMO_PY_FILES}
DESTINATION "${OMPL_DEMO_INSTALL_DIR}"
COMPONENT ompl)
install(DIRECTORY Koules
DESTINATION "${OMPL_DEMO_INSTALL_DIR}"
COMPONENT ompl)
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, 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: Bryant Gipson, Mark Moll */
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/geometric/planners/kpiece/KPIECE1.h>
#include <ompl/geometric/planners/est/EST.h>
#include <ompl/geometric/planners/prm/PRM.h>
#include <ompl/geometric/planners/stride/STRIDE.h>
#include <ompl/tools/benchmark/Benchmark.h>
#include <boost/math/constants/constants.hpp>
#include <boost/format.hpp>
#include <fstream>
#include <libgen.h>
unsigned ndim = 6;
const double edgeWidth = 0.1;
// Only states near some edges of a hypercube are valid. The valid edges form a
// narrow passage from (0,...,0) to (1,...,1). A state s is valid if there exists
// a k s.t. (a) 0<=s[k]<=1, (b) for all i<k s[i]<=edgeWidth, and (c) for all i>k
// s[i]>=1-edgewidth.
bool isStateValid(const ompl::base::State *state)
{
const ompl::base::RealVectorStateSpace::StateType *s
= static_cast<const ompl::base::RealVectorStateSpace::StateType*>(state);
bool foundMaxDim = false;
for (int i = ndim - 1; i >= 0; i--)
if (!foundMaxDim)
{
if ((*s)[i] > edgeWidth)
foundMaxDim = true;
}
else if ((*s)[i] < (1. - edgeWidth))
return false;
return true;
}
void addPlanner(ompl::tools::Benchmark& benchmark, ompl::base::PlannerPtr planner, double range)
{
ompl::base::ParamSet& params = planner->params();
if (params.hasParam(std::string("range")))
params.setParam(std::string("range"), boost::lexical_cast<std::string>(range));
benchmark.addPlanner(planner);
}
int main(int argc, char **argv)
{
if(argc > 1)
ndim = boost::lexical_cast<size_t>(argv[1]);
double range = edgeWidth * 0.5;
ompl::base::StateSpacePtr space(new ompl::base::RealVectorStateSpace(ndim));
ompl::base::RealVectorBounds bounds(ndim);
ompl::geometric::SimpleSetup ss(space);
ompl::base::ScopedState<> start(space), goal(space);
bounds.setLow(0.);
bounds.setHigh(1.);
space->as<ompl::base::RealVectorStateSpace>()->setBounds(bounds);
ss.setStateValidityChecker(&isStateValid);
ss.getSpaceInformation()->setStateValidityCheckingResolution(0.001);
for(unsigned int i = 0; i < ndim; ++i)
{
start[i] = 0.;
goal[i] = 1.;
}
ss.setStartAndGoalStates(start, goal);
// by default, use the Benchmark class
double runtime_limit = 1000, memory_limit = 4096;
int run_count = 20;
ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
ompl::tools::Benchmark b(ss, "HyperCube");
addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())), range);
addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::EST(ss.getSpaceInformation())), range);
addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::KPIECE1(ss.getSpaceInformation())), range);
addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::RRT(ss.getSpaceInformation())), range);
addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::PRM(ss.getSpaceInformation())), range);
b.benchmark(request);
b.saveResultsToFile(boost::str(boost::format("hypercube_%i.log") % ndim).c_str());
exit(0);
}
This diff is collapsed.
......@@ -254,7 +254,7 @@ ob::OptimizationObjectivePtr getBalancedObjective1(const ob::SpaceInformationPtr
ob::OptimizationObjectivePtr clearObj(new ClearanceObjective(si));
ob::MultiOptimizationObjective* opt = new ob::MultiOptimizationObjective(si);
opt->addObjective(lengthObj, 5.0);
opt->addObjective(lengthObj, 10.0);
opt->addObjective(clearObj, 1.0);
return ob::OptimizationObjectivePtr(opt);
......@@ -268,7 +268,7 @@ ob::OptimizationObjectivePtr getBalancedObjective2(const ob::SpaceInformationPtr
ob::OptimizationObjectivePtr lengthObj(new ob::PathLengthOptimizationObjective(si));
ob::OptimizationObjectivePtr clearObj(new ClearanceObjective(si));
return 5.0*lengthObj + clearObj;
return 10.0*lengthObj + clearObj;
}
/** Create an optimization objective for minimizing path length, and
......
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, 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 */
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/tools/benchmark/Benchmark.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
namespace ob = ompl::base;
namespace og = ompl::geometric;
/// @cond IGNORE
//
// In this demo we define a simple 2D planning problem to get from
// (0,0) to (1,1) without obstacles. We're interested in collecting
// data over the execution of a planner using the Benchmark class.
//
// This program will output two files: a .console file, and a .log
// file. You can generate plots in a file called "plot.pdf" from this
// experiment using the ompl/scripts/ompl_benchmark_statistics.py
// script and the .log file like so:
//
// python path/to/ompl/scripts/ompl_benchmark_statistics.py <your_log_filename>.log -p plot.pdf
//
// Toward the bottom of the generated plot.pdf file, you'll see plots
// for how various properties change, on average, during planning
// runs.
int main(int argc, char** argv)
{
ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
space->as<ob::RealVectorStateSpace>()->setBounds(0, 1);
og::SimpleSetup ss(space);
// Set our robot's starting state to be the bottom-left corner of
// the environment, or (0,0).
ob::ScopedState<> start(space);
start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0;
// Set our robot's goal state to be the top-right corner of the
// environment, or (1,1).
ob::ScopedState<> goal(space);
goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;
// Goal tolerance is 0.05
ss.setStartAndGoalStates(start, goal, 0.05);
// Create a new Benchmark object and set the RRTstar algorithm as
// the planner. We're using RRTstar because currently, only the
// RRTstar algorithm reports interesting planner progress
// properties.
ompl::tools::Benchmark b(ss, "my experiment");
og::RRTstar* rrt = new og::RRTstar(ss.getSpaceInformation());
rrt->setName("rrtstar");
// We disable goal biasing so that the straight-line path doesn't
// get found immediately. We want to demonstrate the steady
// downward trend in path cost that characterizes RRTstar.
rrt->setGoalBias(0.0);
b.addPlanner(ob::PlannerPtr(rrt));
// Here we specify options for this benchmark. Maximum time spent
// per planner execution is 2.0 seconds, maximum memory is 100MB,
// number of planning runs is 50, planner progress properties will
// be queried every 0.01 seconds, and a progress bar will be
// displayed to show how the benchmarking is going.
ompl::tools::Benchmark::Request req;
req.maxTime = 2.0;
req.maxMem = 100;
req.runCount = 50;
req.timeBetweenUpdates = 0.01;
req.displayProgress = true;
b.benchmark(req);
b.saveResultsToFile();
return 0;
}
#!/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: 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 random import choice
## @cond IGNORE
class RandomWalkPlanner(ob.Planner):
def __init__(self, si):
super(RandomWalkPlanner, self).__init__(si, "RandomWalkPlanner")
self.states_ = []
self.sampler_ = si.allocStateSampler()
def solve(self, ptc):
pdef = self.getProblemDefinition()
goal = pdef.getGoal()
si = self.getSpaceInformation()
pi = self.getPlannerInputStates()
st = pi.nextStart()
while st:
self.states_.append(st)
st = pi.nextStart()
solution = None
approxsol = 0
approxdif = 1e6
while not ptc():
rstate = si.allocState()
# pick a random state in the state space
self.sampler_.sampleUniform(rstate)
# check motion
if si.checkMotion(self.states_[-1], rstate):
self.states_.append(rstate)
sat = goal.isSatisfied(rstate)
dist = goal.distanceGoal(rstate)
if sat:
approxdif = dist
solution = len(self.states_)
break
if dist < approxdif:
approxdif = dist
approxsol = len(self.states_)
solved = False
approximate = False
if not solution:
solution = approxsol
approximate = True
if solution:
path = og.PathGeometric(si)