├── .gitignore ├── .travis.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── Makefile ├── README.md ├── include └── or_ompl │ ├── OMPLConversions.h │ ├── OMPLModule.h │ ├── OMPLPlanner.h │ ├── OMPLPlannerParameters.h │ ├── OMPLSimplifer.h │ ├── PlannerRegistry.h │ ├── SemiToroidalStateSpace.h │ ├── StateSpaces.h │ ├── TSR.h │ ├── TSRChain.h │ ├── TSRGoal.h │ ├── TSRRobot.h │ ├── config.h.in │ └── or_conversions.h ├── mainpage.dox ├── package.xml ├── planners.yaml ├── scripts ├── example.py └── wrap_planners.py ├── src ├── OMPLConversions.cpp ├── OMPLMain.cpp ├── OMPLPlanner.cpp ├── OMPLSimplifier.cpp ├── SemiToroidalStateSpace.cpp ├── StateSpaces.cpp ├── TSR.cpp ├── TSRChain.cpp ├── TSRGoal.cpp └── TSRRobot.cpp └── tests └── test_Planner.py /.gitignore: -------------------------------------------------------------------------------- 1 | bin/ 2 | build/ 3 | lib/ 4 | 5 | *~ 6 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: trusty 2 | sudo: required 3 | language: generic 4 | cache: 5 | - apt 6 | env: 7 | global: 8 | - REPOSITORY=or_ompl 9 | - DISTRIBUTION=https://raw.githubusercontent.com/personalrobotics/pr-rosinstalls/feature/rosdistro/repositories.yml 10 | # Install test fixture dependencies. 11 | before_install: 12 | - mkdir -p "${HOME}/workspace/src" 13 | - cd "${HOME}/workspace" 14 | - curl -sSo distribution.yml "${DISTRIBUTION}" 15 | - git clone https://github.com/personalrobotics/pr-cleanroom.git scripts 16 | - ./scripts/internal-setup.sh 17 | - export PACKAGE_NAMES="$(./scripts/internal-get-packages.py distribution.yml ${REPOSITORY})" 18 | install: 19 | - mv "${TRAVIS_BUILD_DIR}" src 20 | - ./scripts/internal-distro.py --workspace=src distribution.yml --repository "${REPOSITORY}" 21 | script: 22 | - ./scripts/internal-build.sh ${PACKAGE_NAMES} 23 | - ./scripts/internal-test.sh ${PACKAGE_NAMES} 24 | after_script: 25 | - ./scripts/view-all-results.sh test_results 26 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package or_ompl 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.0 (2015-10-12) 6 | ------------------ 7 | * Added FMT* and BIT* (`#28 `_) 8 | * Added integration tests (`#23 `_) 9 | * Added a print of which planners were found at build time (`#30 `_) 10 | * Fixed codegen to output planner registery code into a package-specific location to avoid conflicts 11 | * Improved style of the auto-generated code 12 | * Upgraded package.xml to format 2 13 | * Contributors: Chris Dellin, David Butterworth, Michael Koval 14 | 15 | 0.6.0 (2015-05-01) 16 | ------------------ 17 | 18 | 0.5.0 (2015-04-08) 19 | ------------------ 20 | * Added `GetTimes` command to measure timing performance 21 | * Added support for multiple initial/goal configs to using standard OpenRAVE 22 | vinitialconfig/vgoalconfig sequencing 23 | * Return trajectories with "linear" interpolation. 24 | * Adding logic to set one element of the state to NaN and logic to the state 25 | validity checker to test for this. 26 | * Support for goal TSRs. 27 | * Throwing exception if construct is called more than once. 28 | * Removed unused `CD_` macros. 29 | * Call planner callbacks from OMPL_Simplifier. 30 | * Switched from `simplify` to `shortcutPath`. 31 | * Made TSRRobot logging more consistent. 32 | * Removed planner type from `PlannerParameters`. 33 | * Switched to `PlannerFactory` architecture. 34 | * Switched to using a custom robot state space. 35 | * Create a separate OpenRAVE Planner for each OMPL planner (e.g. 36 | `OMPL_RRTConnect` wraps OMPL's RRTConnect planner). 37 | * `OMPLSimplifier` now calls `shortcutPath` instead of `simplify`. 38 | * Renamed the `OMPLSimplifier` planner to `OMPL_Simplifier`. 39 | * Added a `GetParameters` command for querying planner parameters. 40 | * Added the `example.py` example script. 41 | * Contributors: Chris Dellin, Jennifer King, Michael Koval, Pras Velagapudi 42 | 43 | 0.4.0 (2014-12-22) 44 | ------------------ 45 | * Added support for building a standalone OpenRAVE plugin. 46 | * Fixed several CMake issues. 47 | * Added licensing information. 48 | * Contributors: Christopher Dellin, Michael Koval 49 | 50 | 0.3.0 (2014-10-09) 51 | ------------------ 52 | * Fixed a bug that could cause OMPLSimplifier to use an incorrect (and very 53 | coarse) resolution for collision checking. 54 | * Only return if an exact solution is available. 55 | * Only check collisions with active DOFs. 56 | * Switched to using openrave_catkin in `catkin.cmake`. 57 | * Added missing dependencies, including TinyXML 58 | * Use headers, instead of version number, to query planner support. 59 | * Contributors: Michael Koval, Pras Velagapudi 60 | 61 | 0.2.0 (2014-06-22 13:12) 62 | ------------------------ 63 | * Wrapped OMPL's PathSimplifier in an OpenRAVE 64 | * Contributors: Michael Koval, Jen King 65 | 66 | 0.1.0 (2014-06-22 02:32) 67 | ------------------------ 68 | * Added missing Python dependencies. 69 | * Forward OMPL log messages to OpenRAVE. 70 | * Generate planner wrappers from a YAML config file. 71 | * Check for non-unit DOF weights. 72 | * Set the collision checking resolution. 73 | * Contributors: Christopher Dellin, Matt Klingensmith, Michael Koval, Michael 74 | Vandeweghe, Pras Velagapudi, Jen King, 75 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(or_ompl) 3 | 4 | include(CheckCXXSourceCompiles) 5 | 6 | find_package(catkin REQUIRED cmake_modules openrave_catkin) 7 | find_package(Boost REQUIRED COMPONENTS chrono system) 8 | find_package(OMPL REQUIRED) 9 | find_package(OpenRAVE REQUIRED) 10 | find_package(TinyXML REQUIRED) 11 | find_package(Eigen REQUIRED) 12 | 13 | # Setup compilation options for the check_cxx_source_compiles tests below. 14 | set(CMAKE_REQUIRED_INCLUDES ${OpenRAVE_INCLUDE_DIRS}) 15 | set(CMAKE_REQUIRED_LIBRARIES 16 | ${Boost_LIBRARIES} 17 | ${OpenRAVE_CORE_LIBRARIES} 18 | ${OpenRAVE_LIBRARIES}) 19 | set(CMAKE_REQUIRED_DEFINITIONS 20 | ${OpenRAVE_CXX_FLAGS} 21 | ${OpenRAVE_LINK_FLAGS}) 22 | 23 | set(CMAKE_REQUIRED_FLAGS) 24 | foreach(dir ${OpenRAVE_LIBRARY_DIRS}) 25 | list(APPEND CMAKE_REQUIRED_FLAGS "-L${dir}") 26 | endforeach() 27 | 28 | # 2013-01-05: int options arg added to PlannerParameters::serialize 29 | check_cxx_source_compiles( 30 | "#include 31 | class P: OpenRAVE::PlannerBase::PlannerParameters 32 | {void f(){bool (P::*x)(std::ostream&,int) const = &P::serialize;}}; 33 | int main(){}" 34 | OR_OMPL_HAS_PPSEROPTS) 35 | 36 | # 1.2.0 ompl (ROS kinetic) switched from boost to std smart pointers 37 | set(CMAKE_REQUIRED_INCLUDES ${OMPL_INCLUDE_DIRS}) 38 | set(CMAKE_REQUIRED_LIBRARIES) 39 | set(CMAKE_REQUIRED_DEFINITIONS) 40 | set(CMAKE_REQUIRED_FLAGS) 41 | 42 | check_cxx_source_compiles( 43 | "#include 44 | int main(){ ompl::base::StateSpacePtr s = boost::shared_ptr(); }" 45 | OR_OMPL_HAS_BOOSTSMARTPTRS) 46 | 47 | configure_file( 48 | "include/${PROJECT_NAME}/config.h.in" 49 | "${CATKIN_DEVEL_PREFIX}/include/${PROJECT_NAME}/config.h" 50 | ) 51 | 52 | catkin_package( 53 | INCLUDE_DIRS include ${CATKIN_DEVEL_PREFIX}/include 54 | LIBRARIES ${PROJECT_NAME} 55 | DEPENDS Boost Eigen OMPL OpenRAVE 56 | ) 57 | 58 | if(NOT OR_OMPL_HAS_BOOSTSMARTPTRS) 59 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 60 | endif(NOT OR_OMPL_HAS_BOOSTSMARTPTRS) 61 | 62 | include_directories( 63 | include 64 | ${CATKIN_DEVEL_PREFIX}/include 65 | ${Boost_INCLUDE_DIRS} 66 | ${Eigen_INCLUDE_DIRS} 67 | ${OMPL_INCLUDE_DIRS} 68 | ${OpenRAVE_INCLUDE_DIRS} 69 | ${TinyXML_INCLUDE_DIRS} 70 | ${catkin_INCLUDE_DIRS} 71 | ) 72 | link_directories( 73 | ${OMPL_LIBRARY_DIRS} 74 | ${OpenRAVE_LIBRARY_DIRS} 75 | ${catkin_LIBRARY_DIRS} 76 | ) 77 | add_definitions( 78 | ${Eigen_DEFINITIONS} 79 | ) 80 | 81 | # Generate the OMPL planner wrappers. 82 | file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/src") 83 | 84 | add_custom_command(OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/src/PlannerRegistry.cpp" 85 | MAIN_DEPENDENCY "${PROJECT_SOURCE_DIR}/planners.yaml" 86 | DEPENDS "${PROJECT_SOURCE_DIR}/scripts/wrap_planners.py" 87 | COMMAND "${PROJECT_SOURCE_DIR}/scripts/wrap_planners.py" 88 | --include-dirs="${OMPL_INCLUDE_DIRS}" 89 | --planners-yaml="${PROJECT_SOURCE_DIR}/planners.yaml" 90 | --generated-cpp="${CMAKE_CURRENT_BINARY_DIR}/src/PlannerRegistry.cpp" 91 | ) 92 | 93 | # Helper library. 94 | add_library(${PROJECT_NAME} 95 | src/OMPLConversions.cpp 96 | src/OMPLPlanner.cpp 97 | src/OMPLSimplifier.cpp 98 | src/SemiToroidalStateSpace.cpp 99 | src/StateSpaces.cpp 100 | src/TSR.cpp 101 | src/TSRChain.cpp 102 | src/TSRGoal.cpp 103 | src/TSRRobot.cpp 104 | "${CMAKE_CURRENT_BINARY_DIR}/src/PlannerRegistry.cpp" 105 | ) 106 | target_link_libraries(${PROJECT_NAME} 107 | ${Boost_LIBRARIES} 108 | ${Eigen_LIBRARIES} 109 | ${OMPL_LIBRARIES} 110 | ${OpenRAVE_LIBRARIES} 111 | ${TinyXML_LIBRARIES} 112 | ) 113 | 114 | # OpenRAVE plugin. 115 | openrave_plugin("${PROJECT_NAME}_plugin" 116 | src/OMPLMain.cpp 117 | ) 118 | target_link_libraries("${PROJECT_NAME}_plugin" 119 | ${PROJECT_NAME} 120 | ${Boost_LIBRARIES} 121 | ${catkin_LIBRARIES} 122 | ) 123 | 124 | install(TARGETS or_ompl 125 | LIBRARY DESTINATION "${CATKIN_PACKAGE_LIB_DESTINATION}" 126 | ) 127 | install(DIRECTORY "include/${PROJECT_NAME}/" 128 | DESTINATION "${CATKIN_PACKAGE_INCLUDE_DESTINATION}" 129 | PATTERN "*.in" EXCLUDE 130 | PATTERN ".svn" EXCLUDE 131 | ) 132 | install(DIRECTORY 133 | "${CATKIN_DEVEL_PREFIX}/include/${PROJECT_NAME}/" 134 | DESTINATION "${CATKIN_PACKAGE_INCLUDE_DESTINATION}" 135 | ) 136 | 137 | # Tests 138 | if(CATKIN_ENABLE_TESTING) 139 | catkin_add_nosetests(tests/test_Planner.py) 140 | endif(CATKIN_ENABLE_TESTING) 141 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, Carnegie Mellon University 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | 11 | Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 16 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 17 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 18 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 21 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 23 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # or_ompl 2 | 3 | [![Build Status](https://travis-ci.org/personalrobotics/or_ompl.svg?branch=master)](https://travis-ci.org/personalrobotics/or_ompl) 4 | 5 | [OpenRAVE](http://openrave.org/) bindings for the 6 | [OMPL](http://ompl.kavrakilab.org/) suite of motion planning algorithms. This 7 | package provides an OMPL plugin that implements the `OpenRAVE::PlannerBase` 8 | interface and delegates planning requests to an OMPL planner. It also includes 9 | the "OMPLSimplifier" plugin that exposes OMPL's `PathSimplifier` to OpenRAVE 10 | through the same interface. See [this demo video](http://youtu.be/6qRRbvNzHG8) 11 | for a brief overview of or_ompl's features. 12 | 13 | This is implemented by initializing OMPL with a state space that matches the 14 | joint limits and resolutions of the robot's active DOFs. Collision checking 15 | is implemented by providing OMPL with a custom "state validity checker" 16 | that uses OpenRAVE's `CheckCollision` and `CheckSelfCollision` calls to check 17 | for collision with the environment. 18 | 19 | or_ompl wraps each OMPL geometric planner as an OpenRAVE planner that 20 | implements the `PlannerBase` interface. E.g. OMPL's RRTConnect algorithm is 21 | exposed through a `OMPL_RRTConnect` planner in OpenRAVE. The planning time 22 | limit and any planner parameters that are exposed through OMPL's `ParamSet` 23 | class can be set by name in the `PlannerParameters` struct in OpenRAVE. 24 | 25 | The wrapper classes necessary to call a planner are automatically generated 26 | from the `planners.yaml` configuration file by the Python script 27 | `scripts/wrap_planners.py`. If you find that a planner is missing, please [open 28 | an issue](https://github.com/personalrobotics/or_ompl/issues/new) or send a 29 | [send us a pull request](https://github.com/personalrobotics/or_ompl/compare/) 30 | with an updated 'planners.yaml' file. The presence or absence of each planner 31 | is determined by testing whether the corresponding header file exists in the 32 | OMPL include directory. 33 | 34 | ## Dependencies 35 | 36 | See the `package.xml` file for a full list of dependencies. These are the major 37 | dependencies: 38 | 39 | - [OpenRAVE](http://openrave.org/) 0.8 or above (primarily developed in 0.9) 40 | - [OMPL](http://ompl.kavrakilab.org/) 0.10 or above (primarily developed in 0.10 and 0.13) 41 | - [ROS](http://ros.org/) optional, see below 42 | 43 | ## Installation Instructions 44 | 45 | The `CMakeLists.txt` file in the root of this repository supports Catkin and 46 | standalone CMake builds. See the appropriate section below for installation 47 | instructions specific to your environment. 48 | 49 | ### Catkin Instructions 50 | 51 | This preferred way of building or_ompl. In this case, you should have OpenRAVE 52 | and OMPL installed as system dependencies. We use a helper package called 53 | [openrave_catkin](https://github.com/personalrobotics/openrave_catkin) to 54 | manage the build process. 55 | 56 | Once the dependencies are satisfied, you can simply clone this repository into 57 | your Catkin workspace and run `catkin_make`: 58 | 59 | ```shell 60 | $ . /my/workspace/devel/setup.bash 61 | $ cd /my/workspace/src 62 | $ git clone https://github.com/personalrobotics/openrave_catkin.git 63 | $ git clone https://github.com/personalrobotics/or_ompl.git 64 | $ cd .. 65 | $ catkin_make 66 | ``` 67 | 68 | This will build the OpenRAVE plugins into the `share/openrave-0.9/plugins` 69 | directory in your devel space. If you run `catkin_make install` the plugin will 70 | be installed to the same directory in your install space. In both cases, the 71 | corresponding directory will be automatically added to your `OPENRAVE_PLUGINS` 72 | path using a [Catkin environment 73 | hook](http://docs.ros.org/fuerte/api/catkin/html/macros.html#catkin_add_env_hooks). 74 | See the [documentation for 75 | openrave_catkin](https://github.com/personalrobotics/openrave_catkin/blob/master/README.md) 76 | for more information. 77 | 78 | ### Standalone CMake Build Instructions 79 | 80 | You can build or_ompl entirely ROS-agnostic by setting the `NO_ROS` variable: 81 | 82 | ```shell 83 | $ git clone https://github.com/personalrobotics/or_ompl.git 84 | $ mkdir build 85 | $ cd build 86 | $ cmake -DNO_ROS:bool=1 .. 87 | $ make 88 | ``` 89 | 90 | Just as in the rosbuild case, this will build the plugin in the `lib/` 91 | directory. You will need to add this directory to your `OPENRAVE_PLUGINS` path 92 | so that OpenRAVE can find it. 93 | 94 | ## Limitations 95 | 96 | Wherever possible we aim to fully support the full breadth of features in both 97 | OMPL and OpenRAVE. However, you should be aware of a few limitations: 98 | 99 | - per-joint weights are not supported (unit weights are assumed) 100 | - per-joint resolutions are not supported (a conservative resolution is selected) 101 | - kineodynamic planning is not supported 102 | - planning with affine DOFs is not implemented 103 | - simplifier does not work with the `SmoothTrajectory` helper function 104 | - no support for multi-query planning (e.g. PRM) 105 | 106 | None of these are fundamental issues and we hope, perhaps with your help, to 107 | shrink this list over time. We would welcome [pull 108 | requests](https://github.com/personalrobotics/or_ompl/compare/) for any of 109 | these features. 110 | 111 | ## Usage 112 | 113 | You can find planners provided by this plugin by looking for planners that 114 | start with the `OMPL_` prefix in the output of `openrave --listplugins`. 115 | 116 | The following Python code will plan using OMPL's implementation of RRT-Connect, 117 | then shortcut the trajectory using OMPL's path simplifier. We assume that the 118 | variable `robot` is an OpenRAVE robot that is configured with an appropriate 119 | set of active DOFs: 120 | 121 | ```python 122 | from openravepy import * 123 | 124 | env = ... # your environment 125 | robot = ... # your robot 126 | planner = RaveCreatePlanner(env, 'OMPL_RRTConnect') 127 | simplifier = RaveCreatePlanner(env, 'OMPL_Simplifier') 128 | 129 | # Setup the planning instance. 130 | params = Planner.PlannerParameters() 131 | params.SetRobotActiveJoints(robot) 132 | params.SetGoalConfig(goal) 133 | 134 | # Set the timeout and planner-specific parameters. You can view a list of 135 | # supported parameters by calling: planner.SendCommand('GetParameters') 136 | params.SetExtraParameters('0.02') 137 | 138 | planner.InitPlan(robot, params) 139 | 140 | # Invoke the planner. 141 | traj = RaveCreateTrajectory(env, '') 142 | result = planner.PlanPath(traj) 143 | assert result == PlannerStatus.HasSolution 144 | 145 | # Shortcut the path. 146 | simplifier.InitPlan(robot, Planner.PlannerParameters()) 147 | result = simplifier.PlanPath(traj) 148 | assert result == PlannerStatus.HasSolution 149 | 150 | # Time the trajectory. 151 | result = planningutils.RetimeTrajectory(traj) 152 | assert result == PlannerStatus.HasSolution 153 | 154 | # Execute the trajectory. 155 | robot.GetController().SetPath(traj) 156 | ``` 157 | 158 | A working version of this script is included in `scripts/example.py`. See the 159 | [documentation on the OpenRAVE website](http://openrave.org/docs/latest_stable/tutorials/openravepy_examples/#directly-launching-planners) 160 | for more information about how to invoke an OpenRAVE planner. 161 | 162 | ## Tuning Planner Performance 163 | 164 | Collision checking is often the bottleneck for motion planning for manipulators 165 | with the sample-based motion planners included in OMPL. You should consider 166 | using a fast collision checker plugin, like 167 | [or_fcl](https://github.com/personalrobotics/or_fcl), to achieve best 168 | performance with `or_ompl`. 169 | 170 | Additionally, you should consider setting the `ActiveDOFs` option during 171 | planning: 172 | 173 | > Allows planners to greatly reduce redundant collision checks. If set and the 174 | > target object is a robot, then only the links controlled by the currently 175 | > set active DOFs and their attached bodies will be checked for collisions. 176 | > 177 | > The things that **will not be** checked for collision are: links that do not 178 | > move with respect to each other as a result of moving the active dofs. 179 | 180 | You can use a `CollisionOptionsStateSaver` to set the flag and automatically 181 | restore the collision detector to its original state after planning is done: 182 | 183 | ```python 184 | with CollisionOptionsStateSaver(env.GetCollisionChecker(), CollisionOptions.ActiveDOFs): 185 | result = planner.PlanPath(traj) 186 | ``` 187 | 188 | ## Available Planners 189 | 190 | The following table shows which OMPL planners are available via `or_ompl`. 191 | 192 | (1 Oct 2015) Note that if you are using the ROS package of OMPL `ros-indigo-ompl` then BIT* and FMT* will not be available. To use those planners♣ you will need to install the latest OMPL from source. There is a catkinized package here [OMPL_catkin_pkg](https://github.com/DavidB-CMU/OMPL_catkin_pkg) 193 | 194 | | Planner Name | OMPL Library | OpenRAVE
Plugin Name | `or_ompl`
Unit Test | 195 | |--------------|-------------------------|------------------------------------|-----------------------------------| 196 | | BIT* (Batch Informed Trees) ♣ | BITstar | OMPL_BITstar | ✗ | 197 | | BKPIECE1 (Bi-directional KPIECE) | BKPIECE1 | OMPL_BKPIECE1 | ✔ | 198 | | EST (Expansive Space Trees) | EST | OMPL_EST | ✔ | 199 | | FMT* (Fast Marching Tree) ♣ | FMT | OMPL_FMT |✗| 200 | | KPIECE1 (Kinematic Planning by Interior-Exterior Cell Exploration) | KPIECE1 | OMPL_KPIECE1 | ✔ | 201 | | LBKPIECE1 (Lazy Bi-directional KPIECE) | LBKPIECE1 | OMPL_LBKPIECE1 | ✔ | 202 | | LazyPRM | LazyPRM | OMPL_LazyPRM | ✔ | 203 | | LazyRRT | LazyRRT | OMPL_LazyRRT | ✔ | 204 | | PDST (Path-Directed Subdivision Trees) | PDST | OMPL_PDST |✗| 205 | | PRM (Probabilistic Road Map) | PRM | OMPL_PRM | ✔ | 206 | | PRM* | PRMstar | OMPL_PRMstar | ✔ | 207 | | RRT (Rapidly Exploring Random Trees) | RRT | OMPL_RRT | ✔ | 208 | | RRTConnect (Bi-directional RRT) | RRTConnect | OMPL_RRTConnect | ✔ | 209 | | RRT* | RRTstar | OMPL_RRTstar |✗| 210 | | SBL (Single-query Bi-directional Lazy collision checking planner) | SBL | OMPL_SBL | ✔ | 211 | | SPARS (SPArse Roadmap Spanner) | SPARS | OMPL_SPARS |✗| 212 | | SPARS2 | SPARStwo | OMPL_SPARStwo |✗| 213 | | T-RRT (Transition-based RRT) | TRRT | OMPL_TRRT |✗| 214 | | pRRT (Parallel RRT) | pRRT | OMPL_pRRT |✗| 215 | | pSBL (Parallel SBL) | pSBL | OMPL_pSBL |✗| 216 | | Cforest (Coupled Forest of Random Engrafting Search Trees - parallelization framework) | CForest | N/A | N/A | 217 | | Thunder | Thunder | N/A | N/A | 218 | | Lightning | Lightning | N/A | N/A | 219 | | LazyPRM* | LazyPRMstar | N/A | N/A | 220 | | BiTRRT (Bidirectional T-RRT) | BiTRRT | N/A | N/A | 221 | | LazyLBTRRT | LazyLBTRRT | N/A | N/A | 222 | | LBTRRT (Lower Bound Tree RRT) | LBTRRT | N/A | N/A | 223 | | STRIDE (Search Tree with Resolution Independent Density Estimation) | STRIDE | N/A | N/A | 224 | 225 | ## License 226 | or_ompl is licensed under a BSD license. See `LICENSE` for more information. 227 | 228 | ## Contributors 229 | or_ompl was developed by the [Personal Robotics Lab](https://personalrobotics.ri.cmu.edu) in the [Robotics 230 | Institute](http://ri.cmu.edu) at [Carnegie Mellon University](http://www.cmu.edu). This plugin was written by [Michael 231 | Koval](http://mkoval.org) and grew out of earlier plugin written by [Christopher Dellin](http://www.ri.cmu.edu/person.html?person_id=2267) and [Matthew Klingensmith](http://www.ri.cmu.edu/person.html?person_id=2744). 232 | -------------------------------------------------------------------------------- /include/or_ompl/OMPLConversions.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Michael Koval 7 | Matthew Klingensmith 8 | Christopher Dellin 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are 12 | met: 13 | 14 | Redistributions of source code must retain the above copyright 15 | notice, this list of conditions and the following disclaimer. 16 | 17 | Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 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 FOR 24 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | 33 | *************************************************************************/ 34 | 35 | #ifndef OR_OMPL_OMPLCONVERSIONS_H_ 36 | #define OR_OMPL_OMPLCONVERSIONS_H_ 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | namespace or_ompl { 46 | 47 | struct OpenRAVEHandler : public ompl::msg::OutputHandler { 48 | public: 49 | virtual void log(std::string const &text, ompl::msg::LogLevel level, 50 | char const *filename, int line); 51 | }; 52 | 53 | std::vector GetContinuousJoints(const OpenRAVE::RobotBasePtr robot, const std::vector idx); 54 | 55 | ompl::base::StateSpacePtr CreateStateSpace(OpenRAVE::RobotBasePtr const robot, 56 | OMPLPlannerParameters const ¶ms); 57 | 58 | OpenRAVE::PlannerStatus ToORTrajectory(OpenRAVE::RobotBasePtr const &robot, 59 | ompl::geometric::PathGeometric &ompl_traj, 60 | OpenRAVE::TrajectoryBasePtr or_traj); 61 | 62 | } // namespace or_ompl 63 | 64 | #endif // OR_OMPL_OMPLCONVERSIONS_H_ 65 | -------------------------------------------------------------------------------- /include/or_ompl/OMPLModule.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Michael Koval 7 | Matthew Klingensmith 8 | Christopher Dellin 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are 12 | met: 13 | 14 | Redistributions of source code must retain the above copyright 15 | notice, this list of conditions and the following disclaimer. 16 | 17 | Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 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 FOR 24 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | 33 | *************************************************************************/ 34 | 35 | /* 36 | * OMPLModule.h 37 | * 38 | * Created on: Jun 2, 2012 39 | * Author: mklingen 40 | */ 41 | 42 | #ifndef OR_OMPL_OMPLMODULE_H_ 43 | #define OR_OMPL_OMPLMODULE_H_ 44 | 45 | #include 46 | 47 | #include 48 | 49 | namespace or_ompl { 50 | 51 | class OMPLModule: public OpenRAVE::ModuleBase { 52 | public: 53 | OMPLModule(OpenRAVE::EnvironmentBasePtr penv); 54 | virtual ~OMPLModule(); 55 | 56 | bool SetRobot(std::ostream& output, std::istream& input); 57 | bool Plan(std::ostream& output, std::istream& input); 58 | 59 | private: 60 | OMPLPlannerPtr m_planner; 61 | OpenRAVE::RobotBasePtr m_robot; 62 | OMPLPlannerParametersPtr m_params; 63 | }; 64 | 65 | } // namespace or_ompl 66 | 67 | #endif // OR_OMPL_OMPLMODULE_H_ 68 | -------------------------------------------------------------------------------- /include/or_ompl/OMPLPlanner.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Michael Koval 7 | Matthew Klingensmith 8 | Christopher Dellin 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are 12 | met: 13 | 14 | Redistributions of source code must retain the above copyright 15 | notice, this list of conditions and the following disclaimer. 16 | 17 | Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 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 FOR 24 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | 33 | *************************************************************************/ 34 | 35 | #ifndef OR_OMPL_OMPLPLANNER_H_ 36 | #define OR_OMPL_OMPLPLANNER_H_ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | namespace or_ompl { 47 | 48 | typedef boost::function PlannerFactory; 49 | 50 | class OMPLPlanner: public OpenRAVE::PlannerBase { 51 | public: 52 | OMPLPlanner(OpenRAVE::EnvironmentBasePtr penv, 53 | PlannerFactory const &planner_factory); 54 | virtual ~OMPLPlanner(); 55 | 56 | virtual bool InitPlan(OpenRAVE::RobotBasePtr robot, 57 | PlannerParametersConstPtr params); 58 | virtual bool InitPlan(OpenRAVE::RobotBasePtr robot, std::istream& input); 59 | 60 | virtual OpenRAVE::PlannerStatus PlanPath (OpenRAVE::TrajectoryBasePtr ptraj); 61 | 62 | virtual PlannerParametersConstPtr GetParameters () const { 63 | return m_parameters; 64 | } 65 | 66 | bool GetTimes(std::ostream & sout, std::istream & sin) const; 67 | bool GetParameterValCommand(std::ostream &sout, std::istream &sin) const; 68 | 69 | protected: 70 | const ompl::base::PlannerPtr & get_planner() { 71 | return m_planner; 72 | } 73 | 74 | private: 75 | bool m_initialized; 76 | PlannerFactory m_planner_factory; 77 | OMPLPlannerParametersPtr m_parameters; 78 | ompl::geometric::SimpleSetupPtr m_simple_setup; 79 | ompl::base::StateSpacePtr m_state_space; 80 | OrStateValidityCheckerPtr m_or_validity_checker; 81 | ompl::base::PlannerPtr m_planner; 82 | OpenRAVE::RobotBasePtr m_robot; 83 | OpenRAVE::CollisionReportPtr m_collisionReport; 84 | double m_totalPlanningTime; 85 | 86 | ompl::base::PlannerPtr CreatePlanner(OMPLPlannerParameters const ¶ms); 87 | 88 | bool GetParametersCommand(std::ostream &sout, std::istream &sin) const; 89 | }; 90 | 91 | typedef boost::shared_ptr OMPLPlannerPtr; 92 | 93 | } // namespace or_ompl 94 | 95 | #endif // OR_OMPL_OMPLPLANNER_H_ 96 | -------------------------------------------------------------------------------- /include/or_ompl/OMPLPlannerParameters.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Michael Koval 7 | Matthew Klingensmith 8 | Christopher Dellin 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are 12 | met: 13 | 14 | Redistributions of source code must retain the above copyright 15 | notice, this list of conditions and the following disclaimer. 16 | 17 | Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 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 FOR 24 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | 33 | *************************************************************************/ 34 | 35 | #ifndef OR_OMPL_OMPLPLANNERPARAMETERS_H_ 36 | #define OR_OMPL_OMPLPLANNERPARAMETERS_H_ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | namespace or_ompl { 47 | 48 | class OMPLPlannerParameters : public OpenRAVE::PlannerBase::PlannerParameters 49 | { 50 | public: 51 | OMPLPlannerParameters() 52 | : m_seed(0) 53 | , m_timeLimit(10) 54 | , m_isProcessingOMPL(false) 55 | , m_dat_filename("") 56 | , m_trajs_fileformat("") 57 | , m_doBaked(false) 58 | { 59 | _vXMLParameters.push_back("seed"); 60 | _vXMLParameters.push_back("time_limit"); 61 | _vXMLParameters.push_back("dat_filename"); 62 | _vXMLParameters.push_back("trajs_fileformat"); 63 | _vXMLParameters.push_back("tsr_chain"); 64 | _vXMLParameters.push_back("do_baked"); 65 | } 66 | 67 | unsigned int m_seed; 68 | double m_timeLimit; 69 | bool m_isProcessingOMPL; 70 | std::string m_dat_filename; 71 | std::string m_trajs_fileformat; 72 | std::vector m_tsrchains; 73 | bool m_doBaked; 74 | 75 | protected: 76 | 77 | #ifdef OR_OMPL_HAS_PPSEROPTS 78 | virtual bool serialize(std::ostream& O, int options=0) const { 79 | if (!PlannerParameters::serialize(O, options)) { 80 | return false; 81 | } 82 | #else 83 | virtual bool serialize(std::ostream& O) const { 84 | if (!PlannerParameters::serialize(O)) { 85 | return false; 86 | } 87 | #endif 88 | 89 | O << "" << m_seed << "\n" 90 | << "" << m_timeLimit << "\n" 91 | << "" << m_dat_filename << "\n" 92 | << "" << m_trajs_fileformat << "\n" 93 | << "" << m_doBaked << "\n"; 94 | BOOST_FOREACH(TSRChain::Ptr chain, m_tsrchains) { 95 | O << ""; 96 | chain->serialize(O); 97 | O << "\n"; 98 | } 99 | 100 | return !!O; 101 | } 102 | 103 | ProcessElement startElement(std::string const &name, 104 | std::list > const &atts) { 105 | if (m_isProcessingOMPL) { 106 | return PE_Ignore; 107 | } 108 | 109 | switch (OpenRAVE::PlannerBase::PlannerParameters::startElement(name, atts)) { 110 | case PE_Pass: 111 | break; 112 | case PE_Support: 113 | return PE_Support; 114 | case PE_Ignore: 115 | return PE_Ignore; 116 | } 117 | 118 | m_isProcessingOMPL = 119 | name == "seed" 120 | || name == "time_limit" 121 | || name == "dat_filename" 122 | || name == "trajs_fileformat" 123 | || name == "tsr_chain" 124 | || name == "do_baked"; 125 | 126 | return m_isProcessingOMPL ? PE_Support : PE_Pass; 127 | } 128 | 129 | virtual bool endElement(std::string const &name) { 130 | if (m_isProcessingOMPL) { 131 | if (name == "seed") { 132 | _ss >> m_seed; 133 | } else if (name == "time_limit") { 134 | _ss >> m_timeLimit; 135 | } else if (name == "dat_filename") { 136 | _ss >> m_dat_filename; 137 | } else if (name == "trajs_fileformat") { 138 | _ss >> m_trajs_fileformat; 139 | } else if (name == "tsr_chain") { 140 | TSRChain::Ptr chain = boost::make_shared(); 141 | bool success = chain->deserialize(_ss); 142 | if(!success){ 143 | RAVELOG_ERROR("failed to deserialize TSRChain\n"); 144 | }else{ 145 | m_tsrchains.push_back(chain); 146 | } 147 | } else if (name == "do_baked") { 148 | std::string strbool; 149 | _ss >> strbool; 150 | if (strbool=="on" || strbool=="yes" || strbool=="1" || strbool=="true") { 151 | m_doBaked = true; 152 | } else if (strbool=="off" || strbool=="no" || strbool=="0" || strbool=="false") { 153 | m_doBaked = false; 154 | } else { 155 | RAVELOG_WARN(str(boost::format("unknown boolean %s, ignoring\n") % strbool)); 156 | } 157 | } else { 158 | RAVELOG_WARN(str(boost::format("unknown tag %s\n") % name)); 159 | } 160 | m_isProcessingOMPL = false; 161 | return false; 162 | } 163 | 164 | return PlannerParameters::endElement(name); 165 | } 166 | }; 167 | 168 | typedef boost::shared_ptr OMPLPlannerParametersPtr; 169 | 170 | } // namespace or_ompl 171 | 172 | #endif // OR_OMPL_OMPLPLANNERPARAMETERS_H_ 173 | -------------------------------------------------------------------------------- /include/or_ompl/OMPLSimplifer.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Michael Koval 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | #ifndef OR_OMPL_OMPLSIMPLIFIER_H_ 34 | #define OR_OMPL_OMPLSIMPLIFIER_H_ 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | namespace or_ompl { 43 | 44 | class OMPLSimplifier : public OpenRAVE::PlannerBase { 45 | public: 46 | OMPLSimplifier(OpenRAVE::EnvironmentBasePtr penv); 47 | virtual ~OMPLSimplifier(); 48 | virtual bool InitPlan(OpenRAVE::RobotBasePtr robot, 49 | PlannerParametersConstPtr params); 50 | virtual bool InitPlan(OpenRAVE::RobotBasePtr robot, std::istream& input); 51 | 52 | virtual OpenRAVE::PlannerStatus PlanPath(OpenRAVE::TrajectoryBasePtr ptraj); 53 | 54 | virtual PlannerParametersConstPtr GetParameters() const { return m_parameters; } 55 | 56 | private: 57 | OpenRAVE::RobotBasePtr m_robot; 58 | OMPLPlannerParametersPtr m_parameters; 59 | ompl::base::StateSpacePtr m_state_space; 60 | OrStateValidityCheckerPtr m_or_validity_checker; 61 | ompl::base::SpaceInformationPtr m_space_info; 62 | ompl::geometric::PathSimplifierPtr m_simplifier; 63 | OpenRAVE::ConfigurationSpecification m_cspec; 64 | }; 65 | 66 | typedef boost::shared_ptr OMPLSimplifierPtr; 67 | 68 | } // namespace or_ompl 69 | 70 | #endif // OR_OMPL_OMPLSIMPLIFIER_H_ 71 | -------------------------------------------------------------------------------- /include/or_ompl/PlannerRegistry.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Michael Koval 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | #ifndef OR_OMPL_PLANNERREGISTRY_H_ 34 | #define OR_OMPL_PLANNERREGISTRY_H_ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace or_ompl { 42 | namespace registry { 43 | 44 | std::vector get_planner_names(); 45 | 46 | ompl::base::Planner *create(std::string const &name, 47 | ompl::base::SpaceInformationPtr space); 48 | 49 | } // namespace registry 50 | } // namespace or_ompl 51 | 52 | #endif // OR_OMPL_PLANNERREGISTRY_H_ 53 | -------------------------------------------------------------------------------- /include/or_ompl/SemiToroidalStateSpace.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2015, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Chris Dellin 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | namespace or_ompl { 34 | 35 | /*! \brief Semi-torus OMPL state space for wrapping dimensions. 36 | * 37 | * This is a generalization of ompl::base::RealVectorStateSpace 38 | * which supports wrapping on each dimension individually 39 | * (that is, the dimension's lower bound and upper bound correspond). 40 | * This is especially useful for handling robots with circular joints. 41 | * It should be funcionally equivalent to building a coupound state 42 | * space with many SO(2) components, except it should be faster because 43 | * all states are stored contiguously. 44 | */ 45 | class SemiToroidalStateSpace: public ompl::base::RealVectorStateSpace 46 | { 47 | public: 48 | SemiToroidalStateSpace(unsigned int dim=0); 49 | 50 | virtual void setIsWrapping(const std::vector &isWrapping); 51 | virtual const std::vector & getIsWrapping() const { return isWrapping_; } 52 | 53 | virtual void addDimension(double minBound=0.0, double maxBound=0.0, bool isWrapping=false); 54 | virtual void addDimension(const std::string &name, double minBound=0.0, double maxBound=0.0, bool isWrapping=false); 55 | 56 | virtual double getMaximumExtent() const; 57 | virtual double distance(const ompl::base::State *state1, const ompl::base::State *state2) const; 58 | virtual bool equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const; 59 | virtual void interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state) const; 60 | 61 | private: 62 | std::vector isWrapping_; 63 | }; 64 | 65 | } // namespace or_ompl 66 | -------------------------------------------------------------------------------- /include/or_ompl/StateSpaces.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Michael Koval 7 | Matthew Klingensmith 8 | Christopher Dellin 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are 12 | met: 13 | 14 | Redistributions of source code must retain the above copyright 15 | notice, this list of conditions and the following disclaimer. 16 | 17 | Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 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 FOR 24 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | 33 | *************************************************************************/ 34 | 35 | #ifndef OR_OMPL_STATESPACES_H_ 36 | #define OR_OMPL_STATESPACES_H_ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | namespace or_ompl { 48 | 49 | /** 50 | * This is like ompl::base::StateValidityChecker, 51 | * except it also knows how to compute forward kinematics 52 | * to match the state 53 | * 54 | * this should work for general state spaces (e.g. CompoundStateSpaces) 55 | */ 56 | class OrStateValidityChecker: public ompl::base::StateValidityChecker 57 | { 58 | public: 59 | OrStateValidityChecker(const ompl::base::SpaceInformationPtr &si, 60 | OpenRAVE::RobotBasePtr robot, std::vector const &indices, 61 | bool do_baked); 62 | void start(); 63 | void stop(); 64 | virtual bool computeFk(const ompl::base::State *state, uint32_t checklimits) const; 65 | virtual bool isValid(const ompl::base::State *state) const; 66 | void resetStatistics() { m_numCollisionChecks = 0; m_totalCollisionTime = 0.0; } 67 | int getNumCollisionChecks() { return m_numCollisionChecks; } 68 | double getTotalCollisionTime() { return m_totalCollisionTime; } 69 | const std::vector & getIndices() { return m_indices; } 70 | protected: 71 | ompl::base::StateSpace * m_stateSpace; 72 | OpenRAVE::EnvironmentBasePtr m_env; 73 | OpenRAVE::RobotBasePtr m_robot; 74 | std::vector const m_indices; 75 | mutable int m_numCollisionChecks; 76 | mutable double m_totalCollisionTime; 77 | // optional baked stuff 78 | const bool m_do_baked; 79 | OpenRAVE::CollisionCheckerBasePtr m_baked_checker; 80 | std::string m_baked_kinbody_type; 81 | OpenRAVE::KinBodyPtr m_baked_kinbody; 82 | }; 83 | 84 | /** 85 | * StateRobotSetter for RealVectorStateSpaces 86 | */ 87 | class RealVectorOrStateValidityChecker: public OrStateValidityChecker 88 | { 89 | public: 90 | RealVectorOrStateValidityChecker(const ompl::base::SpaceInformationPtr &si, 91 | OpenRAVE::RobotBasePtr robot, std::vector const &indices, 92 | bool do_baked); 93 | virtual bool computeFk(const ompl::base::State *state, uint32_t checklimits) const; 94 | private: 95 | const std::size_t m_num_dof; 96 | }; 97 | 98 | #ifdef OR_OMPL_HAS_BOOSTSMARTPTRS 99 | typedef boost::shared_ptr OrStateValidityCheckerPtr; 100 | #else 101 | typedef std::shared_ptr OrStateValidityCheckerPtr; 102 | #endif 103 | 104 | } // namespace or_ompl 105 | 106 | #endif // OR_OMPL_STATESPACES_H_ 107 | -------------------------------------------------------------------------------- /include/or_ompl/TSR.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Jennifer King 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | #ifndef OR_OMPL_TSR_H_ 34 | #define OR_OMPL_TSR_H_ 35 | 36 | #include 37 | #include 38 | 39 | namespace or_ompl { 40 | 41 | class TSR { 42 | 43 | public: 44 | typedef boost::shared_ptr Ptr; 45 | 46 | /** 47 | * Constructor 48 | */ 49 | TSR(); 50 | 51 | /** 52 | * Constructor 53 | * 54 | * @param T0_w Transform from world frame to the TSR frame w 55 | * @param Tw_e End-effector offset transform in the coordinates of w 56 | * @param Bw 6x2 matrix of bounds in the coordinates of w 57 | * bounds are (x, y, z, roll, pitch, yaw - assume RPY Euler angle convention) 58 | */ 59 | TSR(const Eigen::Affine3d &T0_w, 60 | const Eigen::Affine3d &Tw_e, 61 | const Eigen::Matrix &Bw); 62 | 63 | int manipulator_index() const; 64 | std::string relative_body_name() const; 65 | std::string relative_link_name() const; 66 | 67 | /** 68 | * Deserialize a serialized TSR. 69 | * 70 | * @param ss The stream to read the serialized TSR from 71 | */ 72 | bool deserialize(std::stringstream &ss); 73 | bool deserialize(std::istream &ss); 74 | 75 | /** 76 | * Serialize a TSR Chain. 77 | * 78 | * @param ss The stream to read the serialized TSR from 79 | */ 80 | void serialize(std::ostream& ss); 81 | 82 | /** 83 | * Compute the distance to the TSR 84 | * 85 | * @param ee_pose The pose of the end-effector in world frame 86 | */ 87 | Eigen::Matrix distance(const Eigen::Affine3d &ee_pose) const; 88 | 89 | /** 90 | * Compute the displacement to the TSR 91 | * 92 | * @param ee_pose The pose of the end-effector in world frame 93 | */ 94 | Eigen::Matrix displacement(const Eigen::Affine3d &ee_pose) const; 95 | 96 | /** 97 | * Sample a pose from the TSR 98 | * 99 | * @return The sampled pose 100 | */ 101 | Eigen::Affine3d sample(void) const; 102 | 103 | /** 104 | * Sample a displacement transform from the TSR 105 | * @return The sampled transform 106 | */ 107 | Eigen::Affine3d sampleDisplacementTransform(void) const; 108 | 109 | /** 110 | * @return The transform for the frame of the TSR (T0_w) 111 | */ 112 | Eigen::Affine3d getOriginTransform(void) const { return _T0_w; } 113 | 114 | /** 115 | * @return The end-effector offset transform (Tw_e) 116 | */ 117 | Eigen::Affine3d getEndEffectorOffsetTransform(void) const { return _Tw_e; } 118 | 119 | /** 120 | * @return The bounds specified for the TSR (Bw) 121 | */ 122 | Eigen::Matrix getBounds(void) const { return _Bw; } 123 | 124 | /** 125 | * Output operator 126 | */ 127 | friend std::ostream& operator << (std::ostream &out, const TSR &tsr) { 128 | out << "TSR: " << std::endl; 129 | out << "\tT0_w: " << tsr._T0_w.matrix() << std::endl; 130 | out << "\tTw_e: " << tsr._Tw_e.matrix() << std::endl; 131 | out << "\tBw: " << tsr._Bw << std::endl; 132 | 133 | return out; 134 | } 135 | 136 | protected: 137 | Eigen::Affine3d _T0_w; 138 | Eigen::Affine3d _T0_w_inv; 139 | Eigen::Affine3d _Tw_e; 140 | Eigen::Affine3d _Tw_e_inv; 141 | Eigen::Matrix _Bw; 142 | int _manipulator_index; 143 | std::string _relative_body_name; 144 | std::string _relative_link_name; 145 | bool _initialized; 146 | }; 147 | 148 | } // namespace or_ompl 149 | 150 | #endif // OR_OMPL_TSR_H_ 151 | -------------------------------------------------------------------------------- /include/or_ompl/TSRChain.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Jennifer King 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | #ifndef OR_OMPL_TSRCHAIN_H_ 34 | #define OR_OMPL_TSRCHAIN_H_ 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | namespace or_ompl { 44 | 45 | class TSRChain { 46 | 47 | public: 48 | typedef boost::shared_ptr Ptr; 49 | 50 | /** 51 | * Constructor 52 | */ 53 | TSRChain(); 54 | 55 | /** 56 | * Constructor 57 | * 58 | * @param sample_start True if the chain should be used to sample the start configuration 59 | * @param sample_goal True if the chain should be used to sample the start configuration 60 | * @param constrain True if the chain should be applied trajectory wide 61 | * @param tsrs The list of TSRs in the chain 62 | */ 63 | TSRChain(const bool &sample_start, 64 | const bool &sample_goal, 65 | const bool &constrain, 66 | const std::vector &tsrs); 67 | 68 | /** 69 | * Deserialize a serialized TSR Chain. 70 | * 71 | * @param ss The stream to read the serialized TSR from 72 | */ 73 | bool deserialize(std::istream &ss); 74 | bool deserialize(std::stringstream &ss); 75 | 76 | /** 77 | * Serialize a TSR Chain. 78 | * 79 | * @param ss The stream to read the serialized TSR from 80 | */ 81 | void serialize(std::ostream& ss); 82 | 83 | /** 84 | * @return True if this chain should be used to sample a start 85 | */ 86 | bool sampleStart() const { return _sample_start; } 87 | 88 | /** 89 | * @return True if this chain should be used to sample a goal 90 | */ 91 | bool sampleGoal() const { return _sample_goal; } 92 | 93 | /** 94 | * @return True if this chain should be applied as a trajectory wide constraint 95 | */ 96 | bool isTrajectoryConstraint() const { return _constrain; } 97 | 98 | 99 | /** 100 | * @return The list of TSRs that make up this chain 101 | */ 102 | std::vector getTSRs() const { return _tsrs; } 103 | 104 | /** 105 | * @return A sample from the TSR chain 106 | */ 107 | Eigen::Affine3d sample(void) const; 108 | 109 | /** 110 | * Compute the distance to the TSR 111 | * 112 | * @param ee_pose The pose of the end-effector in world frame 113 | */ 114 | Eigen::Matrix distance(const Eigen::Affine3d &ee_pose) const; 115 | 116 | /** 117 | * Set the planning environment. This is required to enable computing 118 | * distance to a TSR. 119 | * @param penv The OpenRAVE environment this TSRChain will be used in 120 | */ 121 | void setEnv(const OpenRAVE::EnvironmentBasePtr &penv); 122 | 123 | private: 124 | bool _initialized; 125 | bool _sample_start; 126 | bool _sample_goal; 127 | bool _constrain; 128 | std::vector _tsrs; 129 | TSRRobot::Ptr _tsr_robot; 130 | }; 131 | 132 | } // namespace or_ompl 133 | 134 | #endif // OR_OMPL_TSRCHAIN_H_ 135 | -------------------------------------------------------------------------------- /include/or_ompl/TSRGoal.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Jennifer King 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | #ifndef OR_OMPL_TSRGOAL_H_ 34 | #define OR_OMPL_TSRGOAL_H_ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | namespace or_ompl { 47 | 48 | /** 49 | * Implements a goal region defined by a center goal position and a radius 50 | */ 51 | class TSRGoal : public ompl::base::GoalSampleableRegion { 52 | 53 | public: 54 | #ifdef OR_OMPL_HAS_BOOSTSMARTPTRS 55 | typedef boost::shared_ptr Ptr; 56 | #else 57 | typedef std::shared_ptr Ptr; 58 | #endif 59 | 60 | /** 61 | * Constructor 62 | * 63 | * @param si The space information object describing the space where the algorithm will be used 64 | * @param tsr The TSR chain describe the goal region 65 | * @param robot The robot to sample a pose for 66 | */ 67 | TSRGoal(const ompl::base::SpaceInformationPtr &si, 68 | const TSR::Ptr &tsr, 69 | OpenRAVE::RobotBasePtr robot, 70 | OrStateValidityCheckerPtr or_validity_checker); 71 | 72 | /** 73 | * Constructor 74 | * 75 | * @param si The space information object describing the space where the algorithm will be used 76 | * @param tsrchain The TSR chain describe the goal region 77 | * @param robot The robot to sample a pose for 78 | */ 79 | TSRGoal(const ompl::base::SpaceInformationPtr &si, 80 | const TSRChain::Ptr &tsrchain, 81 | OpenRAVE::RobotBasePtr robot, 82 | OrStateValidityCheckerPtr or_validity_checker); 83 | 84 | /** 85 | * Constructor 86 | * 87 | * @param si The space information object describing the space where the algorithm will be used 88 | * @param tsrchains The list of TSR chain describe the goal region 89 | * @param robot The robot to sample a pose for 90 | */ 91 | TSRGoal(const ompl::base::SpaceInformationPtr &si, 92 | const std::vector &tsrchains, 93 | OpenRAVE::RobotBasePtr robot, 94 | OrStateValidityCheckerPtr or_validity_checker); 95 | 96 | /** 97 | * Destructor 98 | */ 99 | ~TSRGoal(); 100 | 101 | /** 102 | * Determines if the given state falls within the goal radius 103 | * 104 | * @param state The state to check 105 | * @return True if the state is within the goal radius 106 | */ 107 | virtual bool isSatisfied(const ompl::base::State *state) const; 108 | 109 | /** 110 | * Calculates the distance between the state and the edge of the goal region 111 | * 112 | * @param state The state 113 | * @return The distance to the edge of the goal region (0 if the state is within the goal region) 114 | */ 115 | virtual double distanceGoal(const ompl::base::State *state) const; 116 | 117 | /** 118 | * Samples a state from within the goal region 119 | * 120 | * @param state The sampled state 121 | */ 122 | virtual void sampleGoal(ompl::base::State *state) const; 123 | 124 | /** 125 | * @return max int 126 | */ 127 | virtual unsigned int maxSampleCount() const; 128 | 129 | private: 130 | std::vector _tsr_chains; 131 | OpenRAVE::RobotBasePtr _robot; 132 | ompl::base::StateSpace * _state_space; 133 | OrStateValidityCheckerPtr _or_validity_checker; 134 | }; 135 | 136 | } // namespace or_ompl 137 | 138 | #endif // OR_OMPL_TSRGOAL_H_ 139 | -------------------------------------------------------------------------------- /include/or_ompl/TSRRobot.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Jennifer King 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | #ifndef OR_OMPL_TSRROBOT_H_ 34 | #define OR_OMPL_TSRROBOT_H_ 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | namespace or_ompl { 42 | 43 | /** 44 | * Decorator for an OpenRAVE Robot constructed to represent 45 | * a TSR chain 46 | */ 47 | class TSRRobot { 48 | 49 | public: 50 | 51 | /** 52 | * Expose a shared ptr for the robot 53 | */ 54 | typedef boost::shared_ptr Ptr; 55 | 56 | /** 57 | * Constructor 58 | */ 59 | TSRRobot(const std::vector &tsrs, const OpenRAVE::EnvironmentBasePtr &penv); 60 | 61 | /** 62 | * @return True if the construction was successful, false otherwise 63 | */ 64 | bool construct(); 65 | 66 | /** 67 | * Finds the nearest reachable end-effector transform to the given transform 68 | * @param Ttarget - The target end-effector transform 69 | */ 70 | Eigen::Affine3d findNearestFeasibleTransform(const Eigen::Affine3d &Ttarget); 71 | 72 | /** 73 | * @return True if this is a point TSR chain - meaning no TSRs have any freedom in the Bw matrix 74 | */ 75 | bool isPointRobot() const { return _point_tsr; } 76 | 77 | /** 78 | * @return True if robot has been properly initialized, false otherwise 79 | */ 80 | bool isInitialized() const { return _initialized; } 81 | 82 | private: 83 | 84 | std::vector _tsrs; 85 | OpenRAVE::EnvironmentBasePtr _penv; 86 | OpenRAVE::RobotBasePtr _probot; 87 | std::string _solver; 88 | OpenRAVE::IkSolverBasePtr _ik_solver; 89 | std::vector _upperlimits; 90 | std::vector _lowerlimits; 91 | bool _initialized; 92 | bool _point_tsr; 93 | unsigned int _num_dof; 94 | }; 95 | 96 | } // namespace or_ompl 97 | 98 | #endif // OR_OMPL_TSRROBOT_H_ 99 | -------------------------------------------------------------------------------- /include/or_ompl/config.h.in: -------------------------------------------------------------------------------- 1 | #cmakedefine OR_OMPL_HAS_PPSEROPTS 1 2 | #cmakedefine OR_OMPL_HAS_BOOSTSMARTPTRS 1 3 | -------------------------------------------------------------------------------- /include/or_ompl/or_conversions.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Michael Koval 7 | Matthew Klingensmith 8 | Christopher Dellin 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are 12 | met: 13 | 14 | Redistributions of source code must retain the above copyright 15 | notice, this list of conditions and the following disclaimer. 16 | 17 | Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 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 FOR 24 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | 33 | *************************************************************************/ 34 | 35 | #ifndef OR_OMPL_OR_CONVERSIONS_H_ 36 | #define OR_OMPL_OR_CONVERSIONS_H_ 37 | 38 | namespace or_ompl { 39 | 40 | inline Eigen::Vector3d toEigen3(OpenRAVE::Vector const &or_v) { 41 | Eigen::Vector3d eigen_v; 42 | eigen_v << or_v.x, or_v.y, or_v.z; 43 | return eigen_v; 44 | } 45 | 46 | inline Eigen::Affine3d toEigen(OpenRAVE::Transform const &or_tf) { 47 | OpenRAVE::TransformMatrix or_matrix(or_tf); 48 | Eigen::Affine3d eigen_tf = Eigen::Affine3d::Identity(); 49 | eigen_tf.linear() << or_matrix.m[0], or_matrix.m[1], or_matrix.m[2], 50 | or_matrix.m[4], or_matrix.m[5], or_matrix.m[6], 51 | or_matrix.m[8], or_matrix.m[9], or_matrix.m[10]; 52 | eigen_tf.translation() << or_matrix.trans.x, or_matrix.trans.y, or_matrix.trans.z; 53 | return eigen_tf; 54 | } 55 | 56 | template 57 | inline OpenRAVE::Vector toOR(Derived const &eigen_v) { 58 | BOOST_STATIC_ASSERT(Derived::IsVectorAtCompileTime); 59 | if (eigen_v.size() == 3) { 60 | return OpenRAVE::Vector(eigen_v[0], eigen_v[1], eigen_v[2]); 61 | } else if (eigen_v.size() == 4) { 62 | return OpenRAVE::Vector(eigen_v[0], eigen_v[1], eigen_v[2], eigen_v[3]); 63 | } else { 64 | throw std::invalid_argument(boost::str( 65 | boost::format("Expected a three or four element vector; got a %d elements.") 66 | % eigen_v.size())); 67 | } 68 | } 69 | 70 | template 71 | inline OpenRAVE::Transform toOR(Eigen::Transform const &tf) { 72 | OpenRAVE::TransformMatrix or_matrix; 73 | or_matrix.rotfrommat( 74 | tf.matrix()(0, 0), tf.matrix()(0, 1), tf.matrix()(0, 2), 75 | tf.matrix()(1, 0), tf.matrix()(1, 1), tf.matrix()(1, 2), 76 | tf.matrix()(2, 0), tf.matrix()(2, 1), tf.matrix()(2, 2) 77 | ); 78 | or_matrix.trans.x = tf(0, 3); 79 | or_matrix.trans.y = tf(1, 3); 80 | or_matrix.trans.z = tf(2, 3); 81 | return or_matrix; 82 | } 83 | 84 | } // namespace or_ompl 85 | 86 | #endif // OR_OMPL_OR_CONVERSIONS_H_ 87 | -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b or_ompl is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | or_ompl 4 | 0.7.0 5 | 6 | OpenRAVE bindings for OMPL. 7 | 8 | https://svn.personalrobotics.ri.cmu.edu/pr-ros-pkg/trunk/or_plugins/or_ompl 9 | Michael Koval 10 | Christopher Dellin 11 | Matthew Klingensmith 12 | Michael Koval 13 | BSD 14 | catkin 15 | pkg-config 16 | python 17 | python-argparse 18 | python-yaml 19 | cmake_modules 20 | eigen 21 | openrave_catkin 22 | ompl 23 | tinyxml 24 | eigen 25 | openrave_catkin 26 | ompl 27 | tinyxml 28 | python-nose 29 | python-numpy 30 | 31 | -------------------------------------------------------------------------------- /planners.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | - name: ompl::geometric::BITstar 3 | header: ompl/geometric/planners/bitstar/BITstar.h 4 | - name: ompl::geometric::BKPIECE1 5 | header: ompl/geometric/planners/kpiece/BKPIECE1.h 6 | - name: ompl::geometric::EST 7 | header: ompl/geometric/planners/est/EST.h 8 | - name: ompl::geometric::FMT 9 | header: ompl/geometric/planners/fmt/FMT.h 10 | - name: ompl::geometric::KPIECE1 11 | header: ompl/geometric/planners/kpiece/KPIECE1.h 12 | - name: ompl::geometric::LazyRRT 13 | header: ompl/geometric/planners/rrt/LazyRRT.h 14 | - name: ompl::geometric::LBKPIECE1 15 | header: ompl/geometric/planners/kpiece/LBKPIECE1.h 16 | - name: ompl::geometric::PDST 17 | header: ompl/geometric/planners/pdst/PDST.h 18 | - name: ompl::geometric::PRM 19 | header: ompl/geometric/planners/prm/PRM.h 20 | - name: ompl::geometric::LazyPRM 21 | header: ompl/geometric/planners/prm/LazyPRM.h 22 | - name: ompl::geometric::PRMstar 23 | header: ompl/geometric/planners/prm/PRMstar.h 24 | - name: ompl::geometric::pRRT 25 | header: ompl/geometric/planners/rrt/pRRT.h 26 | - name: ompl::geometric::pSBL 27 | header: ompl/geometric/planners/sbl/pSBL.h 28 | - name: ompl::geometric::RRT 29 | header: ompl/geometric/planners/rrt/RRT.h 30 | - name: ompl::geometric::RRTConnect 31 | header: ompl/geometric/planners/rrt/RRTConnect.h 32 | - name: ompl::geometric::RRTstar 33 | header: ompl/geometric/planners/rrt/RRTstar.h 34 | - name: ompl::geometric::SBL 35 | header: ompl/geometric/planners/sbl/SBL.h 36 | - name: ompl::geometric::SPARS 37 | header: ompl/geometric/planners/prm/SPARS.h 38 | - name: ompl::geometric::SPARStwo 39 | header: ompl/geometric/planners/prm/SPARStwo.h 40 | - name: ompl::geometric::TRRT 41 | header: ompl/geometric/planners/rrt/TRRT.h 42 | -------------------------------------------------------------------------------- /scripts/example.py: -------------------------------------------------------------------------------- 1 | #!//bin/env python 2 | # This script requires the models that are shipped with OpenRAVE to be in your 3 | # OPENRAVE_DATA path. This is true by default, but may not be true if you set 4 | # your OPENRAVE_DATA environmental variable. Notably, this includes if you 5 | # source a Catkin workspace that includes the openrave_catkin package. 6 | # 7 | # If so, you should explicitly add this directory to your path: 8 | # 9 | # export OPENRAVE_DATA="${OPENRAVE_DATA}:/usr/share/openrave-0.9/data" 10 | # 11 | # This path assumes that you installed OpenRAVE to /usr. You may need to alter 12 | # the command to match your acutal install destination. 13 | 14 | from openravepy import * 15 | 16 | start_config = [ 0.80487864, 0.42326865, -0.54016693, 2.28895761, 17 | -0.34930645, -1.19702164, 1.95971213 ] 18 | goal_config = [ 2.41349473, -1.43062044, -2.69016693, 2.12681216, 19 | -0.75643783, -1.52392537, 1.01239878 ] 20 | 21 | # Setup the environment. 22 | env = Environment() 23 | env.SetViewer('qtcoin') 24 | env.Load('wamtest1.env.xml') 25 | robot = env.GetRobot('BarrettWAM') 26 | manipulator = robot.GetManipulator('arm') 27 | 28 | planner = RaveCreatePlanner(env, 'OMPL_RRTConnect') 29 | simplifier = RaveCreatePlanner(env, 'OMPL_Simplifier') 30 | 31 | with env: 32 | robot.SetActiveDOFs(manipulator.GetArmIndices()) 33 | robot.SetActiveDOFValues(start_config) 34 | robot.SetActiveManipulator(manipulator) 35 | 36 | # Setup the planning instance. 37 | params = Planner.PlannerParameters() 38 | params.SetRobotActiveJoints(robot) 39 | params.SetGoalConfig(goal_config) 40 | 41 | # Set the timeout and planner-specific parameters. You can view a list of 42 | # supported parameters by calling: planner.SendCommand('GetParameters') 43 | print 'Parameters:' 44 | print planner.SendCommand('GetParameters') 45 | 46 | params.SetExtraParameters('0.02') 47 | 48 | with env: 49 | with robot: 50 | # Invoke the planner. 51 | print 'Calling the OMPL_RRTConnect planner.' 52 | traj = RaveCreateTrajectory(env, '') 53 | planner.InitPlan(robot, params) 54 | result = planner.PlanPath(traj) 55 | assert result == PlannerStatus.HasSolution 56 | 57 | # Shortcut the path. 58 | print 'Calling the OMPL_Simplifier for shortcutting.' 59 | simplifier.InitPlan(robot, Planner.PlannerParameters()) 60 | result = simplifier.PlanPath(traj) 61 | assert result == PlannerStatus.HasSolution 62 | 63 | # Time the trajectory. 64 | print 'Timing trajectory' 65 | result = planningutils.RetimeTrajectory(traj) 66 | assert result == PlannerStatus.HasSolution 67 | 68 | # Execute the trajectory. 69 | raw_input('Press to execute trajectory.') 70 | robot.GetController().SetPath(traj) 71 | robot.WaitForController(0) 72 | -------------------------------------------------------------------------------- /scripts/wrap_planners.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2014, Carnegie Mellon University 4 | # All rights reserved. 5 | # 6 | # Authors: Michael Koval 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions are 10 | # met: 11 | # 12 | # Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # 15 | # Redistributions in binary form must reproduce the above copyright 16 | # notice, this list of conditions and the following disclaimer in the 17 | # documentation and/or other materials provided with the distribution. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | # A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | # HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | # SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | # DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | # THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | from __future__ import print_function 32 | import argparse, yaml, os.path, sys 33 | 34 | factory_frontmatter = """\ 35 | #include 36 | #include 37 | #include 38 | {includes:s} 39 | #include 40 | 41 | namespace or_ompl {{ 42 | namespace registry {{ 43 | 44 | struct BasePlannerFactory {{ 45 | virtual ~BasePlannerFactory() {{ }} 46 | virtual ompl::base::Planner *create(ompl::base::SpaceInformationPtr space) = 0; 47 | }}; 48 | 49 | /* 50 | * Planner Factories 51 | */ 52 | """ 53 | factory_template = """\ 54 | struct {name:s}Factory : public virtual BasePlannerFactory {{ 55 | virtual ompl::base::Planner *create(ompl::base::SpaceInformationPtr space) 56 | {{ 57 | return new {qualified_name:s}(space); 58 | }} 59 | }}; 60 | """ 61 | 62 | registry_frontmatter = """ 63 | /* 64 | * Planner Registry 65 | */ 66 | typedef std::map PlannerRegistry; 67 | 68 | // The dynamic_cast is necessary to work around a type inference bug when 69 | // using map_list_of on a polymorphic type. 70 | static PlannerRegistry registry = boost::assign::map_list_of 71 | """ 72 | registry_entry = ' ("{name:s}", dynamic_cast(new {name:s}Factory))' 73 | registry_backmatter = """\ 74 | ; 75 | 76 | std::vector get_planner_names() 77 | { 78 | std::vector names; 79 | names.reserve(registry.size()); 80 | 81 | PlannerRegistry::const_iterator it; 82 | for (it = registry.begin(); it != registry.end(); ++it) { 83 | names.push_back(it->first); 84 | } 85 | 86 | return names; 87 | } 88 | 89 | ompl::base::Planner *create(std::string const &name, 90 | ompl::base::SpaceInformationPtr space) 91 | { 92 | PlannerRegistry::const_iterator const it = registry.find(name); 93 | if (it != registry.end()) { 94 | return it->second->create(space); 95 | } else { 96 | throw std::runtime_error("Unknown planner '" + name + "'."); 97 | } 98 | } 99 | 100 | } // namespace registry 101 | } // namespace or_ompl 102 | """ 103 | 104 | def parse_version(version): 105 | return tuple(int(x) for x in version.split('.')) 106 | 107 | def print_colored(colorcode, s): 108 | if sys.stdout.isatty(): 109 | print('\033[{}m{}\033[0m'.format(colorcode, s)) 110 | else: 111 | print(s) 112 | 113 | def main(): 114 | parser = argparse.ArgumentParser() 115 | parser.add_argument('--include-dirs', type=str, 116 | help='OMPL include directories') 117 | parser.add_argument('--planners-yaml', type=str, required=True, 118 | help='input filename for planner list') 119 | parser.add_argument('--generated-cpp', type=str, required=True, 120 | help='output filename for generated code') 121 | args = parser.parse_args() 122 | 123 | include_dirs = args.include_dirs.split(os.path.pathsep) 124 | 125 | # Filter planners by version number. 126 | with open(args.planners_yaml) as fin: 127 | planners = yaml.load(fin) 128 | supported_planners = [] 129 | 130 | print_colored(94, 'Configuring or_ompl planner registry ...') 131 | for planner in planners: 132 | for include_dir in include_dirs: 133 | header_path = os.path.join(include_dir, planner['header']) 134 | if os.path.exists(header_path): 135 | supported_planners.append(planner) 136 | print_colored(92, ' planner {} found'.format(planner['name'])) 137 | break 138 | else: 139 | print_colored(91, ' planner {} not found'.format(planner['name'])) 140 | 141 | planners = supported_planners 142 | 143 | with open(args.generated_cpp,'w') as fout: 144 | 145 | # Include the necessary OMPL 146 | headers = [ planner['header'] for planner in planners ] 147 | includes = [ '#include <{:s}>'.format(path) for path in headers ] 148 | fout.write(factory_frontmatter.format(includes='\n'.join(includes))) 149 | 150 | # Generate the factory class implementations. 151 | names = [ planner['name'] for planner in planners ] 152 | registry_entries = [] 153 | 154 | for qualified_name in names: 155 | _, _, name = qualified_name.rpartition('::') 156 | args = { 'name': name, 157 | 'qualified_name': qualified_name } 158 | fout.write(factory_template.format(**args)) 159 | registry_entries.append(registry_entry.format(**args)) 160 | 161 | # Generate the registry of factory classes. 162 | fout.write(registry_frontmatter) 163 | fout.write('\n'.join(registry_entries)) 164 | fout.write(registry_backmatter) 165 | 166 | if __name__ == '__main__': 167 | main() 168 | -------------------------------------------------------------------------------- /src/OMPLConversions.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Michael Koval 7 | Matthew Klingensmith 8 | Christopher Dellin 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are 12 | met: 13 | 14 | Redistributions of source code must retain the above copyright 15 | notice, this list of conditions and the following disclaimer. 16 | 17 | Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 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 FOR 24 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | 33 | *************************************************************************/ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | namespace or_ompl { 43 | 44 | void OpenRAVEHandler::log(std::string const &text, ompl::msg::LogLevel level, 45 | char const *filename, int line) { 46 | 47 | int const openrave_threshold_level = ( 48 | OpenRAVE::RaveGetDebugLevel() & OpenRAVE::Level_OutputMask); 49 | int openrave_message_level = OpenRAVE::Level_Debug; 50 | 51 | switch (level) { 52 | case ompl::msg::LOG_DEBUG: 53 | openrave_message_level = OpenRAVE::Level_Debug; 54 | break; 55 | 56 | case ompl::msg::LOG_INFO: 57 | openrave_message_level = OpenRAVE::Level_Info; 58 | break; 59 | 60 | case ompl::msg::LOG_WARN: 61 | openrave_message_level = OpenRAVE::Level_Warn; 62 | break; 63 | 64 | case ompl::msg::LOG_ERROR: 65 | openrave_message_level = OpenRAVE::Level_Error; 66 | break; 67 | 68 | case ompl::msg::LOG_NONE: 69 | default: 70 | RAVELOG_ERROR("Unknown OMPL log level '%d'.\n", level); 71 | } 72 | 73 | if (openrave_message_level >= openrave_threshold_level) 74 | { 75 | #ifdef OPENRAVE_LOG4CXX 76 | log4cxx::spi::LocationInfo const location_info( 77 | OpenRAVE::RaveGetSourceFilename(filename), "", line); 78 | OpenRAVE::RavePrintfA_DEBUGLEVEL( 79 | OpenRAVE::RaveGetLogger(), location_info, text); 80 | #else 81 | OpenRAVE::RavePrintfA_DEBUGLEVEL("[%s:%d] %s\n", 82 | OpenRAVE::RaveGetSourceFilename(filename), line, 83 | text.c_str()); 84 | #endif 85 | } 86 | } 87 | 88 | std::vector GetContinuousJoints(const OpenRAVE::RobotBasePtr robot, const std::vector idx) { 89 | const std::vector& joints = robot->GetJoints(); 90 | std::vector isContinuous; 91 | for (size_t j = 0; j < idx.size(); j++) 92 | { 93 | isContinuous.push_back(joints[idx[j]]->IsCircular(0)); 94 | } 95 | return isContinuous; 96 | } 97 | 98 | ompl::base::StateSpacePtr CreateStateSpace(OpenRAVE::RobotBasePtr const robot, 99 | OMPLPlannerParameters const ¶ms) { 100 | if (!robot) { 101 | RAVELOG_ERROR("Robot must not be NULL.\n"); 102 | return ompl::base::StateSpacePtr(); 103 | } else if (robot->GetActiveDOF() == 0) { 104 | RAVELOG_ERROR("Zero DOFs are active.\n"); 105 | return ompl::base::StateSpacePtr(); 106 | } 107 | 108 | if (params.m_seed) { 109 | RAVELOG_DEBUG("Setting OMPL seed to %u.\n", params.m_seed); 110 | ompl::RNG::setSeed(params.m_seed); 111 | if (ompl::RNG::getSeed() != params.m_seed) { 112 | RAVELOG_ERROR("Could not set OMPL seed. Was this the first or_ompl" 113 | " plan attempted?\n"); 114 | return ompl::base::StateSpacePtr(); 115 | } 116 | } else { 117 | RAVELOG_DEBUG("Using default seed of %u for OMPL.\n", 118 | ompl::RNG::getSeed()); 119 | } 120 | 121 | std::vector dof_indices = robot->GetActiveDOFIndices(); 122 | const unsigned int num_dof = dof_indices.size(); 123 | std::vector is_continuous = GetContinuousJoints(robot, dof_indices); 124 | BOOST_ASSERT(is_continuous.size() == num_dof); 125 | bool any_continuous = false; 126 | for (size_t i = 0; i < num_dof; ++i) { 127 | if (is_continuous[i]) { 128 | any_continuous = true; 129 | } 130 | } 131 | 132 | std::vector lowerLimits, upperLimits; 133 | robot->GetActiveDOFLimits(lowerLimits, upperLimits); 134 | BOOST_ASSERT(lowerLimits.size() == num_dof); 135 | BOOST_ASSERT(upperLimits.size() == num_dof); 136 | 137 | ompl::base::RealVectorBounds bounds(num_dof); 138 | for (size_t i = 0; i < num_dof; ++i) { 139 | BOOST_ASSERT(lowerLimits[i] <= upperLimits[i]); 140 | if (is_continuous[i]) 141 | { 142 | double diam = upperLimits[i] - lowerLimits[i]; 143 | if (fabs(diam - 2.0*M_PI) > std::numeric_limits::epsilon() * 2.0) 144 | { 145 | RAVELOG_WARN("Robot DOF [%lu] is circular, but has limits [%f,%f]!\n", 146 | i, lowerLimits[i], upperLimits[i]); 147 | RAVELOG_WARN("Ignoring limits and using [-PI,PI] instead ...\n"); 148 | lowerLimits[i] = -M_PI; 149 | upperLimits[i] = M_PI; 150 | } 151 | } 152 | bounds.setLow(i, lowerLimits[i]); 153 | bounds.setHigh(i, upperLimits[i]); 154 | } 155 | 156 | // construct state space 157 | ompl::base::StateSpacePtr state_space; 158 | if (any_continuous) { 159 | state_space.reset(new SemiToroidalStateSpace(num_dof)); 160 | state_space->as()->setIsWrapping(is_continuous); 161 | RAVELOG_DEBUG("Setting joint limits.\n"); 162 | state_space->as()->setBounds(bounds); 163 | } else { 164 | state_space.reset(new ompl::base::RealVectorStateSpace(num_dof)); 165 | RAVELOG_DEBUG("Setting joint limits.\n"); 166 | state_space->as()->setBounds(bounds); 167 | } 168 | 169 | // Set the resolution at which OMPL should discretize edges for collision 170 | // checking. OpenRAVE supports per-joint resolutions, so we compute 171 | // a conservative minimum resolution required across all joints. 172 | // We then convert this to a fraction 173 | // of the workspace extents to call setLongestValidSegmentFraction. 174 | RAVELOG_DEBUG("Setting resolution.\n"); 175 | std::vector dof_resolutions; 176 | robot->GetActiveDOFResolutions(dof_resolutions); 177 | BOOST_ASSERT(dof_resolutions.size() == num_dof); 178 | 179 | double conservative_resolution = std::numeric_limits::max(); 180 | for (size_t i = 0; i < num_dof; ++i) { 181 | conservative_resolution = std::min(conservative_resolution, dof_resolutions[i]); 182 | } 183 | 184 | double conservative_fraction = conservative_resolution / state_space->getMaximumExtent(); 185 | state_space->setLongestValidSegmentFraction(conservative_fraction); 186 | RAVELOG_DEBUG("Computed resolution of %f (%f fraction of extents).\n", 187 | conservative_resolution, conservative_fraction); 188 | 189 | // Per-DOF weights are not supported by OMPL. 190 | // TODO: Emulate this by scaling joint values. 191 | RAVELOG_DEBUG("Setting joint weights.\n"); 192 | std::vector dof_weights; 193 | robot->GetActiveDOFWeights(dof_weights); 194 | BOOST_ASSERT(dof_weights.size() == num_dof); 195 | 196 | bool has_weights = false; 197 | for (size_t i = 0; !has_weights && i < num_dof; ++i) { 198 | has_weights = dof_weights[i] != 1.0; 199 | } 200 | 201 | if (has_weights) { 202 | RAVELOG_WARN("Robot specifies DOF weights. Only unit weights are" 203 | " supported by OMPL; planning will commence as if" 204 | " there are no weights.\n"); 205 | } 206 | 207 | state_space->setup(); 208 | 209 | return state_space; 210 | } 211 | 212 | OpenRAVE::PlannerStatus ToORTrajectory( 213 | OpenRAVE::RobotBasePtr const &robot, 214 | ompl::geometric::PathGeometric& ompl_traj, 215 | OpenRAVE::TrajectoryBasePtr or_traj) { 216 | using ompl::geometric::PathGeometric; 217 | 218 | size_t const num_dof = robot->GetActiveDOF(); 219 | or_traj->Init(robot->GetActiveConfigurationSpecification("linear")); 220 | 221 | ompl::base::StateSpacePtr space = ompl_traj.getSpaceInformation()->getStateSpace(); 222 | 223 | for (size_t i = 0; i < ompl_traj.getStateCount(); ++i){ 224 | std::vector values; 225 | space->copyToReals(values, ompl_traj.getState(i)); 226 | or_traj->Insert(i, values, true); 227 | } 228 | return OpenRAVE::PS_HasSolution; 229 | } 230 | 231 | } // namespace or_ompl 232 | -------------------------------------------------------------------------------- /src/OMPLMain.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are 8 | met: 9 | 10 | Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | 13 | Redistributions in binary form must reproduce the above copyright 14 | notice, this list of conditions and the following disclaimer in the 15 | documentation and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 20 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 21 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 22 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | *************************************************************************/ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | using namespace OpenRAVE; 47 | 48 | InterfaceBasePtr CreateInterfaceValidated( 49 | InterfaceType type, std::string const &interfacename, 50 | std::istream &sinput, EnvironmentBasePtr penv) { 51 | std::vector const planner_names 52 | = or_ompl::registry::get_planner_names(); 53 | 54 | if (type == PT_Planner && boost::starts_with(interfacename, "ompl_")) { 55 | // Handle OMPLSimplifier as a special case. This doesn't implement the 56 | // planning interface, so we can't auto-generate this. 57 | if (interfacename == "ompl_simplifier") { 58 | return boost::make_shared(penv); 59 | } 60 | 61 | // Check whether this is an automatically-wrapped planner. 62 | std::string const ompl_planner_name = interfacename.substr(5); 63 | BOOST_FOREACH (std::string const &candidate_name, planner_names) { 64 | std::string candidate_name_lower = candidate_name; 65 | boost::algorithm::to_lower(candidate_name_lower); 66 | 67 | if (candidate_name_lower == ompl_planner_name) { 68 | or_ompl::PlannerFactory const factory = boost::bind( 69 | &or_ompl::registry::create, candidate_name, _1); 70 | return boost::make_shared(penv, factory); 71 | } 72 | } 73 | } 74 | return InterfaceBasePtr(); 75 | } 76 | 77 | void GetPluginAttributesValidated(PLUGININFO &info) { 78 | std::vector const planner_names 79 | = or_ompl::registry::get_planner_names(); 80 | 81 | BOOST_FOREACH (std::string const &planner_name, planner_names) { 82 | std::string const or_planner_name = "OMPL_" + planner_name; 83 | info.interfacenames[PT_Planner].push_back(or_planner_name); 84 | } 85 | 86 | info.interfacenames[PT_Planner].push_back("OMPL_Simplifier"); 87 | 88 | // Forward OMPL log messages to OpenRAVE. 89 | ompl::msg::setLogLevel(ompl::msg::LOG_DEBUG); 90 | ompl::msg::useOutputHandler(new or_ompl::OpenRAVEHandler); 91 | } 92 | 93 | RAVE_PLUGIN_API void DestroyPlugin() { 94 | } 95 | -------------------------------------------------------------------------------- /src/OMPLPlanner.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Michael Koval 7 | Matthew Klingensmith 8 | Christopher Dellin 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are 12 | met: 13 | 14 | Redistributions of source code must retain the above copyright 15 | notice, this list of conditions and the following disclaimer. 16 | 17 | Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 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 FOR 24 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | 33 | *************************************************************************/ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | namespace or_ompl { 54 | 55 | OMPLPlanner::OMPLPlanner(OpenRAVE::EnvironmentBasePtr penv, 56 | PlannerFactory const &planner_factory) 57 | : OpenRAVE::PlannerBase(penv) 58 | , m_initialized(false) 59 | , m_planner_factory(planner_factory) { 60 | 61 | RegisterCommand("GetParameters", 62 | boost::bind(&OMPLPlanner::GetParametersCommand, this, _1, _2), 63 | "returns the list of accepted planner parameters" 64 | ); 65 | 66 | RegisterCommand("GetParameterValue", 67 | boost::bind(&OMPLPlanner::GetParameterValCommand, this, _1, _2), 68 | "returns the value of a specific parameter" 69 | ); 70 | 71 | RegisterCommand("GetTimes", 72 | boost::bind(&OMPLPlanner::GetTimes,this,_1,_2), 73 | "get timing information from last plan"); 74 | 75 | } 76 | 77 | OMPLPlanner::~OMPLPlanner() { 78 | } 79 | 80 | bool OMPLPlanner::InitPlan(OpenRAVE::RobotBasePtr robot, std::istream& input) { 81 | OMPLPlannerParametersPtr params = boost::make_shared(); 82 | input >> *params; 83 | return InitPlan(robot, params); 84 | } 85 | 86 | bool OMPLPlanner::InitPlan(OpenRAVE::RobotBasePtr robot, 87 | PlannerParametersConstPtr params_raw) { 88 | m_initialized = false; 89 | 90 | try { 91 | typedef ompl::base::ScopedState ScopedState; 92 | 93 | if (!robot) { 94 | RAVELOG_ERROR("Robot must not be NULL.\n"); 95 | return false; 96 | } else if (!params_raw) { 97 | RAVELOG_ERROR("Parameters must not be NULL.\n"); 98 | return false; 99 | } 100 | 101 | m_robot = robot; 102 | m_totalPlanningTime = 0.0; 103 | 104 | std::vector dof_indices = robot->GetActiveDOFIndices(); 105 | const unsigned int num_dof = dof_indices.size(); 106 | m_parameters = boost::make_shared(); 107 | m_parameters->copy(params_raw); 108 | 109 | RAVELOG_DEBUG("Creating state space.\n"); 110 | m_state_space = CreateStateSpace(robot, *m_parameters); 111 | if (!m_state_space) { 112 | RAVELOG_ERROR("Failed creating state space.\n"); 113 | return false; 114 | } 115 | 116 | RAVELOG_DEBUG("Creating OMPL setup.\n"); 117 | m_simple_setup.reset(new ompl::geometric::SimpleSetup(m_state_space)); 118 | 119 | RAVELOG_DEBUG("Setting state validity checker.\n"); 120 | if (m_state_space->isCompound()) { 121 | m_or_validity_checker.reset(new OrStateValidityChecker( 122 | m_simple_setup->getSpaceInformation(), m_robot, dof_indices, m_parameters->m_doBaked)); 123 | } else { 124 | m_or_validity_checker.reset(new RealVectorOrStateValidityChecker( 125 | m_simple_setup->getSpaceInformation(), m_robot, dof_indices, m_parameters->m_doBaked)); 126 | } 127 | #ifdef OR_OMPL_HAS_BOOSTSMARTPTRS 128 | m_simple_setup->setStateValidityChecker( 129 | boost::static_pointer_cast(m_or_validity_checker)); 130 | #else 131 | m_simple_setup->setStateValidityChecker( 132 | std::static_pointer_cast(m_or_validity_checker)); 133 | #endif 134 | 135 | // start validity checker 136 | m_or_validity_checker->start(); 137 | BOOST_SCOPE_EXIT((m_or_validity_checker)) { 138 | m_or_validity_checker->stop(); 139 | } BOOST_SCOPE_EXIT_END 140 | 141 | RAVELOG_DEBUG("Setting initial configuration.\n"); 142 | if (m_parameters->vinitialconfig.size() % num_dof != 0) { 143 | RAVELOG_ERROR("Start configuration has incorrect DOF;" 144 | " expected multiple of %d, got %d.\n", 145 | num_dof, m_parameters->vinitialconfig.size()); 146 | return false; 147 | } 148 | unsigned int num_starts = m_parameters->vinitialconfig.size() / num_dof; 149 | if (num_starts == 0) { 150 | RAVELOG_ERROR("No initial configurations provided.\n"); 151 | return false; 152 | } 153 | 154 | if (num_starts == 1) { 155 | ScopedState q_start(m_state_space); 156 | for (size_t i = 0; i < num_dof; i++) { 157 | q_start[i] = m_parameters->vinitialconfig[i]; 158 | } 159 | if (!m_or_validity_checker->isValid(q_start.get())) { 160 | RAVELOG_ERROR("Single initial configuration in collision.\n"); 161 | return false; 162 | } 163 | } 164 | 165 | for (unsigned int istart=0; istartvinitialconfig[istart*num_dof + i]; 170 | } 171 | m_simple_setup->addStartState(q_start); 172 | } 173 | 174 | RAVELOG_DEBUG("Setting goal configuration.\n"); 175 | std::vector goal_chains; 176 | BOOST_FOREACH(TSRChain::Ptr tsr_chain, m_parameters->m_tsrchains){ 177 | if(tsr_chain->sampleGoal()){ 178 | tsr_chain->setEnv(robot->GetEnv()); // required to enable distance to TSR chains 179 | goal_chains.push_back(tsr_chain); 180 | }else{ 181 | RAVELOG_ERROR("Only goal TSR chains are supported by OMPL. Failing.\n"); 182 | return false; 183 | } 184 | } 185 | 186 | if(goal_chains.size() > 0 && m_parameters->vgoalconfig.size() > 0){ 187 | RAVELOG_ERROR("A goal TSR chain has been supplied and a goal configuration" 188 | " has been specified. The desired behavior is ambiguous." 189 | " Please specified one or the other.\n"); 190 | return false; 191 | } 192 | 193 | if(goal_chains.size() > 0) { 194 | TSRGoal::Ptr goaltsr(new TSRGoal(m_simple_setup->getSpaceInformation(), 195 | goal_chains, 196 | robot, 197 | m_or_validity_checker)); 198 | m_simple_setup->setGoal(goaltsr); 199 | }else{ 200 | if (m_parameters->vgoalconfig.size() % num_dof != 0) { 201 | RAVELOG_ERROR("End configuration has incorrect DOF;" 202 | " expected multiple of %d, got %d.\n", 203 | num_dof, m_parameters->vgoalconfig.size()); 204 | return false; 205 | } 206 | unsigned int num_goals = m_parameters->vgoalconfig.size() / num_dof; 207 | if (num_goals == 0) { 208 | RAVELOG_ERROR("No goal configurations provided.\n"); 209 | return false; 210 | } 211 | 212 | if (num_goals == 1) { 213 | ScopedState q_goal(m_state_space); 214 | for (size_t i = 0; i < num_dof; i++) { 215 | q_goal[i] = m_parameters->vgoalconfig[i]; 216 | } 217 | 218 | if (!m_or_validity_checker->isValid(q_goal.get())) { 219 | RAVELOG_ERROR("Single goal configuration is in collision.\n"); 220 | return false; 221 | } 222 | 223 | m_simple_setup->setGoalState(q_goal); 224 | } else { 225 | // if multiple possible goals specified, 226 | // don't check them all (this might be expensive) 227 | // and instead lead the planner check some 228 | ompl::base::GoalPtr ompl_goals(new ompl::base::GoalStates( 229 | m_simple_setup->getSpaceInformation())); 230 | for (unsigned int igoal=0; igoalvgoalconfig[igoal*num_dof + i]; 235 | } 236 | ompl_goals->as()->addState(q_goal); 237 | } 238 | m_simple_setup->setGoal(ompl_goals); 239 | } 240 | } 241 | 242 | RAVELOG_DEBUG("Creating planner.\n"); 243 | m_planner = CreatePlanner(*m_parameters); 244 | if (!m_planner) { 245 | RAVELOG_ERROR("Failed creating OMPL planner.\n"); 246 | return false; 247 | } 248 | m_simple_setup->setPlanner(m_planner); 249 | 250 | m_initialized = true; 251 | return true; 252 | } catch (std::runtime_error const &e) { 253 | RAVELOG_ERROR("InitPlan failed: %s\n", e.what()); 254 | return false; 255 | } 256 | } 257 | 258 | ompl::base::PlannerPtr OMPLPlanner::CreatePlanner( 259 | OMPLPlannerParameters const ¶ms) { 260 | // Create the planner. 261 | ompl::base::SpaceInformationPtr const spaceInformation 262 | = m_simple_setup->getSpaceInformation(); 263 | 264 | ompl::base::PlannerPtr planner(m_planner_factory(spaceInformation)); 265 | if (!planner) { 266 | RAVELOG_ERROR("Failed creating planner."); 267 | return ompl::base::PlannerPtr(); 268 | } 269 | 270 | // Populate planner parameters from the PlannerParameters. 271 | std::string const params_str = "" 272 | + m_parameters->_sExtraParameters 273 | + ""; 274 | TiXmlDocument doc_xml; 275 | doc_xml.Parse(params_str.c_str()); 276 | if (doc_xml.Error()) { 277 | RAVELOG_ERROR("Failed parsing XML parameters: %s\n", 278 | doc_xml.ErrorDesc()); 279 | return ompl::base::PlannerPtr(); 280 | } 281 | 282 | TiXmlElement const *root_xml = doc_xml.RootElement(); 283 | std::vector< std::pair > params_vec; 284 | 285 | for (TiXmlElement const *it_ele = root_xml->FirstChildElement(); 286 | it_ele; 287 | it_ele = it_ele->NextSiblingElement()) { 288 | // Extract the property name. 289 | std::string const key = it_ele->ValueStr(); 290 | 291 | // Extract the property value. 292 | TiXmlNode const *node = it_ele->FirstChild(); 293 | if (!node) { 294 | RAVELOG_ERROR("Failed parsing planner parameters:" 295 | " Element '%s' does not contain a value.\n", 296 | key.c_str()); 297 | return ompl::base::PlannerPtr(); 298 | } 299 | TiXmlText const *text = node->ToText(); 300 | TiXmlNode const *next_node = node->NextSibling(); 301 | if (!text || next_node) { 302 | RAVELOG_ERROR("Failed parsing planner parameters:" 303 | " Element '%s' contains complex data.\n", 304 | key.c_str()); 305 | } 306 | std::string const value = text->Value(); 307 | 308 | params_vec.push_back(std::make_pair(key, value)); 309 | } 310 | 311 | ompl::base::ParamSet ¶m_set = planner->params(); 312 | bool is_success = true; 313 | for (std::vector< std::pair >::iterator 314 | it=params_vec.begin(); it!=params_vec.end(); it++) 315 | { 316 | is_success = param_set.setParam(it->first, it->second); 317 | if (!is_success) 318 | break; 319 | } 320 | 321 | // Print out the list of valid parameters. 322 | if (!is_success) { 323 | std::vector param_names; 324 | param_set.getParamNames(param_names); 325 | 326 | std::stringstream param_stream; 327 | BOOST_FOREACH (std::string const ¶m_name, param_names) { 328 | param_stream << " " << param_name; 329 | } 330 | std::string const param_str = param_stream.str(); 331 | 332 | RAVELOG_ERROR("Invalid planner parameters." 333 | " The following parameters are supported:%s\n", 334 | param_str.c_str()); 335 | return ompl::base::PlannerPtr(); 336 | } 337 | return planner; 338 | } 339 | 340 | OpenRAVE::PlannerStatus OMPLPlanner::PlanPath(OpenRAVE::TrajectoryBasePtr ptraj) { 341 | if (!m_initialized) { 342 | RAVELOG_ERROR("Unable to plan. Did you call InitPlan?\n"); 343 | return OpenRAVE::PS_Failed; 344 | } 345 | 346 | boost::chrono::steady_clock::time_point const tic 347 | = boost::chrono::steady_clock::now(); 348 | 349 | OpenRAVE::PlannerStatus planner_status; 350 | try { 351 | // TODO: Configure anytime algorithms to keep planning. 352 | //m_simpleSetup->getGoal()->setMaximumPathLength(0.0); 353 | 354 | // start validity checker 355 | m_or_validity_checker->start(); 356 | BOOST_SCOPE_EXIT((m_or_validity_checker)) { 357 | m_or_validity_checker->stop(); 358 | } BOOST_SCOPE_EXIT_END 359 | 360 | ompl::base::PlannerStatus ompl_status; 361 | ompl_status = m_simple_setup->solve(m_parameters->m_timeLimit); 362 | 363 | // Handle OMPL return codes, set planner_status and ptraj 364 | if (ompl_status == ompl::base::PlannerStatus::EXACT_SOLUTION 365 | || ompl_status == ompl::base::PlannerStatus::APPROXIMATE_SOLUTION) { 366 | 367 | if (m_simple_setup->haveExactSolutionPath()) { 368 | ToORTrajectory(m_robot, m_simple_setup->getSolutionPath(), ptraj); 369 | if (ompl_status == ompl::base::PlannerStatus::EXACT_SOLUTION) { 370 | planner_status = OpenRAVE::PS_HasSolution; 371 | } else { 372 | planner_status = OpenRAVE::PS_InterruptedWithSolution; 373 | } 374 | } else { 375 | RAVELOG_ERROR("Planner returned %s, but no path found!\n", ompl_status.asString().c_str()); 376 | planner_status = OpenRAVE::PS_Failed; 377 | } 378 | 379 | } else { 380 | // Intended to handle: 381 | // - PlannerStatus::INVALID_START 382 | // - PlannerStatus::INVALID_GOAL 383 | // - PlannerStatus::UNRECOGNIZED_GOAL_TYPE 384 | // - PlannerStatus::CRASH 385 | // - PlannerStatus::ABORT 386 | // - PlannerStatus::TIMEOUT 387 | // (these cases are not handled explicitly because different versions 388 | // of OMPL support different error cases) 389 | RAVELOG_ERROR("Planner returned %s.\n", ompl_status.asString().c_str()); 390 | planner_status = OpenRAVE::PS_Failed; 391 | } 392 | 393 | } catch (std::runtime_error const &e) { 394 | RAVELOG_ERROR("Planning failed: %s\n", e.what()); 395 | planner_status = OpenRAVE::PS_Failed; 396 | } 397 | 398 | boost::chrono::steady_clock::time_point const toc 399 | = boost::chrono::steady_clock::now(); 400 | m_totalPlanningTime += boost::chrono::duration_cast< 401 | boost::chrono::duration >(toc - tic).count(); 402 | 403 | return planner_status; 404 | } 405 | 406 | bool OMPLPlanner::GetParametersCommand(std::ostream &sout, std::istream &sin) const { 407 | typedef std::map ParamMap; 408 | 409 | ompl::base::PlannerPtr planner; 410 | if (m_planner) { 411 | planner = m_planner; 412 | } 413 | // We need an instance of the planner to query its ParamSet. Unfortunately, 414 | // constructing the planner requires a SpaceInformationPtr, which can only 415 | // be generated from an existing StateSpace. As a workaround, we construct 416 | // a simple one-DOF state space and make a temporary planner instance. 417 | else { 418 | ompl::base::StateSpacePtr const state_space( 419 | new ompl::base::RealVectorStateSpace(1)); 420 | ompl::base::SpaceInformationPtr const space_information( 421 | new ompl::base::SpaceInformation(state_space)); 422 | planner.reset(m_planner_factory(space_information)); 423 | } 424 | 425 | // Query the supported parameters. Each planner has a name and a "range 426 | // suggestion", which is used to generate the GUI in OMPL.app. 427 | ompl::base::ParamSet const ¶m_set = planner->params(); 428 | ParamMap const ¶m_map = param_set.getParams(); 429 | 430 | ParamMap::const_iterator it; 431 | for (it = param_map.begin(); it != param_map.end(); ++it) { 432 | sout << it->first << " (" << it->second->getRangeSuggestion() << ")\n"; 433 | } 434 | 435 | return true; 436 | } 437 | 438 | bool OMPLPlanner::GetParameterValCommand(std::ostream &sout, std::istream &sin) const { 439 | typedef std::map ParamMap; 440 | //Obtain argument from input stream 441 | std::string inp_arg; 442 | sin >> inp_arg; 443 | 444 | ompl::base::PlannerPtr planner; 445 | if (m_planner) { 446 | planner = m_planner; 447 | } 448 | // We need an instance of the planner to query its ParamSet. Unfortunately, 449 | // constructing the planner requires a SpaceInformationPtr, which can only 450 | // be generated from an existing StateSpace. As a workaround, we construct 451 | // a simple one-DOF state space and make a temporary planner instance. 452 | else { 453 | ompl::base::StateSpacePtr const state_space( 454 | new ompl::base::RealVectorStateSpace(1)); 455 | ompl::base::SpaceInformationPtr const space_information( 456 | new ompl::base::SpaceInformation(state_space)); 457 | planner.reset(m_planner_factory(space_information)); 458 | } 459 | 460 | // Query the supported parameters. Each planner has a name and a "range 461 | // suggestion", which is used to generate the GUI in OMPL.app. 462 | 463 | ompl::base::ParamSet const ¶m_set = planner->params(); 464 | std::string value; 465 | 466 | //Check if in parameter map 467 | bool in_map = param_set.getParam(inp_arg,value); 468 | 469 | if(!in_map){ 470 | RAVELOG_ERROR("Parameter not in set\n"); 471 | throw OpenRAVE::openrave_exception("Parameter not in set",OpenRAVE::ORE_InvalidArguments); 472 | } 473 | else{ 474 | //Output key-value pair 475 | sout<getTotalCollisionTime(); 490 | sout << " totaltime " << m_totalPlanningTime; 491 | sout << " n_checks " << m_or_validity_checker->getNumCollisionChecks(); 492 | return true; 493 | } 494 | 495 | } // namespace or_ompl 496 | -------------------------------------------------------------------------------- /src/OMPLSimplifier.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Michael Koval 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | using OpenRAVE::PA_None; 43 | using OpenRAVE::PA_Interrupt; 44 | using OpenRAVE::PA_ReturnWithAnySolution; 45 | 46 | using OpenRAVE::PS_HasSolution; 47 | using OpenRAVE::PS_InterruptedWithSolution; 48 | 49 | namespace or_ompl { 50 | 51 | OMPLSimplifier::OMPLSimplifier(OpenRAVE::EnvironmentBasePtr env) 52 | : OpenRAVE::PlannerBase(env) { 53 | } 54 | 55 | OMPLSimplifier::~OMPLSimplifier() { 56 | } 57 | 58 | bool OMPLSimplifier::InitPlan(OpenRAVE::RobotBasePtr robot, 59 | PlannerParametersConstPtr params_raw) { 60 | if (!robot) { 61 | RAVELOG_ERROR("Robot must not be NULL.\n"); 62 | return false; 63 | } else if (!params_raw) { 64 | RAVELOG_ERROR("Parameters must not be NULL.\n"); 65 | return false; 66 | } 67 | 68 | m_robot = robot; 69 | m_cspec = m_robot->GetActiveConfigurationSpecification(); 70 | std::vector dof_indices = robot->GetActiveDOFIndices(); 71 | 72 | m_parameters = boost::make_shared(); 73 | m_parameters->copy(params_raw); 74 | 75 | try { 76 | using ompl::base::SpaceInformation; 77 | using ompl::geometric::PathSimplifier; 78 | 79 | m_state_space = CreateStateSpace(robot, *m_parameters); 80 | m_space_info.reset(new SpaceInformation(m_state_space)); 81 | if (m_state_space->isCompound()) { 82 | m_or_validity_checker.reset(new OrStateValidityChecker( 83 | m_space_info, m_robot, dof_indices, m_parameters->m_doBaked)); 84 | } else { 85 | m_or_validity_checker.reset(new RealVectorOrStateValidityChecker( 86 | m_space_info, m_robot, dof_indices, m_parameters->m_doBaked)); 87 | } 88 | #ifdef OR_OMPL_HAS_BOOSTSMARTPTRS 89 | m_space_info->setStateValidityChecker( 90 | boost::static_pointer_cast(m_or_validity_checker)); 91 | #else 92 | m_space_info->setStateValidityChecker( 93 | std::static_pointer_cast(m_or_validity_checker)); 94 | #endif 95 | 96 | m_space_info->setup(); 97 | m_simplifier.reset(new PathSimplifier(m_space_info)); 98 | return true; 99 | } catch (std::runtime_error const &e) { 100 | RAVELOG_ERROR("IntPlan failed: %s\n", e.what()); 101 | return false; 102 | } 103 | } 104 | 105 | bool OMPLSimplifier::InitPlan(OpenRAVE::RobotBasePtr robot, std::istream &input) { 106 | OMPLPlannerParametersPtr params = boost::make_shared(); 107 | input >> *params; 108 | return InitPlan(robot, params); 109 | } 110 | 111 | OpenRAVE::PlannerStatus OMPLSimplifier::PlanPath(OpenRAVE::TrajectoryBasePtr ptraj) { 112 | typedef ompl::base::ScopedState ScopedState; 113 | 114 | if (!m_simplifier) { 115 | RAVELOG_ERROR("Not initialized. Did you call InitPlan?\n"); 116 | return OpenRAVE::PS_Failed; 117 | } else if (!ptraj) { 118 | RAVELOG_ERROR("Input trajectory is NULL.\n"); 119 | return OpenRAVE::PS_Failed; 120 | } else if (m_parameters->m_timeLimit <= 0) { 121 | RAVELOG_ERROR("Time limit must be positive; got %f.", 122 | m_parameters->m_timeLimit); 123 | return OpenRAVE::PS_Failed; 124 | } else if (ptraj->GetNumWaypoints() == 0) { 125 | RAVELOG_WARN("Input trajectory is empty; there is nothing to do.\n"); 126 | return OpenRAVE::PS_HasSolution; 127 | } 128 | 129 | // Convert the OpenRAVE trajectory into an OMPL path. 130 | BOOST_ASSERT(m_space_info); 131 | ompl::geometric::PathGeometric path(m_space_info); 132 | size_t const num_dof = m_cspec.GetDOF(); 133 | 134 | RAVELOG_DEBUG("Create OMPL path with %d DOF and %d waypoints.\n", 135 | num_dof, ptraj->GetNumWaypoints()); 136 | 137 | for (size_t iwaypoint = 0; iwaypoint < ptraj->GetNumWaypoints(); ++iwaypoint) { 138 | // Extract the OpenRAVE waypoint. Default to the current configuration 139 | // for any missing DOFs. 140 | std::vector waypoint_openrave; 141 | m_robot->GetActiveDOFValues(waypoint_openrave); 142 | ptraj->GetWaypoint(iwaypoint, waypoint_openrave, m_cspec); 143 | 144 | // Insert the waypoint into the OMPL path. 145 | ScopedState waypoint_ompl(m_space_info); 146 | for (size_t idof = 0; idof < num_dof; ++idof) { 147 | waypoint_ompl[idof] = waypoint_openrave[idof]; 148 | } 149 | path.append(waypoint_ompl.get()); 150 | } 151 | 152 | // Run path simplification. 153 | OpenRAVE::PlannerBase::PlannerProgress progress; 154 | OpenRAVE::PlannerAction planner_action = PA_None; 155 | double const length_before = path.length(); 156 | int num_changes = 0; 157 | 158 | ompl::time::duration const time_limit 159 | = ompl::time::seconds(m_parameters->m_timeLimit); 160 | ompl::time::point const time_before = ompl::time::now(); 161 | ompl::time::point time_current; 162 | 163 | RAVELOG_DEBUG("Running path simplification for %f seconds.\n", 164 | m_parameters->m_timeLimit); 165 | 166 | // start validity checker 167 | m_or_validity_checker->start(); 168 | BOOST_SCOPE_EXIT((m_or_validity_checker)) { 169 | m_or_validity_checker->stop(); 170 | } BOOST_SCOPE_EXIT_END 171 | 172 | do { 173 | // Run one iteration of shortcutting. This gives us fine control over 174 | // the termination condition and allows us to invoke the planner 175 | // callbacks between iterations. 176 | // 177 | // The numeric arguments are the following: 178 | // - maxSteps: maximum number of iterations 179 | // - maxEmptySteps: maximum number of iterations without improvement 180 | // - rangeRatio: maximum connection distance attempted, specified as a 181 | // ratio of the total number of states 182 | // - snapToVertex: ratio of total path length used to snap samples to 183 | // vertices 184 | bool const changed = m_simplifier->shortcutPath(path, 1, 1, 1.0, 0.005); 185 | num_changes += !!changed; 186 | progress._iteration += 1; 187 | 188 | // Call any user-registered callbacks. These functions can terminate 189 | // planning early. 190 | planner_action = _CallCallbacks(progress); 191 | 192 | time_current = ompl::time::now(); 193 | } while (time_current - time_before <= time_limit 194 | && planner_action == PA_None); 195 | 196 | double const length_after = path.length(); 197 | 198 | RAVELOG_DEBUG( 199 | "Ran %d iterations of smoothing over %.3f seconds. %d of %d iterations" 200 | " (%.2f%%) were effective. Reduced path length from %.3f to %.3f.\n", 201 | progress._iteration, 202 | ompl::time::seconds(time_current - time_before), 203 | num_changes, 204 | progress._iteration, 205 | 100 * static_cast(num_changes) / progress._iteration, 206 | length_before, length_after 207 | ); 208 | 209 | // Store the result in the OpenRAVE trajectory. 210 | RAVELOG_DEBUG("Reconstructing OpenRAVE trajectory with %d waypoints.\n", 211 | path.getStateCount()); 212 | BOOST_ASSERT(ptraj); 213 | ptraj->Remove(0, ptraj->GetNumWaypoints()); 214 | 215 | ToORTrajectory(m_robot, path, ptraj); 216 | 217 | if (planner_action == PA_None) { 218 | return PS_HasSolution; 219 | } else { 220 | return PS_InterruptedWithSolution; 221 | } 222 | } 223 | 224 | } // namespace or_ompl 225 | -------------------------------------------------------------------------------- /src/SemiToroidalStateSpace.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2015, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Chris Dellin 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | or_ompl::SemiToroidalStateSpace::SemiToroidalStateSpace(unsigned int dim): 38 | ompl::base::RealVectorStateSpace(dim), 39 | isWrapping_(dim, false) 40 | { 41 | } 42 | 43 | void or_ompl::SemiToroidalStateSpace::setIsWrapping(const std::vector &isWrapping) 44 | { 45 | if (isWrapping_.size() != dimension_) 46 | throw ompl::Exception("IsWrapping does not match dimension of state space: expected dimension " + 47 | boost::lexical_cast(dimension_) + " but got dimension " + 48 | boost::lexical_cast(isWrapping_.size())); 49 | isWrapping_ = isWrapping; 50 | } 51 | 52 | void or_ompl::SemiToroidalStateSpace::addDimension(double minBound, double maxBound, bool isWrapping) 53 | { 54 | ompl::base::RealVectorStateSpace::addDimension(minBound,maxBound); 55 | isWrapping_.push_back(isWrapping); 56 | } 57 | 58 | void or_ompl::SemiToroidalStateSpace::addDimension(const std::string &name, double minBound, double maxBound, bool isWrapping) 59 | { 60 | addDimension(minBound, maxBound, isWrapping); 61 | setDimensionName(dimension_ - 1, name); 62 | } 63 | 64 | double or_ompl::SemiToroidalStateSpace::getMaximumExtent() const 65 | { 66 | double e = 0.0; 67 | for (unsigned int i=0 ; i(state1)->values; 81 | const double *s2 = static_cast(state2)->values; 82 | 83 | for (unsigned int i=0; i 0.5*diam) 91 | diff = diam - diff; 92 | else if (-diff > 0.5*diam) 93 | diff = diam + diff; 94 | } 95 | dist += diff * diff; 96 | } 97 | return sqrt(dist); 98 | } 99 | 100 | bool or_ompl::SemiToroidalStateSpace::equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const 101 | { 102 | const double *s1 = static_cast(state1)->values; 103 | const double *s2 = static_cast(state2)->values; 104 | for (unsigned int i=0; i std::numeric_limits::epsilon() * 2.0) 108 | { 109 | if (!isWrapping_[i]) 110 | return false; 111 | double diam = bounds_.high[i] - bounds_.low[i]; 112 | if (fabs(diff) - diam > std::numeric_limits::epsilon() * 2.0) 113 | return false; 114 | } 115 | } 116 | return true; 117 | } 118 | 119 | void or_ompl::SemiToroidalStateSpace::interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state) const 120 | { 121 | const StateType *rfrom = static_cast(from); 122 | const StateType *rto = static_cast(to); 123 | const StateType *rstate = static_cast(state); 124 | for (unsigned int i=0; i < dimension_ ; ++i) 125 | { 126 | if (!isWrapping_[i]) 127 | { 128 | rstate->values[i] = rfrom->values[i] + (rto->values[i] - rfrom->values[i]) * t; 129 | } 130 | else 131 | { 132 | 133 | double diam = bounds_.high[i] - bounds_.low[i]; 134 | double from = rfrom->values[i]; 135 | double to = rto->values[i]; 136 | double diff = to - from; 137 | if (diff > 0.5*diam) 138 | { 139 | rstate->values[i] = rfrom->values[i] + ((rto->values[i]-diam) - rfrom->values[i]) * t; 140 | if (rstate->values[i] < bounds_.low[i]) 141 | rstate->values[i] += diam; 142 | } 143 | else if (-diff > 0.5*diam) 144 | { 145 | rstate->values[i] = rfrom->values[i] + ((rto->values[i]+diam) - rfrom->values[i]) * t; 146 | if (rstate->values[i] > bounds_.high[i]) 147 | rstate->values[i] -= diam; 148 | } 149 | else 150 | { 151 | rstate->values[i] = rfrom->values[i] + (rto->values[i] - rfrom->values[i]) * t; 152 | } 153 | } 154 | } 155 | } 156 | -------------------------------------------------------------------------------- /src/StateSpaces.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Michael Koval 7 | Matthew Klingensmith 8 | Christopher Dellin 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are 12 | met: 13 | 14 | Redistributions of source code must retain the above copyright 15 | notice, this list of conditions and the following disclaimer. 16 | 17 | Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 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 FOR 24 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | 33 | *************************************************************************/ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | using namespace or_ompl; 43 | namespace ob = ompl::base; 44 | 45 | or_ompl::OrStateValidityChecker::OrStateValidityChecker( 46 | const ompl::base::SpaceInformationPtr &si, 47 | OpenRAVE::RobotBasePtr robot, std::vector const &indices, 48 | bool do_baked): 49 | ompl::base::StateValidityChecker(si), 50 | m_stateSpace(si->getStateSpace().get()), 51 | m_env(robot->GetEnv()), m_robot(robot), m_indices(indices), 52 | m_do_baked(do_baked) 53 | { 54 | if (m_do_baked) 55 | { 56 | m_baked_checker = m_env->GetCollisionChecker(); 57 | std::stringstream sinput("BakeGetType"), soutput; 58 | try 59 | { 60 | if (!m_baked_checker->SendCommand(soutput, sinput)) 61 | throw std::runtime_error("collision checker doesn't support baked checks!"); 62 | } 63 | catch (const OpenRAVE::openrave_exception & exc) 64 | { 65 | throw std::runtime_error("collision checker doesn't support baked checks!"); 66 | } 67 | m_baked_kinbody_type = soutput.str(); 68 | } 69 | 70 | resetStatistics(); 71 | } 72 | 73 | void or_ompl::OrStateValidityChecker::start() { 74 | if (m_do_baked) 75 | { 76 | // start baking 77 | std::stringstream sinput("BakeBegin BakeEnd"), soutput; 78 | m_baked_checker->SendCommand(soutput, sinput); // BakeBegin 79 | m_baked_kinbody = OpenRAVE::RaveCreateKinBody(m_env, m_baked_kinbody_type); 80 | m_env->CheckCollision(m_robot); 81 | m_robot->CheckSelfCollision(); 82 | m_baked_checker->SendCommand(soutput, sinput); // BakeEnd 83 | } 84 | } 85 | 86 | void or_ompl::OrStateValidityChecker::stop() { 87 | m_baked_kinbody.reset(); 88 | } 89 | 90 | bool or_ompl::OrStateValidityChecker::computeFk(const ompl::base::State *state, uint32_t checklimits) const { 91 | std::vector values; 92 | m_stateSpace->copyToReals(values, state); 93 | 94 | BOOST_FOREACH(double v, values) { 95 | if(std::isnan(v)) { 96 | RAVELOG_ERROR("Invalid value in state.\n"); 97 | return false; 98 | } 99 | } 100 | 101 | m_robot->SetDOFValues(values, checklimits, m_indices); 102 | return true; 103 | } 104 | 105 | bool or_ompl::OrStateValidityChecker::isValid(const ompl::base::State *state) const { 106 | boost::chrono::steady_clock::time_point const tic 107 | = boost::chrono::steady_clock::now(); 108 | 109 | bool collided = !computeFk(state, OpenRAVE::KinBody::CLA_Nothing); 110 | 111 | if (!collided) 112 | { 113 | if (m_do_baked) 114 | collided = collided || m_baked_checker->CheckStandaloneSelfCollision(m_baked_kinbody); 115 | else 116 | collided = collided || m_env->CheckCollision(m_robot) || m_robot->CheckSelfCollision(); 117 | 118 | boost::chrono::steady_clock::time_point const toc 119 | = boost::chrono::steady_clock::now(); 120 | m_totalCollisionTime += boost::chrono::duration_cast< 121 | boost::chrono::duration >(toc - tic).count(); 122 | m_numCollisionChecks++; 123 | } 124 | 125 | return !collided; 126 | } 127 | 128 | or_ompl::RealVectorOrStateValidityChecker::RealVectorOrStateValidityChecker( 129 | const ompl::base::SpaceInformationPtr &si, 130 | OpenRAVE::RobotBasePtr robot, std::vector const &indices, 131 | bool do_baked): 132 | or_ompl::OrStateValidityChecker(si,robot,indices,do_baked), 133 | m_num_dof(si->getStateDimension()) { 134 | } 135 | 136 | bool or_ompl::RealVectorOrStateValidityChecker::computeFk(const ompl::base::State *state, uint32_t checklimits) const { 137 | ompl::base::RealVectorStateSpace::StateType const * real_state 138 | = state->as(); 139 | 140 | std::vector values(real_state->values, real_state->values+m_num_dof); 141 | 142 | BOOST_FOREACH(double v, values) { 143 | if(std::isnan(v)) { 144 | RAVELOG_ERROR("Invalid value in state.\n"); 145 | return false; 146 | } 147 | } 148 | 149 | m_robot->SetDOFValues(values, checklimits, m_indices); 150 | return true; 151 | } 152 | -------------------------------------------------------------------------------- /src/TSR.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Jennifer King 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | using namespace or_ompl; 39 | 40 | TSR::TSR() 41 | : _initialized(false) 42 | { 43 | } 44 | 45 | TSR::TSR( 46 | const Eigen::Affine3d &T0_w, 47 | const Eigen::Affine3d &Tw_e, 48 | const Eigen::Matrix &Bw) 49 | : _T0_w(T0_w) 50 | , _Tw_e(Tw_e) 51 | , _Bw(Bw) 52 | , _manipulator_index(-1) 53 | , _relative_body_name("NULL") 54 | , _relative_link_name("") 55 | , _initialized(true) 56 | { 57 | _T0_w_inv = _T0_w.inverse(); 58 | _Tw_e_inv = _Tw_e.inverse(); 59 | } 60 | 61 | bool TSR::deserialize(std::stringstream &ss) 62 | { 63 | return deserialize(static_cast(ss)); 64 | } 65 | 66 | bool TSR::deserialize(std::istream &ss) 67 | { 68 | // Set _initialized to false in case an error occurs. 69 | _initialized = false; 70 | 71 | ss >> _manipulator_index 72 | >> _relative_body_name; 73 | 74 | if(_relative_body_name != "NULL") 75 | ss >> _relative_link_name; 76 | 77 | // Read in the T0_w matrix 78 | for(unsigned int c=0; c < 3; c++) 79 | for(unsigned int r=0; r < 3; r++) 80 | ss >> _T0_w.matrix()(r,c); 81 | 82 | for(unsigned int idx=0; idx < 3; idx++) 83 | ss >> _T0_w.translation()(idx); 84 | 85 | // Read in the Tw_e matrix 86 | for(unsigned int c=0; c < 3; c++) 87 | for(unsigned int r=0; r < 3; r++) 88 | ss >> _Tw_e.matrix()(r,c); 89 | 90 | for(unsigned int idx=0; idx < 3; idx++) 91 | ss >> _Tw_e.translation()(idx); 92 | 93 | // Read in the Bw matrix 94 | for(unsigned int r=0; r < 6; r++) 95 | for(unsigned int c=0; c < 2; c++) 96 | ss >> _Bw(r,c); 97 | 98 | // Check for an error. 99 | if (!ss) 100 | return false; 101 | 102 | _T0_w_inv = _T0_w.inverse(); 103 | _Tw_e_inv = _Tw_e.inverse(); 104 | _initialized = true; 105 | 106 | return true; 107 | } 108 | 109 | void TSR::serialize(std::ostream& ss) 110 | { 111 | if (!_initialized) 112 | throw std::runtime_error("TSR is not initialized."); 113 | 114 | ss << _manipulator_index 115 | << ' ' << _relative_body_name; 116 | 117 | if(_relative_body_name != "NULL") 118 | ss << ' ' << _relative_link_name; 119 | 120 | // T0_w matrix 121 | for(unsigned int c=0; c < 3; c++) 122 | for(unsigned int r=0; r < 3; r++) 123 | ss << ' ' << _T0_w.matrix()(r, c); 124 | 125 | for(unsigned int idx=0; idx < 3; idx++) 126 | ss << ' ' << _T0_w.translation()(idx); 127 | 128 | // Tw_e matrix 129 | for(unsigned int c=0; c < 3; c++) 130 | for(unsigned int r=0; r < 3; r++) 131 | ss << ' ' << _Tw_e.matrix()(r, c); 132 | 133 | for(unsigned int idx=0; idx < 3; idx++) 134 | ss << ' ' << _Tw_e.translation()(idx); 135 | 136 | // Read in the Bw matrix 137 | for(unsigned int r=0; r < 6; r++) 138 | for(unsigned int c=0; c < 2; c++) 139 | ss << ' ' << _Bw(r, c); 140 | } 141 | 142 | Eigen::Matrix TSR::distance(const Eigen::Affine3d &ee_pose) const { 143 | Eigen::Matrix dist = Eigen::Matrix::Zero(); 144 | 145 | // First compute the pose of the w frame in world coordinates, given the ee_pose 146 | Eigen::Affine3d w_in_world = ee_pose * _Tw_e_inv; 147 | 148 | // Next compute the pose of the w frame relative to its original pose (as specified by T0_w) 149 | Eigen::Affine3d w_offset = _T0_w_inv * w_in_world; 150 | 151 | // Now compute the elements of the distance matrix 152 | dist(0,0) = w_offset.translation()(0); 153 | dist(1,0) = w_offset.translation()(1); 154 | dist(2,0) = w_offset.translation()(2); 155 | dist(3,0) = atan2(w_offset.rotation()(2,1), w_offset.rotation()(2,2)); 156 | dist(4,0) = -asin(w_offset.rotation()(2,0)); 157 | dist(5,0) = atan2(w_offset.rotation()(1,0), w_offset.rotation()(0,0)); 158 | 159 | return dist; 160 | } 161 | 162 | Eigen::Matrix TSR::displacement(const Eigen::Affine3d &ee_pose) const { 163 | 164 | Eigen::Matrix dist = distance(ee_pose); 165 | Eigen::Matrix disp = Eigen::Matrix::Zero(); 166 | 167 | for(unsigned int idx=0; idx < 6; idx++){ 168 | if(dist(idx,0) < _Bw(idx,0)){ 169 | disp(idx,0) = dist(idx,0) - _Bw(idx,0); 170 | }else if(dist(idx,0) > _Bw(idx,1)){ 171 | disp(idx,0) = dist(idx,0) - _Bw(idx,1); 172 | } 173 | } 174 | 175 | return disp; 176 | } 177 | 178 | Eigen::Affine3d TSR::sampleDisplacementTransform(void) const { 179 | 180 | // First sample uniformly betwee each of the bounds of Bw 181 | std::vector d_sample(6); 182 | 183 | ompl::RNG rng; 184 | for(unsigned int idx=0; idx < d_sample.size(); idx++){ 185 | if(_Bw(idx,1) > _Bw(idx,0)){ 186 | d_sample[idx] = rng.uniformReal(_Bw(idx,0), _Bw(idx,1)); 187 | } 188 | } 189 | 190 | Eigen::Affine3d return_tf; 191 | return_tf.translation() << d_sample[0], d_sample[1], d_sample[2]; 192 | 193 | // Convert to a transform matrix 194 | double roll = d_sample[3]; 195 | double pitch = d_sample[4]; 196 | double yaw = d_sample[5]; 197 | 198 | double A = cos(yaw); 199 | double B = sin(yaw); 200 | double C = cos(pitch); 201 | double D = sin(pitch); 202 | double E = cos(roll); 203 | double F = sin(roll); 204 | return_tf.linear() << A*C, A*D*F - B*E, B*F + A*D*E, 205 | B*C, A*E + B*D*F, B*D*E - A*F, 206 | -D, C*F, C*E; 207 | 208 | return return_tf; 209 | } 210 | 211 | Eigen::Affine3d TSR::sample() const { 212 | 213 | Eigen::Affine3d tf = sampleDisplacementTransform(); 214 | 215 | return _T0_w * tf * _Tw_e; 216 | } 217 | -------------------------------------------------------------------------------- /src/TSRChain.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Jennifer King 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | using namespace or_ompl; 41 | using OpenRAVE::openrave_exception; 42 | using OpenRAVE::ORE_Failed; 43 | 44 | 45 | TSRChain::TSRChain() 46 | : _initialized(false) 47 | , _sample_start(false) 48 | , _sample_goal(false) 49 | , _constrain(false) 50 | , _tsr_robot() 51 | { 52 | } 53 | 54 | TSRChain::TSRChain(const bool &sample_start, 55 | const bool &sample_goal, 56 | const bool &constrain, 57 | const std::vector &tsrs) 58 | : _initialized(true) 59 | , _sample_start(sample_start) 60 | , _sample_goal(sample_goal) 61 | , _constrain(constrain) 62 | , _tsrs(tsrs) 63 | , _tsr_robot() 64 | { 65 | } 66 | 67 | bool TSRChain::deserialize(std::stringstream &ss) 68 | { 69 | return deserialize(static_cast(ss)); 70 | } 71 | 72 | bool TSRChain::deserialize(std::istream &ss) 73 | { 74 | int num_tsrs; 75 | 76 | ss >> _sample_start 77 | >> _sample_goal 78 | >> _constrain 79 | >> num_tsrs; 80 | 81 | _tsrs.clear(); 82 | 83 | for(int idx = 0; idx < num_tsrs; idx++) 84 | { 85 | TSR::Ptr new_tsr = boost::make_shared(); 86 | 87 | if (!new_tsr->deserialize(ss)){ 88 | RAVELOG_ERROR("Failed deserializing TSR %d of %d in TSRChain.\n", 89 | idx + 1, num_tsrs); 90 | return false; 91 | } 92 | 93 | _tsrs.push_back(new_tsr); 94 | } 95 | 96 | // TODO: Ignored are mmicbody name and mimicbodyjoints 97 | 98 | if (!ss) 99 | { 100 | RAVELOG_ERROR("Failed deserializing TSRChain.\n"); 101 | return false; 102 | } 103 | 104 | return true; 105 | } 106 | 107 | void TSRChain::serialize(std::ostream& ss) 108 | { 109 | ss << _sample_start 110 | << ' ' << _sample_goal 111 | << ' ' << _constrain 112 | << ' ' << _tsrs.size(); 113 | 114 | BOOST_FOREACH(const boost::shared_ptr &tsr_chain, _tsrs){ 115 | ss << ' '; 116 | tsr_chain->serialize(ss); 117 | } 118 | } 119 | 120 | Eigen::Affine3d TSRChain::sample() const { 121 | 122 | Eigen::Affine3d T0_w; 123 | if(_tsrs.size() == 0){ 124 | throw OpenRAVE::openrave_exception( 125 | "There are no TSRs in this TSR Chain.", 126 | OpenRAVE::ORE_InvalidState 127 | ); 128 | } 129 | 130 | T0_w = _tsrs.front()->getOriginTransform(); 131 | BOOST_FOREACH(TSR::Ptr tsr, _tsrs){ 132 | T0_w = T0_w * tsr->sampleDisplacementTransform() * tsr->getEndEffectorOffsetTransform(); 133 | } 134 | 135 | return T0_w; 136 | } 137 | 138 | Eigen::Matrix TSRChain::distance(const Eigen::Affine3d &ee_pose) const { 139 | 140 | if(_tsrs.size() == 1){ 141 | TSR::Ptr tsr = _tsrs.front(); 142 | return tsr->distance(ee_pose); 143 | } 144 | 145 | if(!_tsr_robot){ 146 | throw OpenRAVE::openrave_exception( 147 | "Failed to compute distance to TSRChain. Did you set the" 148 | " environment by calling the setEnv function?", 149 | OpenRAVE::ORE_InvalidState 150 | ); 151 | } 152 | 153 | if(!_tsr_robot->construct()){ 154 | throw OpenRAVE::openrave_exception( 155 | "Failed to robotize TSR.", 156 | OpenRAVE::ORE_Failed 157 | ); 158 | } 159 | 160 | RAVELOG_DEBUG("[TSRChain] Solving IK to compute distance"); 161 | // Compute the ideal pose of the end-effector 162 | Eigen::Affine3d Ttarget = ee_pose * _tsrs.back()->getEndEffectorOffsetTransform().inverse(); 163 | 164 | // Ask the robot to solve ik to find the closest possible end-effector transform 165 | Eigen::Affine3d Tnear = _tsr_robot->findNearestFeasibleTransform(Ttarget); 166 | 167 | // Compute the distance between the two 168 | Eigen::Affine3d offset = Tnear.inverse() * Ttarget; 169 | Eigen::Matrix dist = Eigen::Matrix::Zero(); 170 | dist[0] = offset.translation()[0]; 171 | dist[1] = offset.translation()[1]; 172 | dist[2] = offset.translation()[2]; 173 | dist[3] = atan2(offset.rotation()(2,1), offset.rotation()(2,2)); 174 | dist[4] = -asin(offset.rotation()(2,0)); 175 | dist[5] = atan2(offset.rotation()(1,0), offset.rotation()(0,0)); 176 | 177 | return dist; 178 | } 179 | 180 | void TSRChain::setEnv(const OpenRAVE::EnvironmentBasePtr &penv) 181 | { 182 | _tsr_robot = boost::make_shared(_tsrs, penv); 183 | 184 | } 185 | -------------------------------------------------------------------------------- /src/TSRGoal.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Jennifer King 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | using namespace or_ompl; 41 | namespace ob = ompl::base; 42 | 43 | TSRGoal::TSRGoal(const ob::SpaceInformationPtr &si, 44 | const TSR::Ptr &tsr, 45 | OpenRAVE::RobotBasePtr robot, 46 | OrStateValidityCheckerPtr or_validity_checker) 47 | : ob::GoalSampleableRegion(si), _robot(robot) 48 | , _state_space(si->getStateSpace().get()) 49 | , _or_validity_checker(or_validity_checker) { 50 | 51 | std::vector tsrs(1); 52 | tsrs.push_back(tsr); 53 | TSRChain::Ptr tsrchain = boost::make_shared(true, false, false, tsrs); 54 | _tsr_chains.push_back(tsrchain); 55 | } 56 | 57 | TSRGoal::TSRGoal(const ob::SpaceInformationPtr &si, 58 | const TSRChain::Ptr &tsrchain, 59 | OpenRAVE::RobotBasePtr robot, 60 | OrStateValidityCheckerPtr or_validity_checker) 61 | : ob::GoalSampleableRegion(si), _robot(robot) 62 | , _state_space(si->getStateSpace().get()) 63 | , _or_validity_checker(or_validity_checker) { 64 | 65 | _tsr_chains.push_back(tsrchain); 66 | } 67 | 68 | TSRGoal::TSRGoal(const ob::SpaceInformationPtr &si, 69 | const std::vector &tsrchains, 70 | OpenRAVE::RobotBasePtr robot, 71 | OrStateValidityCheckerPtr or_validity_checker) 72 | : ob::GoalSampleableRegion(si), _tsr_chains(tsrchains), _robot(robot) 73 | , _state_space(si->getStateSpace().get()) 74 | , _or_validity_checker(or_validity_checker) { 75 | } 76 | 77 | TSRGoal::~TSRGoal() { 78 | 79 | } 80 | 81 | bool TSRGoal::isSatisfied(const ompl::base::State *state) const { 82 | 83 | bool satisfied = (distanceGoal(state) == 0.0); 84 | return satisfied; 85 | } 86 | 87 | double TSRGoal::distanceGoal(const ompl::base::State *state) const { 88 | 89 | // Save the state of the robot 90 | OpenRAVE::EnvironmentMutex::scoped_lock lockenv(_robot->GetEnv()->GetMutex()); 91 | OpenRAVE::KinBody::KinBodyStateSaver rsaver(_robot); 92 | 93 | OpenRAVE::RobotBase::ManipulatorPtr active_manip = _robot->GetActiveManipulator(); 94 | 95 | // Put the robot in the pose that is represented in the state 96 | unsigned int check_limits = 0; // The planner does this 97 | _or_validity_checker->computeFk(state, check_limits); 98 | 99 | // Get the end effector transform 100 | OpenRAVE::Transform or_tf = active_manip->GetEndEffectorTransform(); 101 | OpenRAVE::TransformMatrix or_matrix(or_tf); 102 | 103 | // Convert to Eigen 104 | Eigen::Affine3d ee_pose = Eigen::Affine3d::Identity(); 105 | ee_pose.linear() << or_matrix.m[0], or_matrix.m[1], or_matrix.m[2], 106 | or_matrix.m[4], or_matrix.m[5], or_matrix.m[6], 107 | or_matrix.m[8], or_matrix.m[9], or_matrix.m[10]; 108 | ee_pose.translation() << or_matrix.trans.x, or_matrix.trans.y, or_matrix.trans.z; 109 | 110 | // Get distance to TSR 111 | double distance = std::numeric_limits::infinity(); 112 | BOOST_FOREACH(TSRChain::Ptr tsrchain, _tsr_chains){ 113 | Eigen::Matrix ee_distance = tsrchain->distance(ee_pose); 114 | double tdistance = ee_distance.norm(); 115 | if(tdistance < distance){ 116 | distance = tdistance; 117 | } 118 | } 119 | 120 | // Reset the state of the robot 121 | rsaver.Restore(); 122 | 123 | return distance; 124 | } 125 | 126 | void TSRGoal::sampleGoal(ompl::base::State *state) const { 127 | 128 | bool success = false; 129 | 130 | // TODO: Figure out how to bail correctly if an IK isn't found 131 | for(unsigned int count=0; count < 20 && !success; count++){ 132 | // Pick a TSR to sample 133 | int idx = 0; 134 | if(_tsr_chains.size() > 1){ 135 | ompl::RNG rng; 136 | idx = rng.uniformInt(0, _tsr_chains.size()-1); 137 | } 138 | 139 | // Sample the TSR 140 | Eigen::Affine3d ee_pose = _tsr_chains[idx]->sample(); 141 | 142 | // Find an associated IK 143 | OpenRAVE::TransformMatrix or_matrix; 144 | or_matrix.rotfrommat( 145 | ee_pose.matrix()(0, 0), ee_pose.matrix()(0, 1), ee_pose.matrix()(0, 2), 146 | ee_pose.matrix()(1, 0), ee_pose.matrix()(1, 1), ee_pose.matrix()(1, 2), 147 | ee_pose.matrix()(2, 0), ee_pose.matrix()(2, 1), ee_pose.matrix()(2, 2) 148 | ); 149 | or_matrix.trans.x = ee_pose(0, 3); 150 | or_matrix.trans.y = ee_pose(1, 3); 151 | or_matrix.trans.z = ee_pose(2, 3); 152 | 153 | OpenRAVE::IkParameterization ik_param(or_matrix, OpenRAVE::IKP_Transform6D); 154 | std::vector ik_solution; 155 | success = _robot->GetActiveManipulator()->FindIKSolution(ik_param, ik_solution, OpenRAVE::IKFO_CheckEnvCollisions); 156 | 157 | // Set the state 158 | if(success){ 159 | 160 | std::vector arm_indices = _robot->GetActiveManipulator()->GetArmIndices(); 161 | const std::vector & state_indices = _or_validity_checker->getIndices(); 162 | std::vector values(state_indices.size()); 163 | for(unsigned int idx=0; idx < ik_solution.size(); idx++){ 164 | 165 | unsigned int sidx = std::find(state_indices.begin(), 166 | state_indices.end(), 167 | arm_indices[idx]) - state_indices.begin(); 168 | 169 | values[sidx] = ik_solution[idx]; 170 | } 171 | _state_space->copyFromReals(state, values); 172 | } 173 | } 174 | 175 | if(!success){ 176 | RAVELOG_ERROR("[TSRGoal] Failed to sample valid goal.\n"); 177 | const std::vector & state_indices = _or_validity_checker->getIndices(); 178 | std::vector values(state_indices.size(), std::numeric_limits::quiet_NaN()); 179 | _state_space->copyFromReals(state, values); 180 | } 181 | } 182 | 183 | unsigned int TSRGoal::maxSampleCount() const { 184 | 185 | return std::numeric_limits::max(); 186 | } 187 | -------------------------------------------------------------------------------- /src/TSRRobot.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | 3 | Copyright (c) 2014, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Authors: Jennifer King 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are 10 | met: 11 | 12 | Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 15 | Redistributions in binary form must reproduce the above copyright 16 | notice, this list of conditions and the following disclaimer in the 17 | documentation and/or other materials provided with the distribution. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | *************************************************************************/ 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | using namespace or_ompl; 39 | 40 | TSRRobot::TSRRobot(const std::vector &tsrs, const OpenRAVE::EnvironmentBasePtr &penv) 41 | : _tsrs(tsrs), _penv(penv), _initialized(false), _solver("GeneralIK") { 42 | 43 | } 44 | 45 | bool TSRRobot::construct() { 46 | 47 | if(_initialized){ 48 | RAVELOG_ERROR("[TSRRobot] Already initialized. TSRRobot::construct cannot be called twice."); 49 | throw OpenRAVE::openrave_exception( 50 | "TSRRobot::construct cannot be called twice", 51 | OpenRAVE::ORE_Failed 52 | ); 53 | } 54 | 55 | _initialized = false; 56 | 57 | // Create an emtpy robot of the correct type 58 | _probot = RaveCreateRobot(_penv, "GenericRobot"); 59 | if( _probot.get() == NULL ){ 60 | RAVELOG_ERROR("[TSRRobot] Failed to create robot of type GenericRobot"); 61 | return _initialized; 62 | } 63 | 64 | // TODO: mimic body 65 | 66 | // Build the robot 67 | std::vector link_infos; 68 | std::vector joint_infos; 69 | std::vector manip_infos; 70 | std::vector sensor_infos; 71 | 72 | const std::string bodyprefix = "Body"; 73 | int bodynumber = 1; 74 | Eigen::Affine3d Tw0_e = Eigen::Affine3d::Identity(); 75 | 76 | for(unsigned int i=0; i < _tsrs.size(); i++){ 77 | 78 | TSR::Ptr tsr = _tsrs[i]; 79 | Eigen::Matrix Bw = tsr->getBounds(); 80 | 81 | if (tsr->relative_body_name() != "NULL") 82 | { 83 | RAVELOG_ERROR("[TSRRobot] ERROR: TSRs relative to a body is not supported.\n"); 84 | return _initialized; 85 | } 86 | 87 | for(int j=0; j < 6; j++){ 88 | 89 | // Don't add a body if there is no freedom in this dimension 90 | if(Bw(j,0) == 0.0 && Bw(j,1) == 0.0){ 91 | continue; 92 | } 93 | 94 | // If the bounds are equal and non-zero, we should do something reasonable 95 | // For now, this isn't supported 96 | if(Bw(j,0) == Bw(j,1)){ 97 | RAVELOG_ERROR("[TSRRobot] ERROR: TSR Chains are currently unable to deal with cases where two bounds are equal but non-zero, cannot robotize.\n"); 98 | return _initialized; 99 | } 100 | 101 | // Check for axis flip, marked by Bw values being backwards 102 | bool bFlipAxis = false; 103 | if(Bw(j,0) > Bw(j,1)){ 104 | Bw(j,0) = -Bw(j,0); 105 | Bw(j,1) = -Bw(j,1); 106 | 107 | bFlipAxis = true; 108 | } 109 | 110 | // TODO: Handle mimic joints 111 | 112 | // Store joint limits 113 | _lowerlimits.push_back(Bw(j,0)); 114 | _upperlimits.push_back(Bw(j,1)); 115 | 116 | // Create a Link 117 | std::string prev_bodyname = (boost::format("%s%d") % bodyprefix % (bodynumber-1)).str(); 118 | std::string bodyname = (boost::format("%s%d") % bodyprefix % bodynumber).str(); 119 | OpenRAVE::KinBody::LinkInfoPtr link_info 120 | = boost::make_shared(); 121 | link_info->_name = bodyname; 122 | link_info->_t = toOR(Tw0_e); // transform 123 | 124 | 125 | OpenRAVE::KinBody::GeometryInfoPtr geom_info 126 | = boost::make_shared(); 127 | if(j < 3){ 128 | geom_info->_type = OpenRAVE::GT_Box; 129 | }else{ 130 | geom_info->_type = OpenRAVE::GT_Cylinder; 131 | geom_info->_vGeomData = OpenRAVE::Vector(0.03, 0.07, 0.0); //cylinder radius, height, ignored 132 | } 133 | 134 | if(j == 0){ 135 | geom_info->_vGeomData = OpenRAVE::Vector(0.04, 0.02, 0.02); // box extents 136 | }else if(j == 1){ 137 | geom_info->_vGeomData = OpenRAVE::Vector(0.02, 0.04, 0.02); // box extents 138 | }else if(j == 2){ 139 | geom_info->_vGeomData = OpenRAVE::Vector(0.02, 0.02, 0.04); // box extents 140 | }else if(j == 3){ 141 | OpenRAVE::RaveTransformMatrix t 142 | = OpenRAVE::geometry::matrixFromAxisAngle(OpenRAVE::Vector(0, 0, 1), 90.); 143 | geom_info->_t = t; 144 | }else if(j == 4){ 145 | OpenRAVE::RaveTransformMatrix t 146 | = OpenRAVE::geometry::matrixFromAxisAngle(OpenRAVE::Vector(0, 1, 0), 90.); 147 | geom_info->_t = t; 148 | }else if(j == 5){ 149 | OpenRAVE::RaveTransformMatrix t 150 | = OpenRAVE::geometry::matrixFromAxisAngle(OpenRAVE::Vector(1, 0, 0), 90.); 151 | geom_info->_t = t; 152 | } 153 | 154 | geom_info->_vDiffuseColor = OpenRAVE::Vector(0.3, 0.7, 0.3); 155 | link_info->_vgeometryinfos.push_back(geom_info); 156 | link_infos.push_back(link_info); 157 | 158 | // Now create a joint 159 | OpenRAVE::KinBody::JointInfoPtr joint_info 160 | = boost::make_shared(); 161 | std::string joint_name = (boost::format("J%d") % bodynumber).str(); 162 | joint_info->_name = joint_name; 163 | if(j < 3){ 164 | joint_info->_type = OpenRAVE::KinBody::JointSlider; 165 | }else{ 166 | joint_info->_type = OpenRAVE::KinBody::JointHinge; 167 | } 168 | joint_info->_linkname0 = prev_bodyname; 169 | joint_info->_linkname1 = bodyname; 170 | joint_info->_vweights[0] = 1.; 171 | joint_info->_vmaxvel[0] = 1.; 172 | joint_info->_vresolution[0] = 1.; 173 | joint_info->_vlowerlimit[0] = Bw(j,0); 174 | joint_info->_vupperlimit[0] = Bw(j,1); 175 | 176 | joint_info->_vaxes[0] = OpenRAVE::Vector(0., 0., 0.); 177 | unsigned int aidx = (j % 3); 178 | if(j > 3 && bFlipAxis){ 179 | joint_info->_vaxes[0][aidx] = -1.; 180 | }else{ 181 | joint_info->_vaxes[0][aidx] = 1.; 182 | } 183 | joint_infos.push_back(joint_info); 184 | 185 | bodynumber++; 186 | } 187 | 188 | Tw0_e = Tw0_e * tsr->getEndEffectorOffsetTransform(); 189 | } 190 | _num_dof = bodynumber - 1; 191 | 192 | // now add a geometry to the last body with the offset of the last TSR, this will be the target for the manipulator 193 | TSR::Ptr last_tsr = _tsrs.back(); 194 | Tw0_e = last_tsr->getEndEffectorOffsetTransform(); 195 | 196 | OpenRAVE::KinBody::LinkInfoPtr link_info 197 | = boost::make_shared(); 198 | std::string bodyname = (boost::format("%s%d") % bodyprefix % (bodynumber-1)).str(); 199 | link_info->_name = bodyname; 200 | link_info->_bStatic = false; 201 | 202 | OpenRAVE::KinBody::GeometryInfoPtr geom_info 203 | = boost::make_shared(); 204 | geom_info->_t = toOR(Tw0_e); 205 | geom_info->_type = OpenRAVE::GT_Sphere; 206 | geom_info->_vGeomData = OpenRAVE::Vector(0.03, 0., 0.); //radius, ignored, ignored 207 | geom_info->_vDiffuseColor = OpenRAVE::Vector(0.3, 0.7, 0.3); 208 | link_info->_vgeometryinfos.push_back(geom_info); 209 | link_infos.push_back(link_info); 210 | 211 | if(bodynumber > 1){ 212 | _point_tsr = false; 213 | 214 | OpenRAVE::RobotBase::ManipulatorInfoPtr manip_info 215 | = boost::make_shared(); 216 | manip_info->_name = "dummy"; 217 | manip_info->_sBaseLinkName = (boost::format("%s0") % bodyprefix).str(); 218 | manip_info->_sEffectorLinkName = bodyname; 219 | manip_infos.push_back(manip_info); 220 | }else{ 221 | _point_tsr = true; 222 | RAVELOG_DEBUG("[TSRRobot] This is a point TSR, no robotized TSR needed."); 223 | _initialized = true; 224 | return _initialized; 225 | } 226 | 227 | if(_point_tsr && _tsrs.size() != 1){ 228 | RAVELOG_ERROR("[TSRRobot] Can't yet handle case where the TSRChain has no freedom but multiple TSRs, try making it a chain of length 1.\n"); 229 | _initialized = false; 230 | return _initialized; 231 | } 232 | 233 | // If we made it this far, then we can build the robot. 234 | _probot->Init(link_infos, joint_infos, manip_infos, sensor_infos); 235 | 236 | // Set the name properly 237 | std::string robotname = (boost::format("TSRChain%lu") % (unsigned long int) this).str(); 238 | _probot->SetName(robotname); 239 | 240 | // Add it to the environment 241 | _penv->Add(_probot, true); 242 | 243 | // Set the pose 244 | // TODO: mimic joint stuff 245 | _probot->SetTransform(toOR(_tsrs[0]->getOriginTransform())); 246 | 247 | // Create an IK Solver 248 | _ik_solver = OpenRAVE::RaveCreateIkSolver(_penv, _solver); 249 | if(_ik_solver.get() == NULL){ 250 | RAVELOG_ERROR("[TSRRobot] Cannot create IK solver, make sure you have the GeneralIK plugin loadable by OpenRAVE\n"); 251 | _initialized = false; 252 | return _initialized; 253 | } 254 | 255 | // Grab the active manipulator on our newly created robot 256 | OpenRAVE::RobotBase::ManipulatorPtr pmanip = _probot->GetActiveManipulator(); 257 | _ik_solver->Init(pmanip); 258 | 259 | // Finally, disable the robot so we don't do collision checking against it 260 | _probot->Enable(false); 261 | _initialized = true; 262 | 263 | return _initialized; 264 | } 265 | 266 | Eigen::Affine3d TSRRobot::findNearestFeasibleTransform(const Eigen::Affine3d &Ttarget) { 267 | 268 | OpenRAVE::Transform or_target = toOR(Ttarget); 269 | 270 | if(_solver.compare("GeneralIK") != 0){ 271 | RAVELOG_ERROR("[TSRRobot] Only GeneralIK solver supported."); 272 | throw OpenRAVE::openrave_exception( 273 | "Only GeneralIK solver supported.", 274 | OpenRAVE::ORE_Failed 275 | ); 276 | } 277 | 278 | // Setup the free parameters - the format and meaning of these is defined directly by 279 | // the IK solver - in our case GeneralIK 280 | std::vector ikfreeparams; 281 | 282 | ikfreeparams.resize(12); 283 | ikfreeparams[0] = 1; // The number of targets - in this case always 1 284 | ikfreeparams[1] = 0; // The manipulator associated the target - only one manipulator to always 0 285 | 286 | // Pose of target 287 | ikfreeparams[2] = or_target.rot.x; 288 | ikfreeparams[3] = or_target.rot.y; 289 | ikfreeparams[4] = or_target.rot.z; 290 | ikfreeparams[5] = or_target.rot.w; 291 | ikfreeparams[6] = or_target.trans.x; 292 | ikfreeparams[7] = or_target.trans.y; 293 | ikfreeparams[8] = or_target.trans.z; 294 | 295 | 296 | ikfreeparams[9] = 0; // no balancing 297 | ikfreeparams[10] = 0; // junk parameters - mode in previous versions of GeneralIK 298 | ikfreeparams[11] = 0; // not translation only - aka do rotation 299 | 300 | std::vector q0; 301 | boost::shared_ptr > solution 302 | = boost::make_shared >(); 303 | 304 | // solve ik 305 | _ik_solver->Solve(OpenRAVE::IkParameterization(), q0, ikfreeparams, 306 | OpenRAVE::IKFO_IgnoreSelfCollisions, solution); 307 | 308 | // Set the dof values to the solution and grab the end-effector transform in world coordinates 309 | _probot->SetDOFValues(*solution); 310 | Eigen::Affine3d ee_pose = toEigen(_probot->GetActiveManipulator()->GetEndEffectorTransform()); 311 | 312 | // Convert to proper frame 313 | Eigen::Affine3d closest = ee_pose * _tsrs.back()->getEndEffectorOffsetTransform(); 314 | return closest; 315 | } 316 | -------------------------------------------------------------------------------- /tests/test_Planner.py: -------------------------------------------------------------------------------- 1 | #!//bin/env python 2 | from __future__ import print_function 3 | import numpy 4 | import unittest 5 | import openravepy 6 | import os 7 | import subprocess 8 | import sys 9 | 10 | 11 | # Add the models included with OpenRAVE to the OPENRAVE_DATA path. These may 12 | # not be available if the user manually set the OPENRAVE_DATA environmental 13 | # variable, e.g. through openrave_catkin. 14 | try: 15 | share_path = subprocess.check_output(['openrave-config', '--share-dir']).strip() 16 | os.environ['OPENRAVE_DATA'] = os.path.join(share_path, 'data') 17 | except subprocess.CalledProcessError as e: 18 | print('error: Failed using "openrave-config" to find the default' 19 | ' OPENRAVE_DATA path. Loading assets may fail.', 20 | file=sys.stderr) 21 | 22 | # Initialize OpenRAVE. 23 | openravepy.RaveInitialize(True) 24 | openravepy.misc.InitOpenRAVELogging() 25 | openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Debug) 26 | 27 | 28 | class PlannerTestsMeta(type): 29 | PLANNER_NAMES = [ 30 | "BKPIECE1", 31 | "EST", 32 | "KPIECE1", 33 | "LazyRRT", 34 | "LBKPIECE1", 35 | "PRM", 36 | "LazyPRM", 37 | "PRMstar", 38 | "RRT", 39 | "RRTConnect", 40 | "SBL", 41 | #"PDST", # often fails to find a solution 42 | #"RRTstar", # often fails to find a solution 43 | #"SPARS", # often fails to find a solution 44 | #"SPARStwo", # often fails to find a solution 45 | #"TRRT", # often fails to find a solution 46 | #"pRRT", # immediately SEGFAULTs 47 | #"pSBL", # immediately throws an InvalidState exception 48 | ] 49 | 50 | 51 | def __init__(self, names, bases, attrs): 52 | super(PlannerTestsMeta, self).__init__(names, bases, attrs) 53 | 54 | 55 | def __new__(cls, name, bases, attrs): 56 | # Create a test for each planner in PLANNER_NAMES. 57 | for planner_name in cls.PLANNER_NAMES: 58 | func = cls.create_test(planner_name) 59 | attrs[func.__name__] = func 60 | 61 | return super(PlannerTestsMeta, cls).__new__(cls, name, bases, attrs) 62 | 63 | 64 | @classmethod 65 | def create_test(self, planner_name): 66 | def func(self): 67 | return self.run_planner(planner_name) 68 | 69 | func.__name__ = 'test_' + planner_name 70 | return func 71 | 72 | 73 | class PlannerTests(unittest.TestCase): 74 | __metaclass__ = PlannerTestsMeta 75 | 76 | NUM_ATTEMPTS = 5 77 | START_CONFIG = numpy.array([ 78 | 0.80487864, 0.42326865, -0.54016693, 2.28895761, 79 | -0.34930645, -1.19702164, 1.95971213 ]) 80 | GOAL_CONFIG = numpy.array([ 81 | 2.41349473, -1.43062044, -2.69016693, 2.12681216, 82 | -0.75643783, -1.52392537, 1.01239878 ]) 83 | TRAJECTORY_XML = """\ 84 | 85 | 86 | 87 | 88 | 89 | 0.80487864 0.42326865 -0.5401669299999999 2.28895761 -0.3493064500000005 -1.19702164 1.95971213 -0.3668195289171482 -0.1719619318499316 0.340737234496427 2.338352935126714 -1.140328948362108 -0.4582953771266394 0.1660684228974656 -0.4316746373996911 -0.1254090482184572 1.337385046499522 0.7087144047880871 -1.48802896774604 0.4679460445583862 -1.337254061021721 0.624583530772981 1.117893213994084 2.633833996473851 0.05391426760723994 -1.669197857527277 0.9002033682607622 -2.714157270255535 0.6372202491284563 1.09989072774182 2.596225701405203 0.06855704409449326 -1.662750197409222 0.8830795265988333 -2.687833191529146 1.229311742752304 0.2563870051612129 0.8340948242701349 0.7546420827296624 -1.360646074939481 0.08074456106588879 -1.454422534352764 1.821403236376152 -0.5871167174193935 -0.9280360528649325 1.440727121364831 -1.058541952469741 -0.7215904044670555 -0.2210118771763818 2.41349473 -1.43062044 -2.69016693 2.12681216 -0.75643783 -1.52392537 1.01239878 90 | """ 91 | 92 | 93 | def setUp(self): 94 | self.env = openravepy.Environment() 95 | self.env.Load('wamtest1.env.xml') 96 | self.robot = self.env.GetRobot('BarrettWAM') 97 | self.manipulator = self.robot.GetManipulator('arm') 98 | 99 | with self.env: 100 | self.robot.SetActiveDOFs(self.manipulator.GetArmIndices()) 101 | self.robot.SetActiveDOFValues(self.START_CONFIG) 102 | self.robot.SetActiveManipulator(self.manipulator) 103 | 104 | 105 | def tearDown(self): 106 | self.env.Destroy() 107 | 108 | 109 | def run_planner(self, planner_name): 110 | with self.env: 111 | # Setup 112 | params = openravepy.Planner.PlannerParameters() 113 | params.SetRobotActiveJoints(self.robot) 114 | params.SetGoalConfig(self.GOAL_CONFIG) 115 | params.SetExtraParameters('60') 116 | 117 | cspec = self.robot.GetActiveConfigurationSpecification() 118 | 119 | traj = openravepy.RaveCreateTrajectory(self.env, '') 120 | 121 | for i in xrange(self.NUM_ATTEMPTS): 122 | # Act 123 | planner = openravepy.RaveCreatePlanner( 124 | self.env, 'OMPL_' + planner_name) 125 | planner.InitPlan(self.robot, params) 126 | result = planner.PlanPath(traj) 127 | 128 | if result == openravepy.PlannerStatus.HasSolution: 129 | break 130 | 131 | # Assert 132 | self.assertEqual(result, openravepy.PlannerStatus.HasSolution) 133 | self.assertGreaterEqual(traj.GetNumWaypoints(), 1) 134 | numpy.testing.assert_array_almost_equal( 135 | traj.GetWaypoint(0, cspec), 136 | self.START_CONFIG) 137 | numpy.testing.assert_array_almost_equal( 138 | traj.GetWaypoint(traj.GetNumWaypoints() - 1, cspec), 139 | self.GOAL_CONFIG) 140 | 141 | 142 | def test_Simplifier(self): 143 | with self.env: 144 | # Setup 145 | simplifier = openravepy.RaveCreatePlanner(self.env, 'OMPL_Simplifier') 146 | params = openravepy.Planner.PlannerParameters() 147 | 148 | cspec = self.robot.GetActiveConfigurationSpecification() 149 | 150 | traj = openravepy.RaveCreateTrajectory(self.env, '') 151 | traj.deserialize(self.TRAJECTORY_XML) 152 | 153 | # Act 154 | simplifier.InitPlan(self.robot, params) 155 | result = simplifier.PlanPath(traj) 156 | 157 | # Assert 158 | self.assertEqual(result, openravepy.PlannerStatus.HasSolution) 159 | self.assertGreaterEqual(traj.GetNumWaypoints(), 1) 160 | numpy.testing.assert_array_almost_equal( 161 | traj.GetWaypoint(0, cspec), 162 | self.START_CONFIG) 163 | numpy.testing.assert_array_almost_equal( 164 | traj.GetWaypoint(traj.GetNumWaypoints() - 1, cspec), 165 | self.GOAL_CONFIG) 166 | 167 | 168 | if __name__ == '__main__': 169 | unittest.main() 170 | --------------------------------------------------------------------------------