Imported Upstream version 1.0.0+ds1

parent b5914d39
.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/header.html
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
package.xml
.dir-locals.el
\ No newline at end of file
cmake_minimum_required(VERSION 2.8)
if(NOT ${CMAKE_VERSION} VERSION_LESS 3.0.0)
cmake_policy(SET CMP0042 OLD)
endif()
project(ompl CXX C)
# set the default build type
......@@ -21,6 +24,7 @@ if(MSVC OR MSVC90 OR MSVC10)
endif (MSVC OR MSVC90 OR MSVC10)
set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules")
include(GNUInstallDirs)
include(CompilerSettings)
include(OMPLVersion)
include(OMPLUtils)
......@@ -33,8 +37,6 @@ set(OMPL_CMAKE_UTIL_DIR "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules"
set(OMPL_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/src")
set(OMPL_DEMO_INSTALL_DIR "share/ompl${OMPL_INSTALL_SUFFIX}/demos"
CACHE STRING "Relative path to directory where demos will be installed")
set(OMPL_DOC_INSTALL_DIR "share/ompl${OMPL_INSTALL_SUFFIX}/doc"
CACHE STRING "Relative path to directory where documentation will be installed")
include_directories("${OMPL_INCLUDE_DIR}")
......@@ -74,15 +76,10 @@ else()
endif()
include_directories(SYSTEM ${Boost_INCLUDE_DIR})
if (NOT ${Boost_VERSION} LESS 104400)
option(OMPL_ODESOLVER "Enable OMPL ODE solver classes" ON)
if(${Boost_VERSION} LESS 105300)
# Include bundled version of boost::odeint if it isn't installed natively
set(ODEINT_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/src/external")
include_directories(SYSTEM "${ODEINT_INCLUDE_DIR}")
endif()
else()
option(OMPL_ODESOLVER "Enable OMPL ODE solver classes" OFF)
if(${Boost_VERSION} LESS 105300)
# Include bundled version of boost::odeint if it isn't installed natively
set(ODEINT_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/src/external")
include_directories(SYSTEM "${ODEINT_INCLUDE_DIR}")
endif()
# on OS X we need to check whether to use libc++ or libstdc++ with clang++
......@@ -108,6 +105,9 @@ find_package(Threads QUIET)
enable_testing()
find_package(Python QUIET)
find_package(Boost COMPONENTS python QUIET)
# MORSE is only needed for Modular OpenRobots Simulation Engine bindings
find_package(MORSE QUIET)
set(OMPL_EXTENSION_MORSE ${MORSE_FOUND})
......@@ -156,23 +156,14 @@ if (NOT MSVC)
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/
DESTINATION ${CMAKE_INSTALL_LIBDIR}/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}"
DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/ompl${OMPL_INSTALL_SUFFIX}"
COMPONENT ompl
RENAME ompl-config.cmake)
if (NOT ${CMAKE_VERSION} VERSION_LESS 2.8.6)
include(WriteBasicConfigVersionFile)
write_basic_config_version_file(
"${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}"
COMPONENT ompl)
endif()
endif()
# uninstall target
......
......@@ -47,7 +47,9 @@ set(CPACK_SOURCE_IGNORE_FILES
".registered$"
"download.md$"
"mainpage.md$"
"binding_generator.py$")
"binding_generator.py$"
"gen_validatorv31.js"
"/php/")
set(CPACK_SOURCE_GENERATOR "TGZ;ZIP")
set(CPACK_GENERATOR "TGZ")
......
# set the version in a way CMake can use
set(OMPL_MAJOR_VERSION 0)
set(OMPL_MINOR_VERSION 14)
set(OMPL_PATCH_VERSION 2)
set(OMPL_MAJOR_VERSION 1)
set(OMPL_MINOR_VERSION 0)
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 9)
set(OMPL_ABI_VERSION 10)
......@@ -23,8 +23,7 @@ endif(APPLE)
if(PYTHON_FOUND AND Boost_PYTHON_LIBRARY)
include_directories(${PYTHON_INCLUDE_DIRS})
# make sure target is defined only once
get_target_property(_target py_ompl EXCLUDE_FROM_ALL)
if(NOT _target)
if(NOT TARGET py_ompl)
# top-level target for compiling python modules
add_custom_target(py_ompl)
endif()
......@@ -38,8 +37,7 @@ endif()
if(PYTHON_FOUND AND Boost_PYTHON_LIBRARY AND PY_PYPLUSPLUS
AND PY_PYGCCXML AND GCCXML)
# make sure targets are defined only once
get_target_property(_target generate_headers EXCLUDE_FROM_ALL)
if(NOT _target)
if(NOT TARGET generate_headers)
# top-level target for updating all-in-one header file for each module
add_custom_target(generate_headers)
# top-level target for regenerating code for all python modules
......@@ -121,16 +119,15 @@ function(create_module_target module dir)
${Boost_PYTHON_LIBRARY}
${PYTHON_LIBRARIES})
add_dependencies(py_ompl py_ompl_${module})
get_target_property(PY${module}_NAME py_ompl_${module} LOCATION)
if(WIN32)
add_custom_command(TARGET py_ompl_${module} POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy "${PY${module}_NAME}"
COMMAND ${CMAKE_COMMAND} -E copy "$<TARGET_FILE:py_ompl_${module}>"
"${_dest_dir}/${module}/_${module}.pyd"
WORKING_DIRECTORY ${LIBRARY_OUTPUT_PATH}
COMMENT "Copying python module ${module} into place")
else(WIN32)
add_custom_command(TARGET py_ompl_${module} POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy "${PY${module}_NAME}"
COMMAND ${CMAKE_COMMAND} -E copy "$<TARGET_FILE:py_ompl_${module}>"
"${_dest_dir}/${module}/_${module}${CMAKE_SHARED_MODULE_SUFFIX}"
WORKING_DIRECTORY ${LIBRARY_OUTPUT_PATH}
COMMENT "Copying python module ${module} into place")
......
#!/bin/sh
if [ ! -d .symlinks ]; then
mkdir -p .symlinks/share
cd .symlinks/share
prefix=`pwd`
mkdir -p .symlinks/@CMAKE_INSTALL_DATAROOTDIR@
cd .symlinks/@CMAKE_INSTALL_DATAROOTDIR@
ln -s ompl@OMPL_INSTALL_SUFFIX@ ompl
cd ../..
mkdir -p .symlinks/lib/pkgconfig
cd .symlinks/lib/pkgconfig
cd ${prefix}
mkdir -p .symlinks/@CMAKE_INSTALL_LIBDIR@/pkgconfig
cd .symlinks/@CMAKE_INSTALL_LIBDIR@/pkgconfig
ln -s ompl@OMPL_INSTALL_SUFFIX@.pc ompl.pc
cd ../../..
mkdir -p .symlinks/include
cd ${prefix}
mkdir -p .symlinks/@CMAKE_INSTALL_INCLUDEDIR@
cd .symlinks/include
ln -s ompl@OMPL_INSTALL_SUFFIX@ ompl
ln -s omplext_odeint@OMPL_INSTALL_SUFFIX@ omplext_odeint
if [[ @Boost_VERSION@ < 105300 ]]; then
ln -s omplext_odeint@OMPL_INSTALL_SUFFIX@ omplext_odeint
fi
if [ $1 ]; then
ln -s omplapp@OMPL_INSTALL_SUFFIX@ omplapp
fi
......
#!/bin/sh
cd @CMAKE_INSTALL_PREFIX@/share
cd @CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_DATAROOTDIR@
rm -f ompl
cd @CMAKE_INSTALL_PREFIX@/lib/pkgconfig
cd @CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@/pkgconfig
rm -f ompl.pc
cd @CMAKE_INSTALL_PREFIX@/include
cd @CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_INCLUDEDIR@
rm -f ompl omplext_odeint omplapp
The Open Motion Planning Library (OMPL) is released as Open Source
under the terms of a 3-clause BSD license.
Copyright (c) 2010, Rice University
Copyright (c) 2010-2014, Rice University
All rights reserved.
Redistribution and use in source and binary forms, with or without
......
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2014, 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: Javier V. Gomez, Mark Moll */
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/ReedsSheppStateSpace.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/cforest/CForest.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/tools/benchmark/Benchmark.h>
#include <boost/program_options.hpp>
namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace ot = ompl::tools;
namespace po = boost::program_options;
ompl::base::OptimizationObjectivePtr getPathLengthObjective(const ompl::base::SpaceInformationPtr& si)
{
return ompl::base::OptimizationObjectivePtr(new ompl::base::PathLengthOptimizationObjective(si));
}
bool isStateValid(double radiusSquared, const ob::State *state)
{
const ob::SE2StateSpace::StateType *s = state->as<ob::SE2StateSpace::StateType>();
double x=s->getX(), y=s->getY();
x = std::abs(x - std::floor(x));
y = std::abs(y - std::floor(y));
x = std::min(x, 1. - x);
y = std::min(y, 1. - y);
return x*x + y*y > radiusSquared;
}
int main(int argc, char **argv)
{
int distance, gridLimit, runCount;
double obstacleRadius, turningRadius, runtimeLimit;
ob::StateSpacePtr space(new ob::SE2StateSpace());
po::options_description desc("Options");
desc.add_options()
("help", "show help message")
("dubins", "use Dubins state space")
("dubinssym", "use symmetrized Dubins state space")
("reedsshepp", "use Reeds-Shepp state space")
("distance", po::value<int>(&distance)->default_value(3), "integer grid distance between start and goal")
("obstacle-radius", po::value<double>(&obstacleRadius)->default_value(.25), "radius of obstacles")
("turning-radius", po::value<double>(&turningRadius)->default_value(.5), "turning radius of robot (ignored for default point robot)")
("grid-limit", po::value<int>(&gridLimit)->default_value(10), "size of the grid")
("runtime-limit", po::value<double>(&runtimeLimit)->default_value(2), "time limit for every test")
("run-count", po::value<int>(&runCount)->default_value(100), "number of times to run each planner")
;
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
po::notify(vm);
if (vm.count("help"))
{
std::cout << desc << "\n";
return 1;
}
if (vm.count("dubins"))
space = ob::StateSpacePtr(new ob::DubinsStateSpace(turningRadius));
if (vm.count("dubinssym"))
space = ob::StateSpacePtr(new ob::DubinsStateSpace(turningRadius, true));
if (vm.count("reedsshepp"))
space = ob::StateSpacePtr(new ob::ReedsSheppStateSpace(turningRadius));
// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(-.5 * gridLimit);
bounds.setHigh(.5 * gridLimit);
space->as<ob::SE2StateSpace>()->setBounds(bounds);
// define a simple setup class
og::SimpleSetup ss(space);
// set state validity checking for this space
ss.setStateValidityChecker(boost::bind(&isStateValid, obstacleRadius*obstacleRadius, _1));
// define start & goal states
ob::ScopedState<ob::SE2StateSpace> start(space), goal(space);
start->setXY(0., 0.5);
start->setYaw(0.);
goal->setXY(0., (double)distance + .5);
goal->setYaw(0);
ss.setStartAndGoalStates(start, goal);
// setting collision checking resolution to 0.05 (absolute)
ss.getSpaceInformation()->setStateValidityCheckingResolution(0.05 / gridLimit);
ss.getProblemDefinition()->setOptimizationObjective(getPathLengthObjective(ss.getSpaceInformation()));
// by default, use the Benchmark class
double memoryLimit = 4096;
ot::Benchmark::Request request(runtimeLimit, memoryLimit, runCount);
ot::Benchmark b(ss, "CircleGrid");
b.addPlanner(ob::PlannerPtr(new og::RRTstar(ss.getSpaceInformation())));
b.addPlanner(ob::PlannerPtr(new og::CForest(ss.getSpaceInformation())));
b.benchmark(request);
b.saveResultsToFile("circleGrid.log");
exit(0);
}
......@@ -17,10 +17,7 @@ if (OMPL_BUILD_DEMOS)
add_ompl_demo(demo_RigidBodyPlanningWithIK RigidBodyPlanningWithIK.cpp)
add_ompl_demo(demo_RigidBodyPlanningWithControls RigidBodyPlanningWithControls.cpp)
add_ompl_demo(demo_RigidBodyPlanningWithIntegrationAndControls RigidBodyPlanningWithIntegrationAndControls.cpp)
if (OMPL_ODESOLVER)
add_ompl_demo(demo_RigidBodyPlanningWithODESolverAndControls RigidBodyPlanningWithODESolverAndControls.cpp)
endif()
add_ompl_demo(demo_RigidBodyPlanningWithODESolverAndControls RigidBodyPlanningWithODESolverAndControls.cpp)
add_ompl_demo(demo_StateSampling StateSampling.cpp)
add_ompl_demo(demo_GeometricCarPlanning GeometricCarPlanning.cpp)
add_ompl_demo(demo_Point2DPlanning Point2DPlanning.cpp)
......@@ -29,9 +26,11 @@ if (OMPL_BUILD_DEMOS)
add_ompl_demo(demo_HypercubeBenchmark HypercubeBenchmark.cpp)
aux_source_directory(Koules Koules_SRC)
add_ompl_demo(demo_Koules ${Koules_SRC})
add_ompl_demo(demo_PlannerData PlannerData.cpp)
add_ompl_demo(demo_OptimalPlanning OptimalPlanning.cpp)
add_ompl_demo(demo_PlannerProgressProperties PlannerProgressProperties.cpp)
add_ompl_demo(demo_CForestCircleGridBenchmark CForestCircleGridBenchmark.cpp)
if (OMPL_EXTENSION_OPENDE)
add_ompl_demo(demo_OpenDERigidBodyPlanning OpenDERigidBodyPlanning.cpp)
......@@ -39,12 +38,9 @@ if (OMPL_BUILD_DEMOS)
if (OMPL_EXTENSION_TRIANGLE)
add_ompl_demo(demo_TriangulationDemo TriangulationDemo.cpp)
add_ompl_demo(demo_LTLWithTriangulation LTLWithTriangulation)
endif()
# Only build the PlannerData demo on Boost >= 1.44
if(NOT "${Boost_VERSION}" LESS 104400)
add_ompl_demo(demo_PlannerData PlannerData.cpp)
endif(NOT "${Boost_VERSION}" LESS 104400)
endif(OMPL_BUILD_DEMOS)
......
......@@ -103,14 +103,14 @@ public:
double distance(const ompl::base::State *state1, const ompl::base::State *state2) const
{
const StateType *cstate1 = static_cast<const StateType*>(state1);
const StateType *cstate2 = static_cast<const StateType*>(state2);
const StateType *cstate1 = state1->as<StateType>();
const StateType *cstate2 = state2->as<StateType>();
double theta1 = 0., theta2 = 0., dx = 0., dy = 0., dist = 0.;
for (unsigned int i = 0; i < getSubspaceCount(); ++i)
{
theta1 += cstate1->components[i]->as<ompl::base::SO2StateSpace::StateType>()->value;
theta2 += cstate2->components[i]->as<ompl::base::SO2StateSpace::StateType>()->value;
theta1 += cstate1->as<ompl::base::SO2StateSpace::StateType>(i)->value;
theta2 += cstate2->as<ompl::base::SO2StateSpace::StateType>(i)->value;
dx += cos(theta1) - cos(theta2);
dy += sin(theta1) - sin(theta2);
dist += sqrt(dx * dx + dy * dy);
......@@ -142,8 +142,8 @@ public:
bool isValid(const ompl::base::State *state) const
{
const KinematicChainSpace* space = static_cast<const KinematicChainSpace*>(si_->getStateSpace().get());
const KinematicChainSpace::StateType *s = static_cast<const KinematicChainSpace::StateType*>(state);
const KinematicChainSpace* space = si_->getStateSpace()->as<KinematicChainSpace>();
const KinematicChainSpace::StateType *s = state->as<KinematicChainSpace::StateType>();
unsigned int n = si_->getStateDimension();
Environment segments;
double linkLength = space->linkLength();
......@@ -152,7 +152,7 @@ public:
segments.reserve(n + 1);
for(unsigned int i = 0; i < n; ++i)
{
theta += static_cast<ompl::base::SO2StateSpace::StateType*>((*s)[i])->value;
theta += s->as<ompl::base::SO2StateSpace::StateType>(i)->value;
xN = x + cos(theta) * linkLength;
yN = y + sin(theta) * linkLength;
segments.push_back(Segment(x, y, xN, yN));
......@@ -207,7 +207,7 @@ protected:
double t_numer = s32_x * s02_y - s32_y * s02_x;
if ((t_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
return false; // No collision
if (((s_numer - denom > -std::numeric_limits<float>::epsilon()) == denomPositive)
if (((s_numer - denom > -std::numeric_limits<float>::epsilon()) == denomPositive)
|| ((t_numer - denom > std::numeric_limits<float>::epsilon()) == denomPositive))
return false; // No collision
return true;
......@@ -304,10 +304,10 @@ int main(int argc, char **argv)
}
// by default, use the Benchmark class
double runtime_limit = 7200, memory_limit = 1024;
double runtime_limit = 60, memory_limit = 1024;
int run_count = 20;
ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
ompl::tools::Benchmark b(ss, "KinematicChain");
ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count, 0.5);
ompl::tools::Benchmark b(ss, boost::str(boost::format("KinematicChain%i") % numLinks));
b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())));
b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::EST(ss.getSpaceInformation())));
......
......@@ -126,7 +126,7 @@ def makeMovie(fname):
interval = 1000. / step, blit = True)
(base,ext) = splitext(basename(fname))
outfname = base + '.mp4'
ani.save(outfname, bitrate = 300, fps = targetFrameRate)
ani.save(outfname, bitrate = 300, fps = targetFrameRate, writer='mencoder')
print('')
if __name__ == '__main__':
......
This diff is collapsed.
......@@ -20,8 +20,9 @@ class MyTriangularDecomposition : public oc::TriangularDecomposition
{
public:
MyTriangularDecomposition(const ob::RealVectorBounds& bounds)
: oc::TriangularDecomposition(2, bounds, createObstacles())
: oc::TriangularDecomposition(bounds, createObstacles())
{
setup();
}
virtual void project(const ob::State* s, std::vector<double>& coord) const
{
......@@ -106,90 +107,6 @@ void propagate(const ob::State *start, const oc::Control *control, const double
rot->value + (*rctrl)[1];
}
void plan(void)
{
// construct the state space we are planning in
ob::StateSpacePtr space(new ob::SE2StateSpace());
// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(-1);
bounds.setHigh(1);