diff --git a/.clang-format b/.clang-format index bef3326796..9a6ec509a4 100644 --- a/.clang-format +++ b/.clang-format @@ -1,66 +1,75 @@ --- BasedOnStyle: Google +ColumnLimit: 120 +MaxEmptyLinesToKeep: 1 +SortIncludes: false + +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never AccessModifierOffset: -2 ConstructorInitializerIndentWidth: 2 +NamespaceIndentation: None +ContinuationIndentWidth: 4 +IndentCaseLabels: true +IndentFunctionDeclarationAfterType: false + AlignEscapedNewlinesLeft: false AlignTrailingComments: true + AllowAllParametersOfDeclarationOnNextLine: false +ExperimentalAutoDetectBinPacking: false +ObjCSpaceBeforeProtocolList: true +Cpp11BracedListStyle: false + +AllowShortBlocksOnASingleLine: true AllowShortIfStatementsOnASingleLine: false AllowShortLoopsOnASingleLine: false AllowShortFunctionsOnASingleLine: None -AllowShortLoopsOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false + AlwaysBreakTemplateDeclarations: true AlwaysBreakBeforeMultilineStrings: false BreakBeforeBinaryOperators: false BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true + BinPackParameters: true -ColumnLimit: 120 ConstructorInitializerAllOnOneLineOrOnePerLine: true DerivePointerBinding: false PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 + +PenaltyExcessCharacter: 50 +PenaltyBreakBeforeFirstCallParameter: 30 +PenaltyBreakComment: 1000 +PenaltyBreakFirstLessLess: 10 PenaltyBreakString: 100 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 70 +PenaltyReturnTypeOnItsOwnLine: 50 + SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false SpacesInParentheses: false SpacesInAngles: false SpaceInEmptyParentheses: false SpacesInCStyleCastParentheses: false +SpaceAfterCStyleCast: false SpaceAfterControlStatementKeyword: true SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false # Configure each individual brace in BraceWrapping BreakBeforeBraces: Custom # Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} +BraceWrapping: + AfterCaseLabel: true + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false ... diff --git a/.docker/README.md b/.docker/README.md index a0cf726f20..d250ee12a1 100644 --- a/.docker/README.md +++ b/.docker/README.md @@ -1,3 +1,4 @@ -# MoveIt! Docker Containers +# MoveIt Docker Containers -For more information see [Continuous Integration and Docker](http://moveit.ros.org/documentation/contributing/continuous_integration.html) documentation. + +For more information see the pages [Continuous Integration and Docker](http://moveit.ros.org/documentation/contributing/continuous_integration.html) and [Using Docker Containers with MoveIt](https://moveit.ros.org/install/docker/). diff --git a/.docker/ci-shadow-fixed/Dockerfile b/.docker/ci-shadow-fixed/Dockerfile index fb053bb6c0..8b824de580 100644 --- a/.docker/ci-shadow-fixed/Dockerfile +++ b/.docker/ci-shadow-fixed/Dockerfile @@ -1,7 +1,7 @@ -# moveit/moveit:master-ci-shadow-fixed +# moveit/moveit:noetic-ci-shadow-fixed # Sets up a base image to use for running Continuous Integration on Travis -FROM moveit/moveit:master-ci +FROM moveit/moveit:noetic-ci MAINTAINER Dave Coleman dave@picknik.ai # Switch to ros-shadow-fixed diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index dd4c77c91f..5ce021548f 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -1,7 +1,7 @@ -# moveit/moveit:master-ci +# moveit/moveit:noetic-ci # Sets up a base image to use for running Continuous Integration on Travis -FROM ros:melodic-ros-base +FROM ros:noetic-ros-base MAINTAINER Dave Coleman dave@picknik.ai ENV TERM xterm @@ -12,35 +12,28 @@ WORKDIR /root/ws_moveit # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers RUN \ - mkdir src && \ - cd src && \ - # - # Download moveit source, so that we can get necessary dependencies - wstool init --shallow . https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall && \ - # # Update apt package list as previous containers clear the cache apt-get -qq update && \ apt-get -qq dist-upgrade && \ # # Install some base dependencies - apt-get -qq install -y \ - # Some source builds require a package.xml be downloaded via wget from an external location - wget \ - # Required for rosdep command - sudo \ + apt-get -qq install --no-install-recommends -y \ + # Some basic requirements + wget git sudo \ # Preferred build tools - python-catkin-tools \ - clang clang-format-3.9 clang-tidy clang-tools \ + python3-catkin-tools python3-osrf-pycommon \ + clang clang-format-10 clang-tidy clang-tools \ ccache && \ # - # Download all dependencies of MoveIt! - rosdep update && \ - rosdep install -y --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \ + # Download MoveIt source, so that we can fetch all necessary dependencies + wstool init --shallow src https://raw.githubusercontent.com/ros-planning/moveit/${ROS_DISTRO}-devel/moveit.rosinstall && \ # + # Download all dependencies of MoveIt + rosdep update && \ + DEBIAN_FRONTEND=noninteractive \ + rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \ # Remove the source code from this container - # TODO: in the future we may want to keep this here for further optimization of later containers - cd .. && \ - rm -rf src/ && \ + rm -rf src && \ # # Clear apt-cache to reduce image size rm -rf /var/lib/apt/lists/* diff --git a/.docker/gui-docker b/.docker/gui-docker index 2688c52cf3..871b04b0f6 100755 --- a/.docker/gui-docker +++ b/.docker/gui-docker @@ -1,11 +1,16 @@ #!/bin/bash -u # This script is used to run a docker container with graphics support. -# http://wiki.ros.org/docker/Tutorials/Hardware%20Acceleration +# All arguments to this script except "-c " will be appended to a docker run command. +# If a container name is specified, and this container already exists, the container is re-entered, +# which easily allows entering the same persistent container from multiple terminals. +# See documentation for detailed examples: https://moveit.ros.org/install/docker/ -# All arguments to this script will be appended to a docker run command. -# Example command line: -# ./gui-docker -it --rm moveit/moveit:master-source /bin/bash +# Example commands: +# ./gui-docker --rm -it moveit/moveit:master-source /bin/bash # Run a (randomly named) container that is removed on exit +# ./gui-docker -v ~/ros_ws:/root/ros_ws --rm -it moveit/moveit:master-source /bin/bash # Same, but also link host volume ~/ros_ws to /root/ros_ws in the container +# ./gui-docker -c container_name # Start (or continue) an interactive bash in a moveit/moveit:master-source container +# ./gui-docker # Same, but use the default container name "default_moveit_container" function check_nvidia2() { # If we don't have an NVIDIA graphics card, bail out @@ -78,8 +83,8 @@ function count_positional_args() { } if [ $# -eq 0 ] ; then - # If not options are specified at all, just run the default_container - CONTAINER_NAME=default_container + # If no options are specified at all, use the name "default_moveit_container" + CONTAINER_NAME=default_moveit_container else # Check for option -c or --container in first position case "$1" in @@ -91,7 +96,7 @@ else shift fi # Set default container name if still undefined - CONTAINER_NAME="${CONTAINER_NAME:-default_container}" + CONTAINER_NAME="${CONTAINER_NAME:-default_moveit_container}" ;; esac fi diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile index e06998b1a2..79d673f1b6 100644 --- a/.docker/release/Dockerfile +++ b/.docker/release/Dockerfile @@ -1,7 +1,7 @@ # moveit/moveit:melodic-release -# Full debian-based install of MoveIt! using apt-get +# Full debian-based install of MoveIt using apt-get -FROM ros:melodic-ros-base +FROM ros:noetic-ros-base MAINTAINER Dave Coleman dave@picknik.ai # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile index 1021b4db18..4328ec4ded 100644 --- a/.docker/source/Dockerfile +++ b/.docker/source/Dockerfile @@ -1,7 +1,7 @@ -# moveit/moveit:master-source +# moveit/moveit:noetic-source # Downloads the moveit source code and install remaining debian dependencies -FROM moveit/moveit:master-ci-shadow-fixed +FROM moveit/moveit:noetic-ci-shadow-fixed MAINTAINER Dave Coleman dave@picknik.ai ENV ROS_UNDERLAY /root/ws_moveit/install @@ -11,7 +11,7 @@ WORKDIR $ROS_UNDERLAY/../src # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers RUN \ # Download moveit source so that we can get necessary dependencies - wstool init . https://raw.githubusercontent.com/ros-planning/moveit/master/moveit.rosinstall && \ + wstool init . https://raw.githubusercontent.com/ros-planning/moveit/${ROS_DISTRO}-devel/moveit.rosinstall && \ # # Update apt package list as cache is cleared in previous container # Usually upgrading involves a few packages only (if container builds became out-of-sync) @@ -24,7 +24,11 @@ RUN \ ENV PYTHONIOENCODING UTF-8 RUN cd .. && \ - catkin config --extend /opt/ros/$ROS_DISTRO --install --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + catkin config --extend /opt/ros/$ROS_DISTRO --install --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON && \ # Status rate is limited so that just enough info is shown to keep Docker from timing out, # but not too much such that the Docker log gets too long (another form of timeout) catkin build --limit-status-rate 0.001 --no-notify + +# Environment variable used in instructions on moveit.ros.org website for running clang-tidy +ENV CATKIN_WS /root/ws_moveit + diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index a6d58c6912..6175b31cf6 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,4 +1,4 @@ -# MoveIt! is a large project with pull requests being created often. In order +# MoveIt is a large project with pull requests being created often. In order # to ensure quick review turn around time from our maintainer team, we're # leveraging an automated "triage" approach to auto-assign reviews to new pull # requests. If you already know who should review your PR, then you can assign @@ -32,31 +32,31 @@ # FOLDER-SPECIFIC REVIEWERS: -/moveit_plugins/moveit_ros_control_interface/ @ipa-mdl @bmagyar -/moveit_plugins/moveit_fake_controller_manager/ @v4hn @rhaschke -/moveit_plugins/moveit_simple_controller_manager/ @mikeferguson @v4hn -/moveit_plugins/moveit_controller_manager_example/ @v4hn +/moveit_plugins/moveit_ros_control_interface/ @ipa-mdl @bmagyar +/moveit_plugins/moveit_fake_controller_manager/ @v4hn @rhaschke +/moveit_plugins/moveit_simple_controller_manager/ @mikeferguson @v4hn +/moveit_plugins/moveit_controller_manager_example/ @v4hn -/moveit_core/background_processing/ @mlautman -/moveit_core/backtrace/ @mlautman -/moveit_core/collision_detection/ @BryceStevenWilley -/moveit_core/collision_detection_fcl/ @rhaschke -/moveit_core/collision_distance_field/ @rhaschke +/moveit_core/background_processing/ @tylerjw +/moveit_core/backtrace/ @tylerjw +/moveit_core/collision_detection/ @j-petit +/moveit_core/collision_detection_fcl/ @j-petit +/moveit_core/collision_distance_field/ @j-petit /moveit_core/constraint_samplers/ @v4hn /moveit_core/controller_manager/ @v4hn /moveit_core/distance_field/ @v4hn -/moveit_core/dynamics_solver/ @mlautman -/moveit_core/exceptions/ @mlautman +/moveit_core/dynamics_solver/ @tylerjw +/moveit_core/exceptions/ @tylerjw /moveit_core/kinematic_constraints/ @rhaschke /moveit_core/kinematics_base/ @rhaschke @mlautman /moveit_core/kinematics_metrics/ @gavanderhoorn -/moveit_core/macros/ @mlautman -/moveit_core/planning_interface/ @rhaschke +/moveit_core/macros/ @tylerjw +/moveit_core/planning_interface/ @rhaschke @felixvd /moveit_core/planning_request_adapter/ @rhaschke -/moveit_core/planning_scene/ @rhaschke -/moveit_core/profiler/ @mlautman -/moveit_core/robot_model/ @jonbinney -/moveit_core/robot_state/ @rhaschke @mlautman +/moveit_core/planning_scene/ @rhaschke @felixvd +/moveit_core/profiler/ @tylerjw +/moveit_core/robot_model/ @tylerjw +/moveit_core/robot_state/ @rhaschke @mlautman @felixvd /moveit_core/robot_trajectory/ @mlautman /moveit_core/sensor_manager/ @mlautman /moveit_core/trajectory_processing/ @mlautman @@ -66,36 +66,39 @@ /moveit_commander/ @rhaschke @willcbaker -/moveit/ @130s +/moveit/ @130s -/moveit_kinematics/ @rhaschke @gavanderhoorn @jrgnicho +/moveit_kinematics/ @rhaschke @gavanderhoorn @jrgnicho -/moveit_experimental/ @AndyZe +/moveit_experimental/ @AndyZe -/moveit_ros/perception/ @mikeferguson @jonbinney -/moveit_ros/manipulation/ @v4hn @felixvd -/moveit_ros/benchmarks/ @henningkayser @MohmadAyman -/moveit_ros/planning_interface/ @mintar @rhaschke -/moveit_ros/robot_interaction/ @mikeferguson @rhaschke -/moveit_ros/warehouse/ @mikeferguson @dg-shadow -/moveit_ros/move_group/ @rhaschke @IanTheEngineer -/moveit_ros/visualization/ @rhaschke @jonbinney @christian-rauch +/moveit_ros/perception/ @jliukkonen @RoboticsYY +/moveit_ros/manipulation/ @v4hn @felixvd +/moveit_ros/benchmarks/ @henningkayser @MohmadAyman +/moveit_ros/planning_interface/ @mintar @rhaschke @felixvd +/moveit_ros/robot_interaction/ @mikeferguson @rhaschke +/moveit_ros/warehouse/ @mikeferguson @dg-shadow +/moveit_ros/move_group/ @rhaschke @IanTheEngineer +/moveit_ros/visualization/ @rhaschke @tylerjw @RoboticsYY @felixvd -/moveit_ros/planning/collision_plugin_loader/ @rhaschke -/moveit_ros/planning/constraint_sampler_manager_loader/ @henningkayser +/moveit_ros/planning/collision_plugin_loader/ @j-petit +/moveit_ros/planning/constraint_sampler_manager_loader/ @nbbrooks /moveit_ros/planning/kinematics_plugin_loader/ @gavanderhoorn /moveit_ros/planning/plan_execution/ @v4hn /moveit_ros/planning/planning_components_tools/ @henningkayser /moveit_ros/planning/planning_pipeline/ @v4hn /moveit_ros/planning/planning_request_adapter_plugins/ @v4hn /moveit_ros/planning/planning_scene_monitor/ @rhaschke -/moveit_ros/planning/rdf_loader/ @henningkayser -/moveit_ros/planning/robot_model_loader/ @henningkayser +/moveit_ros/planning/rdf_loader/ @nbbrooks +/moveit_ros/planning/robot_model_loader/ @nbbrooks /moveit_ros/planning/trajectory_execution_manager/ @rhaschke -/moveit_setup_assistant/ @davetcoleman @rhaschke @MohmadAyman +/moveit_setup_assistant/ @RoboticsYY @rhaschke @MohmadAyman -/moveit_planners/ompl/ @BryceStevenWilley @zkingston +/moveit_planners/ompl/ @BryceStevenWilley @mamoll /moveit_planners/chomp/chomp_interface/ @raghavendersahdev @knorth55 @bmagyar -/moveit_planners/chomp/chomp_optimizer_adapter/ @raghavendersahdev @knorth55 @bmagyar -/moveit_planners/chomp/chomp_motion_planner/ @raghavendersahdev @knorth55 @bmagyar +/moveit_planners/chomp/chomp_optimizer_adapter/ @raghavendersahdev @knorth55 @bmagyar +/moveit_planners/chomp/chomp_motion_planner/ @raghavendersahdev @knorth55 @bmagyar +/moveit_planners/trajopt @ommmid @mamoll +/moveit_planners/pilz_industrial_motion_planner @jschleicher @ct2034 +/moveit_planners/pilz_industrial_motion_planner_testutils/ @jschleicher @ct2034 diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE/bug_report.md similarity index 70% rename from .github/ISSUE_TEMPLATE.md rename to .github/ISSUE_TEMPLATE/bug_report.md index 62f551be4a..6806c4801e 100644 --- a/.github/ISSUE_TEMPLATE.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -1,13 +1,22 @@ +--- +name: Bug report +about: Create a report to help us improve +title: '' +labels: bug +assignees: '' + +--- + ### Description Overview of your issue here. ### Your environment -* ROS Distro: [Indigo|Jade|Kinetic] -* OS Version: e.g. Ubuntu 16.04 +* ROS Distro: [Kinetic|Melodic|Noetic] +* OS Version: e.g. Ubuntu 18.04 * Source or Binary build? * If binary, which release version? -* If source, which git commit or tag? +* If source, which branch? ### Steps to reproduce Tell us how to reproduce this issue. Attempt to provide a working demo, perhaps using Docker. diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 0000000000..11fc491ef1 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,20 @@ +--- +name: Feature request +about: Suggest an idea for this project +title: '' +labels: enhancement +assignees: '' + +--- + +**Is your feature request related to a problem? Please describe.** +A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] + +**Describe the solution you'd like** +A clear and concise description of what you want to happen. + +**Describe alternatives you've considered** +A clear and concise description of any alternative solutions or features you've considered. + +**Additional context** +Add any other context or screenshots about the feature request here. diff --git a/.github/ISSUE_TEMPLATE/first_timer_only.md b/.github/ISSUE_TEMPLATE/first_timer_only.md new file mode 100644 index 0000000000..57cdf54dfa --- /dev/null +++ b/.github/ISSUE_TEMPLATE/first_timer_only.md @@ -0,0 +1,67 @@ +--- +name: First Timer Only Issue +about: Create an issue to welcome a new contributor into the community. +title: '' +labels: first-timer-only +assignees: '' + +--- + +## Background + +Overview of your issue here. + +## Instructions +Hi, this is a `first-timer-only` issue. This means we've worked to make it more legible to people who either **haven't contributed to our codebase before, or even folks who haven't contributed to open source before**. + +We're interested in helping you take the first step, and can answer questions and help you out along the way. Note that we're especially interested in contributions from underrepresented groups! + +We know that creating a pull request is the biggest barrier for new contributors. This issue is for you πŸ’ + +If you have contributed before, **consider leaving this PR for someone new**, and looking through our general [bug](https://github.com/ros-planning/moveit/labels/bug) issues. Thanks! + +### πŸ€” What you will need to know. + +Nothing. This issue is meant to welcome you to Open Source :) We are happy to walk you through the process. + +### πŸ“‹ Step by Step + +- [ ] πŸ™‹ **Claim this issue**: Comment below. If someone else has claimed it, ask if they've opened a pull request already and if they're stuck -- maybe you can help them solve a problem or move it along! + +- [ ] πŸ—„οΈ **Create a local workspace** for making your changes and testing [following these instructions](https://moveit.ros.org/install/source/) + +- [ ] 🍴 **Fork the repository** using the handy button at the top of the repository page and **clone** it into `~/ws_moveit/src/moveit`, [here is a guide that you can follow](https://guides.github.com/activities/forking/) (You will have to remove or empty the existing `moveit` folder before cloning your own fork) + +- [ ] πŸ“ **Update** the file(s) `$FILENAME`by applying the change shown in the diff below. + +```diff +$DIFF +``` + +- [ ] πŸ€– **Apply `clang-format-10`** auto formatting, [following these instructions](https://moveit.ros.org//documentation/contributing/code/?#clang-format-auto-code-formatting) + +- [ ] πŸ’Ύ **Commit and Push** your changes + +- [ ] πŸ”€ **Start a Pull Request** to request to merge your code into `master`. There are two ways that you can start a pull request: +1. If you are not familiar with GitHub or how to create a pull request, [here is a guide you can follow](https://guides.github.com/activities/hello-world/) on how GitHub works. +2. If you are familiar with the terminal or would like to learn to use it, [here is a great tutorial](https://egghead.io/series/how-to-contribute-to-an-open-source-project-on-github) on how to send a pull request using the terminal. + +- [ ] 🏁 **Done** Ask in comments for a review :) + +### Is someone else already working on this? + +πŸ”—- We encourage contributors to link to the original issue in their pull request so all users can easily see if someone's already started on it. + +πŸ‘₯- **If someone seems stuck, offer them some help!** + +### πŸ€”β“ Questions? + +Don’t hesitate to ask questions or to get help if you feel like you are getting stuck. For example leave a comment below! +Furthermore, you find helpful resources here: +* [MoveIt FAQ](https://moveit.ros.org/documentation/faqs/) +* [MoveIt Tutorials](https://ros-planning.github.io/moveit_tutorials/) +* [MoveIt contribution guide](https://moveit.ros.org/documentation/contributing/) +* [ROS Tutorials](https://wiki.ros.org/ROS/Tutorials) +* [ROS Answers](https://answers.ros.org/questions/) + +**Good luck with your first issue!** diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 81764f54b6..fbd1d34b45 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -4,11 +4,10 @@ Please explain the changes you made, including a reference to the related issue ### Checklist - [ ] **Required by CI**: Code is auto formatted using [clang-format](http://moveit.ros.org/documentation/contributing/code) -- [ ] Extended the tutorials / documentation, if necessary [reference](http://moveit.ros.org/documentation/contributing/) +- [ ] Extend the tutorials / documentation [reference](http://moveit.ros.org/documentation/contributing/) +- [ ] Document API changes relevant to the user in the [MIGRATION.md](https://github.com/ros-planning/moveit/blob/master/MIGRATION.md) notes +- [ ] Create tests, which fail without this PR [reference](https://ros-planning.github.io/moveit_tutorials/doc/tests/tests_tutorial.html) - [ ] Include a screenshot if changing a GUI -- [ ] Document API changes relevant to the user in the moveit/MIGRATION.md notes -- [ ] Created tests, which fail without this PR [reference](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/tests.html) -- [ ] Decide if this should be cherry-picked to other current ROS branches -- [ ] While waiting for someone to review your request, please consider reviewing [another open pull request](https://github.com/ros-planning/moveit/pulls) to support the maintainers +- [ ] While waiting for someone to review your request, please help review [another open pull request](https://github.com/ros-planning/moveit/pulls) to support the maintainers [//]: # "You can expect a response from a maintainer within 7 days. If you haven't heard anything by then, feel free to ping the thread. Thank you!" diff --git a/.github/config.yml b/.github/config.yml index fed71ab355..3dd3ae1f8c 100644 --- a/.github/config.yml +++ b/.github/config.yml @@ -4,19 +4,18 @@ # Comment to be posted to on first time issues newIssueWelcomeComment: > - Thanks for reporting an issue. We will have a look asap. - If you can think of a fix, please consider providing it as a pull request. + Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed. # Configuration for new-pr-welcome - https://github.com/behaviorbot/new-pr-welcome # Comment to be posted to on PRs from first time contributors in your repository newPRWelcomeComment: > - Thanks for helping in improving MoveIt! + Thanks for helping in improving MoveIt and open source robotics! # Configuration for first-pr-merge - https://github.com/behaviorbot/first-pr-merge # Comment to be posted to on pull requests merged by a first time user firstPRMergeComment: > - Congrats on getting your first MoveIt! pull request merged and improving open source robotics! + Congrats on getting your first MoveIt pull request merged and improving open source robotics! # It is recommended to include as many gifs and emojis as possible! diff --git a/.gitignore b/.gitignore index 9877f9248d..808fe85341 100644 --- a/.gitignore +++ b/.gitignore @@ -51,10 +51,10 @@ qtcreator-* # Vim *.swp -# Catkin custom files -CATKIN_IGNORE - # Continous Integration .moveit_ci *.pyc + +#gdb +*.gdb diff --git a/.travis.yml b/.travis.yml index f37cd1bfb5..ac6d1d5f18 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,6 +1,6 @@ # This config file for Travis CI utilizes https://github.com/ros-planning/moveit_ci/ package. -sudo: required -dist: xenial # distro used by Travis, moveit_ci uses the docker image's distro +os: linux +dist: bionic # distro used by Travis, moveit_ci uses the docker image's distro services: - docker language: cpp @@ -8,32 +8,33 @@ cache: ccache compiler: gcc notifications: - email: - recipients: - - 130s@2000.jukuin.keio.ac.jp + email: true + env: global: - MOVEIT_CI_TRAVIS_TIMEOUT=85 # Travis grants us 90 min, but we add a safety margin of 5 min - - ROS_DISTRO=melodic - - ROS_REPO=ros + - ROS_DISTRO=noetic + - ROS_REPO=ros-testing - UPSTREAM_WORKSPACE=moveit.rosinstall - # moveit_ros_perception: mesh_filter_test fails due to broken Mesa OpenGL - - TEST_BLACKLIST="moveit_ros_perception" - - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-unused-function" + - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" - WARNINGS_OK=false - matrix: - - TEST="clang-format catkin_lint" - - ROS_DISTRO=melodic - - ROS_DISTRO=kinetic -matrix: # Add a separate config to the matrix, using clang as compiler +jobs: + fast_finish: true include: - # add a config to the matrix using clang as compiler and also test ikfast plugin creation + - env: TEST="clang-format catkin_lint" + - env: TEST=code-coverage + - env: ROS_DISTRO=noetic ROS_REPO=ros-testing + - env: ROS_DISTRO=melodic BEFORE_DOCKER_SCRIPT="source moveit_kinematics/test/test_ikfast_plugins.sh" - compiler: clang - env: ROS_REPO=ros-shadow-fixed - TEST=clang-tidy-fix + # test_ikfast_plugins takes ~10 minutes: include here to keep the main jobs shorter + env: TEST=clang-tidy-fix BEFORE_DOCKER_SCRIPT="source moveit_kinematics/test/test_ikfast_plugins.sh" - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unused-parameter -Wno-unused-function -Wno-overloaded-virtual" + CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-overloaded-virtual" + - if: branch =~ /^(.*-devel|master)$/ + env: ROS_REPO=ros-shadow-fixed + allow_failures: + - env: TEST=code-coverage before_script: - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md new file mode 100644 index 0000000000..185db208a8 --- /dev/null +++ b/CODE_OF_CONDUCT.md @@ -0,0 +1,15 @@ +# MoveIt Code of Conduct + +Like the technical community as a whole, the MoveIt team and community is made up of a mixture of professionals and volunteers from all over the world, working on every aspect of the mission - including mentorship, teaching, and connecting people. + +Diversity is one of our huge strengths, but it can also lead to communication issues and unhappiness. To that end, we have a few basic rules that we ask people to adhere to. This code applies equally to maintainers, contributors, and those seeking help and guidance. +It applies to all kinds of communication, including discussions on GitHub (issue tracker, pull requests), MoveIt maintainer meetings and any other forums created by the project team, which the community uses for communication. + +- **Be friendly and patient.** +- **Be welcoming.** We strive to be a community that welcomes and supports people of all backgrounds and identities. +- **Be considerate.** Your work will be used by other people, and you in turn will depend on the work of others. Any decision you take will affect users and colleagues, and you should take those consequences into account when making decisions. Remember that we're a world-wide community, so you might not be communicating in someone else's primary language. +- **Be respectful.** Not all of us will agree all the time, but disagreement is no excuse for poor behavior and poor manners. We might all experience some frustration now and then, but we cannot allow that frustration to turn into a personal attack. It's important to remember that a community where people feel uncomfortable or threatened is not a productive one. Members of the MoveIt community should be respectful when dealing with other members as well as with people outside the MoveIt community. +- **Be careful in the words that you choose.** We are a community of professionals, and we conduct ourselves professionally. Be kind to others. Do not insult or put down other participants. Harassment and other exclusionary behavior aren't acceptable. Treat others like you want to be treated. +- **When we disagree, try to understand why.** Disagreements, both social and technical, happen all the time and MoveIt is no exception. It is important that we resolve disagreements and differing views constructively. Remember that we're different. The strength of MoveIt comes from its varied community, people from a wide range of backgrounds. Different people have different perspectives on issues. Being unable to understand why someone holds a viewpoint doesn't mean that they're wrong. Don't forget that it is human to err and blaming each other doesn't get us anywhere. Instead, focus on helping to resolve issues and learning from mistakes. + +Original text courtesy of the [Django project](https://www.djangoproject.com/conduct/). diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index eeafb30ddf..609c244cf6 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,4 +1,4 @@ -# Contributing to MoveIt! +# Contributing to MoveIt Thanks for getting involved! Information on contributing can be found at [http://moveit.ros.org/documentation/contributing/](http://moveit.ros.org/documentation/contributing/) diff --git a/MIGRATION.md b/MIGRATION.md index 05740746a4..4dbbf006a2 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -1,13 +1,22 @@ # Migration Notes -API changes in MoveIt! releases +API changes in MoveIt releases -## ROS Noetic (upcoming changes in master) +## ROS Noetic +- RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link. +- Planned trajectories are *slow* by default. + The default values of the `velocity_scaling_factor` and `acceleration_scaling_factor` can now be customized and default to 0.1 instead of 1.0. + The values can be changed by setting the parameters `default_acceleration_scaling_factor` and `default_velocity_scaling_factor` in the `joint_limits.yaml` of your robot's `moveit_config` package. + Consider setting them to values between 0.2 and 0.6, to allow for significant speedup/slowdown of your application. + Additionally, you can always change the factors per request with the corresponding methods in the [move_group_interface](http://docs.ros.org/melodic/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroupInterface.html#a3e2bd2edccca8aa49a6bec9d039d5bf3), the [MoveGroupCommander](http://docs.ros.org/melodic/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html#a7706effa66a0847496de477cf219a562) or in the Rviz interface. ([1890](https://github.com/ros-planning/moveit/pull/1890)) - Extended the return value of `MoveitCommander.MoveGroup.plan()` from `trajectory` to a tuple of `(success, trajectory, planning_time, error_code)` to better match the C++ MoveGroupInterface ([790](https://github.com/ros-planning/moveit/pull/790/)) - `moveit_rviz.launch`, generated by MSA, provides an argument `rviz_config` to configure the rviz config to be used. The old boolean config argument was dropped. ([1397](https://github.com/ros-planning/moveit/pull/1397)) - Moved the example package `moveit_controller_manager_example` into [moveit_tutorials](https://github.com/ros-planning/moveit_tutorials) - Requests to `get_planning_scene` service without explicitly setting "components" now return full scene -- `moveit_ros_plannning` no longer depends on `moveit_ros_perception` +- `moveit_ros_planning` no longer depends on `moveit_ros_perception` +- `CollisionRobot` and `CollisionWorld` are combined into a single `CollisionEnv` class. This applies for all derived collision checkers as `FCL`, `ALL_VALID`, `HYBRID` and `DISTANCE_FIELD`. Consequently, `getCollisionRobot[Unpadded] / getCollisionWorld` functions are replaced through a `getCollisionEnv` in the planning scene and return the new combined environment. This unified collision environment provides the union of all member functions of `CollisionRobot` and `CollisionWorld`. Note that calling `checkRobotCollision` of the `CollisionEnv` does not take a `CollisionRobot` as an argument anymore as it is implicitly contained in the `CollisionEnv`. +- `RobotTrajectory` provides a copy constructor that allows a shallow (default) and deep copy of waypoints +- Replace the redundant namespaces `robot_state::` and `robot_model::` with the actual namespace `moveit::core::`. The additional namespaces will disappear in the future (ROS2). ## ROS Melodic diff --git a/README.md b/README.md index 0ffd0e0e3b..785b82079e 100644 --- a/README.md +++ b/README.md @@ -1,62 +1,86 @@ -MoveIt! Logo +MoveIt Logo -The MoveIt! Motion Planning Framework +**The MoveIt Motion Planning Framework** -Currently we support ROS Indigo, Kinetic, and Melodic. +*Easy-to-use open source robotics manipulation platform for developing commercial applications, prototyping designs, and benchmarking algorithms.* -- [Overview of MoveIt!](http://moveit.ros.org) -- [Installation Instructions](http://moveit.ros.org/install/) -- [Documentation](http://moveit.ros.org/documentation/) -- [Get Involved](http://moveit.ros.org/documentation/contributing/) +- [Overview of MoveIt](https://moveit.ros.org) +- [Installation Instructions](https://moveit.ros.org/install/) +- [Documentation](https://moveit.ros.org/documentation/source-code-api/) +- [Get Involved](https://moveit.ros.org/about/get_involved/) ## Branches Policy -We develop all new 1.0 features on ``master``. The ``*-devel`` branches correspond to -released and stable versions of MoveIt for specific distributions of ROS. -Bug fixes occationally get backported to these released versions of MoveIt. -The next version of MoveIt 1.0 will be branched to ``noetic-devel`` around May 2020. +- We develop latest features on ``master``. +- The ``*-devel`` branches correspond to released and stable versions of MoveIt for specific distributions of ROS. +- Bug fixes occationally get backported to these released versions of MoveIt. +- The next version of MoveIt 1.0 will be branched to ``noetic-devel`` around June 2020. +- For MoveIt 2 development, see [moveit2](https://github.com/ros-planning/moveit2). -For MoveIt 2.0 development, see [moveit2](https://github.com/ros-planning/moveit2). +## MoveIt Status -## Continuous Integration Status +### Continuous Integration -service | Indigo | Kinetic | Melodic | Master ----------- | ------ | ------- | ------- | ------ -Travis | [![Build Status](https://travis-ci.org/ros-planning/moveit.svg?branch=indigo-devel)](https://travis-ci.org/ros-planning/moveit/branches) | [![Build Status](https://travis-ci.org/ros-planning/moveit.svg?branch=kinetic-devel)](https://travis-ci.org/ros-planning/moveit/branches) | [![Build Status](https://travis-ci.org/ros-planning/moveit.svg?branch=melodic-devel)](https://travis-ci.org/ros-planning/moveit/branches) | [![Build Status](https://travis-ci.org/ros-planning/moveit.svg?branch=master)](https://travis-ci.org/ros-planning/moveit/branches) | -build farm | [![Build Status](http://build.ros.org/buildStatus/icon?job=Idev__moveit__ubuntu_trusty_amd64)](http://build.ros.org/job/Idev__moveit__ubuntu_trusty_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__moveit__ubuntu_xenial_amd64)](http://build.ros.org/job/Kdev__moveit__ubuntu_xenial_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__moveit__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__moveit__ubuntu_bionic_amd64) | N/A | +service | Kinetic | Melodic | Master +---------- | ------- | ------- | ------ +Travis | [![Build Status](https://travis-ci.com/ros-planning/moveit.svg?branch=kinetic-devel)](https://travis-ci.com/ros-planning/moveit/branches) | [![Build Status](https://travis-ci.com/ros-planning/moveit.svg?branch=melodic-devel)](https://travis-ci.com/ros-planning/moveit/branches) | [![Build Status](https://travis-ci.com/ros-planning/moveit.svg?branch=master)](https://travis-ci.com/ros-planning/moveit/branches) | +build farm | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__moveit__ubuntu_xenial_amd64)](http://build.ros.org/job/Kdev__moveit__ubuntu_xenial_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__moveit__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__moveit__ubuntu_bionic_amd64) | N/A | +### Licenses -## Docker Containers +[![FOSSA Status](https://app.fossa.com/api/projects/git%2Bgithub.com%2Fros-planning%2Fmoveit.svg?type=shield)](https://app.fossa.com/projects/git%2Bgithub.com%2Fros-planning%2Fmoveit?ref=badge_shield) + +### Docker Containers [![Docker Build](https://img.shields.io/docker/build/moveit/moveit.svg)](https://hub.docker.com/r/moveit/moveit/builds) [![Docker Automated build](https://img.shields.io/docker/automated/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit/) [![Docker Pulls](https://img.shields.io/docker/pulls/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit/) [![Docker Stars](https://img.shields.io/docker/stars/moveit/moveit.svg)](https://registry.hub.docker.com/moveit/moveit/) -## ROS Buildfarm - -MoveIt! Package | Indigo Source | Indigo Debian | Kinetic Source | Kinetic Debian | Melodic Source | Melodic Debian ---------------- | ------------- | ------------- | -------------- | -------------- | -------------- | -------------- -moveit | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit__ubuntu_bionic_amd64__binary) -moveit_chomp_optimizer_adapter | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary) -moveit_commander | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_commander__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_commander__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_commander__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_commander__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_commander__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_commander__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_commander__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_commander__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary) -moveit_core | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_core__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_core__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_core__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_core__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_core__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_core__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_core__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_core__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary) -moveit_experimental | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_experimental__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_experimental__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_experimental__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_experimental__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_experimental__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_experimental__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_experimental__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_experimental__ubuntu_bionic_amd64__binary) -moveit_fake_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_fake_controller_manager__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_fake_controller_manager__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_fake_controller_manager__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_fake_controller_manager__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary) -moveit_kinematics | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_kinematics__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_kinematics__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_kinematics__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_kinematics__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_kinematics__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_kinematics__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_kinematics__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_kinematics__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary) -moveit_planners | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_planners__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_planners__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_planners__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_planners__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary) -moveit_planners_chomp | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary) -moveit_planners_ompl | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_planners_ompl__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_planners_ompl__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_planners_ompl__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_planners_ompl__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary) -moveit_plugins | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_plugins__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_plugins__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_plugins__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_plugins__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_plugins__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_plugins__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_plugins__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_plugins__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary) -moveit_ros | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_ros__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_ros__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary) -moveit_ros_benchmarks | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_ros_benchmarks__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_benchmarks__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_ros_benchmarks__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_benchmarks__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary) -moveit_ros_control_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_ros_control_interface__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_control_interface__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_ros_control_interface__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_control_interface__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary) -moveit_ros_manipulation | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_ros_manipulation__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_manipulation__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_ros_manipulation__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_manipulation__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary) -moveit_ros_move_group | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_ros_move_group__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_move_group__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_ros_move_group__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_move_group__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary) -moveit_ros_perception | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_ros_perception__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_perception__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_ros_perception__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_perception__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_perception__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_perception__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary) -moveit_ros_planning | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_ros_planning__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_planning__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_ros_planning__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_planning__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary) -moveit_ros_planning_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_ros_planning_interface__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_planning_interface__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_ros_planning_interface__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_planning_interface__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary) -moveit_ros_robot_interaction | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_ros_robot_interaction__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_robot_interaction__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_ros_robot_interaction__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_robot_interaction__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary) -moveit_ros_visualization | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_ros_visualization__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_visualization__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_ros_visualization__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_visualization__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary) -moveit_ros_warehouse | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_ros_warehouse__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_warehouse__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_ros_warehouse__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_warehouse__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary) -moveit_runtime | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_runtime__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_runtime__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_runtime__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_runtime__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_runtime__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_runtime__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_runtime__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_runtime__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary) -moveit_setup_assistant | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_setup_assistant__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_setup_assistant__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_setup_assistant__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_setup_assistant__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary) -moveit_simple_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_simple_controller_manager__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_simple_controller_manager__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_simple_controller_manager__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_simple_controller_manager__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary) -chomp_motion_planner | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__chomp_motion_planner__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__chomp_motion_planner__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary) + +### ROS Buildfarm + +MoveIt Package | Kinetic Source | Kinetic Debian | Melodic Source | Melodic Debian | Noetic Source | Noetic Debian +-------------- | -------------- | -------------- | -------------- | -------------- | ------------- | ------------- +moveit | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit__ubuntu_focal_amd64__binary) +moveit_chomp_optimizer_adapter | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_chomp_optimizer_adapter__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_chomp_optimizer_adapter__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_chomp_optimizer_adapter__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_chomp_optimizer_adapter__ubuntu_focal_amd64__binary) +moveit_commander | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_commander__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_commander__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_commander__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_commander__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_commander__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_commander__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_commander__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_commander__ubuntu_focal_amd64__binary) +moveit_core | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_core__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_core__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_core__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_core__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_core__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_core__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_core__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_core__ubuntu_focal_amd64__binary) +moveit_fake_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_fake_controller_manager__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_fake_controller_manager__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_fake_controller_manager__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_fake_controller_manager__ubuntu_focal_amd64__binary) +moveit_grasps | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_grasps__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_grasps__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_grasps__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_grasps__ubuntu_focal_amd64__binary) +moveit_jog_arm | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_jog_arm__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_jog_arm__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_jog_arm__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_jog_arm__ubuntu_focal_amd64__binary) +moveit_kinematics | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_kinematics__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_kinematics__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_kinematics__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_kinematics__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_kinematics__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_kinematics__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_kinematics__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_kinematics__ubuntu_focal_amd64__binary) +moveit_msgs | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_msgs__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_msgs__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_msgs__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_msgs__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_msgs__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_msgs__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_msgs__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_msgs__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_msgs__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_msgs__ubuntu_focal_amd64__binary) +moveit_planners | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners__ubuntu_focal_amd64__binary) +moveit_planners_chomp | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners_chomp__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners_chomp__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners_chomp__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners_chomp__ubuntu_focal_amd64__binary) +moveit_planners_ompl | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners_ompl__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners_ompl__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners_ompl__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners_ompl__ubuntu_focal_amd64__binary) +moveit_plugins | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_plugins__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_plugins__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_plugins__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_plugins__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_plugins__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_plugins__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_plugins__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_plugins__ubuntu_focal_amd64__binary) +moveit_resources | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_resources__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_resources__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_resources__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_resources__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_resources__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_resources__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_resources__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_resources__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_resources__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_resources__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_resources__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_resources__ubuntu_focal_amd64__binary) +moveit_ros | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros__ubuntu_focal_amd64__binary) +moveit_ros_benchmarks | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_benchmarks__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_benchmarks__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_benchmarks__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_benchmarks__ubuntu_focal_amd64__binary) +moveit_ros_control_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_control_interface__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_control_interface__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_control_interface__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_control_interface__ubuntu_focal_amd64__binary) +moveit_ros_manipulation | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_manipulation__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_manipulation__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_manipulation__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_manipulation__ubuntu_focal_amd64__binary) +moveit_ros_move_group | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_move_group__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_move_group__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_move_group__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_move_group__ubuntu_focal_amd64__binary) +moveit_ros_occupancy_map_monitor | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_occupancy_map_monitor__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_occupancy_map_monitor__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_occupancy_map_monitor__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_occupancy_map_monitor__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_occupancy_map_monitor__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_occupancy_map_monitor__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_occupancy_map_monitor__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_occupancy_map_monitor__ubuntu_focal_amd64__binary) +moveit_ros_perception | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_perception__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_perception__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_perception__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_perception__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_perception__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_perception__ubuntu_focal_amd64__binary) +moveit_ros_planning | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_planning__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_planning__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_planning__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_planning__ubuntu_focal_amd64__binary) +moveit_ros_planning_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_planning_interface__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_planning_interface__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_planning_interface__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_planning_interface__ubuntu_focal_amd64__binary) +moveit_ros_robot_interaction | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_robot_interaction__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_robot_interaction__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_robot_interaction__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_robot_interaction__ubuntu_focal_amd64__binary) +moveit_ros_visualization | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_visualization__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_visualization__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_visualization__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_visualization__ubuntu_focal_amd64__binary) +moveit_ros_warehouse | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_warehouse__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_warehouse__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_warehouse__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_warehouse__ubuntu_focal_amd64__binary) +moveit_runtime | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_runtime__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_runtime__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_runtime__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_runtime__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_runtime__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_runtime__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary) +moveit_setup_assistant | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_setup_assistant__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_setup_assistant__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_setup_assistant__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_setup_assistant__ubuntu_focal_amd64__binary) +moveit_simple_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary) +moveit_task_constructor_capabilities | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_capabilities__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_capabilities__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_capabilities__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_capabilities__ubuntu_focal_amd64__binary) +moveit_task_constructor_core | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_core__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_core__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_core__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_core__ubuntu_focal_amd64__binary) +moveit_task_constructor_demo | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_demo__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_demo__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_demo__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_demo__ubuntu_focal_amd64__binary) +moveit_task_constructor_msgs | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_msgs__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_msgs__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_msgs__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_msgs__ubuntu_focal_amd64__binary) +moveit_task_constructor_visualization | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_visualization__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_visualization__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_visualization__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_visualization__ubuntu_focal_amd64__binary) +moveit_visual_tools | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_visual_tools__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_visual_tools__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_visual_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_visual_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_visual_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_visual_tools__ubuntu_focal_amd64__binary) +chomp_motion_planner | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__chomp_motion_planner__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__chomp_motion_planner__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__chomp_motion_planner__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__chomp_motion_planner__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__chomp_motion_planner__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__chomp_motion_planner__ubuntu_focal_amd64__binary) +geometric_shapes | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__geometric_shapes__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__geometric_shapes__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__geometric_shapes__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__geometric_shapes__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__geometric_shapes__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__geometric_shapes__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary) +rviz_marker_tools | | | | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__rviz_marker_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__rviz_marker_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__rviz_marker_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__rviz_marker_tools__ubuntu_focal_amd64__binary) +rviz_visual_tools | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__rviz_visual_tools__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__rviz_visual_tools__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__rviz_visual_tools__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__rviz_visual_tools__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__rviz_visual_tools__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__rviz_visual_tools__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__rviz_visual_tools__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__rviz_visual_tools__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__rviz_visual_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__rviz_visual_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__rviz_visual_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__rviz_visual_tools__ubuntu_focal_amd64__binary) +srdfdom | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__srdfdom__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__srdfdom__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__srdfdom__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__srdfdom__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__srdfdom__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__srdfdom__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary) + + +## Stargazers over time + +[![Stargazers over time](https://starchart.cc/ros-planning/moveit.svg)](https://starchart.cc/ros-planning/moveit) diff --git a/codecov.yaml b/codecov.yaml new file mode 100644 index 0000000000..3999cdb452 --- /dev/null +++ b/codecov.yaml @@ -0,0 +1,10 @@ +coverage: + precision: 2 + round: up + range: "45...70" + status: + project: + default: + target: auto + threshold: 5% + patch: off diff --git a/moveit.rosinstall b/moveit.rosinstall index f0ef21afa0..5b2a6c27bd 100644 --- a/moveit.rosinstall +++ b/moveit.rosinstall @@ -1,13 +1,17 @@ -# This file is intended for users who want to build MoveIt! from source. -# Used with wstool, users can download source of all packages of MoveIt!. +# This file is intended for users who want to build MoveIt from source. +# Used with wstool, users can download source of all packages of MoveIt. - git: local-name: moveit_msgs uri: https://github.com/ros-planning/moveit_msgs.git - version: melodic-devel + version: master +- git: + local-name: moveit_resources + uri: https://github.com/ros-planning/moveit_resources.git + version: master - git: local-name: geometric_shapes uri: https://github.com/ros-planning/geometric_shapes.git - version: melodic-devel + version: noetic-devel - git: local-name: moveit uri: https://github.com/ros-planning/moveit.git @@ -24,3 +28,7 @@ local-name: moveit_tutorials uri: https://github.com/ros-planning/moveit_tutorials.git version: master +- git: + local-name: panda_moveit_config + uri: https://github.com/ros-planning/panda_moveit_config.git + version: melodic-devel diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst index 9f03a155e1..bafe0e5fc4 100644 --- a/moveit/CHANGELOG.rst +++ b/moveit/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature][visualization] Clean up Rviz Motion Planning plugin, add tooltips (`#2310 `_) +* [feature][moveit_servo] A library for servoing toward a moving pose (`#2203 `_) +* [feature][moveit_setup_assistant] Allow showing both, visual and collision geometry (`#2352 `_) +* [fix][moveit_setup_assistant] layout (`#2349 `_) +* [fix][moveit_setup_assistant] group editing (`#2350 `_) +* [fix][moveit_setup_assistant] disappearing robot on change of reference frame (`#2335 `_) +* Contributors: Felix von Drigalski, Michael GΓΆrner, Robert Haschke, Tyler Weaver, Yoan Mollard + +1.1.0 (2020-09-04) +------------------ +* [maint] Use standard cmake text for metapackages (`#1620 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Jonathan Binney, Robert Haschke, Sean Yen + 1.0.1 (2019-03-08) ------------------ * [fix] segfault in chomp adapter (`#1377 `_) @@ -28,7 +45,7 @@ Changelog for package moveit * [improve][moveit_kinematics] KDL IK solver improvements (`#1321 `_) * [improve][moveit_setup_assistant] support dark themes (`#1173 `_) * [improve][moveit_ros_robot_interaction] cleanup RobotInteraction (`#1287 `_) -* [improve][moveit_ros_robot_interaction] limit IK timeout to 0.1s for a responsive interaction behaviour (`#1291 `_) +* [improve][moveit_ros_robot_interaction] limit IK timeout to 0.1s for a responsive interaction behaviour (`#1291 `_) * [maintenance] cleanup SimpleControllerManager https://github.com/ros-planning/moveit/pull/1352 * [maintenance][moveit_core] Add coverage analysis for moveit_core (`#1133 `_) * Contributors: Alexander Gutenkunst, Dave Coleman, Jonathan Binney, Keerthana Subramanian Manivannan, Martin Oehler, Michael GΓΆrner, Mike Lautman, Robert Haschke, Simon Schmeisser @@ -64,7 +81,7 @@ Changelog for package moveit 0.10.2 (2018-10-24) ------------------- -* [fix] Text refrences to MoveIt! (`#1020 `_) +* [fix] Text refrences to MoveIt (`#1020 `_) * [fix] Eigen alignment issuses due to missing aligned allocation (`#1039 `_) * [fix][chomp] changelogs: migration from tf -> tf2 only accidentally became part of 0.9.12's changelog * [fix] Chomp package handling issue `#1086 `_ that was introduced in `ubi-agni/hotfix-#1012 `_ @@ -82,7 +99,7 @@ Changelog for package moveit * [capability][chomp] Failure recovery options for CHOMP by tweaking parameters (`#987 `_) * [capability] New screen for automatically generating interfaces to low level controllers(`#951 `_, `#994 `_, `#908 `_) * [capability][moveit_setup_assistant] Perception screen for using laser scanner point clouds. (`#969 `_) -* [enhancement][GUI][moveit_setup_assistant] Logo for MoveIt! 2.0, cleanup appearance (`#1059 `_) +* [enhancement][GUI][moveit_setup_assistant] Logo for MoveIt 2.0, cleanup appearance (`#1059 `_) * [enhancement][GUI][moveit_setup_assistant] added a setup assistant window icon (`#1028 `_) * [capability][chomp] Addition of CHOMP planning adapter for optimizing result of other planners (`#1012 `_) * [capability][chomp] Failure recovery options for CHOMP by tweaking parameters (`#987 `_) @@ -162,7 +179,7 @@ Changelog for package moveit * [fix][moveit_ros_visualization] RobotStateVisualization: clear before load to avoid segfault `#572 `_ * [fix][setup_assistant] Fix for lunar (`#542 `_) (fix `#506 `_) * [fix][moveit_core] segfault due to missing string format parameter. (`#547 `_) - * [fix][moveit_core] doc-comment for robot_state::computeAABB (`#516 `_) + * [fix][moveit_core] doc-comment for robot_state::computeAABB (`#516 `_) * Improvement in the contained packages: * [improve][moveit_ros_planning] Chomp use PlanningScene (`#546 `_) to partially address `#305 `_ @@ -216,12 +233,12 @@ Changelog for package moveit 0.9.5 (2017-03-08) ------------------ -* [fix] correct "simplify widget handling" `#452 `_ This reverts "simplify widget handling (`#442 `_)" -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix] correct "simplify widget handling" `#452 `_ This reverts "simplify widget handling (`#442 `_)" +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [fix] Regression on Ubuntu Xenial; numpy.ndarray indices bug (from `#86 `_) (`#450 `_). * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * [enhancement][MoveGroup] Add getLinkNames function (`#440 `_) -* [doc][moveit_commander] added description for set_start_state (`#447 `_) +* [doc][moveit_commander] added description for set_start_state (`#447 `_) * Contributors: Adam Allevato, Dave Coleman, Bence Magyar, Dave Coleman, Isaac I.Y. Saito, Yannick Jonetzko, Ravi Prakash Joshi 0.9.4 (2017-02-06) diff --git a/moveit/CMakeLists.txt b/moveit/CMakeLists.txt index 9d8f3239d4..9acc0a9b9e 100644 --- a/moveit/CMakeLists.txt +++ b/moveit/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit/package.xml b/moveit/package.xml index 0a60b4860e..0af3c9d8f5 100644 --- a/moveit/package.xml +++ b/moveit/package.xml @@ -1,8 +1,8 @@ moveit - 1.0.1 - Meta package that contains all essential package of MoveIt!. Until Summer 2016 MoveIt! had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See the detailed discussion for the merge of several repositories. + 1.1.1 + Meta package that contains all essential package of MoveIt. Until Summer 2016 MoveIt had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See the detailed discussion for the merge of several repositories. Dave Coleman Michael Ferguson Michael GΓΆrner diff --git a/moveit/scripts/README.md b/moveit/scripts/README.md index ddc705b86f..e6fde97ef7 100644 --- a/moveit/scripts/README.md +++ b/moveit/scripts/README.md @@ -1,15 +1,23 @@ -# Utility Scripts for MoveIt! +# Utility Scripts for MoveIt ## README Markdown Buildfarm Table Generator -This Python script will generate Github-style Markdown table with Jenkins badges for every package recursively from the current folder. It will order all packages alphabetically, but start with all MoveIt! packages first. For generating an updated README for a new ROS distribution, first update the `create_readme_table.py` script and add/remove the current MoveIt!-supported ROS distros and their corresponding Ubuntu distribution in the specified Python dictionary. Then, in the main MoveIt! `README.md` file, remove the "## ROS Buildfarm" header and the following table. Finally, from your catkin workspace with every MoveIt! package, run the `create_readme_table.py` script. For example: +This Python script will generate Github-style Markdown table with Jenkins badges for every package recursively from the current folder. +It will order all packages alphabetically, but start with all MoveIt packages first. +For generating an updated README for a new ROS distribution: +First update the `create_readme_table.py` script and add/remove the current MoveIt-supported ROS distros and their corresponding Ubuntu distribution in the specified Python dictionary. +Then, in the main MoveIt `README.md` file, remove the "## ROS Buildfarm" header and the following table. +Finally, from your catkin workspace with every MoveIt package, run the `create_readme_table.py` script. +For example: cd ~/ws_moveit/src python moveit/moveit/scripts/create_readme_table.py >> moveit/README.md ## HTML Table Generator -This python script will generate an html page with useful data about every package in the current folder the script is run in. Therefore, to summarize moveit, run within a catkin workspace. For example: +This python script will generate an html page with useful data about every package in the current folder the script is run in. +Therefore, to summarize moveit, run within a catkin workspace. +For example: cd ~/ws_moveit/src python moveit/moveit/scripts/create_maintainer_table.py diff --git a/moveit/scripts/create_maintainer_table.py b/moveit/scripts/create_maintainer_table.py index af7615c2a9..1557530ee2 100644 --- a/moveit/scripts/create_maintainer_table.py +++ b/moveit/scripts/create_maintainer_table.py @@ -98,7 +98,7 @@ def populate_package_data(path, package): def list_moveit_packages(): """ - Creates MoveIt! List + Creates MoveIt List """ output = '' packages = find_packages(os.getcwd()) diff --git a/moveit/scripts/create_readme_table.py b/moveit/scripts/create_readme_table.py index 9a5af08554..0b52973892 100644 --- a/moveit/scripts/create_readme_table.py +++ b/moveit/scripts/create_readme_table.py @@ -10,8 +10,8 @@ def create_header(ros_ubuntu_dict): ros_distros = sorted(ros_ubuntu_dict.keys()) - section_header = "## ROS Buildfarm\n" - header="MoveIt! Package" + section_header = "### ROS Buildfarm\n" + header="MoveIt Package" header_lines = '-'*len(header) for ros in ros_distros: source = ' '.join([ros.capitalize(), "Source"]) @@ -39,7 +39,8 @@ def create_line(package, ros_ubuntu_dict): for target in ['src', 'bin']: define_urls(target, params) response = requests.get(params['url']).status_code - if response < 400: # success + # we want to show a particular OS's badges to indicate they are not released / working yet + if response < 400 or ubuntu == "focal": # success line += ' | [![Build Status]({base_url}/buildStatus/icon?job={job})]({url})'.format(**params) else: # error line += ' | ' @@ -49,13 +50,13 @@ def create_line(package, ros_ubuntu_dict): def create_moveit_buildfarm_table(): """ - Creates MoveIt! buildfarm badge table + Creates MoveIt buildfarm badge table """ # Update the following dictionary with the appropriate ROS-Ubuntu # combinations for supported distribitions. For instance, in Noetic, # remove {"indigo":"trusty"} and add {"noetic":"fbuntu"} with "fbuntu" # being whatever the 20.04 distro is named - supported_distro_ubuntu_dict = {"indigo":"trusty", "kinetic":"xenial", "melodic":"bionic"} + supported_distro_ubuntu_dict = {"kinetic":"xenial", "melodic":"bionic", "noetic":"focal"} all_packages = sorted([package.name for _, package in find_packages(os.getcwd()).items()]) moveit_packages = list() diff --git a/moveit/scripts/maintainer_table_template.html b/moveit/scripts/maintainer_table_template.html index b5c94f4020..584b1d68d6 100644 --- a/moveit/scripts/maintainer_table_template.html +++ b/moveit/scripts/maintainer_table_template.html @@ -2,7 +2,7 @@ - MoveIt! Package Tracking + MoveIt Package Tracking @@ -23,7 +23,7 @@ -

MoveIt! Package Tracking

+

MoveIt Package Tracking

diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst index 4184e8de90..0b6b4fb7e2 100644 --- a/moveit_commander/CHANGELOG.rst +++ b/moveit_commander/CHANGELOG.rst @@ -2,6 +2,58 @@ Changelog for package moveit_commander ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature] MGC: Improve exception messages (`#2318 `_) +* [fix] ROS namespacing in moveit_commander's PSI (`#2347 `_) +* [fix] python3 issues (`#2323 `_) +* Contributors: Michael GΓΆrner, Peter Mitrano, Robert Haschke + +1.1.0 (2020-09-04) +------------------ +* [feature] Add missing variants of place from list of PlaceLocations and Poses in the python interface (`#2231 `_) +* [fix] Add default velocity/acceleration scaling factors (`#1890 `_) +* [fix] Handle the updated plan() function of MoveGroupCommander (`#1640 `_) +* [fix] Fix `failing tutorial `_ (`#1459 `_) +* [maint] Update dependencies for python3 in noetic (`#2131 `_) +* [maint] Better align MoveGroupInterface.plan() with C++ MoveGroup::plan() (`#790 `_) +* Contributors: Bence Magyar, Bjar Ne, Dave Coleman, Felix von Drigalski, Gerard Canal, Jafar Abdi, Masaki Murooka, Michael Ferguson, Michael GΓΆrner, Pavel-P, Raphael Druon, Robert Haschke, Ryodo Tanaka, Ryosuke Tajima, Sean Yen, v4hn + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [feature] Exposed parameter wait_for_servers and getPlannerId() API in MoveGroup's Python API (`#2201 `_) +* Contributors: Gerard Canal, Robert Haschke, Michael GΓΆrner + +1.0.5 (2020-07-08) +------------------ +* [fix] Python 3 fix (`#2030 `_) +* Contributors: Henning Kayser, Michael GΓΆrner, Robert Haschke + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [feature] Expose reference_point_position parameter in getJacobian() (`#1595 `_) +* [maint] Improve Python 3 compatibility (`#1870 `_) + * Replaced StringIO with BytesIO for python msg serialization + * Use py_bindings_tools::ByteString as byte-based serialization buffer on C++ side +* [fix] Fix service call to utilize original name space (`#1959 `_) +* [maint] Windows compatibility: fallback to using `pyreadline` (`#1635 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] Fix planning scene interface not respecting custom namespace (`#1815 `_) +* [maint] moveit_commander: python3 import fixes (`#1786 `_) +* [fix] python planning_scene_interface: fix attaching objects (`#1624 `_) +* [feature] Select time parametrization algorithm in retime_trajectory (`#1508 `_) +* Contributors: Bjar Ne, Felix von Drigalski, Masaki Murooka, Pavel-P, Raphael Druon, Robert Haschke, Ryodo Tanaka, Sean Yen, v4hn + +1.0.2 (2019-06-28) +------------------ +* [feature] Add get_jacobian_matrix to moveit_commander (`#1501 `_) +* [maintanance] Cleanup Python PlanningSceneInterface (`#1405 `_, `#789 `_) +* Contributors: Bence Magyar, Robert Haschke, Ryosuke Tajima + 1.0.1 (2019-03-08) ------------------ * [capability] python PlanningSceneInterface.add_cylinder() (`#1372 `_) @@ -70,7 +122,7 @@ Changelog for package moveit_commander 0.9.5 (2017-03-08) ------------------ * [fix] Regression on Ubuntu Xenial; numpy.ndarray indices bug (from `#86 `_) (`#450 `_). -* [doc][moveit_commander] added description for set_start_state (`#447 `_) +* [doc][moveit_commander] added description for set_start_state (`#447 `_) * Contributors: Adam Allevato, Ravi Prakash Joshi 0.9.4 (2017-02-06) @@ -126,7 +178,7 @@ Changelog for package moveit_commander 0.5.6 (2014-03-24) ------------------ -* Added the calls necessary to manage path constraints. +* Added the calls necessary to manage path constraints. * fix joint and link acces on __getattr__ when trying to acces a joint and its paramaters throught * Contributors: Acorn, Emili Boronat, Sachin Chitta diff --git a/moveit_commander/CMakeLists.txt b/moveit_commander/CMakeLists.txt index 70ba103732..cb0618035e 100644 --- a/moveit_commander/CMakeLists.txt +++ b/moveit_commander/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_commander) find_package(catkin REQUIRED) @@ -7,7 +7,8 @@ catkin_python_setup() catkin_package() -install(PROGRAMS bin/${PROJECT_NAME}_cmdline.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +catkin_install_python( + PROGRAMS bin/${PROJECT_NAME}_cmdline.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) add_subdirectory(test) diff --git a/moveit_commander/README.md b/moveit_commander/README.md index ecd51ed3b1..1b8f538576 100644 --- a/moveit_commander/README.md +++ b/moveit_commander/README.md @@ -1 +1 @@ -# MoveIt! Commander +# MoveIt Commander diff --git a/moveit_commander/bin/moveit_commander_cmdline.py b/moveit_commander/bin/moveit_commander_cmdline.py index eb3c524c74..821b4ffed5 100755 --- a/moveit_commander/bin/moveit_commander_cmdline.py +++ b/moveit_commander/bin/moveit_commander_cmdline.py @@ -4,7 +4,10 @@ import roslib import rospy -import readline +try: + import readline +except ImportError: + import pyreadline as readline # for Windows import sys import os import signal @@ -42,7 +45,7 @@ def complete(self, text, state): prefix = "" if len(cmds) > 0: prefix = cmds[0] - if not self.options.has_key(prefix): + if not prefix in self.options: prefix = "" if state == 0: @@ -151,7 +154,7 @@ def sigint_handler(signal, frame): rospy.init_node('move_group_interface_cmdline', anonymous=True, disable_signals=True) parser = argparse.ArgumentParser(usage = """%(prog)s [options] []""", - description = "Command Line Interface to MoveIt!") + description = "Command Line Interface to MoveIt") parser.add_argument("-i", "--interactive", action="store_true", dest="interactive", default=True, help="Run the command processing script in interactive mode (default)") parser.add_argument("-s", "--service", action="store_true", dest="service", default=False, diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml index 6680232adc..e0fc5eb801 100644 --- a/moveit_commander/package.xml +++ b/moveit_commander/package.xml @@ -1,13 +1,14 @@ - + + moveit_commander - 1.0.1 + 1.1.1 Python interfaces to MoveIt Ioan Sucan Michael GΓΆrner Robert Haschke - MoveIt! Release Team + MoveIt Release Team BSD @@ -16,20 +17,22 @@ https://github.com/ros-planning/moveit catkin - python-catkin-pkg + python-catkin-pkg + python3-catkin-pkg - python + python + python3 geometry_msgs moveit_msgs moveit_ros_planning_interface - python - python-pyassimp + python-pyassimp + python3-pyassimp rospy sensor_msgs shape_msgs tf rostest - moveit_resources + moveit_resources_fanuc_moveit_config diff --git a/moveit_commander/src/moveit_commander/conversions.py b/moveit_commander/src/moveit_commander/conversions.py index 9a241d3992..f9129ffd8f 100644 --- a/moveit_commander/src/moveit_commander/conversions.py +++ b/moveit_commander/src/moveit_commander/conversions.py @@ -32,14 +32,20 @@ # # Author: Ioan Sucan -import StringIO +try: + # Try Python 2.7 behaviour first + from StringIO import StringIO +except ImportError: + # Use Python 3.x behaviour as fallback and choose the non-unicode version + from io import BytesIO as StringIO + from moveit_commander import MoveItCommanderException from geometry_msgs.msg import Pose, PoseStamped, Transform import rospy import tf def msg_to_string(msg): - buf = StringIO.StringIO() + buf = StringIO() msg.serialize(buf) return buf.getvalue() diff --git a/moveit_commander/src/moveit_commander/interpreter.py b/moveit_commander/src/moveit_commander/interpreter.py index 3505e6deb5..cd4777e626 100644 --- a/moveit_commander/src/moveit_commander/interpreter.py +++ b/moveit_commander/src/moveit_commander/interpreter.py @@ -103,7 +103,7 @@ def execute_generic_command(self, cmd): clist[1] = self._prev_group_name if len(clist[1]) == 0: return (MoveGroupInfoLevel.DEBUG, "OK") - if self._gdict.has_key(clist[1]): + if clist[1] in self._gdict: self._prev_group_name = self._group_name self._group_name = clist[1] return (MoveGroupInfoLevel.DEBUG, "OK") @@ -267,7 +267,7 @@ def execute_group_command(self, g, cmdorig): assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd) if assign_match: known = g.get_remembered_joint_values() - if known.has_key(assign_match.group(2)): + if assign_match.group(2) in known: g.remember_joint_values(assign_match.group(1), known[assign_match.group(2)]) return (MoveGroupInfoLevel.SUCCESS, assign_match.group(1) + " is now the same as " + assign_match.group(2)) else: @@ -286,7 +286,7 @@ def execute_group_command(self, g, cmdorig): component_match = re.match(r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd) if component_match: known = g.get_remembered_joint_values() - if known.has_key(component_match.group(1)): + if component_match.group(1) in known: try: val = known[component_match.group(1)] val[int(component_match.group(2))] = float(component_match.group(3)) @@ -303,7 +303,7 @@ def execute_group_command(self, g, cmdorig): # if this is an unknown one-word command, it is probably a variable if len(clist) == 1: known = g.get_remembered_joint_values() - if known.has_key(cmd): + if cmd in known: return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[cmd]]) + "]") else: return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") @@ -336,11 +336,11 @@ def execute_group_command(self, g, cmdorig): if clist[1] == "rand" or clist[1] == "random": vals = g.get_random_joint_values() g.set_joint_value_target(vals) - self._last_plan = g.plan() + self._last_plan = g.plan()[1] # The trajectory msg else: try: g.set_named_target(clist[1]) - self._last_plan = g.plan() + self._last_plan = g.plan()[1] # The trajectory msg except MoveItCommanderException as e: return (MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e)) except: @@ -381,7 +381,7 @@ def execute_group_command(self, g, cmdorig): return (MoveGroupInfoLevel.SUCCESS, "Forgot joint values under the name " + clist[1]) elif clist[0] == "show": known = g.get_remembered_joint_values() - if known.has_key(clist[1]): + if clist[1] in known: return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[clist[1]]]) + "]") else: return (MoveGroupInfoLevel.WARN, "Joint values for " + clist[1] + " are not known") @@ -423,7 +423,7 @@ def execute_group_command(self, g, cmdorig): return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") if len(clist) == 3: - if clist[0] == "go" and self.GO_DIRS.has_key(clist[1]): + if clist[0] == "go" and clist[1] in self.GO_DIRS: self._last_plan = None try: offset = float(clist[2]) @@ -587,7 +587,7 @@ def get_keywords(self): known_vars = self.get_active_group().get_remembered_joint_values().keys() known_constr = self.get_active_group().get_known_constraints() groups = self._robot.get_group_names() - return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + known_vars, + return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + list(known_vars), 'help':[], 'record':known_vars, 'show':known_vars, diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index a05c23cf85..df1f267361 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/moveit_commander/src/moveit_commander/move_group.py @@ -48,9 +48,9 @@ class MoveGroupCommander(object): Execution of simple commands for a particular group """ - def __init__(self, name, robot_description="robot_description", ns=""): + def __init__(self, name, robot_description="robot_description", ns="", wait_for_servers=5.0): """ Specify the group name for which to construct this commander instance. Throws an exception if there is an initialization error. """ - self._g = _moveit_move_group_interface.MoveGroupInterface(name, robot_description, ns) + self._g = _moveit_move_group_interface.MoveGroupInterface(name, robot_description, ns, wait_for_servers) def get_name(self): """ Get the name of the group this instance was initialized for """ @@ -83,7 +83,7 @@ def get_end_effector_link(self): def set_end_effector_link(self, link_name): """ Set the name of the link to be considered as an end effector """ if not self._g.set_end_effector_link(link_name): - raise MoveItCommanderException("Unable to set end efector link") + raise MoveItCommanderException("Unable to set end effector link") def get_interface_description(self): """ Get the description of the planner interface (list of planner ids) """ @@ -156,6 +156,20 @@ def set_start_state(self, msg): """ self._g.set_start_state(conversions.msg_to_string(msg)) + def get_current_state_bounded(self): + """ Get the current state of the robot bounded.""" + s = RobotState() + c_str = self._g.get_current_state_bounded() + conversions.msg_from_string(s, c_str) + return s + + def get_current_state(self): + """ Get the current state of the robot.""" + s = RobotState() + c_str = self._g.get_current_state() + conversions.msg_from_string(s, c_str) + return s + def get_joint_value_target(self): return self._g.get_joint_value_target() @@ -219,7 +233,7 @@ def set_joint_value_target(self, arg1, arg2=None, arg3=None): if approx: raise MoveItCommanderException("Error setting joint target. Does your IK solver support approximate IK?") else: - raise MoveItCommanderException("Error setting joint target. Is IK running?") + raise MoveItCommanderException("Error setting joint target. Is the IK solver functional?") elif (hasattr(arg1, '__iter__')): if (arg2 is not None or arg3 is not None): @@ -230,7 +244,6 @@ def set_joint_value_target(self, arg1, arg2=None, arg3=None): else: raise MoveItCommanderException("Unsupported argument of type %s" % type(arg1)) - def set_rpy_target(self, rpy, end_effector_link=""): """ Specify a target orientation for the end-effector. Any position of the end-effector is acceptable.""" if len(end_effector_link) > 0 or self.has_end_effector_link(): @@ -448,7 +461,7 @@ def set_planner_id(self, planner_id): def get_planner_id(self): """ Get the current planner_id """ - self._g.get_planner_id() + return self._g.get_planner_id() def set_num_planning_attempts(self, num_planning_attempts): """ Set the number of times the motion plan is to be computed from scratch before the shortest solution is returned. The default value is 1. """ @@ -468,14 +481,16 @@ def set_workspace(self, ws): raise MoveItCommanderException("Expected 0, 4 or 6 values in list specifying workspace") def set_max_velocity_scaling_factor(self, value): - """ Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,1]. """ + """ Set a scaling factor to reduce the maximum joint velocities. Allowed values are in (0,1]. + The default value is set in the joint_limits.yaml of the moveit_config package. """ if value > 0 and value <= 1: self._g.set_max_velocity_scaling_factor(value) else: raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor") def set_max_acceleration_scaling_factor(self, value): - """ Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]. """ + """ Set a scaling factor to reduce the maximum joint accelerations. Allowed values are in (0,1]. + The default value is set in the joint_limits.yaml of the moveit_config package. """ if value > 0 and value <= 1: self._g.set_max_acceleration_scaling_factor(value) else: @@ -569,7 +584,7 @@ def pick(self, object_name, grasp=[], plan_only=False): def place(self, object_name, location=None, plan_only=False): """Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided""" result = False - if location is None: + if not location: result = self._g.place(object_name, plan_only) elif type(location) is PoseStamped: old = self.get_pose_reference_frame() @@ -580,8 +595,16 @@ def place(self, object_name, location=None, plan_only=False): result = self._g.place(object_name, conversions.pose_to_list(location), plan_only) elif type(location) is PlaceLocation: result = self._g.place(object_name, conversions.msg_to_string(location), plan_only) + elif type(location) is list: + if location: + if type(location[0]) is PlaceLocation: + result = self._g.place_locations_list(object_name, [conversions.msg_to_string(x) for x in location], plan_only) + elif type(location[0]) is PoseStamped: + result = self._g.place_poses_list(object_name, [conversions.msg_to_string(x) for x in location], plan_only) + else: + raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped, PlaceLocation, list of PoseStamped or list of PlaceLocation object") else: - raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped or PlaceLocation object") + raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped, PlaceLocation, list of PoseStamped or list of PlaceLocation object") return result def set_support_surface_name(self, value): @@ -596,7 +619,13 @@ def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor=1.0, traj_out.deserialize(ser_traj_out) return traj_out - def get_jacobian_matrix(self, joint_values): + def get_jacobian_matrix(self, joint_values, reference_point=None): """ Get the jacobian matrix of the group as a list""" - return self._g.get_jacobian_matrix(joint_values) - + return self._g.get_jacobian_matrix(joint_values, [0.0, 0.0, 0.0] if reference_point is None else reference_point) + + def enforce_bounds(self, robot_state_msg): + """ Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds() """ + s = RobotState() + c_str = self._g.enforce_bounds(conversions.msg_to_string(robot_state_msg)) + conversions.msg_from_string(s, c_str) + return s diff --git a/moveit_commander/src/moveit_commander/planning_scene_interface.py b/moveit_commander/src/moveit_commander/planning_scene_interface.py index a7fa6c9ad2..9de2da057f 100644 --- a/moveit_commander/src/moveit_commander/planning_scene_interface.py +++ b/moveit_commander/src/moveit_commander/planning_scene_interface.py @@ -33,13 +33,14 @@ # Author: Ioan Sucan, Felix Messmer import rospy -import conversions +from rosgraph.names import ns_join +from . import conversions from moveit_msgs.msg import PlanningScene, CollisionObject, AttachedCollisionObject from moveit_ros_planning_interface import _moveit_planning_scene_interface from geometry_msgs.msg import Pose, Point from shape_msgs.msg import SolidPrimitive, Plane, Mesh, MeshTriangle -from exception import MoveItCommanderException +from .exception import MoveItCommanderException from moveit_msgs.srv import ApplyPlanningScene, ApplyPlanningSceneRequest try: @@ -60,16 +61,16 @@ def __init__(self, ns='', synchronous=False, service_timeout=5.0): """ Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics. """ self._psi = _moveit_planning_scene_interface.PlanningSceneInterface(ns) - self._pub_co = rospy.Publisher('/collision_object', CollisionObject, queue_size=100) - self._pub_aco = rospy.Publisher('/attached_collision_object', AttachedCollisionObject, queue_size=100) + self._pub_co = rospy.Publisher(ns_join(ns, 'collision_object'), CollisionObject, queue_size=100) + self._pub_aco = rospy.Publisher(ns_join(ns, 'attached_collision_object'), AttachedCollisionObject, queue_size=100) self.__synchronous = synchronous if self.__synchronous: - self._apply_planning_scene_diff = rospy.ServiceProxy('apply_planning_scene', ApplyPlanningScene) + self._apply_planning_scene_diff = rospy.ServiceProxy(ns_join(ns, 'apply_planning_scene'), ApplyPlanningScene) self._apply_planning_scene_diff.wait_for_service(service_timeout) def __submit(self, collision_object, attach=False): if self.__synchronous: - diff_req = self.__make_planning_scene_diff_req(collision_object) + diff_req = self.__make_planning_scene_diff_req(collision_object, attach) self._apply_planning_scene_diff.call(diff_req) else: if attach: @@ -306,10 +307,14 @@ def __make_cylinder(name, pose, height, radius): return co @staticmethod - def __make_planning_scene_diff_req(collision_object): + def __make_planning_scene_diff_req(collision_object, attach=False): scene = PlanningScene() scene.is_diff = True - scene.world.collision_objects = [collision_object] + scene.robot_state.is_diff = True + if attach: + scene.robot_state.attached_collision_objects = [collision_object] + else: + scene.world.collision_objects = [collision_object] planning_scene_diff_req = ApplyPlanningSceneRequest() planning_scene_diff_req.scene = scene return planning_scene_diff_req diff --git a/moveit_commander/src/moveit_commander/robot.py b/moveit_commander/src/moveit_commander/robot.py index 6336600aca..137fe4b4b4 100644 --- a/moveit_commander/src/moveit_commander/robot.py +++ b/moveit_commander/src/moveit_commander/robot.py @@ -261,7 +261,7 @@ def get_group(self, name): @param name str: Name of movegroup @rtype: moveit_commander.MoveGroupCommander """ - if not self._groups.has_key(name): + if not name in self._groups: if not self.has_group(name): raise MoveItCommanderException("There is no group named %s" % name) self._groups[name] = MoveGroupCommander(name, self._robot_description, self._ns) @@ -279,7 +279,7 @@ def get_default_owner_group(self, joint_name): Get the name of the smallest group (fewest joints) that includes the joint name specified as argument. """ - if not self._joint_owner_groups.has_key(joint_name): + if not joint_name in self._joint_owner_groups: group = None for g in self.get_group_names(): if joint_name in self.get_joint_names(g): diff --git a/moveit_commander/test/CMakeLists.txt b/moveit_commander/test/CMakeLists.txt index 157406a545..b097091a89 100644 --- a/moveit_commander/test/CMakeLists.txt +++ b/moveit_commander/test/CMakeLists.txt @@ -1,5 +1,4 @@ if (CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) find_package(rostest REQUIRED) add_rostest(python_moveit_commander.test) diff --git a/moveit_commander/test/python_moveit_commander.py b/moveit_commander/test/python_moveit_commander.py index 48d01ce524..3c6178562d 100755 --- a/moveit_commander/test/python_moveit_commander.py +++ b/moveit_commander/test/python_moveit_commander.py @@ -35,11 +35,15 @@ # Author: William Baker import unittest + +import genpy import numpy as np import rospy import rostest import os +from moveit_msgs.msg import RobotState + from moveit_commander import RobotCommander, PlanningSceneInterface @@ -55,6 +59,31 @@ def setUpClass(self): def tearDown(self): pass + def test_enforce_bounds_empty_state(self): + in_msg = RobotState() + with self.assertRaises(genpy.DeserializationError): + self.group.enforce_bounds(in_msg) + + def test_enforce_bounds(self): + in_msg = RobotState() + in_msg.joint_state.header.frame_id = 'base_link' + in_msg.joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] + in_msg.joint_state.position = [0] * 6 + in_msg.joint_state.position[0] = 1000 + + out_msg = self.group.enforce_bounds(in_msg) + + self.assertEqual(in_msg.joint_state.position[0], 1000) + self.assertLess(out_msg.joint_state.position[0], 1000) + + def test_get_current_state(self): + expected_state = RobotState() + expected_state.joint_state.header.frame_id = 'base_link' + expected_state.multi_dof_joint_state.header.frame_id = 'base_link' + expected_state.joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] + expected_state.joint_state.position = [0] * 6 + self.assertEqual(self.group.get_current_state(), expected_state) + def check_target_setting(self, expect, *args): if len(args) == 0: args = [expect] diff --git a/moveit_commander/test/python_moveit_commander.test b/moveit_commander/test/python_moveit_commander.test index c97a74198c..eb99c5935d 100644 --- a/moveit_commander/test/python_moveit_commander.test +++ b/moveit_commander/test/python_moveit_commander.test @@ -1,5 +1,5 @@ - + diff --git a/moveit_commander/test/python_moveit_commander_ns.test b/moveit_commander/test/python_moveit_commander_ns.test index feda452a7e..eefed9aec6 100644 --- a/moveit_commander/test/python_moveit_commander_ns.test +++ b/moveit_commander/test/python_moveit_commander_ns.test @@ -1,5 +1,5 @@ - + diff --git a/moveit_commander/test/python_moveit_commander_ros_namespace.py b/moveit_commander/test/python_moveit_commander_ros_namespace.py new file mode 100755 index 0000000000..2b36e937a2 --- /dev/null +++ b/moveit_commander/test/python_moveit_commander_ros_namespace.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, Open Source Robotics Foundation +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Open Source Robotics Foundation. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Peter Mitrano +# +# This test is used to ensure planning with a RobotCommander is +# possible if the robot's move_group node is in a different namespace + +import unittest +import rospy +import rostest +import os + +from moveit_commander import PlanningSceneInterface + + +class PythonMoveitCommanderRosNamespaceTest(unittest.TestCase): + + def test_namespace(self): + self.scene = PlanningSceneInterface() + expected_resolved_co_name = "/test_ros_namespace/collision_object" + expected_resolved_aco_name = "/test_ros_namespace/attached_collision_object" + self.assertEqual(self.scene._pub_co.resolved_name, expected_resolved_co_name) + self.assertEqual(self.scene._pub_aco.resolved_name, expected_resolved_aco_name) + + def test_namespace_synchronous(self): + self.scene = PlanningSceneInterface(synchronous=True) + expected_resolved_apply_diff_name = "/test_ros_namespace/apply_planning_scene" + self.assertEqual(self.scene._apply_planning_scene_diff.resolved_name, expected_resolved_apply_diff_name) + + +if __name__ == '__main__': + PKGNAME = 'moveit_ros_planning_interface' + NODENAME = 'moveit_test_python_moveit_commander_ros_namespace' + rospy.init_node(NODENAME) + rostest.rosrun(PKGNAME, NODENAME, PythonMoveitCommanderRosNamespaceTest) + + # suppress cleanup segfault + os._exit(0) diff --git a/moveit_commander/test/python_moveit_commander_ros_namespace.test b/moveit_commander/test/python_moveit_commander_ros_namespace.test new file mode 100644 index 0000000000..a2c3800c57 --- /dev/null +++ b/moveit_commander/test/python_moveit_commander_ros_namespace.test @@ -0,0 +1,9 @@ + + + + + + + diff --git a/moveit_commander/test/python_time_parameterization.test b/moveit_commander/test/python_time_parameterization.test index 61c5fa50fd..3a83385699 100644 --- a/moveit_commander/test/python_time_parameterization.test +++ b/moveit_commander/test/python_time_parameterization.test @@ -1,5 +1,5 @@ - + diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst index b9990334d2..552ada72b3 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -2,6 +2,141 @@ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature] Handle multiple link libraries for FCL (`#2325 `_) +* [feature] Adapt to API changes in geometric_shapes (`#2324 `_) +* [fix] clang-tidy issues (`#2337 `_) +* [fix] various issues with Noetic build (`#2327 `_) +* [maint] Depend on ros-noetic-fcl (0.6) in Noetic (`#2359 `_) +* [maint] Cleanup MSA includes (`#2351 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, G.A. vd. Hoorn, Robert Haschke + +1.1.0 (2020-09-04) +------------------ +* [feature] Add a utility to print collision pairs (`#2275 `_) +* [feature] Fix subframes disappearing when object is detached/scaled/renamed (`#1866 `_) +* [feature] Use Eigen::Transform::linear() instead of rotation() (`#1964 `_) +* [feature] Utilize new geometric_shapes functions to improve performance (`#2038 `_) +* [feature] move_group pick place test (`#2031 `_) +* [feature] Split collision proximity threshold (`#2008 `_) +* [feature] Integration test to defend subframe tutorial (`#1757 `_) +* [feature] List missing joints in group states (`#1935 `_) +* [feature] Improve documentation for setJointPositions() (`#1921 `_) +* [feature] Installs an empty plugin description xml file if bullet is not found (`#1898 `_) +* [feature] Bullet collision detection (`#1839 `_) +* [feature] Improve RobotState documentation (`#1846 `_) +* [feature] Adapt cmake for Bullet (`#1744 `_) +* [feature] Unified Collision Environment Bullet (`#1572 `_) +* [feature] Adding continuous collision detection to Bullet (`#1551 `_) +* [feature] Bullet Collision Detection (`#1504 `_) +* [feature] Generic collision detection test suite (`#1543 `_) +* [feature] Empty collision checker template for usage with tesseract and bullet (`#1499 `_) +* [feature] Add deepcopy option for RobotTrajectory's copy constructor (`#1760 `_) +* [feature] Enable code-coverage test (`#1776 `_) +* [feature] Provide UniquePtr macros (`#1771 `_) +* [feature] Improve variable name in RobotModel (`#1752 `_) +* [feature] Adding documentation to collision detection (`#1645 `_) +* [feature] Unified Collision Environment Integration (`#1584 `_) +* [feature] Document discretization behavior in KinematicsBase (`#1602 `_) +* [feature] Rename lm to link_model (`#1592 `_) +* [feature] Allow ROS namespaces for planning request adapters (`#1530 `_) +* [feature] Add named frames to CollisionObjects (`#1439 `_) +* [feature] More verbose "id" argument in PlanningScene, RobotState & CollisionWorld functions (`#1450 `_) +* [feature] Separate source file for CartesianInterpolator (`#1149 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Change FloatingJointModel::getStateSpaceDimension return value to 7 +* [fix] collision world: check for empty shapes vector before access (`#2026 `_) +* [fix] Fix Condition for Adding current DistanceResultData to DistanceMap for DistanceRequestType::SINGLE (`#1963 `_) +* [fix] Do not override empty URDF link collision geometry (`#1952 `_) +* [fix] Fix issue in unpadded collision checking (`#1899 `_) +* [fix] Remove object from collision world only once (`#1900 `_) +* [fix] Initialize zero dynamics in CurrentStateMonitor (`#1883 `_) +* [fix] getFrameInfo(): Avoid double search for link name (`#1853 `_) +* [fix] Fix RobotTrajectory's copy constructor (`#1834 `_) +* [fix] Fix flaky moveit_cpp test (`#1781 `_) +* [fix] Fix doc string OrientationConstraint (`#1793 `_) +* [fix] Move ASSERT() into test setup (`#1657 `_) +* [fix] Add missing dependencies to library (`#1746 `_) +* [fix] Fix clang-tidy for unified collision environment (`#1638 `_) +* [fix] PlanningRequestAdapter::initialize() = 0 (`#1621 `_) +* [fix] Fix World::getTransform (`#1553 `_) +* [fix] Link moveit_robot_model from moveit_test_utils (`#1534 `_) +* [maint] Move constraint representation dox to moveit_tutorials (`#2147 `_) +* [maint] Update dependencies for python3 in noetic (`#2131 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#2004 `_, `#1419 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Rename PR2-related collision test files (`#1856 `_) +* [maint] Fix compiler warnings (`#1773 `_) +* [maint] Add missing licenses (`#1716 `_) (`#1720 `_) +* [maint] Move isEmpty() test functions to moveit_core/utils (`#1627 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: AndyZe, Aris Synodinos, Ayush Garg, Bryce Willey, Dale Koenig, Dave Coleman, Felix von Drigalski, Henning Kayser, Jafar Abdi, Jens P, Jere Liukkonen, Jeroen, John Stechschulte, Jonas Wittmann, Jonathan Binney, Markus Vieth, Martin Pecka, Michael Ferguson, Michael GΓΆrner, Mike Lautman, Niklas Fiedler, Patrick Beeson, Robert Haschke, Sean Yen, Shivang Patel, Tyler Weaver, Wolfgang Merkt, Yu, Yan, tsijs, v4hn + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10, Fix warnings +* [maint] Optimize includes (`#2229 `_) +* [maint] Fix docs in robot_state.h (`#2215 `_) +* Contributors: Jeroen, Markus Vieth, Michael GΓΆrner, Robert Haschke + +1.0.5 (2020-07-08) +------------------ +* [fix] Fix memory leaks related to geometric shapes usage (`#2138 `_) +* [fix] Prevent collision checking segfault if octomap has NULL root pointer (`#2104 `_) +* [feature] Allow to parameterize input trajectory density of Time Optimal trajectory generation (`#2185 `_) +* [maint] Optional C++ version setting (`#2166 `_) +* [maint] Added missing boost::regex dependency (`#2163 `_) +* [maint] PropagationDistanceField: Replace eucDistSq with squaredNorm (`#2101 `_) +* [fix] Fix getTransform() (`#2113 `_) + - PlanningScene::getTransforms().getTransform() -> PlanningScene::getFrameTransform() + - PlanningScene::getTransforms().canTransform() -> PlanningScene::knowsFrameTransform() +* [fix] Change FloatingJointModel::getStateSpaceDimension return value to 7 (`#2106 `_) +* [fix] Check for empty quaternion message (`#2089 `_) +* [fix] TOTG: Fix parameterization for single-waypoint trajectories (`#2054 `_) + - RobotState: Added interfaces to zero and remove dynamics +* [maint] Remove unused angles.h includes (`#1985 `_) +* Contributors: Felix von Drigalski, Henning Kayser, Michael GΓΆrner, Jere Liukkonen, John Stechschulte, Patrick Beeson, Robert Haschke, Tyler Weaver, Wolfgang Merkt + +1.0.4 (2020-05-30) +------------------ +* Fix broken IKFast generator (`#2116 `_) +* Contributors: Robert Haschke + +1.0.3 (2020-04-26) +------------------ +* [feature] Allow to filter for joint when creating a RobotTrajectory message (`#1927 `_) +* [fix] Fix RobotState::copyFrom() +* [fix] Fix segfault in totg (`#1861 `_) +* [fix] Handle incomplete group states +* [fix] Fix issue in totg giving invalid accelerations (`#1729 `_) +* [feature] New isValidVelocityMove() for checking time between two waypoints given velocity (`#684 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [fix] Fix Condition for adding current DistanceResultData to DistanceMap (`#1968 `_) +* [maint] Fix various build issues on Windows (`#1880 `_) + * remove GCC extensions (`#1583 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] Delete attached body before adding a new one with same id (`#1821 `_) +* [maint] Provide UniquePtr macros (`#1771 `_) +* [maint] Updated deprecation method: MOVEIT_DEPRECATED -> [[deprecated]] (`#1748 `_) +* [feature] Add RobotTrajectory::getDuration() (`#1554 `_) +* Contributors: Ayush Garg, Dale Koenig, Dave Coleman, Felix von Drigalski, Jafar Abdi, Jeroen, Michael GΓΆrner, Mike Lautman, Niklas Fiedler, Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ +* [fix] Removed MessageFilter for /collision_object messages (`#1406 `_) +* [fix] Update robot state transforms when initializing a planning scene (`#1474 `_) +* [fix] Fix segfault when detaching attached collision object (`#1438 `_) +* [fix] Normalize quaternions when adding new or moving collision objects (`#1420 `_) +* [fix] Minor bug fixes in (collision) distance field (`#1392 `_) +* [fix] Remove obsolete moveit_resources/config.h () +* [fix] Fix test utilities in moveit_core (`#1391 `_, `#1409 `_, `#1412 `_) +* Contributors: Bryce Willey, Henning Kayser, Mike Lautman, Robert Haschke, tsijs + 1.0.1 (2019-03-08) ------------------ * [capability] Graphically print current robot joint states with joint limits (`#1358 `_) @@ -122,7 +257,7 @@ Changelog for package moveit_core 0.9.9 (2017-08-06) ------------------ * [fix][moveit_core] segfault due to missing string format parameter. (`#547 `_) -* [fix][moveit_core] doc-comment for robot_state::computeAABB (`#516 `_) +* [fix][moveit_core] doc-comment for robot_state::computeAABB (`#516 `_) * Contributors: Martin Pecka, henhenhen 0.9.8 (2017-06-21) @@ -140,7 +275,7 @@ Changelog for package moveit_core 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman @@ -198,7 +333,7 @@ Changelog for package moveit_core * [fix] New getOnlyOneEndEffectorTip() function `#262 `_ * [fix] issue `#258 `_ in jade-devel `#266 `_ * [fix] Segfault in parenthesis operator `#254 `_ -* [fix] API Change of shape_tools `#242 `_ +* [fix] API Change of shape_tools `#242 `_ * [fix] Fixed bug in KinematicConstraintSet::decide that makes it evaluate only joint_constraints. `#250 `_ * [fix] Prevent divide by zero `#246 `_ * [fix] removed the 'f' float specifiers and corrected misspelled method name diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 9fd0c3bc6e..1aa2450ff0 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -1,33 +1,60 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_core) -add_compile_options(-std=c++14) - -# Warnings -add_compile_options(-Wall -Wextra - -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual - -Wno-unused-parameter -Wno-unused-function) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") + # Enable warnings + add_compile_options(-Wall -Wextra + -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual + -Wno-unused-parameter -Wno-unused-function) +endif() if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") # This too often has false-positives add_compile_options(-Wno-maybe-uninitialized) elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") endif() -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance") set(CMAKE_BUILD_TYPE Release) endif() -find_package(Boost REQUIRED system filesystem date_time thread iostreams) +# boost::iostreams on Windows depends on boost::zlib +if(WIN32) + set(EXTRA_BOOST_COMPONENTS zlib) +endif() +find_package(Boost REQUIRED system filesystem date_time thread iostreams regex ${EXTRA_BOOST_COMPONENTS}) find_package(Eigen3 REQUIRED) +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") + +# TODO: Move collision detection into separate packages +find_package(Bullet 2.87) + +# TODO(j-petit): Version check can be dropped when Xenial reaches end-of-life +if(BULLET_FOUND) + set(BULLET_ENABLE "BULLET") + set(BULLET_LIB "moveit_collision_detection_bullet") + set(BULLET_INC "collision_detection_bullet/include") + message(STATUS "Compiling with Bullet") +else() + message(STATUS "Version of Bullet too old or not available: disabling Bullet collision detection plugin. Try using Ubuntu 18.04 or later.") +endif() + find_package(PkgConfig REQUIRED) pkg_check_modules(LIBFCL_PC REQUIRED fcl) -# find *absolute* paths to LIBFCL_INCLUDE_DIRS and LIBFCL_LIBRARIES -find_path(LIBFCL_INCLUDE_DIRS fcl/config.h HINTS ${LIBFCL_PC_INCLUDE_DIR} ${LIBFCL_PC_INCLUDE_DIRS}) -find_library(LIBFCL_LIBRARIES fcl HINTS ${LIBFCL_PC_LIBRARY_DIRS}) +set(LIBFCL_INCLUDE_DIRS ${LIBFCL_PC_INCLUDE_DIRS}) +# find *absolute* paths to LIBFCL_LIBRARIES +set(LIBFCL_LIBRARIES) +foreach(_lib ${LIBFCL_PC_LIBRARIES}) + find_library(_lib_${_lib} ${_lib} HINTS ${LIBFCL_PC_LIBRARY_DIRS}) + list(APPEND LIBFCL_LIBRARIES ${_lib_${_lib}}) +endforeach() find_package(octomap REQUIRED) find_package(urdfdom REQUIRED) @@ -38,6 +65,7 @@ COMPONENTS tf2_eigen tf2_geometry_msgs eigen_stl_containers + eigen_conversions geometric_shapes geometry_msgs kdl_parser @@ -70,6 +98,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS backtrace/include collision_detection/include collision_detection_fcl/include + ${BULLET_INC} constraint_samplers/include controller_manager/include distance_field/include @@ -106,6 +135,7 @@ catkin_package( moveit_planning_interface moveit_collision_detection moveit_collision_detection_fcl + ${BULLET_LIB} moveit_kinematic_constraints moveit_planning_scene moveit_constraint_samplers @@ -142,31 +172,22 @@ catkin_package( OCTOMAP urdfdom urdfdom_headers + ${BULLET_ENABLE} ) - -# to run: catkin_make -DENABLE_COVERAGE_TESTING=ON package_name_coverage -if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) - find_package(code_coverage REQUIRED) # catkin package ros-*-code-coverage - include(CodeCoverage) - APPEND_COVERAGE_COMPILER_FLAGS() - set(COVERAGE_EXCLUDES "*/test/*") - add_code_coverage(NAME ${PROJECT_NAME}_coverage) -endif() - -include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${urdfdom_INCLUDE_DIRS} + ${urdfdom_headers_INCLUDE_DIRS} ${LIBFCL_INCLUDE_DIRS} - ) + ${BULLET_INCLUDE_DIRS} + ${OCTOMAP_INCLUDE_DIRS} + ) #catkin_lint: ignore_once external_directory (${VERSION_FILE_PATH}) include_directories(${THIS_PACKAGE_INCLUDE_DIRS} - ${VERSION_FILE_PATH} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${urdfdom_INCLUDE_DIRS} - ${urdfdom_headers_INCLUDE_DIRS} - ${OCTOMAP_INCLUDE_DIRS} - ) + ${VERSION_FILE_PATH}) # Generate and install version.h string(REGEX REPLACE "^([0-9]+)\\..*" "\\1" MOVEIT_VERSION_MAJOR "${${PROJECT_NAME}_VERSION}") @@ -174,7 +195,7 @@ string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_MINOR "${${PROJ string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_PATCH "${${PROJECT_NAME}_VERSION}") set(MOVEIT_VERSION_EXTRA "Alpha") set(MOVEIT_VERSION "${MOVEIT_VERSION_MAJOR}.${MOVEIT_VERSION_MINOR}.${MOVEIT_VERSION_PATCH}-${MOVEIT_VERSION_EXTRA}") -message(STATUS " *** Building MoveIt! ${MOVEIT_VERSION} ***") +message(STATUS " *** Building MoveIt ${MOVEIT_VERSION} ***") configure_file("version/version.h.in" "${VERSION_FILE_PATH}/moveit/version.h") #catkin_lint: ignore_once external_file install(FILES "${VERSION_FILE_PATH}/moveit/version.h" DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/moveit) @@ -205,3 +226,9 @@ add_subdirectory(distance_field) add_subdirectory(collision_distance_field) add_subdirectory(kinematics_metrics) add_subdirectory(dynamics_solver) + +if(BULLET_ENABLE) + add_subdirectory(collision_detection_bullet) +else() + install(FILES collision_detection_bullet/empty_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} RENAME collision_detector_bullet_description.xml) +endif() diff --git a/moveit_core/CMakeModules/FindBullet.cmake b/moveit_core/CMakeModules/FindBullet.cmake new file mode 100644 index 0000000000..1dff096d89 --- /dev/null +++ b/moveit_core/CMakeModules/FindBullet.cmake @@ -0,0 +1,10 @@ +include(FindPackageHandleStandardArgs) +find_package(PkgConfig) + +if(PKGCONFIG_FOUND) + pkg_check_modules(BULLET bullet) +endif() + +find_package_handle_standard_args(Bullet + REQUIRED_VARS BULLET_LIBRARIES BULLET_INCLUDE_DIRS + VERSION_VAR BULLET_VERSION) diff --git a/moveit_core/README.md b/moveit_core/README.md index 2a54efedd4..5e602a457c 100644 --- a/moveit_core/README.md +++ b/moveit_core/README.md @@ -1,4 +1,4 @@ -# MoveIt! Core +# MoveIt Core This repository includes core libraries used by MoveIt: - representation of kinematic models diff --git a/moveit_core/background_processing/CMakeLists.txt b/moveit_core/background_processing/CMakeLists.txt index 0e09ddc409..f0b7ab4712 100644 --- a/moveit_core/background_processing/CMakeLists.txt +++ b/moveit_core/background_processing/CMakeLists.txt @@ -7,6 +7,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/background_processing/include/moveit/background_processing/background_processing.h b/moveit_core/background_processing/include/moveit/background_processing/background_processing.h index 30cd7380ae..3ecb98b526 100644 --- a/moveit_core/background_processing/include/moveit/background_processing/background_processing.h +++ b/moveit_core/background_processing/include/moveit/background_processing/background_processing.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_BACKGROUND_PROCESSING_ -#define MOVEIT_BACKGROUND_PROCESSING_ +#pragma once #include #include @@ -47,7 +46,7 @@ namespace moveit { /** \brief This namespace includes classes and functions that are - helpful in the implementation of other MoveIt! components. This is + helpful in the implementation of other MoveIt components. This is not code specific to the functionality provided by MoveIt. */ namespace tools { @@ -111,7 +110,5 @@ class BackgroundProcessing : private boost::noncopyable void processingThread(); }; -} -} - -#endif +} // namespace tools +} // namespace moveit diff --git a/moveit_core/background_processing/src/background_processing.cpp b/moveit_core/background_processing/src/background_processing.cpp index 37991357b7..e2b05c11c1 100644 --- a/moveit_core/background_processing/src/background_processing.cpp +++ b/moveit_core/background_processing/src/background_processing.cpp @@ -46,7 +46,7 @@ BackgroundProcessing::BackgroundProcessing() // spin a thread that will process user events run_processing_thread_ = true; processing_ = false; - processing_thread_.reset(new boost::thread(boost::bind(&BackgroundProcessing::processingThread, this))); + processing_thread_ = std::make_unique(boost::bind(&BackgroundProcessing::processingThread, this)); } BackgroundProcessing::~BackgroundProcessing() diff --git a/moveit_core/backtrace/include/moveit/backtrace/backtrace.h b/moveit_core/backtrace/include/moveit/backtrace/backtrace.h index c00033f9bc..c00af92322 100644 --- a/moveit_core/backtrace/include/moveit/backtrace/backtrace.h +++ b/moveit_core/backtrace/include/moveit/backtrace/backtrace.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_BACKTRACE_ -#define MOVEIT_BACKTRACE_ +#pragma once #include @@ -62,6 +61,4 @@ void get_backtrace(std::ostream& out) out << "Unable to get backtrace with the used compiler." << std::endl; } #endif -} - -#endif +} // namespace moveit diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt index bfc4e8db6b..f263ec9a08 100644 --- a/moveit_core/collision_detection/CMakeLists.txt +++ b/moveit_core/collision_detection/CMakeLists.txt @@ -1,15 +1,14 @@ set(MOVEIT_LIB_NAME moveit_collision_detection) add_library(${MOVEIT_LIB_NAME} - src/allvalid/collision_robot_allvalid.cpp - src/allvalid/collision_world_allvalid.cpp + src/allvalid/collision_env_allvalid.cpp + src/collision_common.cpp src/collision_matrix.cpp src/collision_octomap_filter.cpp - src/collision_robot.cpp src/collision_tools.cpp - src/collision_world.cpp src/world.cpp src/world_diff.cpp + src/collision_env.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -31,6 +30,7 @@ endif() install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h index 549a6d6990..fdc9aec03b 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h @@ -34,23 +34,18 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALL_VALID_H_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALL_VALID_H_ +#pragma once #include -#include -#include +#include namespace collision_detection { /** \brief An allocator for AllValid collision detectors */ class CollisionDetectorAllocatorAllValid - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_world_allvalid.cpp + static const std::string NAME; // defined in collision_env_allvalid.cpp }; -} - -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h similarity index 52% rename from moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h rename to moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h index c950579668..50b543d837 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h @@ -32,48 +32,45 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Jia Pan, Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_ALLVALID_COLLISION_WORLD_ALLVALID_ -#define MOVEIT_COLLISION_DETECTION_ALLVALID_COLLISION_WORLD_ALLVALID_ +#pragma once -#include +#include namespace collision_detection { -class CollisionRobotAllValid; - -class CollisionWorldAllValid : public CollisionWorld +/** \brief Collision environment which always just returns no collisions. + * + * This can be used to save resources if collision checking is not important. */ +class CollisionEnvAllValid : public CollisionEnv { public: - CollisionWorldAllValid(); - explicit CollisionWorldAllValid(const WorldPtr& world); - CollisionWorldAllValid(const CollisionWorld& other, const WorldPtr& world); + CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); + CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, double padding = 0.0, + double scale = 1.0); + CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world); - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const override; - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, - const AllowedCollisionMatrix& acm) const override; + virtual double distanceRobot(const moveit::core::RobotState& state) const; + virtual double distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; + void distanceRobot(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const override; - virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state) const; - virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const; - void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override; - virtual double distanceWorld(const CollisionWorld& world) const; - virtual double distanceWorld(const CollisionWorld& world, const AllowedCollisionMatrix& acm) const; - void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override; -}; -} + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; -#endif + void distanceSelf(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const override; +}; +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h deleted file mode 100644 index 9ac86aa6bb..0000000000 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h +++ /dev/null @@ -1,88 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#ifndef MOVEIT_COLLISION_DETECTION_ALLVALID_COLLISION_ROBOT_ALLVALID_ -#define MOVEIT_COLLISION_DETECTION_ALLVALID_COLLISION_ROBOT_ALLVALID_ - -#include - -namespace collision_detection -{ -class CollisionRobotAllValid : public CollisionRobot -{ -public: - CollisionRobotAllValid(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); - CollisionRobotAllValid(const CollisionRobot& other); - - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const override; - - virtual double distanceSelf(const robot_state::RobotState& state) const; - virtual double distanceSelf(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; - void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override; - - virtual double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const; - virtual double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, const AllowedCollisionMatrix& acm) const; - void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override; -}; -} - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 601755b0bb..fde530f553 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_COMMON_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_COMMON_ +#pragma once #include #include @@ -48,7 +47,7 @@ namespace collision_detection { -MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); +MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc /** \brief The types of bodies that are considered for collision */ namespace BodyTypes @@ -65,10 +64,10 @@ enum Type /** \brief A body in the environment */ WORLD_OBJECT }; -} +} // namespace BodyTypes /** \brief The types of bodies that are considered for collision */ -typedef BodyTypes::Type BodyType; +using BodyType = BodyTypes::Type; /** \brief Definition of a contact point */ struct Contact @@ -95,6 +94,15 @@ struct Contact /** \brief The type of the second body involved in the contact */ BodyType body_type_2; + + /** \brief The distance percentage between casted poses until collision. + * + * If the value is 0, then the collision occured in the start pose. If the value is 1, then the collision occured in + * the end pose. */ + double percent_interpolation; + + /** \brief The two nearest points connecting the two bodies */ + Eigen::Vector3d nearest_points[2]; }; /** \brief When collision costs are computed, this structure contains information about the partial cost incurred in a @@ -139,7 +147,7 @@ struct CollisionResult CollisionResult() : collision(false), distance(std::numeric_limits::max()), contact_count(0) { } - typedef std::map, std::vector > ContactMap; + using ContactMap = std::map, std::vector >; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -153,6 +161,9 @@ struct CollisionResult cost_sources.clear(); } + /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */ + void print() const; + /** \brief True if collision was found, false otherwise */ bool collision; @@ -162,11 +173,10 @@ struct CollisionResult /** \brief Number of contacts returned */ std::size_t contact_count; - /** \brief A map returning the pairs of ids of the bodies in contact, plus information about the contacts themselves - */ + /** \brief A map returning the pairs of body ids in contact, plus their contact details */ ContactMap contacts; - /** \brief When costs are computed, the individual cost sources are */ + /** \brief These are the individual cost sources when costs are computed */ std::set cost_sources; }; @@ -180,7 +190,6 @@ struct CollisionRequest , max_contacts(1) , max_contacts_per_pair(1) , max_cost_sources(1) - , min_cost_density(0.2) , verbose(false) { } @@ -197,7 +206,7 @@ struct CollisionRequest /** \brief If true, a collision cost is computed */ bool cost; - /** \brief If true, compute contacts */ + /** \brief If true, compute contacts. Otherwise only a binary collision yes/no is reported. */ bool contacts; /** \brief Overall maximum number of contacts to compute */ @@ -210,9 +219,6 @@ struct CollisionRequest /** \brief When costs are computed, this value defines how many of the top cost sources should be returned */ std::size_t max_cost_sources; - /** \brief When costs are computed, this is the minimum cost density for a CostSource to be included in the results */ - double min_cost_density; - /** \brief Function call that decides whether collision detection should stop. */ boost::function is_done; @@ -224,14 +230,15 @@ namespace DistanceRequestTypes { enum DistanceRequestType { - GLOBAL, /// Find the global minimum - SINGLE, /// Find the global minimum for each pair - LIMITED, /// Find a limited(max_contacts_per_body) set of contacts for a given pair - ALL /// Find all the contacts for a given pair + GLOBAL, ///< Find the global minimum + SINGLE, ///< Find the global minimum for each pair + LIMITED, ///< Find a limited(max_contacts_per_body) set of contacts for a given pair + ALL ///< Find all the contacts for a given pair }; } -typedef DistanceRequestTypes::DistanceRequestType DistanceRequestType; +using DistanceRequestType = DistanceRequestTypes::DistanceRequestType; +/** \brief Representation of a distance-reporting request */ struct DistanceRequest { DistanceRequest() @@ -248,7 +255,7 @@ struct DistanceRequest } /// Compute \e active_components_only_ based on \e req_ - void enableGroup(const robot_model::RobotModelConstPtr& robot_model) + void enableGroup(const moveit::core::RobotModelConstPtr& robot_model) { if (robot_model->hasJointModelGroup(group_name)) active_components_only = &robot_model->getJointModelGroup(group_name)->getUpdatedLinkModelsSet(); @@ -264,7 +271,7 @@ struct DistanceRequest /// Indicate the type of distance request. If using type=ALL, it is /// recommended to set max_contacts_per_body to the expected number - /// of contacts per pair becaused it is uesed to reserving space. + /// of contacts per pair because it is used to reserve space. DistanceRequestType type; /// Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold) @@ -274,13 +281,13 @@ struct DistanceRequest std::string group_name; /// The set of active components to check - const std::set* active_components_only; + const std::set* active_components_only; /// The allowed collision matrix used to filter checks const AllowedCollisionMatrix* acm; /// Only calculate distances for objects within this threshold to each other. - /// If set this can significantly to reduce number of queries. + /// If set, this can significantly reduce the number of queries. double distance_threshold; /// Log debug information @@ -291,7 +298,8 @@ struct DistanceRequest bool compute_gradient; }; -struct DistanceResultsData +/** \brief Generic representation of the distance information for a pair of objects */ +struct DistanceResultsData // NOLINT(readability-identifier-naming) - suppress spurious clang-tidy warning { DistanceResultsData() { @@ -330,19 +338,6 @@ struct DistanceResultsData normal.setZero(); } - /// Update structure data given DistanceResultsData object - void operator=(const DistanceResultsData& other) - { - distance = other.distance; - nearest_points[0] = other.nearest_points[0]; - nearest_points[1] = other.nearest_points[1]; - link_names[0] = other.link_names[0]; - link_names[1] = other.link_names[1]; - body_types[0] = other.body_types[0]; - body_types[1] = other.body_types[1]; - normal = other.normal; - } - /// Compare if the distance is less than another bool operator<(const DistanceResultsData& other) { @@ -356,8 +351,10 @@ struct DistanceResultsData } }; -typedef std::map, std::vector > DistanceMap; +/** \brief Mapping between the names of the collision objects and the DistanceResultData. */ +using DistanceMap = std::map, std::vector >; +/** \brief Result of a distance request. */ struct DistanceResult { DistanceResult() : collision(false) @@ -381,6 +378,4 @@ struct DistanceResult distances.clear(); } }; -} - -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h index aed7eac019..05df2799a0 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h @@ -34,16 +34,14 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ +#pragma once -#include -#include +#include #include namespace collision_detection { -MOVEIT_CLASS_FORWARD(CollisionDetectorAllocator); +MOVEIT_CLASS_FORWARD(CollisionDetectorAllocator); // Defines CollisionDetectorAllocatorPtr, ConstPtr, WeakPtr... etc /** \brief An allocator for a compatible CollisionWorld/CollisionRobot pair. */ class CollisionDetectorAllocator @@ -57,22 +55,20 @@ class CollisionDetectorAllocator virtual const std::string& getName() const = 0; /** create a new CollisionWorld for checking collisions with the supplied world. */ - virtual CollisionWorldPtr allocateWorld(const WorldPtr& world) const = 0; + virtual CollisionEnvPtr allocateEnv(const WorldPtr& world, + const moveit::core::RobotModelConstPtr& robot_model) const = 0; /** create a new CollisionWorld by copying an existing CollisionWorld of the same type.s * The world must be either the same world as used by \orig or a copy of that world which has not yet been modified. */ - virtual CollisionWorldPtr allocateWorld(const CollisionWorldConstPtr& orig, const WorldPtr& world) const = 0; + virtual CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const = 0; - /** create a new CollisionRobot given a robot_model */ - virtual CollisionRobotPtr allocateRobot(const robot_model::RobotModelConstPtr& robot_model) const = 0; - - /** create a new CollisionRobot by copying an existing CollisionRobot of the same type. */ - virtual CollisionRobotPtr allocateRobot(const CollisionRobotConstPtr& orig) const = 0; + /** create a new CollisionEnv given a robot_model with a new empty world */ + virtual CollisionEnvPtr allocateEnv(const moveit::core::RobotModelConstPtr& robot_model) const = 0; }; /** \brief Template class to make it easy to create an allocator for a specific CollisionWorld/CollisionRobot pair. */ -template +template class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator { public: @@ -81,32 +77,25 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator return CollisionDetectorAllocatorType::NAME; } - CollisionWorldPtr allocateWorld(const WorldPtr& world) const override + CollisionEnvPtr allocateEnv(const WorldPtr& world, const moveit::core::RobotModelConstPtr& robot_model) const override { - return CollisionWorldPtr(new CollisionWorldType(world)); + return CollisionEnvPtr(new CollisionEnvType(robot_model, world)); } - CollisionWorldPtr allocateWorld(const CollisionWorldConstPtr& orig, const WorldPtr& world) const override + CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const override { - return CollisionWorldPtr(new CollisionWorldType(dynamic_cast(*orig), world)); + return CollisionEnvPtr(new CollisionEnvType(dynamic_cast(*orig), world)); } - CollisionRobotPtr allocateRobot(const robot_model::RobotModelConstPtr& robot_model) const override + CollisionEnvPtr allocateEnv(const moveit::core::RobotModelConstPtr& robot_model) const override { - return CollisionRobotPtr(new CollisionRobotType(robot_model)); + return CollisionEnvPtr(new CollisionEnvType(robot_model)); } - CollisionRobotPtr allocateRobot(const CollisionRobotConstPtr& orig) const override - { - return CollisionRobotPtr(new CollisionRobotType(dynamic_cast(*orig))); - } - - /** Create an allocator for FCL collision detectors */ + /** Create an allocator for collision detectors. */ static CollisionDetectorAllocatorPtr create() { return CollisionDetectorAllocatorPtr(new CollisionDetectorAllocatorType()); } }; -} - -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h similarity index 54% rename from moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h rename to moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h index 1b6d476a89..9177fae307 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h @@ -14,7 +14,7 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * Neither the name of Willow Garage nor the names of its + * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -32,49 +32,57 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Jens Petit */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_ROBOT_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_ROBOT_ +#pragma once #include #include #include #include #include +#include namespace collision_detection { -MOVEIT_CLASS_FORWARD(CollisionRobot); +MOVEIT_CLASS_FORWARD(CollisionEnv); // Defines CollisionEnvPtr, ConstPtr, WeakPtr... etc -/** @brief This class represents a collision model of the robot and can be used for self collision checks - (to check if the robot is in collision with itself) or in collision checks with a different robot. Collision checks - with - the environment are performed using the CollisionWorld class. */ -class CollisionRobot +/** \brief Provides the interface to the individual collision checking libraries. */ +class CollisionEnv { public: + CollisionEnv() = delete; + /** @brief Constructor * @param model A robot model to construct the collision robot from * @param padding The padding to use for all objects/links on the robot * @scale scale A common scaling to use for all objects/links on the robot */ - CollisionRobot(const robot_model::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); + CollisionEnv(const moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); - /** @brief A copy constructor*/ - CollisionRobot(const CollisionRobot& other); + /** @brief Constructor + * @param model A robot model to construct the collision robot from + * @param padding The padding to use for all objects/links on the robot + * @scale scale A common scaling to use for all objects/links on the robot + */ + CollisionEnv(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, + double scale = 1.0); + + /** \brief Copy constructor */ + CollisionEnv(const CollisionEnv& other, const WorldPtr& world); - virtual ~CollisionRobot() + virtual ~CollisionEnv() { } - /** @brief Check for self collision. Any collision between any pair of links is checked for, NO collisions are + /** @brief Check for robot self collision. Any collision between any pair of links is checked for, NO collisions are * ignored. + * * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result * @param state The kinematic state for which checks are being made */ virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const = 0; + const moveit::core::RobotState& state) const = 0; /** \brief Check for self collision. Allowed collisions specified by the allowed collision matrix are * taken into account. @@ -83,87 +91,80 @@ class CollisionRobot * @param state The kinematic state for which checks are being made * @param acm The allowed collision matrix. */ virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; - /** @brief Check for self collision in a continuous manner. Any collision between any pair of links is checked for, - * NO collisions are ignored. + /** \brief Check whether the robot model is in collision with itself or the world at a particular state. + * Any collision between any pair of links is checked for, NO collisions are ignored. * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result - * @param state1 The kinematic state at the start of the segment for which checks are being made - * @param state2 The kinematic state at the end of the segment for which checks are being made */ - virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const = 0; + * @param state The kinematic state for which checks are being made */ + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const; - /** \brief Check for self collision. Allowed collisions specified by the allowed collision matrix are - * taken into account. + /** \brief Check whether the robot model is in collision with itself or the world at a particular state. + * Allowed collisions specified by the allowed collision matrix are taken into account. * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result - * @param state1 The kinematic state at the start of the segment for which checks are being made - * @param state2 The kinematic state at the end of the segment for which checks are being made + * @param state The kinematic state for which checks are being made * @param acm The allowed collision matrix. */ - virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const = 0; + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const; - /** \brief Check for collision with a different robot (possibly a different kinematic model as well). - * Any collision between any pair of links is checked for, NO collisions are ignored. + /** \brief Check whether the robot model is in collision with the world. Any collisions between a robot link + * and the world are considered. Self collisions are not checked. * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result - * @param state The kinematic state for which checks are being made. - * @param other_robot The collision representation for the other robot - * @param other_state The kinematic state corresponding to the other robot */ - virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const = 0; - - /** \brief Check for collision with a different robot (possibly a different kinematic model as well). - * Allowed collisions specified by the allowed collision matrix are taken into account. + * @robot robot The collision model for the robot + * @param state The kinematic state for which checks are being made + */ + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const = 0; + + /** \brief Check whether the robot model is in collision with the world. + * Allowed collisions are ignored. Self collisions are not checked. * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result - * @param state The kinematic state for which checks are being made. - * @param other_robot The collision representation for the other robot - * @param other_state The kinematic state corresponding to the other robot - * @param acm The allowed collision matrix. */ - virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const = 0; + * @robot robot The collision model for the robot + * @param state The kinematic state for which checks are being made + * @param acm The allowed collision matrix.*/ + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; - /** \brief Check for collision with a different robot (possibly a different kinematic model as well), in a continuous - * fashion. - * Any collision between any pair of links is checked for, NO collisions are ignored. + /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot + * states). + * Allowed collisions are ignored. Self collisions are not checked. * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result - * @param state1 The kinematic state at the start of the segment for which checks are being made (this robot) - * @param state2 The kinematic state at the end of the segment for which checks are being made (this robot) - * @param other_robot The collision representation for the other robot - * @param other_state1 The kinematic state at the start of the segment for which checks are being made (other robot) - * @param other_state2 The kinematic state at the end of the segment for which checks are being made (other robot) */ - virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const = 0; - - /** \brief Check for collision with a different robot (possibly a different kinematic model as well), in a continuous - * fashion. - * Allowed collisions specified by the allowed collision matrix are taken into account. + * @robot robot The collision model for the robot + * @param state1 The kinematic state at the start of the segment for which checks are being made + * @param state2 The kinematic state at the end of the segment for which checks are being made + * @param acm The allowed collision matrix.*/ + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, + const AllowedCollisionMatrix& acm) const = 0; + + /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot + * states). + * Allowed collisions are ignored. Self collisions are not checked. * @param req A CollisionRequest object that encapsulates the collision request * @param res A CollisionResult object that encapsulates the collision result - * @param state1 The kinematic state at the start of the segment for which checks are being made (this robot) - * @param state2 The kinematic state at the end of the segment for which checks are being made (this robot) - * @param other_robot The collision representation for the other robot - * @param other_state1 The kinematic state at the start of the segment for which checks are being made (other robot) - * @param other_state2 The kinematic state at the end of the segment for which checks are being made (other robot) - * @param acm The allowed collision matrix. */ - virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const = 0; + * @robot robot The collision model for the robot + * @param state1 The kinematic state at the start of the segment for which checks are being made + * @param state2 The kinematic state at the end of the segment for which checks are being made + * @param acm The allowed collision matrix.*/ + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const = 0; + + /** \brief The distance to self-collision given the robot is at state \e state. + @param req A DistanceRequest object that encapsulates the distance request + @param res A DistanceResult object that encapsulates the distance result + @param state The state of this robot to consider */ + virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const = 0; /** \brief The distance to self-collision given the robot is at state \e state. */ - inline double distanceSelf(const robot_state::RobotState& state) const + inline double distanceSelf(const moveit::core::RobotState& state) const { DistanceRequest req; DistanceResult res; @@ -175,7 +176,7 @@ class CollisionRobot /** \brief The distance to self-collision given the robot is at state \e state, ignoring the distances between links that are allowed to always collide (as specified by \e acm) */ - inline double distanceSelf(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const + inline double distanceSelf(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { DistanceRequest req; DistanceResult res; @@ -186,61 +187,72 @@ class CollisionRobot return res.minimum_distance.distance; } - /** \brief The distance to self-collision given the robot is at state \e state. - @param req A DistanceRequest object that encapsulates the distance request - @param res A DistanceResult object that encapsulates the distance result - @param state The state of this robot to consider */ - virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const = 0; - - /** \brief The distance to another robot instance. - - - @param state The state of this robot to consider - @param other_robot The other robot instance to measure distance to - @param other_state The state of the other robot */ - inline double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const + /** \brief Compute the distance between a robot and the world + * @param req A DistanceRequest object that encapsulates the distance request + * @param res A DistanceResult object that encapsulates the distance result + * @param robot The robot to check distance for + * @param state The state for the robot to check distances from */ + virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const = 0; + + /** \brief Compute the shortest distance between a robot and the world + * @param robot The robot to check distance for + * @param state The state for the robot to check distances from + * @param verbose Output debug information about distance checks */ + inline double distanceRobot(const moveit::core::RobotState& state, bool verbose = false) const { DistanceRequest req; DistanceResult res; + req.verbose = verbose; req.enableGroup(getRobotModel()); - distanceOther(req, res, state, other_robot, other_state); + + distanceRobot(req, res, state); return res.minimum_distance.distance; } - /** \brief The distance to another robot instance, ignoring distances between links that are allowed to always - collide. - - - @param state The state of this robot to consider - @param other_robot The other robot instance to measure distance to - @param other_state The state of the other robot - @param acm The collision matrix specifying which links are allowed to always collide */ - inline double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, const AllowedCollisionMatrix& acm) const + /** \brief Compute the shortest distance between a robot and the world + * @param robot The robot to check distance for + * @param state The state for the robot to check distances from + * @param acm Using an allowed collision matrix has the effect of ignoring distances from links that are always + * allowed to be in collision. + * @param verbose Output debug information about distance checks */ + inline double distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, + bool verbose = false) const { DistanceRequest req; DistanceResult res; - req.enableGroup(getRobotModel()); req.acm = &acm; - distanceOther(req, res, state, other_robot, other_state); + req.verbose = verbose; + req.enableGroup(getRobotModel()); + + distanceRobot(req, res, state); return res.minimum_distance.distance; } - /** \brief The distance to self-collision given the robot is at state \e state. - @param req A DistanceRequest object that encapsulates the distance request - @param res A DistanceResult object that encapsulates the distance result - @param state The state of this robot to consider - @param other_robot The other robot instance to measure distance to - @param other_state The state of the other robot */ - virtual void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const = 0; + /** set the world to use. + * This can be expensive unless the new and old world are empty. + * Passing NULL will result in a new empty world being created. */ + virtual void setWorld(const WorldPtr& world); + + /** access the world geometry */ + const WorldPtr& getWorld() + { + return world_; + } + + /** access the world geometry */ + const WorldConstPtr& getWorld() const + { + return world_const_; + } + + using ObjectPtr = World::ObjectPtr; + using ObjectConstPtr = World::ObjectConstPtr; /** @brief The kinematic model corresponding to this collision model*/ - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -300,14 +312,16 @@ class CollisionRobot virtual void updatedPaddingOrScaling(const std::vector& links); /** @brief The kinematic model corresponding to this collision model*/ - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; /** @brief The internally maintained map (from link names to padding)*/ std::map link_padding_; /** @brief The internally maintained map (from link names to scaling)*/ std::map link_scale_; -}; -} -#endif +private: + WorldPtr world_; // The world always valid, never nullptr. + WorldConstPtr world_const_; // always same as world_ +}; +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index 59eb26e535..4b7a72910b 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, E. Gil Jones */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_MATRIX_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_MATRIX_ +#pragma once #include #include @@ -66,13 +65,13 @@ enum Type imply that the two bodies are in collision*/ CONDITIONAL }; -} +} // namespace AllowedCollision /** \brief Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type is * CONDITIONAL) */ -typedef boost::function DecideContactFn; +using DecideContactFn = boost::function; -MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); +MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc /** @class AllowedCollisionMatrix * @brief Definition of a structure for the allowed collision matrix. All elements in the collision world are referred @@ -93,7 +92,7 @@ class AllowedCollisionMatrix AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg); /** @brief Copy constructor */ - AllowedCollisionMatrix(const AllowedCollisionMatrix& acm); + AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) = default; /** @brief Get the type of the allowed collision between two elements. Return true if the entry is included in the * collision matrix. @@ -277,6 +276,4 @@ class AllowedCollisionMatrix std::map default_entries_; std::map default_allowed_contacts_; }; -} - -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h index 466d58a685..36069f95a5 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h @@ -34,11 +34,10 @@ /* Author: Adam Leeper */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_OCTOMAP_FILTER_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_OCTOMAP_FILTER_ +#pragma once #include -#include +#include namespace collision_detection { @@ -61,6 +60,4 @@ namespace collision_detection int refineContactNormals(const World::ObjectConstPtr& object, CollisionResult& res, double cell_bbx_search_distance = 1.0, double allowed_angle_divergence = 0.0, bool estimate_depth = false, double iso_value = 0.5, double metaball_radius_multiple = 1.5); -} - -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h index 156318f4bc..0873e6a816 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h @@ -32,17 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_PLUGIN_H -#define MOVEIT_COLLISION_DETECTION_COLLISION_PLUGIN_H +#pragma once #include -#include -#include +#include #include namespace collision_detection { -MOVEIT_CLASS_FORWARD(CollisionPlugin); +MOVEIT_CLASS_FORWARD(CollisionPlugin); // Defines CollisionPluginPtr, ConstPtr, WeakPtr... etc /** * @brief Plugin API for loading a custom collision detection robot/world. @@ -53,8 +51,7 @@ MOVEIT_CLASS_FORWARD(CollisionPlugin); * { * * class MyCollisionDetectorAllocator : - * public collision_detection::CollisionDetectorAllocatorTemplate + * public collision_detection::CollisionDetectorAllocatorTemplate * { * public: * static const std::string NAME_; @@ -94,5 +91,3 @@ class CollisionPlugin }; } // namespace collision_detection - -#endif // MOVEIT_COLLISION_DETECTION_COLLISION_PLUGIN_H diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h index 6893c41444..bf7e2430bc 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_TOOLS_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_TOOLS_ +#pragma once #include #include @@ -71,6 +70,4 @@ bool getSensorPositioning(geometry_msgs::Point& point, const std::set - -#include -#include -#include -#include - -/** \brief Generic interface to collision detection */ -namespace collision_detection -{ -MOVEIT_CLASS_FORWARD(CollisionWorld); - -/** \brief Perform collision checking with the environment. The - * collision world maintains a representation of the environment - * that the robot is operating in. */ -class CollisionWorld : private boost::noncopyable -{ -public: - CollisionWorld(); - - explicit CollisionWorld(const WorldPtr& world); - - /** \brief A copy constructor. \e other should not be changed while the copy constructor is running. - * world must be the same world as used by other or a (not-yet-modified) copy of the world used by other */ - CollisionWorld(const CollisionWorld& other, const WorldPtr& world); - - virtual ~CollisionWorld() - { - } - - /**********************************************************************/ - /* Collision Checking Routines */ - /**********************************************************************/ - - /** \brief Check whether the robot model is in collision with itself or the world at a particular state. - * Any collision between any pair of links is checked for, NO collisions are ignored. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @param state The kinematic state for which checks are being made */ - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const; - - /** \brief Check whether the robot model is in collision with itself or the world at a particular state. - * Allowed collisions specified by the allowed collision matrix are taken into account. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @param state The kinematic state for which checks are being made - * @param acm The allowed collision matrix. */ - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; - - /** \brief Check whether the robot model is in collision with itself or the world in a continuous manner - * (between two robot states) - * Any collision between any pair of links is checked for, NO collisions are ignored. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @param state1 The kinematic state at the start of the segment for which checks are being made - * @param state2 The kinematic state at the end of the segment for which checks are being made */ - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2) const; - - /** \brief Check whether the robot model is in collision with itself or the world in a continuous manner - * (between two robot states). - * Allowed collisions specified by the allowed collision matrix are taken into account. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @param state1 The kinematic state at the start of the segment for which checks are being made - * @param state2 The kinematic state at the end of the segment for which checks are being made - * @param acm The allowed collision matrix. */ - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const; - - /** \brief Check whether the robot model is in collision with the world. Any collisions between a robot link - * and the world are considered. Self collisions are not checked. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @robot robot The collision model for the robot - * @param state The kinematic state for which checks are being made - */ - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const = 0; - - /** \brief Check whether the robot model is in collision with the world. - * Allowed collisions are ignored. Self collisions are not checked. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @robot robot The collision model for the robot - * @param state The kinematic state for which checks are being made - * @param acm The allowed collision matrix.*/ - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; - - /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot - * states). - * Any collisions between a robot link and the world are considered. Self collisions are not checked. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @robot robot The collision model for the robot - * @param state1 The kinematic state at the start of the segment for which checks are being made - * @param state2 The kinematic state at the end of the segment for which checks are being made */ - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const = 0; - - /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot - * states). - * Allowed collisions are ignored. Self collisions are not checked. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @robot robot The collision model for the robot - * @param state1 The kinematic state at the start of the segment for which checks are being made - * @param state2 The kinematic state at the end of the segment for which checks are being made - * @param acm The allowed collision matrix.*/ - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const = 0; - - /** \brief Check whether a given set of objects is in collision with objects from another world. - * Any contacts are considered. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @param other_world The other collision world - */ - virtual void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const = 0; - - /** \brief Check whether a given set of objects is in collision with objects from another world. - * Allowed collisions are ignored. Any contacts are considered. - * @param req A CollisionRequest object that encapsulates the collision request - * @param res A CollisionResult object that encapsulates the collision result - * @param other_world The other collision world - * @param acm The allowed collision matrix.*/ - virtual void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, - const AllowedCollisionMatrix& acm) const = 0; - - /** \brief Compute the shortest distance between a robot and the world - * @param robot The robot to check distance for - * @param state The state for the robot to check distances from - * @param verbose Output debug information about distance checks */ - inline double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state, - bool verbose = false) const - { - DistanceRequest req; - DistanceResult res; - - req.verbose = verbose; - req.enableGroup(robot.getRobotModel()); - - distanceRobot(req, res, robot, state); - return res.minimum_distance.distance; - } - - /** \brief Compute the shortest distance between a robot and the world - * @param robot The robot to check distance for - * @param state The state for the robot to check distances from - * @param acm Using an allowed collision matrix has the effect of ignoring distances from links that are always - * allowed to be in collision. - * @param verbose Output debug information about distance checks */ - inline double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm, bool verbose = false) const - { - DistanceRequest req; - DistanceResult res; - - req.acm = &acm; - req.verbose = verbose; - req.enableGroup(robot.getRobotModel()); - - distanceRobot(req, res, robot, state); - return res.minimum_distance.distance; - } - - /** \brief Compute the distance between a robot and the world - * @param req A DistanceRequest object that encapsulates the distance request - * @param res A DistanceResult object that encapsulates the distance result - * @param robot The robot to check distance for - * @param state The state for the robot to check distances from */ - virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const = 0; - - /** \brief The shortest distance to another world instance (\e world) - * @param verbose Output debug information about distance checks */ - inline double distanceWorld(const CollisionWorld& world, bool verbose = false) const - { - DistanceRequest req; - DistanceResult res; - - req.verbose = verbose; - distanceWorld(req, res, world); - - return res.minimum_distance.distance; - } - - /** \brief The shortest distance to another world instance (\e world), ignoring the distances between world elements - * that are allowed to collide (as specified by \e acm) - * @param verbose Output debug information about distance checks */ - inline double distanceWorld(const CollisionWorld& world, const AllowedCollisionMatrix& acm, - bool verbose = false) const - { - DistanceRequest req; - DistanceResult res; - - req.acm = &acm; - req.verbose = verbose; - distanceWorld(req, res, world); - - return res.minimum_distance.distance; - } - - /** \brief Compute the distance between another world - * @param req A DistanceRequest object that encapsulates the distance request - * @param res A DistanceResult object that encapsulates the distance result - * @param world The world to check distance for */ - virtual void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const = 0; - - /** set the world to use. - * This can be expensive unless the new and old world are empty. - * Passing NULL will result in a new empty world being created. */ - virtual void setWorld(const WorldPtr& world); - - /** access the world geometry */ - const WorldPtr& getWorld() - { - return world_; - } - - /** access the world geometry */ - const WorldConstPtr& getWorld() const - { - return world_const_; - } - - typedef World::ObjectPtr ObjectPtr; - typedef World::ObjectConstPtr ObjectConstPtr; - -private: - WorldPtr world_; // The world. Always valid. Never NULL. - WorldConstPtr world_const_; // always same as world_ -}; -} - -#endif diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h new file mode 100644 index 0000000000..35d8002977 --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -0,0 +1,310 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +/** \brief Brings the panda robot in user defined home position */ +inline void setToHome(moveit::core::RobotState& panda_state) +{ + panda_state.setToDefaultValues(); + double joint2 = -0.785; + double joint4 = -2.356; + double joint6 = 1.571; + double joint7 = 0.785; + panda_state.setJointPositions("panda_joint2", &joint2); + panda_state.setJointPositions("panda_joint4", &joint4); + panda_state.setJointPositions("panda_joint6", &joint6); + panda_state.setJointPositions("panda_joint7", &joint7); + panda_state.update(); +} + +template +class CollisionDetectorPandaTest : public testing::Test +{ +public: + std::shared_ptr value_; + +protected: + void SetUp() override + { + value_.reset(new CollisionAllocatorType); + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + robot_model_ok_ = static_cast(robot_model_); + + acm_.reset(new collision_detection::AllowedCollisionMatrix()); + // Use default collision operations in the SRDF to setup the acm + const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); + acm_->setEntry(collision_links, collision_links, false); + + // allow collisions for pairs that have been disabled + const std::vector& dc = robot_model_->getSRDF()->getDisabledCollisionPairs(); + for (const srdf::Model::DisabledCollision& it : dc) + acm_->setEntry(it.link1_, it.link2_, true); + + cenv_ = value_->allocateEnv(robot_model_); + + robot_state_.reset(new moveit::core::RobotState(robot_model_)); + setToHome(*robot_state_); + } + + void TearDown() override + { + } + + bool robot_model_ok_; + + moveit::core::RobotModelPtr robot_model_; + + collision_detection::CollisionEnvPtr cenv_; + + collision_detection::AllowedCollisionMatrixPtr acm_; + + moveit::core::RobotStatePtr robot_state_; +}; + +TYPED_TEST_CASE_P(CollisionDetectorPandaTest); + +/** \brief Correct setup testing. */ +TYPED_TEST_P(CollisionDetectorPandaTest, InitOK) +{ + ASSERT_TRUE(this->robot_model_ok_); +} + +/** \brief Tests the default values specified in the SRDF if they are collision free. */ +TYPED_TEST_P(CollisionDetectorPandaTest, DefaultNotInCollision) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); +} + +/** \brief A configuration where the robot should collide with itself. */ +TYPED_TEST_P(CollisionDetectorPandaTest, LinksInCollision) +{ + // Sets the joints into a colliding configuration + double joint2 = 0.15; + double joint4 = -3.0; + this->robot_state_->setJointPositions("panda_joint2", &joint2); + this->robot_state_->setJointPositions("panda_joint4", &joint4); + this->robot_state_->update(); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); +} + +/** \brief Adding obstacles to the world which are tested against the robot. Simple cases. */ +TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_1) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + shapes::Shape* shape = new shapes::Box(.1, .1, .1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + pos1.translation().z() = 0.3; + this->cenv_->getWorld()->addToObject("box", shape_ptr, pos1); + + this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + this->cenv_->getWorld()->moveObject("box", pos1); + ASSERT_FALSE(res.collision); +} + +/** \brief Adding obstacles to the world which are tested against the robot. */ +TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_2) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + req.max_contacts = 10; + req.contacts = true; + req.verbose = true; + + shapes::Shape* shape = new shapes::Box(.4, .4, .4); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + pos1.translation().z() = 0.3; + this->cenv_->getWorld()->addToObject("box", shape_ptr, pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + ASSERT_GE(res.contact_count, 3u); + res.clear(); +} + +/** \brief Tests the padding through expanding the link geometry in such a way that a collision occurs. */ +TYPED_TEST_P(CollisionDetectorPandaTest, PaddingTest) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 10; + collision_detection::CollisionResult res; + + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + // Adding the box right in front of the robot hand + shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + pos.translation().x() = 0.43; + pos.translation().y() = 0; + pos.translation().z() = 0.55; + this->cenv_->getWorld()->addToObject("box", shape_ptr, pos); + + this->cenv_->setLinkPadding("panda_hand", 0.08); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + this->cenv_->setLinkPadding("panda_hand", 0.0); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); +} + +/** \brief Tests the distance reporting with the robot itself */ +TYPED_TEST_P(CollisionDetectorPandaTest, DistanceSelf) +{ + collision_detection::CollisionRequest req; + req.distance = true; + collision_detection::CollisionResult res; + this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + EXPECT_NEAR(res.distance, 0.022, 0.001); +} + +TYPED_TEST_P(CollisionDetectorPandaTest, DistanceWorld) +{ + collision_detection::CollisionRequest req; + req.distance = true; + collision_detection::CollisionResult res; + + // Adding the box right in front of the robot hand + shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + pos.translation().x() = 0.43; + pos.translation().y() = 0; + pos.translation().z() = 0.55; + this->cenv_->getWorld()->addToObject("box", shape_ptr, pos); + + this->cenv_->setLinkPadding("panda_hand", 0.0); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + EXPECT_NEAR(res.distance, 0.029, 0.01); +} + +template +class DistanceCheckPandaTest : public CollisionDetectorPandaTest +{ +}; + +TYPED_TEST_CASE_P(DistanceCheckPandaTest); + +TYPED_TEST_P(DistanceCheckPandaTest, DistanceSingle) +{ + std::set active_components{ this->robot_model_->getLinkModel("panda_hand") }; + collision_detection::DistanceRequest req; + req.type = collision_detection::DistanceRequestTypes::SINGLE; + req.active_components_only = &active_components; + req.enable_signed_distance = true; + + random_numbers::RandomNumberGenerator rng(0x47110815); + double min_distance = std::numeric_limits::max(); + for (int i = 0; i < 10; ++i) + { + collision_detection::DistanceResult res; + + shapes::ShapeConstPtr shape(new shapes::Cylinder(rng.uniform01(), rng.uniform01())); + Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; + pose.translation() = + Eigen::Vector3d(rng.uniformReal(0.1, 2.0), rng.uniformReal(0.1, 2.0), rng.uniformReal(1.2, 1.7)); + double quat[4]; + rng.quaternion(quat); + pose.linear() = Eigen::Quaterniond(quat[0], quat[1], quat[2], quat[3]).toRotationMatrix(); + + this->cenv_->getWorld()->addToObject("collection", shape, pose); + this->cenv_->getWorld()->removeObject("object"); + this->cenv_->getWorld()->addToObject("object", shape, pose); + + this->cenv_->distanceRobot(req, res, *this->robot_state_); + auto& distances1 = res.distances[std::pair("collection", "panda_hand")]; + auto& distances2 = res.distances[std::pair("object", "panda_hand")]; + ASSERT_EQ(distances1.size(), 1u) << "no distance reported for collection/panda_hand"; + ASSERT_EQ(distances2.size(), 1u) << "no distance reported for object/panda_hand"; + + double collection_distance = distances1[0].distance; + min_distance = std::min(min_distance, distances2[0].distance); + ASSERT_NEAR(collection_distance, min_distance, 1e-5) + << "distance to collection is greater than distance to minimum of individual objects after " << i << " rounds"; + } +} + +REGISTER_TYPED_TEST_CASE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision, + RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, DistanceSelf, DistanceWorld); + +REGISTER_TYPED_TEST_CASE_P(DistanceCheckPandaTest, DistanceSingle); diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h similarity index 68% rename from moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp rename to moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h index e606f632a3..75030d8a56 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h @@ -36,10 +36,10 @@ #include #include -#include -#include #include - +#include +#include +#include #include #include @@ -49,59 +49,64 @@ #include #include -typedef collision_detection::CollisionWorldFCL DefaultCWorldType; -typedef collision_detection::CollisionRobotFCL DefaultCRobotType; - -class FclCollisionDetectionTester : public testing::Test +template +class CollisionDetectorTest : public ::testing::Test { +public: + std::shared_ptr value_; + protected: + CollisionDetectorTest() + { + } + void SetUp() override { + value_.reset(new CollisionAllocatorType); robot_model_ = moveit::core::loadTestingRobotModel("pr2"); robot_model_ok_ = static_cast(robot_model_); - kinect_dae_resource_ = "package://moveit_resources/pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae"; + kinect_dae_resource_ = "package://moveit_resources_pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae"; acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); - crobot_.reset(new DefaultCRobotType(robot_model_)); - cworld_.reset(new DefaultCWorldType()); + cenv_ = value_->allocateEnv(robot_model_); } void TearDown() override { } -protected: bool robot_model_ok_; - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; - collision_detection::CollisionRobotPtr crobot_; - collision_detection::CollisionWorldPtr cworld_; + collision_detection::CollisionEnvPtr cenv_; collision_detection::AllowedCollisionMatrixPtr acm_; std::string kinect_dae_resource_; }; -TEST_F(FclCollisionDetectionTester, InitOK) +TYPED_TEST_CASE_P(CollisionDetectorTest); + +TYPED_TEST_P(CollisionDetectorTest, InitOK) { - ASSERT_TRUE(robot_model_ok_); + ASSERT_TRUE(this->robot_model_ok_); } -TEST_F(FclCollisionDetectionTester, DefaultNotInCollision) +TYPED_TEST_P(CollisionDetectorTest, DefaultNotInCollision) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_FALSE(res.collision); } -TEST_F(FclCollisionDetectionTester, LinksInCollision) +TYPED_TEST_P(CollisionDetectorTest, LinksInCollision) { collision_detection::CollisionRequest req; collision_detection::CollisionResult res1; @@ -110,7 +115,7 @@ TEST_F(FclCollisionDetectionTester, LinksInCollision) // req.contacts = true; // req.max_contacts = 100; - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -123,12 +128,12 @@ TEST_F(FclCollisionDetectionTester, LinksInCollision) robot_state.updateStateWithLinkAt("base_bellow_link", offset); robot_state.update(); - acm_->setEntry("base_link", "base_bellow_link", false); - crobot_->checkSelfCollision(req, res1, robot_state, *acm_); + this->acm_->setEntry("base_link", "base_bellow_link", false); + this->cenv_->checkSelfCollision(req, res1, robot_state, *this->acm_); ASSERT_TRUE(res1.collision); - acm_->setEntry("base_link", "base_bellow_link", true); - crobot_->checkSelfCollision(req, res2, robot_state, *acm_); + this->acm_->setEntry("base_link", "base_bellow_link", true); + this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_); ASSERT_FALSE(res2.collision); // req.verbose = true; @@ -138,18 +143,18 @@ TEST_F(FclCollisionDetectionTester, LinksInCollision) robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset); robot_state.update(); - acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); - crobot_->checkSelfCollision(req, res3, robot_state, *acm_); + this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); + this->cenv_->checkSelfCollision(req, res3, robot_state, *this->acm_); ASSERT_TRUE(res3.collision); } -TEST_F(FclCollisionDetectionTester, ContactReporting) +TYPED_TEST_P(CollisionDetectorTest, ContactReporting) { collision_detection::CollisionRequest req; req.contacts = true; req.max_contacts = 1; - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -167,11 +172,11 @@ TEST_F(FclCollisionDetectionTester, ContactReporting) robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset); robot_state.update(); - acm_->setEntry("base_link", "base_bellow_link", false); - acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); + this->acm_->setEntry("base_link", "base_bellow_link", false); + this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); collision_detection::CollisionResult res; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); EXPECT_EQ(res.contacts.size(), 1u); EXPECT_EQ(res.contacts.begin()->second.size(), 1u); @@ -180,7 +185,7 @@ TEST_F(FclCollisionDetectionTester, ContactReporting) req.max_contacts = 2; req.max_contacts_per_pair = 1; // req.verbose = true; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); EXPECT_EQ(res.contacts.size(), 2u); EXPECT_EQ(res.contacts.begin()->second.size(), 1u); @@ -190,20 +195,20 @@ TEST_F(FclCollisionDetectionTester, ContactReporting) req.max_contacts = 10; req.max_contacts_per_pair = 2; - acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false)); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + this->acm_.reset(new collision_detection::AllowedCollisionMatrix(this->robot_model_->getLinkModelNames(), false)); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); EXPECT_LE(res.contacts.size(), 10u); EXPECT_LE(res.contact_count, 10u); } -TEST_F(FclCollisionDetectionTester, ContactPositions) +TYPED_TEST_P(CollisionDetectorTest, ContactPositions) { collision_detection::CollisionRequest req; req.contacts = true; req.max_contacts = 1; - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -219,10 +224,10 @@ TEST_F(FclCollisionDetectionTester, ContactPositions) robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); robot_state.update(); - acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); + this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); collision_detection::CollisionResult res; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); ASSERT_EQ(res.contacts.size(), 1u); ASSERT_EQ(res.contacts.begin()->second.size(), 1u); @@ -234,7 +239,8 @@ TEST_F(FclCollisionDetectionTester, ContactPositions) } pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); - pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0)); + pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * + Eigen::Quaterniond(sqrt(1 - pow(0.258, 2)), 0.0, 0.258, 0.0)); // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); @@ -242,7 +248,7 @@ TEST_F(FclCollisionDetectionTester, ContactPositions) robot_state.update(); collision_detection::CollisionResult res2; - crobot_->checkSelfCollision(req, res2, robot_state, *acm_); + this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_); ASSERT_TRUE(res2.collision); ASSERT_EQ(res2.contacts.size(), 1u); ASSERT_EQ(res2.contacts.begin()->second.size(), 1u); @@ -254,7 +260,8 @@ TEST_F(FclCollisionDetectionTester, ContactPositions) } pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); - pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0)); + pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * + Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0).normalized()); // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); @@ -262,18 +269,18 @@ TEST_F(FclCollisionDetectionTester, ContactPositions) robot_state.update(); collision_detection::CollisionResult res3; - crobot_->checkSelfCollision(req, res2, robot_state, *acm_); + this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_); ASSERT_FALSE(res3.collision); } -TEST_F(FclCollisionDetectionTester, AttachedBodyTester) +TYPED_TEST_P(CollisionDetectorTest, AttachedBodyTester) { collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); + this->acm_.reset(new collision_detection::AllowedCollisionMatrix(this->robot_model_->getLinkModelNames(), true)); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -283,18 +290,18 @@ TEST_F(FclCollisionDetectionTester, AttachedBodyTester) // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); robot_state.update(); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_FALSE(res.collision); shapes::Shape* shape = new shapes::Box(.1, .1, .1); - cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1); + this->cenv_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1); res = collision_detection::CollisionResult(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_); + this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); // deletes shape - cworld_->getWorld()->removeObject("box"); + this->cenv_->getWorld()->removeObject("box"); shape = new shapes::Box(.1, .1, .1); std::vector shapes; @@ -305,7 +312,7 @@ TEST_F(FclCollisionDetectionTester, AttachedBodyTester) robot_state.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link"); res = collision_detection::CollisionResult(); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); // deletes shape @@ -318,39 +325,38 @@ TEST_F(FclCollisionDetectionTester, AttachedBodyTester) robot_state.update(); res = collision_detection::CollisionResult(); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); ASSERT_FALSE(res.collision); pos1.translation().x() = 5.01; shapes::Shape* coll = new shapes::Box(.1, .1, .1); - cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1); + this->cenv_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1); res = collision_detection::CollisionResult(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_); + this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); - acm_->setEntry("coll", "r_gripper_palm_link", true); + this->acm_->setEntry("coll", "r_gripper_palm_link", true); res = collision_detection::CollisionResult(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_); + this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_); ASSERT_TRUE(res.collision); } -TEST_F(FclCollisionDetectionTester, DiffSceneTester) +TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - collision_detection::CollisionRobotFCL new_crobot( - *(dynamic_cast(crobot_.get()))); + collision_detection::CollisionEnvPtr new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld()); ros::WallTime before = ros::WallTime::now(); - new_crobot.checkSelfCollision(req, res, robot_state); + new_cenv->checkSelfCollision(req, res, robot_state); double first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); - new_crobot.checkSelfCollision(req, res, robot_state); + new_cenv->checkSelfCollision(req, res, robot_state); double second_check = (ros::WallTime::now() - before).toSec(); EXPECT_LT(fabs(first_check - second_check), .05); @@ -358,7 +364,7 @@ TEST_F(FclCollisionDetectionTester, DiffSceneTester) std::vector shapes; shapes.resize(1); - shapes[0].reset(shapes::createMeshFromResource(kinect_dae_resource_)); + shapes[0].reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); EigenSTL::vector_Isometry3d poses; poses.push_back(Eigen::Isometry3d::Identity()); @@ -367,57 +373,56 @@ TEST_F(FclCollisionDetectionTester, DiffSceneTester) robot_state.attachBody("kinect", shapes, poses, touch_links, "r_gripper_palm_link"); before = ros::WallTime::now(); - new_crobot.checkSelfCollision(req, res, robot_state); + new_cenv->checkSelfCollision(req, res, robot_state); first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); - new_crobot.checkSelfCollision(req, res, robot_state); + new_cenv->checkSelfCollision(req, res, robot_state); second_check = (ros::WallTime::now() - before).toSec(); // the first check is going to take a while, as data must be constructed EXPECT_LT(second_check, .1); - collision_detection::CollisionRobotFCL other_new_crobot( - *(dynamic_cast(crobot_.get()))); + collision_detection::CollisionEnvPtr other_new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld()); before = ros::WallTime::now(); - new_crobot.checkSelfCollision(req, res, robot_state); + new_cenv->checkSelfCollision(req, res, robot_state); first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); - new_crobot.checkSelfCollision(req, res, robot_state); + new_cenv->checkSelfCollision(req, res, robot_state); second_check = (ros::WallTime::now() - before).toSec(); EXPECT_LT(fabs(first_check - second_check), .05); } -TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached) +TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) { collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - shapes::ShapeConstPtr shape(shapes::createMeshFromResource(kinect_dae_resource_)); + shapes::ShapeConstPtr shape(shapes::createMeshFromResource(this->kinect_dae_resource_)); Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity(); pos2.translation().x() = 10.0; - cworld_->getWorld()->addToObject("kinect", shape, pos1); + this->cenv_->getWorld()->addToObject("kinect", shape, pos1); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(this->robot_model_); robot_state.setToDefaultValues(); robot_state.update(); ros::WallTime before = ros::WallTime::now(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state); + this->cenv_->checkRobotCollision(req, res, robot_state); double first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state); + this->cenv_->checkRobotCollision(req, res, robot_state); double second_check = (ros::WallTime::now() - before).toSec(); EXPECT_LT(second_check, .05); - collision_detection::CollisionWorld::ObjectConstPtr object = cworld_->getWorld()->getObject("kinect"); - cworld_->getWorld()->removeObject("kinect"); + collision_detection::CollisionEnv::ObjectConstPtr object = this->cenv_->getWorld()->getObject("kinect"); + this->cenv_->getWorld()->removeObject("kinect"); - robot_state::RobotState robot_state1(robot_model_); - robot_state::RobotState robot_state2(robot_model_); + moveit::core::RobotState robot_state1(this->robot_model_); + moveit::core::RobotState robot_state2(this->robot_model_); robot_state1.setToDefaultValues(); robot_state2.setToDefaultValues(); robot_state1.update(); @@ -434,24 +439,24 @@ TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached) // going to take a while, but that's fine res = collision_detection::CollisionResult(); - crobot_->checkSelfCollision(req, res, robot_state1); + this->cenv_->checkSelfCollision(req, res, robot_state1); EXPECT_TRUE(res.collision); before = ros::WallTime::now(); - crobot_->checkSelfCollision(req, res, robot_state1, *acm_); + this->cenv_->checkSelfCollision(req, res, robot_state1, *this->acm_); first_check = (ros::WallTime::now() - before).toSec(); before = ros::WallTime::now(); req.verbose = true; res = collision_detection::CollisionResult(); - crobot_->checkSelfCollision(req, res, robot_state2, *acm_); + this->cenv_->checkSelfCollision(req, res, robot_state2, *this->acm_); second_check = (ros::WallTime::now() - before).toSec(); EXPECT_LT(first_check, .05); EXPECT_LT(fabs(first_check - second_check), .1); } -TEST_F(FclCollisionDetectionTester, TestCollisionMapAdditionSpeed) +TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed) { EigenSTL::vector_Isometry3d poses; std::vector shapes; @@ -461,41 +466,42 @@ TEST_F(FclCollisionDetectionTester, TestCollisionMapAdditionSpeed) shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01))); } ros::WallTime start = ros::WallTime::now(); - cworld_->getWorld()->addToObject("map", shapes, poses); + this->cenv_->getWorld()->addToObject("map", shapes, poses); double t = (ros::WallTime::now() - start).toSec(); - EXPECT_GE(1.0, t); + // TODO: investigate why bullet collision checking is considerably slower here + EXPECT_GE(5.0, t); // this is not really a failure; it is just that slow; // looking into doing collision checking with a voxel grid. - ROS_INFO_NAMED("collision_detection.fcl", "Adding boxes took %g", t); + ROS_INFO_NAMED("collision_detection.bullet", "Adding boxes took %g", t); } -TEST_F(FclCollisionDetectionTester, MoveMesh) +TYPED_TEST_P(CollisionDetectorTest, MoveMesh) { - robot_state::RobotState robot_state1(robot_model_); + moveit::core::RobotState robot_state1(this->robot_model_); robot_state1.setToDefaultValues(); robot_state1.update(); Eigen::Isometry3d kinect_pose; kinect_pose.setIdentity(); shapes::ShapePtr kinect_shape; - kinect_shape.reset(shapes::createMeshFromResource(kinect_dae_resource_)); + kinect_shape.reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); - cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); + this->cenv_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); Eigen::Isometry3d np; for (unsigned int i = 0; i < 5; i++) { np = Eigen::Translation3d(i * .001, i * .001, i * .001) * Eigen::Quaterniond::Identity(); - cworld_->getWorld()->moveShapeInObject("kinect", kinect_shape, np); + this->cenv_->getWorld()->moveShapeInObject("kinect", kinect_shape, np); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - cworld_->checkCollision(req, res, *crobot_, robot_state1, *acm_); + this->cenv_->checkCollision(req, res, robot_state1, *this->acm_); } } -TEST_F(FclCollisionDetectionTester, TestChangingShapeSize) +TYPED_TEST_P(CollisionDetectorTest, TestChangingShapeSize) { - robot_state::RobotState robot_state1(robot_model_); + moveit::core::RobotState robot_state1(this->robot_model_); robot_state1.setToDefaultValues(); robot_state1.update(); @@ -508,43 +514,41 @@ TEST_F(FclCollisionDetectionTester, TestChangingShapeSize) std::vector shapes; for (unsigned int i = 0; i < 5; i++) { - cworld_->getWorld()->removeObject("shape"); + this->cenv_->getWorld()->removeObject("shape"); shapes.clear(); poses.clear(); shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001))); poses.push_back(Eigen::Isometry3d::Identity()); - cworld_->getWorld()->addToObject("shape", shapes, poses); + this->cenv_->getWorld()->addToObject("shape", shapes, poses); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - cworld_->checkCollision(req, res, *crobot_, robot_state1, *acm_); + this->cenv_->checkCollision(req, res, robot_state1, *this->acm_); ASSERT_TRUE(res.collision); } Eigen::Isometry3d kinect_pose = Eigen::Isometry3d::Identity(); shapes::ShapePtr kinect_shape; - kinect_shape.reset(shapes::createMeshFromResource(kinect_dae_resource_)); - cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); + kinect_shape.reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); + this->cenv_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); collision_detection::CollisionRequest req2; collision_detection::CollisionResult res2; - cworld_->checkCollision(req2, res2, *crobot_, robot_state1, *acm_); + this->cenv_->checkCollision(req2, res2, robot_state1, *this->acm_); ASSERT_TRUE(res2.collision); for (unsigned int i = 0; i < 5; i++) { - cworld_->getWorld()->removeObject("shape"); + this->cenv_->getWorld()->removeObject("shape"); shapes.clear(); poses.clear(); shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001))); poses.push_back(Eigen::Isometry3d::Identity()); - cworld_->getWorld()->addToObject("shape", shapes, poses); + this->cenv_->getWorld()->addToObject("shape", shapes, poses); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - cworld_->checkCollision(req, res, *crobot_, robot_state1, *acm_); + this->cenv_->checkCollision(req, res, robot_state1, *this->acm_); ASSERT_TRUE(res.collision); } } -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +REGISTER_TYPED_TEST_CASE_P(CollisionDetectorTest, InitOK, DefaultNotInCollision, LinksInCollision, ContactReporting, + ContactPositions, AttachedBodyTester, DiffSceneTester, ConvertObjectToAttached, + TestCollisionMapAdditionSpeed, MoveMesh, TestChangingShapeSize); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index f461043f4a..c9a52c3e6b 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -34,15 +34,13 @@ /* Author: Ioan Sucan, Acorn Pooley, Sachin Chitta */ -#ifndef MOVEIT_COLLISION_DETECTION_WORLD_ -#define MOVEIT_COLLISION_DETECTION_WORLD_ +#pragma once #include #include #include #include -#include #include #include #include @@ -50,12 +48,12 @@ namespace shapes { -MOVEIT_CLASS_FORWARD(Shape); +MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc } namespace collision_detection { -MOVEIT_CLASS_FORWARD(World); +MOVEIT_CLASS_FORWARD(World); // Defines WorldPtr, ConstPtr, WeakPtr... etc /** \brief Maintain a representation of the environment */ class World @@ -121,7 +119,7 @@ class World ObjectConstPtr getObject(const std::string& object_id) const; /** iterator over the objects in the world. */ - typedef std::map::const_iterator const_iterator; + using const_iterator = std::map::const_iterator; /** iterator pointing to first change */ const_iterator begin() const { @@ -152,12 +150,14 @@ class World /** \brief Get the transform to an object or subframe with given name. * If name does not exist, a std::runtime_error is thrown. - * A subframe name needs to be prefixed with the object's name separated by a slash. */ + * A subframe name needs to be prefixed with the object's name separated by a slash. + * The returned transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getTransform(const std::string& name) const; /** \brief Get the transform to an object or subframe with given name. * If name does not exist, returns an identity transform and sets frame_found to false. - * A subframe name needs to be prefixed with the object's name separated by a slash. */ + * A subframe name needs to be prefixed with the object's name separated by a slash. + * The returned transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getTransform(const std::string& name, bool& frame_found) const; /** \brief Add shapes to an object in the map. @@ -242,7 +242,7 @@ class World class ObserverHandle { public: - ObserverHandle() : observer_(NULL) + ObserverHandle() : observer_(nullptr) { } @@ -254,7 +254,7 @@ class World friend class World; }; - typedef boost::function ObserverCallbackFn; + using ObserverCallbackFn = boost::function; /** \brief register a callback function for notification of changes. * \e callback will be called right after any change occurs to any Object. @@ -271,7 +271,7 @@ class World private: /** notify all observers of a change */ - void notify(const ObjectConstPtr&, Action); + void notify(const ObjectConstPtr& /*obj*/, Action /*action*/); /** send notification of change to all objects. */ void notifyAll(Action action); @@ -288,7 +288,7 @@ class World /** The objects maintained in the world */ std::map objects_; - /* observers to call when something changes */ + /** Wrapper for a callback function to call when something changes in the world */ class Observer { public: @@ -297,8 +297,8 @@ class World } ObserverCallbackFn callback_; }; + + /// All registered observers of this world representation std::vector observers_; }; -} - -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h index 5f2013d7f0..1ffbceafda 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h @@ -34,17 +34,14 @@ /* Author: Acorn Pooley, Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_COLLISION_DETECTION_WORLD_DIFF_ -#define MOVEIT_COLLISION_DETECTION_WORLD_DIFF_ +#pragma once #include #include -#include - namespace collision_detection { -MOVEIT_CLASS_FORWARD(WorldDiff); +MOVEIT_CLASS_FORWARD(WorldDiff); // Defines WorldDiffPtr, ConstPtr, WeakPtr... etc /** \brief Maintain a diff list of changes that have happened to a World. */ class WorldDiff @@ -79,7 +76,7 @@ class WorldDiff return changes_; } - typedef std::map::const_iterator const_iterator; + using const_iterator = std::map::const_iterator; /** iterator pointing to first change */ const_iterator begin() const { @@ -114,7 +111,7 @@ class WorldDiff private: /** \brief Notification function */ - void notify(const World::ObjectConstPtr&, World::Action); + void notify(const World::ObjectConstPtr& /*obj*/, World::Action /*action*/); /** keep changes in a map so they can be coalesced */ std::map changes_; @@ -125,6 +122,4 @@ class WorldDiff /* used to unregister the notifier */ WorldWeakPtr world_; }; -} - -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp similarity index 53% rename from moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp rename to moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp index 023f308a62..9f6c842b5a 100644 --- a/moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp +++ b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp @@ -32,118 +32,104 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Jia Pan, Jens Petit */ -#include +#include +#include -collision_detection::CollisionWorldAllValid::CollisionWorldAllValid() : CollisionWorld() -{ -} +const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME("ALL_VALID"); -collision_detection::CollisionWorldAllValid::CollisionWorldAllValid(const WorldPtr& world) : CollisionWorld(world) +collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, + double padding, double scale) + : CollisionEnv(robot_model, padding, scale) { } -collision_detection::CollisionWorldAllValid::CollisionWorldAllValid(const CollisionWorld& other, const WorldPtr& world) - : CollisionWorld(other, world) +collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, + const WorldPtr& world, double padding, double scale) + : CollisionEnv(robot_model, world, padding, scale) { } -void collision_detection::CollisionWorldAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state) const +collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world) + : CollisionEnv(other, world) { - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionWorldAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const +void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const { res.collision = false; if (req.verbose) ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionWorldAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const +void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const { res.collision = false; if (req.verbose) ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionWorldAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const +void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const { res.collision = false; if (req.verbose) ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionWorldAllValid::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const +void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, + const AllowedCollisionMatrix& acm) const { res.collision = false; if (req.verbose) ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionWorldAllValid::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world, - const AllowedCollisionMatrix& acm) const +void collision_detection::CollisionEnvAllValid::distanceRobot(const collision_detection::DistanceRequest& req, + collision_detection::DistanceResult& res, + const moveit::core::RobotState& state) const { res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -double collision_detection::CollisionWorldAllValid::distanceRobot(const CollisionRobot& robot, - const robot_state::RobotState& state) const +double collision_detection::CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& state) const { return 0.0; } -double collision_detection::CollisionWorldAllValid::distanceRobot(const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const +double collision_detection::CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const { return 0.0; } -void collision_detection::CollisionWorldAllValid::distanceRobot(const collision_detection::DistanceRequest& req, - collision_detection::DistanceResult& res, - const collision_detection::CollisionRobot& robot, - const moveit::core::RobotState& state) const +void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const { res.collision = false; + if (req.verbose) + ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -double collision_detection::CollisionWorldAllValid::distanceWorld(const CollisionWorld& world) const -{ - return 0.0; -} - -double collision_detection::CollisionWorldAllValid::distanceWorld(const CollisionWorld& world, - const AllowedCollisionMatrix& acm) const +void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const { - return 0.0; + res.collision = false; + if (req.verbose) + ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); } -void collision_detection::CollisionWorldAllValid::distanceWorld(const collision_detection::DistanceRequest& req, - collision_detection::DistanceResult& res, - const collision_detection::CollisionWorld& world) const +void collision_detection::CollisionEnvAllValid::distanceSelf(const collision_detection::DistanceRequest& req, + collision_detection::DistanceResult& res, + const moveit::core::RobotState& state) const { res.collision = false; } - -#include -const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME("ALL_VALID"); diff --git a/moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp deleted file mode 100644 index 963f374711..0000000000 --- a/moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp +++ /dev/null @@ -1,171 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan, Jia Pan */ - -#include - -collision_detection::CollisionRobotAllValid::CollisionRobotAllValid(const robot_model::RobotModelConstPtr& robot_model, - double padding, double scale) - : CollisionRobot(robot_model, padding, scale) -{ -} - -collision_detection::CollisionRobotAllValid::CollisionRobotAllValid(const CollisionRobot& other) : CollisionRobot(other) -{ -} - -void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const -{ - res.collision = false; - if (req.verbose) - ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed."); -} - -double collision_detection::CollisionRobotAllValid::distanceSelf(const robot_state::RobotState& state) const -{ - return 0.0; -} - -double collision_detection::CollisionRobotAllValid::distanceSelf(const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - return 0.0; -} - -void collision_detection::CollisionRobotAllValid::distanceSelf(const collision_detection::DistanceRequest& req, - collision_detection::DistanceResult& res, - const moveit::core::RobotState& state) const -{ - res.collision = false; -} - -double collision_detection::CollisionRobotAllValid::distanceOther(const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - return 0.0; -} - -double collision_detection::CollisionRobotAllValid::distanceOther(const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const -{ - return 0.0; -} - -void collision_detection::CollisionRobotAllValid::distanceOther(const collision_detection::DistanceRequest& req, - collision_detection::DistanceResult& res, - const moveit::core::RobotState& state, - const collision_detection::CollisionRobot& other_robot, - const moveit::core::RobotState& other_state) const -{ - res.collision = false; -} diff --git a/moveit_core/collision_detection/src/collision_common.cpp b/moveit_core/collision_detection/src/collision_common.cpp new file mode 100644 index 0000000000..0f54dfdfd3 --- /dev/null +++ b/moveit_core/collision_detection/src/collision_common.cpp @@ -0,0 +1,61 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +static const char LOGNAME[] = "collision_common"; +constexpr size_t LOG_THROTTLE_PERIOD = 5; + +namespace collision_detection +{ +void CollisionResult::print() const +{ + if (!contacts.empty()) + { + ROS_WARN_STREAM_THROTTLE_NAMED(LOG_THROTTLE_PERIOD, LOGNAME, + "Objects in collision (printing 1st of " + << contacts.size() << " pairs): " << contacts.begin()->first.first << ", " + << contacts.begin()->first.second); + + // Log all collisions at the debug level + ROS_DEBUG_STREAM_THROTTLE_NAMED(LOG_THROTTLE_PERIOD, LOGNAME, "Objects in collision:"); + for (const auto& contact : contacts) + { + ROS_DEBUG_STREAM_THROTTLE_NAMED(LOG_THROTTLE_PERIOD, LOGNAME, + "\t" << contact.first.first << ", " << contact.first.second); + } + } +} + +} // namespace collision_detection diff --git a/moveit_core/collision_detection/src/collision_robot.cpp b/moveit_core/collision_detection/src/collision_env.cpp similarity index 62% rename from moveit_core/collision_detection/src/collision_robot.cpp rename to moveit_core/collision_detection/src/collision_env.cpp index c3e27751a6..701ca6f354 100644 --- a/moveit_core/collision_detection/src/collision_robot.cpp +++ b/moveit_core/collision_detection/src/collision_env.cpp @@ -14,7 +14,7 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * Neither the name of Willow Garage nor the names of its + * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -32,9 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Jens Petit */ -#include +#include #include static inline bool validateScale(double scale) @@ -69,15 +69,15 @@ static inline bool validatePadding(double padding) namespace collision_detection { -CollisionRobot::CollisionRobot(const robot_model::RobotModelConstPtr& model, double padding, double scale) - : robot_model_(model) +CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, double padding, double scale) + : robot_model_(model), world_(new World()), world_const_(world_) { if (!validateScale(scale)) scale = 1.0; if (!validatePadding(padding)) padding = 0.0; - const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); for (auto link : links) { link_padding_[link->getName()] = padding; @@ -85,18 +85,35 @@ CollisionRobot::CollisionRobot(const robot_model::RobotModelConstPtr& model, dou } } -CollisionRobot::CollisionRobot(const CollisionRobot& other) : robot_model_(other.robot_model_) +CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding, + double scale) + : robot_model_(model), world_(world), world_const_(world_) +{ + if (!validateScale(scale)) + scale = 1.0; + if (!validatePadding(padding)) + padding = 0.0; + + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + for (auto link : links) + { + link_padding_[link->getName()] = padding; + link_scale_[link->getName()] = scale; + } +} + +CollisionEnv::CollisionEnv(const CollisionEnv& other, const WorldPtr& world) + : robot_model_(other.robot_model_), world_(world), world_const_(world) { link_padding_ = other.link_padding_; link_scale_ = other.link_scale_; } - -void CollisionRobot::setPadding(double padding) +void CollisionEnv::setPadding(double padding) { if (!validatePadding(padding)) return; std::vector u; - const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); for (auto link : links) { if (getLinkPadding(link->getName()) != padding) @@ -107,12 +124,12 @@ void CollisionRobot::setPadding(double padding) updatedPaddingOrScaling(u); } -void CollisionRobot::setScale(double scale) +void CollisionEnv::setScale(double scale) { if (!validateScale(scale)) return; std::vector u; - const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); for (auto link : links) { if (getLinkScale(link->getName()) != scale) @@ -123,8 +140,9 @@ void CollisionRobot::setScale(double scale) updatedPaddingOrScaling(u); } -void CollisionRobot::setLinkPadding(const std::string& link_name, double padding) +void CollisionEnv::setLinkPadding(const std::string& link_name, double padding) { + validatePadding(padding); bool update = getLinkPadding(link_name) != padding; link_padding_[link_name] = padding; if (update) @@ -134,7 +152,7 @@ void CollisionRobot::setLinkPadding(const std::string& link_name, double padding } } -double CollisionRobot::getLinkPadding(const std::string& link_name) const +double CollisionEnv::getLinkPadding(const std::string& link_name) const { auto it = link_padding_.find(link_name); if (it != link_padding_.end()) @@ -143,11 +161,12 @@ double CollisionRobot::getLinkPadding(const std::string& link_name) const return 0.0; } -void CollisionRobot::setLinkPadding(const std::map& padding) +void CollisionEnv::setLinkPadding(const std::map& padding) { std::vector u; for (const auto& link_pad_pair : padding) { + validatePadding(link_pad_pair.second); bool update = getLinkPadding(link_pad_pair.first) != link_pad_pair.second; link_padding_[link_pad_pair.first] = link_pad_pair.second; if (update) @@ -157,13 +176,14 @@ void CollisionRobot::setLinkPadding(const std::map& padding updatedPaddingOrScaling(u); } -const std::map& CollisionRobot::getLinkPadding() const +const std::map& CollisionEnv::getLinkPadding() const { return link_padding_; } -void CollisionRobot::setLinkScale(const std::string& link_name, double scale) +void CollisionEnv::setLinkScale(const std::string& link_name, double scale) { + validateScale(scale); bool update = getLinkScale(link_name) != scale; link_scale_[link_name] = scale; if (update) @@ -173,7 +193,7 @@ void CollisionRobot::setLinkScale(const std::string& link_name, double scale) } } -double CollisionRobot::getLinkScale(const std::string& link_name) const +double CollisionEnv::getLinkScale(const std::string& link_name) const { auto it = link_scale_.find(link_name); if (it != link_scale_.end()) @@ -182,7 +202,7 @@ double CollisionRobot::getLinkScale(const std::string& link_name) const return 1.0; } -void CollisionRobot::setLinkScale(const std::map& scale) +void CollisionEnv::setLinkScale(const std::map& scale) { std::vector u; for (const auto& link_scale_pair : scale) @@ -196,16 +216,17 @@ void CollisionRobot::setLinkScale(const std::map& scale) updatedPaddingOrScaling(u); } -const std::map& CollisionRobot::getLinkScale() const +const std::map& CollisionEnv::getLinkScale() const { return link_scale_; } -void CollisionRobot::setPadding(const std::vector& padding) +void CollisionEnv::setPadding(const std::vector& padding) { std::vector u; for (const auto& p : padding) { + validatePadding(p.padding); bool update = getLinkPadding(p.link_name) != p.padding; link_padding_[p.link_name] = p.padding; if (update) @@ -215,11 +236,12 @@ void CollisionRobot::setPadding(const std::vector& pad updatedPaddingOrScaling(u); } -void CollisionRobot::setScale(const std::vector& scale) +void CollisionEnv::setScale(const std::vector& scale) { std::vector u; for (const auto& s : scale) { + validateScale(s.scale); bool update = getLinkScale(s.link_name) != s.scale; link_scale_[s.link_name] = s.scale; if (update) @@ -229,7 +251,7 @@ void CollisionRobot::setScale(const std::vector& scale) updatedPaddingOrScaling(u); } -void CollisionRobot::getPadding(std::vector& padding) const +void CollisionEnv::getPadding(std::vector& padding) const { padding.clear(); for (const auto& lp : link_padding_) @@ -241,7 +263,7 @@ void CollisionRobot::getPadding(std::vector& padding) } } -void CollisionRobot::getScale(std::vector& scale) const +void CollisionEnv::getScale(std::vector& scale) const { scale.clear(); for (const auto& ls : link_scale_) @@ -253,8 +275,33 @@ void CollisionRobot::getScale(std::vector& scale) const } } -void CollisionRobot::updatedPaddingOrScaling(const std::vector& links) +void CollisionEnv::updatedPaddingOrScaling(const std::vector& links) +{ +} + +void CollisionEnv::setWorld(const WorldPtr& world) +{ + world_ = world; + if (!world_) + world_.reset(new World()); + + world_const_ = world; +} + +void CollisionEnv::checkCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const +{ + checkSelfCollision(req, res, state); + if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) + checkRobotCollision(req, res, state); +} + +void CollisionEnv::checkCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const { + checkSelfCollision(req, res, state, acm); + if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) + checkRobotCollision(req, res, state, acm); } } // end of namespace collision_detection diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index 7d359abb3b..05ea5b66ba 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -73,14 +73,6 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisi } } -AllowedCollisionMatrix::AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) -{ - entries_ = acm.entries_; - allowed_contacts_ = acm.allowed_contacts_; - default_entries_ = acm.default_entries_; - default_allowed_contacts_ = acm.default_allowed_contacts_; -} - bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const { auto it1 = allowed_contacts_.find(name1); @@ -191,8 +183,7 @@ void AllowedCollisionMatrix::removeEntry(const std::string& name1, const std::st } } -void AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector& other_names, - bool allowed) +void AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector& other_names, bool allowed) { for (const auto& other_name : other_names) if (other_name != name) diff --git a/moveit_core/collision_detection/src/collision_octomap_filter.cpp b/moveit_core/collision_detection/src/collision_octomap_filter.cpp index 5305727973..051a7afd25 100644 --- a/moveit_core/collision_detection/src/collision_octomap_filter.cpp +++ b/moveit_core/collision_detection/src/collision_octomap_filter.cpp @@ -36,7 +36,6 @@ #include #include -#include #include #include #include diff --git a/moveit_core/collision_detection/src/collision_tools.cpp b/moveit_core/collision_detection/src/collision_tools.cpp index d1ba1f4b39..8071c6ddfa 100644 --- a/moveit_core/collision_detection/src/collision_tools.cpp +++ b/moveit_core/collision_detection/src/collision_tools.cpp @@ -168,8 +168,7 @@ void intersectCostSources(std::set& cost_sources, const std::set= tmp.aabb_max[0] || tmp.aabb_min[1] >= tmp.aabb_max[1] || - tmp.aabb_min[2] >= tmp.aabb_max[2]) + if (tmp.aabb_min[0] >= tmp.aabb_max[0] || tmp.aabb_min[1] >= tmp.aabb_max[1] || tmp.aabb_min[2] >= tmp.aabb_max[2]) continue; tmp.cost = std::max(source_a.cost, source_b.cost); cost_sources.insert(tmp); diff --git a/moveit_core/collision_detection/src/collision_world.cpp b/moveit_core/collision_detection/src/collision_world.cpp deleted file mode 100644 index ec2f3abcf9..0000000000 --- a/moveit_core/collision_detection/src/collision_world.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include - -namespace collision_detection -{ -CollisionWorld::CollisionWorld() : world_(new World()), world_const_(world_) -{ -} - -CollisionWorld::CollisionWorld(const WorldPtr& world) : world_(world), world_const_(world) -{ -} - -CollisionWorld::CollisionWorld(const CollisionWorld& other, const WorldPtr& world) : world_(world), world_const_(world) -{ -} - -void CollisionWorld::checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - robot.checkSelfCollision(req, res, state); - if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) - checkRobotCollision(req, res, robot, state); -} - -void CollisionWorld::checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const -{ - robot.checkSelfCollision(req, res, state, acm); - if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) - checkRobotCollision(req, res, robot, state, acm); -} - -void CollisionWorld::checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2) const -{ - robot.checkSelfCollision(req, res, state1, state2); - if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) - checkRobotCollision(req, res, robot, state1, state2); -} - -void CollisionWorld::checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - robot.checkSelfCollision(req, res, state1, state2, acm); - if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) - checkRobotCollision(req, res, robot, state1, state2, acm); -} - -void CollisionWorld::setWorld(const WorldPtr& world) -{ - world_ = world; - if (!world_) - world_.reset(new World()); - - world_const_ = world; -} - -} // end of namespace collision_detection \ No newline at end of file diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index 4fbf16e76c..383f298471 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -35,6 +35,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ #include +#include #include #include @@ -59,6 +60,7 @@ inline void World::addToObjectInternal(const ObjectPtr& obj, const shapes::Shape const Eigen::Isometry3d& pose) { obj->shapes_.push_back(shape); + ASSERT_ISOMETRY(pose) // unsanitized input, could contain a non-isometry obj->shape_poses_.push_back(pose); } @@ -143,10 +145,10 @@ bool World::knowsTransform(const std::string& name) const std::map::const_iterator it = objects_.find(name); if (it != objects_.end()) // only accept object name as frame if it is associated to a unique shape - return it->second->shape_poses_.size() == 1; + return !it->second->shape_poses_.empty(); else // Then objects' subframes { - for (const std::pair& object : objects_) + for (const std::pair& object : objects_) { // if "object name/" matches start of object_id, we found the matching object if (boost::starts_with(name, object.first) && name[object.first.length()] == '/') @@ -168,26 +170,30 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name) const const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& frame_found) const { + // assume found + frame_found = true; + std::map::const_iterator it = objects_.find(name); if (it != objects_.end()) - return it->second->shape_poses_[0]; + { + if (!it->second->shape_poses_.empty()) + return it->second->shape_poses_[0]; + } else // Search within subframes { - for (const std::pair& object : objects_) + for (const std::pair& object : objects_) { // if "object name/" matches start of object_id, we found the matching object if (boost::starts_with(name, object.first) && name[object.first.length()] == '/') { auto it = object.second->subframe_poses_.find(name.substr(object.first.length() + 1)); if (it != object.second->subframe_poses_.end()) - { - frame_found = true; return it->second; - } } } } + // we need a persisting isometry for the API static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity(); frame_found = false; return IDENTITY_TRANSFORM; @@ -204,6 +210,7 @@ bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeC if (it->second->shapes_[i] == shape) { ensureUnique(it->second); + ASSERT_ISOMETRY(pose) // unsanitized input, could contain a non-isometry it->second->shape_poses_[i] = pose; notify(it->second, MOVE_SHAPE); @@ -223,6 +230,7 @@ bool World::moveObject(const std::string& object_id, const Eigen::Isometry3d& tr ensureUnique(it->second); for (size_t i = 0, n = it->second->shapes_.size(); i < n; ++i) { + ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry it->second->shape_poses_[i] = transform * it->second->shape_poses_[i]; } notify(it->second, MOVE_SHAPE); @@ -282,6 +290,10 @@ bool World::setSubframesOfObject(const std::string& object_id, const moveit::cor { return false; } + for (const auto& t : subframe_poses) + { + ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry + } obj_pair->second->subframe_poses_ = subframe_poses; return true; } @@ -314,8 +326,8 @@ void World::notifyAll(Action action) void World::notify(const ObjectConstPtr& obj, Action action) { - for (std::vector::const_iterator obs = observers_.begin(); obs != observers_.end(); ++obs) - (*obs)->callback_(obj, action); + for (Observer* observer : observers_) + observer->callback_(obj, action); } void World::notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const diff --git a/moveit_core/collision_detection/test/test_all_valid.cpp b/moveit_core/collision_detection/test/test_all_valid.cpp index 47a33a81cf..94c0bc36d7 100644 --- a/moveit_core/collision_detection/test/test_all_valid.cpp +++ b/moveit_core/collision_detection/test/test_all_valid.cpp @@ -36,26 +36,24 @@ #include #include -#include -#include +#include TEST(AllValid, Instantiate) { using namespace collision_detection; CollisionDetectorAllocatorAllValid allocator; - CollisionWorldAllValid world; urdf::ModelInterfaceSharedPtr urdf_model; urdf_model.reset(new urdf::ModelInterface()); srdf::ModelConstSharedPtr srdf_model; srdf_model.reset(new srdf::Model()); - robot_model::RobotModelConstPtr robot_model; - robot_model.reset(new robot_model::RobotModel(urdf_model, srdf_model)); - CollisionRobotAllValid robot(robot_model); + moveit::core::RobotModelConstPtr robot_model; + robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model)); + CollisionEnvAllValid env(robot_model); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt new file mode 100644 index 0000000000..775fba1bd3 --- /dev/null +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -0,0 +1,43 @@ +set(MOVEIT_LIB_NAME moveit_collision_detection_bullet) + +add_library(${MOVEIT_LIB_NAME} + src/bullet_integration/bullet_utils.cpp + src/bullet_integration/bullet_discrete_bvh_manager.cpp + src/bullet_integration/bullet_cast_bvh_manager.cpp + src/collision_env_bullet.cpp + src/bullet_integration/bullet_bvh_manager.cpp +) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +target_link_libraries(${MOVEIT_LIB_NAME} moveit_collision_detection + ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} + ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES} + ${BULLET_LIBRARIES}) +add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) + +add_library(collision_detector_bullet_plugin src/collision_detector_bullet_plugin_loader.cpp) +set_target_properties(collision_detector_bullet_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(collision_detector_bullet_plugin ${catkin_LIBRARIES} ${MOVEIT_LIB_NAME}) + +install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_bullet_plugin + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + +install(FILES ../collision_detector_bullet_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +if(CATKIN_ENABLE_TESTING) + catkin_add_gtest(test_bullet_collision_detection test/test_bullet_collision_detection_pr2.cpp) + target_link_libraries(test_bullet_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_bullet_collision_detection PRIVATE -Wno-deprecated-declarations) + + catkin_add_gtest(test_bullet_collision_detection_panda test/test_bullet_collision_detection_panda.cpp) + target_link_libraries(test_bullet_collision_detection_panda moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_bullet_collision_detection_panda PRIVATE -Wno-deprecated-declarations) + + catkin_add_gtest(test_bullet_continuous_collision_checking test/test_bullet_continuous_collision_checking.cpp) + target_link_libraries(test_bullet_continuous_collision_checking moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) +endif() diff --git a/moveit_core/collision_detection_bullet/README.md b/moveit_core/collision_detection_bullet/README.md new file mode 100644 index 0000000000..63b95739f6 --- /dev/null +++ b/moveit_core/collision_detection_bullet/README.md @@ -0,0 +1,8 @@ +### Using Bullet as a collision checker [experimental] +To use Bullet as a collision checker you can manually switch to Bullet using the planning_scene.setActiveCollisionDetector() + +### Speed Benchmarks +For speed comparisons to FCL please see (relative link to README of benchmark script will be added). + +### Current Limitations +The collision detection using Bullet is not thread safe as the internal collision managers are `mutable` members of the collision environment. diff --git a/moveit_core/collision_detection_bullet/empty_description.xml b/moveit_core/collision_detection_bullet/empty_description.xml new file mode 100644 index 0000000000..3ef0fa6791 --- /dev/null +++ b/moveit_core/collision_detection_bullet/empty_description.xml @@ -0,0 +1,2 @@ + + diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h new file mode 100644 index 0000000000..d243c030be --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h @@ -0,0 +1,83 @@ +/********************************************************************* + * Software License Agreement (Apache License) + * + * Copyright (c) 2013, Southwest Research Institute + * All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace collision_detection_bullet +{ +template +using AlignedVector = std::vector>; + +template +using AlignedMap = std::map, Eigen::aligned_allocator>>; + +template +using AlignedUnorderedMap = std::unordered_map, std::equal_to, + Eigen::aligned_allocator>>; + +enum class CollisionObjectType +{ + USE_SHAPE_TYPE = 0, /**< @brief Infer the type from the type specified in the shapes::Shape class */ + + // all of the following convert the meshes to custom collision objects + CONVEX_HULL = 1, /**< @brief Use the mesh in shapes::Shape but make it a convex hulls collision object (if not convex + it will be converted) */ + MULTI_SPHERE = 2, /**< @brief Use the mesh and represent it by multiple spheres collision object */ + SDF = 3 /**< @brief Use the mesh and rpresent it by a signed distance fields collision object */ +}; + +/** \brief Bundles the data for a collision query */ +struct ContactTestData +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ContactTestData(const std::vector& active, const double& contact_distance, + collision_detection::CollisionResult& res, const collision_detection::CollisionRequest& req) + : active(active), contact_distance(contact_distance), res(res), req(req), done(false), pair_done(false) + { + } + + const std::vector& active; + + /** \brief If after a positive broadphase check the distance is below this threshold, a contact is added. */ + const double& contact_distance; + + collision_detection::CollisionResult& res; + const collision_detection::CollisionRequest& req; + + /// Indicates if search is finished + bool done; + + /// Indicates if search between a single pair is finished + bool pair_done; +}; + +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h new file mode 100644 index 0000000000..140f684f16 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h @@ -0,0 +1,157 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Levi Armstrong, Jens Petit */ + +#pragma once + +#include +#include + +namespace collision_detection_bullet +{ +MOVEIT_CLASS_FORWARD(BulletBVHManager) + +/** @brief A bounding volume hierarchy (BVH) implementation of a tesseract contact manager */ +class BulletBVHManager +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Constructor */ + BulletBVHManager(); + + virtual ~BulletBVHManager(); + + /** @brief Clone the manager + * + * This is to be used for multi threaded applications. A user should make a clone for each thread. */ + BulletBVHManagerPtr clone() const; + + /**@brief Find if a collision object already exists + * @param name The name of the collision object + * @return true if it exists, otherwise false. */ + bool hasCollisionObject(const std::string& name) const; + + /**@brief Remove an object from the checker + * @param name The name of the object + * @return true if successfully removed, otherwise false. */ + bool removeCollisionObject(const std::string& name); + + /**@brief Enable an object + * @param name The name of the object + * @return true if successfully enabled, otherwise false. */ + bool enableCollisionObject(const std::string& name); + + /**@brief Disable an object + * @param name The name of the object + * @return true if successfully disabled, otherwise false. */ + bool disableCollisionObject(const std::string& name); + + /**@brief Set a single static collision object's tansform + * @param name The name of the object + * @param pose The tranformation in world */ + void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose); + + /**@brief Set which collision objects are active + * @param names A vector of collision object names */ + void setActiveCollisionObjects(const std::vector& names); + + /**@brief Get which collision objects are active + * @return A vector of collision object names */ + const std::vector& getActiveCollisionObjects() const; + + /**@brief Set the contact distance threshold for which collision should be considered through expanding the AABB by + * the + * contact_distance for all links. + * @param contact_distance The contact distance */ + void setContactDistanceThreshold(double contact_distance); + + /**@brief Get the contact distance threshold + * @return The contact distance */ + double getContactDistanceThreshold() const; + + /**@brief Perform a contact test for all objects + * @param collisions The Contact results data + * @param req The collision request data + * @param acm The allowed collision matrix + * @param self Used for indicating self collision checks */ + virtual void contactTest(collision_detection::CollisionResult& collisions, + const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, bool self) = 0; + + /**@brief Add a collision object to the checker + * + * All objects are added should initially be added as static objects. Use the setContactRequest method of defining + * which collision objects are moving. + * + * @param name The name of the object, must be unique. + * @param mask_id User defined id which gets stored in the results structure. + * @param shapes A vector of shapes that make up the collision object. + * @param shape_poses A vector of poses for each shape, must be same length as shapes + * @param shape_types A vector of shape types for encode the collision object. If the vector is of length 1 it is + * used for all shapes. + * @param collision_object_types A int identifying a conversion mode for the object. (ex. convert meshes to convex + * hulls) + * @param enabled Indicate if the object is enabled for collision checking. + * @return true if successfully added, otherwise false. */ + /**@brief Add a tesseract collision object to the manager + * @param cow The tesseract bullet collision object */ + virtual void addCollisionObject(const CollisionObjectWrapperPtr& cow) = 0; + + const std::map& getCollisionObjects() const; + +protected: + /** @brief A map of collision objects being managed */ + std::map link2cow_; + + /** @brief A list of the active collision links */ + std::vector active_; + + /** @brief The contact distance threshold */ + double contact_distance_; + + /** @brief The bullet collision dispatcher used for getting object to object collison algorithm */ + std::unique_ptr dispatcher_; + + /** @brief The bullet collision dispatcher configuration information */ + btDispatcherInfo dispatch_info_; + + /** @brief The bullet collision configuration */ + btDefaultCollisionConfiguration coll_config_; + + /** @brief The bullet broadphase interface */ + std::unique_ptr broadphase_; + + /** \brief Callback function for culling objects before a broadphase check */ + BroadphaseFilterCallback filter_callback_; +}; +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h new file mode 100644 index 0000000000..69c856575b --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h @@ -0,0 +1,81 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Levi Armstrong, Jens Petit */ + +#pragma once + +#include +#include +#include + +namespace collision_detection_bullet +{ +MOVEIT_CLASS_FORWARD(BulletCastBVHManager) + +/** @brief A bounding volume hierarchy (BVH) implementation of a tesseract contact manager */ +class BulletCastBVHManager : public BulletBVHManager +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Constructor */ + BulletCastBVHManager() = default; + + ~BulletCastBVHManager() override = default; + + /**@brief Clone the manager + * + * This is to be used for multi threaded applications. A user should make a clone for each thread. */ + BulletCastBVHManagerPtr clone() const; + + /**@brief Set a single cast (moving) collision object's tansforms + * + * This should only be used for moving objects. + * + * @param name The name of the object + * @param pose1 The start tranformation in world + * @param pose2 The end tranformation in world */ + void setCastCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose1, + const Eigen::Isometry3d& pose2); + + /**@brief Perform a contact test for all objects + * @param collisions The Contact results data + * @param req The collision request data + * @param acm The allowed collision matrix */ + void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, bool self) override; + + /**@brief Add a tesseract collision object to the manager + * @param cow The tesseract bullet collision object */ + void addCollisionObject(const CollisionObjectWrapperPtr& cow) override; +}; +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h new file mode 100644 index 0000000000..d749d5c209 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h @@ -0,0 +1,71 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Levi Armstrong, Jens Petit */ + +#pragma once + +#include +#include +#include + +namespace collision_detection_bullet +{ +MOVEIT_CLASS_FORWARD(BulletDiscreteBVHManager) + +/** @brief A bounding volume hierarchy (BVH) implementaiton of a discrete bullet manager */ +class BulletDiscreteBVHManager : public BulletBVHManager +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Constructor */ + BulletDiscreteBVHManager() = default; + + ~BulletDiscreteBVHManager() override = default; + + /**@brief Clone the manager + * + * This is to be used for multi threaded applications. A user should make a clone for each thread. */ + BulletDiscreteBVHManagerPtr clone() const; + + /**@brief Perform a contact test for all objects in the manager + * @param collisions The Contact results data + * @param acm The allowed collision matrix + * @param req The contact request */ + void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, bool self) override; + + /**@brief Add a bullet collision object to the manager + * @param cow The bullet collision object */ + void addCollisionObject(const CollisionObjectWrapperPtr& cow) override; +}; +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h new file mode 100644 index 0000000000..26ed9e5928 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h @@ -0,0 +1,970 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * Copyright (c) 2013, John Schulman + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: John Schulman, Levi Armstrong */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace collision_detection_bullet +{ +#define METERS + +const btScalar BULLET_MARGIN = 0.0f; +const btScalar BULLET_SUPPORT_FUNC_TOLERANCE = 0.01f METERS; +const btScalar BULLET_LENGTH_TOLERANCE = 0.001f METERS; +const btScalar BULLET_EPSILON = 1e-3f; // numerical precision limit +const btScalar BULLET_DEFAULT_CONTACT_DISTANCE = 0.00f; // All pairs closer than this distance get reported +const bool BULLET_COMPOUND_USE_DYNAMIC_AABB = true; + +MOVEIT_CLASS_FORWARD(CollisionObjectWrapper) + +/** \brief Allowed = true */ +inline bool acmCheck(const std::string& body_1, const std::string& body_2, + const collision_detection::AllowedCollisionMatrix* acm) +{ + collision_detection::AllowedCollision::Type allowed_type; + + if (acm != nullptr) + { + if (acm->getEntry(body_1, body_2, allowed_type)) + { + if (allowed_type == collision_detection::AllowedCollision::Type::NEVER) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not allowed entry in ACM found, collision check between " + << body_1 << " and " << body_2); + return false; + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Entry in ACM found, skipping collision check as allowed " + << body_1 << " and " << body_2); + return true; + } + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "No entry in ACM found, collision check between " << body_1 << " and " << body_2); + return false; + } + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "No ACM, collision check between " << body_1 << " and " << body_2); + return false; + } +} + +/** \brief Converts eigen vector to bullet vector */ +inline btVector3 convertEigenToBt(const Eigen::Vector3d& v) +{ + return btVector3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2])); +} + +/** \brief Converts bullet vector to eigen vector */ +inline Eigen::Vector3d convertBtToEigen(const btVector3& v) +{ + return Eigen::Vector3d(static_cast(v.x()), static_cast(v.y()), static_cast(v.z())); +} + +/** \brief Converts eigen quaternion to bullet quaternion */ +inline btQuaternion convertEigenToBt(const Eigen::Quaterniond& q) +{ + return btQuaternion(static_cast(q.x()), static_cast(q.y()), static_cast(q.z()), + static_cast(q.w())); +} + +/** \brief Converts eigen matrix to bullet matrix */ +inline btMatrix3x3 convertEigenToBt(const Eigen::Matrix3d& r) +{ + return btMatrix3x3(static_cast(r(0, 0)), static_cast(r(0, 1)), static_cast(r(0, 2)), + static_cast(r(1, 0)), static_cast(r(1, 1)), static_cast(r(1, 2)), + static_cast(r(2, 0)), static_cast(r(2, 1)), static_cast(r(2, 2))); +} + +/** \brief Converts bullet transform to eigen transform */ +inline btTransform convertEigenToBt(const Eigen::Isometry3d& t) +{ + const Eigen::Matrix3d& rot = t.matrix().block<3, 3>(0, 0); + const Eigen::Vector3d& tran = t.translation(); + + btMatrix3x3 mat = convertEigenToBt(rot); + btVector3 translation = convertEigenToBt(tran); + + return btTransform(mat, translation); +} + +/** @brief Tesseract bullet collision object. + * + * A wrapper around bullet's collision object which contains specific information related to bullet. One of the main + * differences is that a bullet collision object has a single world transformation and all shapes have transformation + * relative to this world transform. The default collision object category is active and active objects are checked + * against active objects and static objects whereas static objects are only checked against active ones. */ +class CollisionObjectWrapper : public btCollisionObject +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Standard constructor + * + * \param shape_poses Assumes all poses are in a single global frame */ + CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, bool active = true); + + /** \brief Constructor for attached robot objects */ + CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::set& touch_links); + + /** \brief Bitfield specifies to which group the object belongs */ + short int m_collisionFilterGroup; + + /** \brief Bitfield specifies against which other groups the object is collision checked */ + short int m_collisionFilterMask; + + /** \brief Indicates if the collision object is used for a collision check */ + bool m_enabled{ true }; + + /** \brief The robot links the collision objects is allowed to touch */ + std::set m_touch_links; + + /** @brief Get the collision object name */ + const std::string& getName() const + { + return m_name; + } + + /** @brief Get a user defined type */ + const collision_detection::BodyType& getTypeID() const + { + return m_type_id; + } + + /** \brief Check if two CollisionObjectWrapper objects point to the same source object + * \return True if same objects, false otherwise */ + bool sameObject(const CollisionObjectWrapper& other) const + { + return m_name == other.m_name && m_type_id == other.m_type_id && m_shapes.size() == other.m_shapes.size() && + m_shape_poses.size() == other.m_shape_poses.size() && + std::equal(m_shapes.begin(), m_shapes.end(), other.m_shapes.begin()) && + std::equal(m_shape_poses.begin(), m_shape_poses.end(), other.m_shape_poses.begin(), + [](const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2) { return t1.isApprox(t2); }); + } + + /** @brief Get the collision objects axis aligned bounding box + * @param aabb_min The minimum point + * @param aabb_max The maximum point */ + void getAABB(btVector3& aabb_min, btVector3& aabb_max) const + { + getCollisionShape()->getAabb(getWorldTransform(), aabb_min, aabb_max); + const btScalar& distance = getContactProcessingThreshold(); + // note that bullet expands each AABB by 4 cm + btVector3 contact_threshold(distance, distance, distance); + aabb_min -= contact_threshold; + aabb_max += contact_threshold; + } + + /** @brief Clones the collision objects but not the collision shape wich is const. + * @return Shared Pointer to the cloned collision object */ + std::shared_ptr clone() + { + std::shared_ptr clone_cow( + new CollisionObjectWrapper(m_name, m_type_id, m_shapes, m_shape_poses, m_collision_object_types, m_data)); + clone_cow->setCollisionShape(getCollisionShape()); + clone_cow->setWorldTransform(getWorldTransform()); + clone_cow->m_collisionFilterGroup = m_collisionFilterGroup; + clone_cow->m_collisionFilterMask = m_collisionFilterMask; + clone_cow->m_enabled = m_enabled; + clone_cow->setBroadphaseHandle(nullptr); + clone_cow->m_touch_links = m_touch_links; + clone_cow->setContactProcessingThreshold(this->getContactProcessingThreshold()); + return clone_cow; + } + + /** \brief Manage memory of a raw pointer shape */ + template + void manage(T* t) + { + m_data.push_back(std::shared_ptr(t)); + } + + /** \brief Manage memory of a shared pointer shape */ + template + void manage(std::shared_ptr t) + { + m_data.push_back(t); + } + +protected: + /** @brief Special constructor used by the clone method */ + CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::vector>& data); + + /** \brief The name of the object, must be unique. */ + std::string m_name; + + collision_detection::BodyType m_type_id; + + /** @brief The shapes that define the collison object */ + std::vector m_shapes; + + /** @brief The poses of the shapes, must be same length as m_shapes */ + AlignedVector m_shape_poses; + + /** @brief The shape collision object type to be used. */ + std::vector m_collision_object_types; + + /** @brief Manages the collision shape pointer so they get destroyed */ + std::vector> m_data; +}; + +/** @brief Casted collision shape used for checking if an object is collision free between two discrete poses + * + * The cast is not explicitely computed but implicitely represented through the single shape and the transformation + * between the first and second pose. */ +struct CastHullShape : public btConvexShape +{ +public: + /** \brief The casted shape */ + btConvexShape* m_shape; + + /** \brief Transformation from the first pose to the second pose */ + btTransform m_shape_transform; + + CastHullShape(btConvexShape* shape, const btTransform& t01) : m_shape(shape), m_shape_transform(t01) + { + m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE; + } + + void updateCastTransform(const btTransform& cast_transform) + { + m_shape_transform = cast_transform; + } + + /** \brief From both shape poses computes the support vertex and returns the larger one. */ + btVector3 localGetSupportingVertex(const btVector3& vec) const override + { + btVector3 support_vector_0 = m_shape->localGetSupportingVertex(vec); + btVector3 support_vector_1 = + m_shape_transform * m_shape->localGetSupportingVertex(vec * m_shape_transform.getBasis()); + return (vec.dot(support_vector_0) > vec.dot(support_vector_1)) ? support_vector_0 : support_vector_1; + } + + void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* /*vectors*/, + btVector3* /*supportVerticesOut*/, + int /*numVectors*/) const override + { + throw std::runtime_error("not implemented"); + } + + /** \brief Shape specific fast recalculation of the AABB at a certain pose + * + * The AABB is not recalculated from scratch but updated depending on the given transformation. */ + void getAabb(const btTransform& transform_world, btVector3& aabbMin, btVector3& aabbMax) const override + { + m_shape->getAabb(transform_world, aabbMin, aabbMax); + btVector3 min1, max1; + m_shape->getAabb(transform_world * m_shape_transform, min1, max1); + aabbMin.setMin(min1); + aabbMax.setMax(max1); + } + + void getAabbSlow(const btTransform& /*t*/, btVector3& /*aabbMin*/, btVector3& /*aabbMax*/) const override + { + throw std::runtime_error("shouldn't happen"); + } + + void setLocalScaling(const btVector3& /*scaling*/) override + { + } + + const btVector3& getLocalScaling() const override + { + static btVector3 out(1, 1, 1); + return out; + } + + void setMargin(btScalar /*margin*/) override + { + } + + btScalar getMargin() const override + { + return 0; + } + + int getNumPreferredPenetrationDirections() const override + { + return 0; + } + + void getPreferredPenetrationDirection(int /*index*/, btVector3& /*penetrationVector*/) const override + { + throw std::runtime_error("not implemented"); + } + + void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const override + { + throw std::runtime_error("not implemented"); + } + + const char* getName() const override + { + return "CastHull"; + } + + btVector3 localGetSupportingVertexWithoutMargin(const btVector3& v) const override + { + return localGetSupportingVertex(v); + } +}; + +/** \brief Computes the local supporting vertex of a convex shape. + * + * If multiple vertices with equal support products exists, their average is calculated and returned. + * + * \param shape The convex shape to check + * \param localNormal The support direction to search for in shape local coordinates + * \param outsupport The value of the calculated support mapping + * \param outpt The computed support point */ +inline void getAverageSupport(const btConvexShape* shape, const btVector3& localNormal, float& outsupport, + btVector3& outpt) +{ + btVector3 pt_sum(0, 0, 0); + float pt_count = 0; + float max_support = -1000; + + const btPolyhedralConvexShape* pshape = dynamic_cast(shape); + if (pshape) + { + int n_pts = pshape->getNumVertices(); + + for (int i = 0; i < n_pts; ++i) + { + btVector3 pt; + pshape->getVertex(i, pt); + + float sup = pt.dot(localNormal); + if (sup > max_support + BULLET_EPSILON) + { + pt_count = 1; + pt_sum = pt; + max_support = sup; + } + else if (sup < max_support - BULLET_EPSILON) + { + } + else + { + pt_count += 1; + pt_sum += pt; + } + } + outsupport = max_support; + outpt = pt_sum / pt_count; + } + else + { + outpt = shape->localGetSupportingVertexWithoutMargin(localNormal); + outsupport = localNormal.dot(outpt); + } +} + +/** \brief Converts a bullet contact result to MoveIt format and adds it to the result data structure */ +inline btScalar addDiscreteSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, + const btCollisionObjectWrapper* colObj1Wrap, ContactTestData& collisions) +{ + assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); + assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); + const CollisionObjectWrapper* cd0 = static_cast(colObj0Wrap->getCollisionObject()); + const CollisionObjectWrapper* cd1 = static_cast(colObj1Wrap->getCollisionObject()); + + std::pair pc = getObjectPairKey(cd0->getName(), cd1->getName()); + + const auto& it = collisions.res.contacts.find(pc); + bool found = (it != collisions.res.contacts.end()); + + collision_detection::Contact contact; + contact.body_name_1 = cd0->getName(); + contact.body_name_2 = cd1->getName(); + contact.depth = static_cast(cp.m_distance1); + contact.normal = convertBtToEigen(-1 * cp.m_normalWorldOnB); + contact.pos = convertBtToEigen(cp.m_positionWorldOnA); + contact.nearest_points[0] = contact.pos; + contact.nearest_points[1] = convertBtToEigen(cp.m_positionWorldOnB); + + contact.body_type_1 = cd0->getTypeID(); + contact.body_type_2 = cd0->getTypeID(); + + if (!processResult(collisions, contact, pc, found)) + { + return 0; + } + + return 1; +} + +inline btScalar addCastSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*index0*/, + const btCollisionObjectWrapper* colObj1Wrap, int /*index1*/, + ContactTestData& collisions) +{ + assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); + assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); + + const CollisionObjectWrapper* cd0 = static_cast(colObj0Wrap->getCollisionObject()); + const CollisionObjectWrapper* cd1 = static_cast(colObj1Wrap->getCollisionObject()); + + std::pair pc = getObjectPairKey(cd0->getName(), cd1->getName()); + + const auto& it = collisions.res.contacts.find(pc); + bool found = (it != collisions.res.contacts.end()); + + collision_detection::Contact contact; + contact.body_name_1 = cd0->getName(); + contact.body_name_2 = cd1->getName(); + contact.depth = static_cast(cp.m_distance1); + contact.normal = convertBtToEigen(-1 * cp.m_normalWorldOnB); + contact.pos = convertBtToEigen(cp.m_positionWorldOnA); + + contact.body_type_1 = cd0->getTypeID(); + contact.body_type_2 = cd0->getTypeID(); + + collision_detection::Contact* col = processResult(collisions, contact, pc, found); + + if (!col) + { + return 0; + } + + assert(!((cd0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) && + (cd1->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter))); + + bool cast_shape_is_first = cd0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter; + + btVector3 normal_world_from_cast = (cast_shape_is_first ? -1 : 1) * cp.m_normalWorldOnB; + const btCollisionObjectWrapper* first_col_obj_wrap = (cast_shape_is_first ? colObj0Wrap : colObj1Wrap); + + // we want the contact information of the non-casted object come first, therefore swap values + if (cast_shape_is_first) + { + std::swap(col->nearest_points[0], col->nearest_points[1]); + contact.pos = convertBtToEigen(cp.m_positionWorldOnB); + std::swap(col->body_name_1, col->body_name_2); + std::swap(col->body_type_1, col->body_type_2); + col->normal *= -1; + } + + btTransform tf_world0, tf_world1; + const CastHullShape* shape = static_cast(first_col_obj_wrap->getCollisionShape()); + + tf_world0 = first_col_obj_wrap->getWorldTransform(); + tf_world1 = first_col_obj_wrap->getWorldTransform() * shape->m_shape_transform; + + // transform normals into pose local coordinate systems + btVector3 normal_local0 = normal_world_from_cast * tf_world0.getBasis(); + btVector3 normal_local1 = normal_world_from_cast * tf_world1.getBasis(); + + btVector3 pt_local0; + float localsup0; + getAverageSupport(shape->m_shape, normal_local0, localsup0, pt_local0); + btVector3 pt_world0 = tf_world0 * pt_local0; + btVector3 pt_local1; + float localsup1; + getAverageSupport(shape->m_shape, normal_local1, localsup1, pt_local1); + btVector3 pt_world1 = tf_world1 * pt_local1; + + float sup0 = normal_world_from_cast.dot(pt_world0); + float sup1 = normal_world_from_cast.dot(pt_world1); + + // TODO: this section is potentially problematic. think hard about the math + if (sup0 - sup1 > BULLET_SUPPORT_FUNC_TOLERANCE) + { + col->percent_interpolation = 0; + } + else if (sup1 - sup0 > BULLET_SUPPORT_FUNC_TOLERANCE) + { + col->percent_interpolation = 1; + } + else + { + const btVector3& pt_on_cast = cast_shape_is_first ? cp.m_positionWorldOnA : cp.m_positionWorldOnB; + float l0c = (pt_on_cast - pt_world0).length(); + float l1c = (pt_on_cast - pt_world1).length(); + + if (l0c + l1c < BULLET_LENGTH_TOLERANCE) + { + col->percent_interpolation = .5; + } + else + { + col->percent_interpolation = static_cast(l0c / (l0c + l1c)); + } + } + + return 1; +} + +/** \brief Checks if the collision pair is kinematic vs kinematic objects */ +inline bool isOnlyKinematic(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) +{ + return cow0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter && + cow1->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter; +} + +/** @brief Callback structure for both discrete and continuous broadphase collision pair + * + * /e needsCollision is the callback executed before a narrowphase check is executed. + * /e addSingleResult is the callback executed after the narrowphase check delivers a result. */ +struct BroadphaseContactResultCallback +{ + ContactTestData& collisions_; + double contact_distance_; + const collision_detection::AllowedCollisionMatrix* acm_{ nullptr }; + + /** \brief Indicates if the callback is used for only self-collision checking */ + bool self_; + + /** \brief Indicates if the callback is used for casted collisions */ + bool cast_{ false }; + + BroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, + const collision_detection::AllowedCollisionMatrix* acm, bool self, bool cast = false) + : collisions_(collisions), contact_distance_(contact_distance), acm_(acm), self_(self), cast_(cast) + { + } + + ~BroadphaseContactResultCallback() = default; + + /** \brief This callback is used for each overlapping pair in a pair cache of the broadphase interface to check if a + * collision check should done for the pair. */ + // TODO: Add check for two objects attached to the same link + bool needsCollision(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) const + { + if (cast_) + { + return !collisions_.done && !isOnlyKinematic(cow0, cow1) && !acmCheck(cow0->getName(), cow1->getName(), acm_); + } + else + { + return !collisions_.done && (self_ ? isOnlyKinematic(cow0, cow1) : !isOnlyKinematic(cow0, cow1)) && + !acmCheck(cow0->getName(), cow1->getName(), acm_); + } + } + + /** \brief This callback is used after btManifoldResult processed a collision result. */ + btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, + const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) + { + if (cp.m_distance1 > static_cast(contact_distance_)) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Not close enough for collision with " << cp.m_distance1); + return 0; + } + + if (cast_) + { + return addCastSingleResult(cp, colObj0Wrap, index0, colObj1Wrap, index1, collisions_); + } + else + { + return addDiscreteSingleResult(cp, colObj0Wrap, colObj1Wrap, collisions_); + } + } +}; + +struct TesseractBroadphaseBridgedManifoldResult : public btManifoldResult +{ + BroadphaseContactResultCallback& result_callback_; + + TesseractBroadphaseBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, + const btCollisionObjectWrapper* obj1Wrap, + BroadphaseContactResultCallback& result_callback) + : btManifoldResult(obj0Wrap, obj1Wrap), result_callback_(result_callback) + { + } + + void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) override + { + if (result_callback_.collisions_.done || result_callback_.collisions_.pair_done || + depth > static_cast(result_callback_.contact_distance_)) + { + return; + } + + bool is_swapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + btVector3 point_a = pointInWorld + normalOnBInWorld * depth; + btVector3 local_a; + btVector3 local_b; + if (is_swapped) + { + local_a = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); + local_b = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + else + { + local_a = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); + local_b = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + + btManifoldPoint new_pt(local_a, local_b, normalOnBInWorld, depth); + new_pt.m_positionWorldOnA = point_a; + new_pt.m_positionWorldOnB = pointInWorld; + + // BP mod, store contact triangles. + if (is_swapped) + { + new_pt.m_partId0 = m_partId1; + new_pt.m_partId1 = m_partId0; + new_pt.m_index0 = m_index1; + new_pt.m_index1 = m_index0; + } + else + { + new_pt.m_partId0 = m_partId0; + new_pt.m_partId1 = m_partId1; + new_pt.m_index0 = m_index0; + new_pt.m_index1 = m_index1; + } + + // experimental feature info, for per-triangle material etc. + const btCollisionObjectWrapper* obj0_wrap = is_swapped ? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1_wrap = is_swapped ? m_body0Wrap : m_body1Wrap; + result_callback_.addSingleResult(new_pt, obj0_wrap, new_pt.m_partId0, new_pt.m_index0, obj1_wrap, new_pt.m_partId1, + new_pt.m_index1); + } +}; + +/** @brief A callback function that is called as part of the broadphase collision checking. + * + * If the AABB of two collision objects are overlapping the processOverlap method is called and they are checked for + * collision/distance and the results are stored in collision_. */ +class TesseractCollisionPairCallback : public btOverlapCallback +{ + const btDispatcherInfo& dispatch_info_; + btCollisionDispatcher* dispatcher_; + + /** \brief Callback executed for each broadphase pair to check if needs collision */ + BroadphaseContactResultCallback& results_callback_; + +public: + TesseractCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher, + BroadphaseContactResultCallback& results_callback) + : dispatch_info_(dispatchInfo), dispatcher_(dispatcher), results_callback_(results_callback) + { + } + + ~TesseractCollisionPairCallback() override = default; + + bool processOverlap(btBroadphasePair& pair) override + { + results_callback_.collisions_.pair_done = false; + + if (results_callback_.collisions_.done) + { + return false; + } + + const CollisionObjectWrapper* cow0 = static_cast(pair.m_pProxy0->m_clientObject); + const CollisionObjectWrapper* cow1 = static_cast(pair.m_pProxy1->m_clientObject); + + std::pair pair_names{ cow0->getName(), cow1->getName() }; + if (results_callback_.needsCollision(cow0, cow1)) + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Processing " << cow0->getName() << " vs " << cow1->getName()); + btCollisionObjectWrapper obj0_wrap(nullptr, cow0->getCollisionShape(), cow0, cow0->getWorldTransform(), -1, -1); + btCollisionObjectWrapper obj1_wrap(nullptr, cow1->getCollisionShape(), cow1, cow1->getWorldTransform(), -1, -1); + + // dispatcher will keep algorithms persistent in the collision pair + if (!pair.m_algorithm) + { + pair.m_algorithm = dispatcher_->findAlgorithm(&obj0_wrap, &obj1_wrap, nullptr, BT_CLOSEST_POINT_ALGORITHMS); + } + + if (pair.m_algorithm) + { + TesseractBroadphaseBridgedManifoldResult contact_point_result(&obj0_wrap, &obj1_wrap, results_callback_); + contact_point_result.m_closestPointDistanceThreshold = + static_cast(results_callback_.contact_distance_); + + // discrete collision detection query + pair.m_algorithm->processCollision(&obj0_wrap, &obj1_wrap, dispatch_info_, &contact_point_result); + } + } + else + { + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Not processing " << cow0->getName() << " vs " << cow1->getName()); + } + return false; + } +}; + +/** \brief Casts a geometric shape into a btCollisionShape */ +btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom, + const CollisionObjectType& collision_object_type, CollisionObjectWrapper* cow); + +/** @brief Update a collision objects filters + * @param active A list of active collision objects + * @param cow The collision object to update. + * @param continuous Indicate if the object is a continuous collision object. + * + * Currently continuous collision objects can only be checked against static objects. Continuous to Continuous + * collision checking is currently not supports. TODO LEVI: Add support for Continuous to Continuous collision + * checking. */ +inline void updateCollisionObjectFilters(const std::vector& active, CollisionObjectWrapper& cow) +{ + // if not active make cow part of static + if (!isLinkActive(active, cow.getName())) + { + cow.m_collisionFilterGroup = btBroadphaseProxy::StaticFilter; + cow.m_collisionFilterMask = btBroadphaseProxy::KinematicFilter; + } + else + { + cow.m_collisionFilterGroup = btBroadphaseProxy::KinematicFilter; + cow.m_collisionFilterMask = btBroadphaseProxy::KinematicFilter | btBroadphaseProxy::StaticFilter; + } + + if (cow.getBroadphaseHandle()) + { + cow.getBroadphaseHandle()->m_collisionFilterGroup = cow.m_collisionFilterGroup; + cow.getBroadphaseHandle()->m_collisionFilterMask = cow.m_collisionFilterMask; + } + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "COW " << cow.getName() << " group " + << cow.m_collisionFilterGroup << " mask " + << cow.m_collisionFilterMask); +} + +inline CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr& cow) +{ + CollisionObjectWrapperPtr new_cow = cow->clone(); + + btTransform tf; + tf.setIdentity(); + + if (btBroadphaseProxy::isConvex(new_cow->getCollisionShape()->getShapeType())) + { + assert(dynamic_cast(new_cow->getCollisionShape()) != nullptr); + btConvexShape* convex = static_cast(new_cow->getCollisionShape()); + + // This checks if the collision object is already a cast collision object + assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); + + CastHullShape* shape = new CastHullShape(convex, tf); + + new_cow->manage(shape); + new_cow->setCollisionShape(shape); + } + else if (btBroadphaseProxy::isCompound(new_cow->getCollisionShape()->getShapeType())) + { + btCompoundShape* compound = static_cast(new_cow->getCollisionShape()); + btCompoundShape* new_compound = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, compound->getNumChildShapes()); + + for (int i = 0; i < compound->getNumChildShapes(); ++i) + { + if (btBroadphaseProxy::isConvex(compound->getChildShape(i)->getShapeType())) + { + btConvexShape* convex = static_cast(compound->getChildShape(i)); + assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); // This checks if already a cast collision object + + btTransform geom_trans = compound->getChildTransform(i); + + btCollisionShape* subshape = new CastHullShape(convex, tf); + + new_cow->manage(subshape); + subshape->setMargin(BULLET_MARGIN); + new_compound->addChildShape(geom_trans, subshape); + } + else if (btBroadphaseProxy::isCompound(compound->getChildShape(i)->getShapeType())) + { + btCompoundShape* second_compound = static_cast(compound->getChildShape(i)); + btCompoundShape* new_second_compound = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, second_compound->getNumChildShapes()); + for (int j = 0; j < second_compound->getNumChildShapes(); ++j) + { + assert(!btBroadphaseProxy::isCompound(second_compound->getChildShape(j)->getShapeType())); + + btConvexShape* convex = static_cast(second_compound->getChildShape(j)); + assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); // This checks if already a cast collision object + + btTransform geom_trans = second_compound->getChildTransform(j); + + btCollisionShape* subshape = new CastHullShape(convex, tf); + + new_cow->manage(subshape); + subshape->setMargin(BULLET_MARGIN); + new_second_compound->addChildShape(geom_trans, subshape); + } + + btTransform geom_trans = compound->getChildTransform(i); + + new_cow->manage(new_second_compound); + + // margin on compound seems to have no effect when positive but has an effect when negative + new_second_compound->setMargin(BULLET_MARGIN); + new_compound->addChildShape(geom_trans, new_second_compound); + } + else + { + ROS_ERROR_NAMED("collision_detection.bullet", + "I can only collision check convex shapes and compound shapes made of convex shapes"); + throw std::runtime_error("I can only collision check convex shapes and compound shapes made of convex shapes"); + } + } + + // margin on compound seems to have no effect when positive but has an effect when negative + new_compound->setMargin(BULLET_MARGIN); + new_cow->manage(new_compound); + new_cow->setCollisionShape(new_compound); + new_cow->setWorldTransform(cow->getWorldTransform()); + } + else + { + ROS_ERROR_NAMED("collision_detection.bullet", + "I can only collision check convex shapes and compound shapes made of convex shapes"); + throw std::runtime_error("I can only collision check convex shapes and compound shapes made of convex shapes"); + } + + return new_cow; +} + +/** @brief Update the Broadphase AABB for the input collision object + * @param cow The collision objects + * @param broadphase The bullet broadphase interface + * @param dispatcher The bullet collision dispatcher */ +inline void updateBroadphaseAABB(const CollisionObjectWrapperPtr& cow, + const std::unique_ptr& broadphase, + const std::unique_ptr& dispatcher) +{ + btVector3 aabb_min, aabb_max; + cow->getAABB(aabb_min, aabb_max); + + assert(cow->getBroadphaseHandle() != nullptr); + broadphase->setAabb(cow->getBroadphaseHandle(), aabb_min, aabb_max, dispatcher.get()); +} + +/** @brief Remove the collision object from broadphase + * @param cow The collision objects + * @param broadphase The bullet broadphase interface + * @param dispatcher The bullet collision dispatcher */ +inline void removeCollisionObjectFromBroadphase(const CollisionObjectWrapperPtr& cow, + const std::unique_ptr& broadphase, + const std::unique_ptr& dispatcher) +{ + btBroadphaseProxy* bp = cow->getBroadphaseHandle(); + if (bp) + { + // only clear the cached algorithms + broadphase->getOverlappingPairCache()->cleanProxyFromPairs(bp, dispatcher.get()); + broadphase->destroyProxy(bp, dispatcher.get()); + cow->setBroadphaseHandle(nullptr); + } +} + +/** @brief Add the collision object to broadphase + * @param cow The collision objects + * @param broadphase The bullet broadphase interface + * @param dispatcher The bullet collision dispatcher */ +inline void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr& cow, + const std::unique_ptr& broadphase, + const std::unique_ptr& dispatcher) +{ + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", "Added " << cow->getName() << " to broadphase"); + btVector3 aabb_min, aabb_max; + cow->getAABB(aabb_min, aabb_max); + + int type = cow->getCollisionShape()->getShapeType(); + cow->setBroadphaseHandle(broadphase->createProxy(aabb_min, aabb_max, type, cow.get(), cow->m_collisionFilterGroup, + cow->m_collisionFilterMask, dispatcher.get())); +} + +struct BroadphaseFilterCallback : public btOverlapFilterCallback +{ + bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override + { + bool cull = !(proxy0->m_collisionFilterMask & proxy1->m_collisionFilterGroup); + cull = cull || !(proxy1->m_collisionFilterMask & proxy0->m_collisionFilterGroup); + + if (cull) + return false; + + const CollisionObjectWrapper* cow0 = static_cast(proxy0->m_clientObject); + const CollisionObjectWrapper* cow1 = static_cast(proxy1->m_clientObject); + + if (!cow0->m_enabled) + return false; + + if (!cow1->m_enabled) + return false; + + if (cow0->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && + cow1->getTypeID() == collision_detection::BodyType::ROBOT_LINK) + if (cow0->m_touch_links.find(cow1->getName()) != cow0->m_touch_links.end()) + return false; + + if (cow1->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && + cow0->getTypeID() == collision_detection::BodyType::ROBOT_LINK) + if (cow1->m_touch_links.find(cow0->getName()) != cow1->m_touch_links.end()) + return false; + + if (cow0->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED && + cow1->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED) + if (cow0->m_touch_links == cow1->m_touch_links) + return false; + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Broadphase pass " << cow0->getName() << " vs " << cow1->getName()); + return true; + } +}; + +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h new file mode 100644 index 0000000000..c77afce1b0 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h @@ -0,0 +1,214 @@ +/********************************************************************* + * Software License Agreement (Apache License) + * + * Copyright (c) 2017, Southwest Research Institute + * All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +namespace collision_detection_bullet +{ +/** + * @brief Get a key for two object to search the collision matrix + * @param obj1 First collision object name + * @param obj2 Second collision object name + * @return The collision pair key + */ +inline std::pair getObjectPairKey(const std::string& obj1, const std::string& obj2) +{ + return obj1 < obj2 ? std::make_pair(obj1, obj2) : std::make_pair(obj2, obj1); +} + +/** + * @brief This will check if a link is active provided a list. If the list is empty the link is considered active. + * @param active List of active link names + * @param name The name of link to check if it is active. + */ +inline bool isLinkActive(const std::vector& active, const std::string& name) +{ + return active.empty() || (std::find(active.begin(), active.end(), name) != active.end()); +} + +/** \brief Stores a single contact result in the requested way. + * \param found Indicates if a contact for this pair of objects has already been found + * \return Pointer to the newly inserted contact */ +inline collision_detection::Contact* processResult(ContactTestData& cdata, collision_detection::Contact& contact, + const std::pair& key, bool found) +{ + // add deepest penetration / smallest distance to result + if (cdata.req.distance) + { + if (contact.depth < cdata.res.distance) + { + cdata.res.distance = contact.depth; + } + } + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Contact btw " << key.first << " and " << key.second << " dist: " << contact.depth); + // case if pair hasn't a contact yet + if (!found) + { + if (contact.depth <= 0) + { + cdata.res.collision = true; + } + + std::vector data; + + // if we dont want contacts we are done here + if (!cdata.req.contacts) + { + if (!cdata.req.distance) + { + cdata.done = true; + } + return nullptr; + } + else + { + data.reserve(cdata.req.max_contacts_per_pair); + data.emplace_back(contact); + cdata.res.contact_count++; + } + + if (cdata.res.contact_count >= cdata.req.max_contacts) + { + if (!cdata.req.distance) + { + cdata.done = true; + } + } + + if (cdata.req.max_contacts_per_pair == 1u) + { + cdata.pair_done = true; + } + + return &(cdata.res.contacts.insert(std::make_pair(key, data)).first->second.back()); + } + else + { + std::vector& dr = cdata.res.contacts[key]; + dr.emplace_back(contact); + cdata.res.contact_count++; + + if (dr.size() >= cdata.req.max_contacts_per_pair) + { + cdata.pair_done = true; + } + + if (cdata.res.contact_count >= cdata.req.max_contacts) + { + if (!cdata.req.distance) + { + cdata.done = true; + } + } + + return &(dr.back()); + } + + return nullptr; +} + +/** + * @brief Create a convex hull from vertices using Bullet Convex Hull Computer + * @param (Output) vertices A vector of vertices + * @param (Output) faces The first values indicates the number of vertices that define the face followed by the vertice + * index + * @param (input) input A vector of point to create a convex hull from + * @param (input) shrink If positive, the convex hull is shrunken by that amount (each face is moved by "shrink" length + * units towards the center along its normal). + * @param (input) shrinkClamp If positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where + * "innerRadius" is the minimum distance of a face to the center of the convex hull. + * @return The number of faces. If less than zero an error occured when trying to create the convex hull + */ +inline int createConvexHull(AlignedVector& vertices, std::vector& faces, + const AlignedVector& input, double shrink = -1, double shrinkClamp = -1) +{ + vertices.clear(); + faces.clear(); + + btConvexHullComputer conv; + btAlignedObjectArray points; + points.reserve(static_cast(input.size())); + for (const Eigen::Vector3d& v : input) + { + points.push_back(btVector3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2]))); + } + + btScalar val = conv.compute(&points[0].getX(), sizeof(btVector3), points.size(), static_cast(shrink), + static_cast(shrinkClamp)); + if (val < 0) + { + ROS_ERROR("Failed to create convex hull"); + return -1; + } + + int num_verts = conv.vertices.size(); + vertices.reserve(static_cast(num_verts)); + for (int i = 0; i < num_verts; i++) + { + btVector3& v = conv.vertices[i]; + vertices.push_back(Eigen::Vector3d(v.getX(), v.getY(), v.getZ())); + } + + int num_faces = conv.faces.size(); + faces.reserve(static_cast(3 * num_faces)); + for (int i = 0; i < num_faces; i++) + { + std::vector face; + face.reserve(3); + + const btConvexHullComputer::Edge* source_edge = &(conv.edges[conv.faces[i]]); + int a = source_edge->getSourceVertex(); + face.push_back(a); + + int b = source_edge->getTargetVertex(); + face.push_back(b); + + const btConvexHullComputer::Edge* edge = source_edge->getNextEdgeOfFace(); + int c = edge->getTargetVertex(); + face.push_back(c); + + edge = edge->getNextEdgeOfFace(); + c = edge->getTargetVertex(); + while (c != a) + { + face.push_back(c); + + edge = edge->getNextEdgeOfFace(); + c = edge->getTargetVertex(); + } + faces.push_back(static_cast(face.size())); + faces.insert(faces.end(), face.begin(), face.end()); + } + + return num_faces; +} + +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h new file mode 100644 index 0000000000..d518c09169 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h @@ -0,0 +1,107 @@ +/********************************************************************* + * Software License Agreement (Apache License) + * + * Copyright (c) 2018, Southwest Research Institute + * All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace collision_detection_bullet +{ +/** \brief Recursively traverses robot from root to get all active links + * + * \param active_links Stores the active links + * \param urdf_link The current urdf link representation + * \param active Indicates if link is considered active */ +static inline void getActiveLinkNamesRecursive(std::vector& active_links, + const urdf::LinkConstSharedPtr& urdf_link, bool active) +{ + if (active) + { + active_links.push_back(urdf_link->name); + for (const auto& child_link : urdf_link->child_links) + { + getActiveLinkNamesRecursive(active_links, child_link, active); + } + } + else + { + for (std::size_t i = 0; i < urdf_link->child_links.size(); ++i) + { + const urdf::LinkConstSharedPtr child_link = urdf_link->child_links[i]; + if ((child_link->parent_joint) && (child_link->parent_joint->type != urdf::Joint::FIXED)) + getActiveLinkNamesRecursive(active_links, child_link, true); + else + getActiveLinkNamesRecursive(active_links, child_link, active); + } + } +} + +inline shapes::ShapePtr constructShape(const urdf::Geometry* geom) +{ + shapes::Shape* result = nullptr; + switch (geom->type) + { + case urdf::Geometry::SPHERE: + result = new shapes::Sphere(static_cast(geom)->radius); + break; + case urdf::Geometry::BOX: + { + urdf::Vector3 dim = static_cast(geom)->dim; + result = new shapes::Box(dim.x, dim.y, dim.z); + } + break; + case urdf::Geometry::CYLINDER: + result = new shapes::Cylinder(static_cast(geom)->radius, + static_cast(geom)->length); + break; + case urdf::Geometry::MESH: + { + const urdf::Mesh* mesh = static_cast(geom); + if (!mesh->filename.empty()) + { + Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); + shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale); + result = m; + } + } + break; + default: + ROS_ERROR("Unknown geometry type: %d", static_cast(geom->type)); + break; + } + + return shapes::ShapePtr(result); +} + +inline Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose& pose) +{ + Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z); + Eigen::Isometry3d result; + result.translation() = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z); + result.linear() = q.toRotationMatrix(); + return result; +} +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h new file mode 100644 index 0000000000..8b3c11057e --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#pragma once + +#include +#include + +namespace collision_detection +{ +/** \brief An allocator for Bullet collision detectors */ +class CollisionDetectorAllocatorBullet + : public CollisionDetectorAllocatorTemplate +{ +public: + static const std::string NAME; // defined in collision_env_bullet.cpp +}; +} // namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h new file mode 100644 index 0000000000..e8612f08fe --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h @@ -0,0 +1,49 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#pragma once + +#include +#include + +namespace collision_detection +{ +class CollisionDetectorBtPluginLoader : public CollisionPlugin +{ +public: + bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const override; +}; +} // namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h new file mode 100644 index 0000000000..e210322ebd --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h @@ -0,0 +1,144 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#pragma once + +#include +#include +#include + +namespace collision_detection +{ +/** \brief */ +class CollisionEnvBullet : public CollisionEnv +{ +public: + CollisionEnvBullet() = delete; + + CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); + + CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, + double scale = 1.0); + + CollisionEnvBullet(const CollisionEnvBullet& other, const WorldPtr& world); + + ~CollisionEnvBullet() override; + + CollisionEnvBullet(CollisionEnvBullet&) = delete; + + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; + + void distanceSelf(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const override; + + void distanceRobot(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const override; + + void setWorld(const WorldPtr& world) override; + +protected: + /** \brief Updates the poses of the objects in the manager according to given robot state */ + void updateTransformsFromState(const moveit::core::RobotState& state, + const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const; + + /** \brief Updates the collision objects saved in the manager to reflect a new padding or scaling of the robot links + */ + void updatedPaddingOrScaling(const std::vector& links) override; + + /** \brief All of the attached objects in the robot state are wrapped into bullet collision objects */ + void addAttachedOjects(const moveit::core::RobotState& state, + std::vector& cows) const; + + /** \brief Bundles the different checkSelfCollision functions into a single function */ + void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; + + void checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, + const AllowedCollisionMatrix* acm) const; + + void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; + + /** \brief Construts a bullet collision object out of a robot link */ + void addLinkAsCollisionObject(const urdf::LinkSharedPtr& link); + + /** \brief Handles self collision checks */ + mutable collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_{ + new collision_detection_bullet::BulletDiscreteBVHManager() + }; + + /** \brief Handles continuous robot world collision checks */ + mutable collision_detection_bullet::BulletCastBVHManagerPtr manager_CCD_{ + new collision_detection_bullet::BulletCastBVHManager() + }; + + /** \brief Adds a world object to the collision managers */ + void addToManager(const World::Object* obj); + + /** \brief Updates a managed collision object with its world representation. + * + * We have three cases: 1) the object is part of the manager and not of world --> delete it + * 2) the object is not in the manager, therefore register to manager, + * 3) the object is in the manager then delete and add the modified */ + void updateManagedObject(const std::string& id); + + /** \brief The active links where active refers to the group which can collide with everything */ + std::vector active_; + +private: + /** \brief Callback function executed for each change to the world environment */ + void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); + + World::ObserverHandle observer_handle_; +}; +} // namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp new file mode 100644 index 0000000000..c59d8de93e --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp @@ -0,0 +1,168 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Levi Armstrong, Jens Petit */ + +#include +#include +#include + +namespace collision_detection_bullet +{ +BulletBVHManager::BulletBVHManager() +{ + dispatcher_.reset(new btCollisionDispatcher(&coll_config_)); + + dispatcher_->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE, + coll_config_.getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE, + CONVEX_SHAPE_PROXYTYPE)); + + dispatcher_->setDispatcherFlags(dispatcher_->getDispatcherFlags() & + ~btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD); + + broadphase_.reset(new btDbvtBroadphase()); + + broadphase_->getOverlappingPairCache()->setOverlapFilterCallback(&filter_callback_); + + contact_distance_ = BULLET_DEFAULT_CONTACT_DISTANCE; +} + +BulletBVHManager::~BulletBVHManager() +{ + // clean up remaining objects + for (const std::pair& cow : link2cow_) + removeCollisionObjectFromBroadphase(cow.second, broadphase_, dispatcher_); +} + +bool BulletBVHManager::hasCollisionObject(const std::string& name) const +{ + return (link2cow_.find(name) != link2cow_.end()); +} + +bool BulletBVHManager::removeCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + CollisionObjectWrapperPtr& cow = it->second; + removeCollisionObjectFromBroadphase(cow, broadphase_, dispatcher_); + link2cow_.erase(name); + return true; + } + + return false; +} + +bool BulletBVHManager::enableCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + it->second->m_enabled = true; + return true; + } + + return false; +} + +bool BulletBVHManager::disableCollisionObject(const std::string& name) +{ + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + it->second->m_enabled = false; + return true; + } + + return false; +} + +void BulletBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) +{ + // TODO(j-petit): Find a way to remove this check. Need to store information in CollisionEnv transforms with geometry + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + CollisionObjectWrapperPtr& cow = it->second; + btTransform tf = convertEigenToBt(pose); + cow->setWorldTransform(tf); + + // Now update Broadphase AABB (See BulletWorld updateSingleAabb function) + if (cow->getBroadphaseHandle()) + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } +} + +void BulletBVHManager::setActiveCollisionObjects(const std::vector& names) +{ + active_ = names; + + // Now need to update the broadphase with correct aabb + for (std::pair& co : link2cow_) + { + CollisionObjectWrapperPtr& cow = co.second; + updateCollisionObjectFilters(active_, *cow); + + // The broadphase tree structure has to be updated, therefore remove and add is necessary + removeCollisionObjectFromBroadphase(cow, broadphase_, dispatcher_); + addCollisionObjectToBroadphase(cow, broadphase_, dispatcher_); + } +} + +const std::vector& BulletBVHManager::getActiveCollisionObjects() const +{ + return active_; +} + +void BulletBVHManager::setContactDistanceThreshold(double contact_distance) +{ + contact_distance_ = contact_distance; + + for (std::pair& co : link2cow_) + { + CollisionObjectWrapperPtr& cow = co.second; + cow->setContactProcessingThreshold(static_cast(contact_distance)); + if (cow->getBroadphaseHandle()) + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } +} + +double BulletBVHManager::getContactDistanceThreshold() const +{ + return contact_distance_; +} + +const std::map& BulletBVHManager::getCollisionObjects() const +{ + return link2cow_; +} + +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp new file mode 100644 index 0000000000..64e9df962c --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp @@ -0,0 +1,170 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Levi Armstrong, Jens Petit */ + +#include +#include +#include + +namespace collision_detection_bullet +{ +BulletCastBVHManagerPtr BulletCastBVHManager::clone() const +{ + BulletCastBVHManagerPtr manager(new BulletCastBVHManager()); + + for (const std::pair& cow : link2cow_) + { + CollisionObjectWrapperPtr new_cow = cow.second->clone(); + + assert(new_cow->getCollisionShape()); + assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); + + new_cow->setWorldTransform(cow.second->getWorldTransform()); + new_cow->setContactProcessingThreshold(static_cast(contact_distance_)); + manager->addCollisionObject(new_cow); + } + + manager->setActiveCollisionObjects(active_); + manager->setContactDistanceThreshold(contact_distance_); + + return manager; +} + +void BulletCastBVHManager::setCastCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose1, + const Eigen::Isometry3d& pose2) +{ + // TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with + // geometry + auto it = link2cow_.find(name); + if (it != link2cow_.end()) + { + CollisionObjectWrapperPtr& cow = it->second; + assert(cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter); + + btTransform tf1 = convertEigenToBt(pose1); + btTransform tf2 = convertEigenToBt(pose2); + + cow->setWorldTransform(tf1); + link2cow_[name]->setWorldTransform(tf1); + + // If collision object is disabled dont proceed + if (cow->m_enabled) + { + if (btBroadphaseProxy::isConvex(cow->getCollisionShape()->getShapeType())) + { + static_cast(cow->getCollisionShape())->updateCastTransform(tf1.inverseTimes(tf2)); + } + else if (btBroadphaseProxy::isCompound(cow->getCollisionShape()->getShapeType())) + { + btCompoundShape* compound = static_cast(cow->getCollisionShape()); + + for (int i = 0; i < compound->getNumChildShapes(); ++i) + { + if (btBroadphaseProxy::isConvex(compound->getChildShape(i)->getShapeType())) + { + const btTransform& local_tf = compound->getChildTransform(i); + + btTransform delta_tf = (tf1 * local_tf).inverseTimes(tf2 * local_tf); + static_cast(compound->getChildShape(i))->updateCastTransform(delta_tf); + compound->updateChildTransform(i, local_tf, false); // This is required to update the BVH tree + } + else if (btBroadphaseProxy::isCompound(compound->getChildShape(i)->getShapeType())) + { + btCompoundShape* second_compound = static_cast(compound->getChildShape(i)); + + for (int j = 0; j < second_compound->getNumChildShapes(); ++j) + { + assert(!btBroadphaseProxy::isCompound(second_compound->getChildShape(j)->getShapeType())); + const btTransform& local_tf = second_compound->getChildTransform(j); + + btTransform delta_tf = (tf1 * local_tf).inverseTimes(tf2 * local_tf); + static_cast(second_compound->getChildShape(j))->updateCastTransform(delta_tf); + second_compound->updateChildTransform(j, local_tf, false); // This is required to update the BVH tree + } + second_compound->recalculateLocalAabb(); + } + } + compound->recalculateLocalAabb(); + } + else + { + ROS_ERROR_NAMED("collision_detection.bullet", "I can only continuous collision check convex shapes and " + "compound shapes made of convex shapes"); + throw std::runtime_error( + "I can only continuous collision check convex shapes and compound shapes made of convex shapes"); + } + + // Now update Broadphase AABB (See BulletWorld updateSingleAabb function) + updateBroadphaseAABB(cow, broadphase_, dispatcher_); + } + } +} + +void BulletCastBVHManager::contactTest(collision_detection::CollisionResult& collisions, + const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, bool self = false) +{ + ContactTestData cdata(active_, contact_distance_, collisions, req); + broadphase_->calculateOverlappingPairs(dispatcher_.get()); + btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Number overlapping candidates " << pair_cache->getNumOverlappingPairs()); + + BroadphaseContactResultCallback cc(cdata, contact_distance_, acm, false, true); + TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); + pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get()); +} + +void BulletCastBVHManager::addCollisionObject(const CollisionObjectWrapperPtr& cow) +{ + std::string name = cow->getName(); + if (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) + { + CollisionObjectWrapperPtr cast_cow = makeCastCollisionObject(cow); + link2cow_[name] = cast_cow; + } + else + { + link2cow_[name] = cow; + } + + btVector3 aabb_min, aabb_max; + link2cow_[name]->getAABB(aabb_min, aabb_max); + + int type = link2cow_[name]->getCollisionShape()->getShapeType(); + link2cow_[name]->setBroadphaseHandle( + broadphase_->createProxy(aabb_min, aabb_max, type, link2cow_[name].get(), link2cow_[name]->m_collisionFilterGroup, + link2cow_[name]->m_collisionFilterMask, dispatcher_.get())); +} + +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp new file mode 100644 index 0000000000..df78daba46 --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp @@ -0,0 +1,87 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Levi Armstrong, Jens Petit */ + +#include "moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h" + +namespace collision_detection_bullet +{ +BulletDiscreteBVHManagerPtr BulletDiscreteBVHManager::clone() const +{ + BulletDiscreteBVHManagerPtr manager(new BulletDiscreteBVHManager()); + + for (const std::pair& cow : link2cow_) + { + CollisionObjectWrapperPtr new_cow = cow.second->clone(); + + assert(new_cow->getCollisionShape()); + assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE); + + new_cow->setWorldTransform(cow.second->getWorldTransform()); + new_cow->setContactProcessingThreshold(static_cast(contact_distance_)); + manager->addCollisionObject(new_cow); + } + + manager->setActiveCollisionObjects(active_); + manager->setContactDistanceThreshold(contact_distance_); + + return manager; +} + +void BulletDiscreteBVHManager::contactTest(collision_detection::CollisionResult& collisions, + const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, bool self) +{ + ContactTestData cdata(active_, contact_distance_, collisions, req); + + broadphase_->calculateOverlappingPairs(dispatcher_.get()); + btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache(); + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", + "Num overlapping candidates " << pair_cache->getNumOverlappingPairs()); + + BroadphaseContactResultCallback cc(cdata, contact_distance_, acm, self); + TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc); + pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get()); + + ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", (collisions.collision ? "In" : "No") + << " collision with " << collisions.contact_count + << " collisions"); +} + +void BulletDiscreteBVHManager::addCollisionObject(const CollisionObjectWrapperPtr& cow) +{ + link2cow_[cow->getName()] = cow; + addCollisionObjectToBroadphase(cow, broadphase_, dispatcher_); +} + +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp new file mode 100644 index 0000000000..2f2b771964 --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp @@ -0,0 +1,353 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * Copyright (c) 2013, John Schulman + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: John Schulman, Levi Armstrong */ + +#include "moveit/collision_detection_bullet/bullet_integration/bullet_utils.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace collision_detection_bullet +{ +btCollisionShape* createShapePrimitive(const shapes::Box* geom, const CollisionObjectType& collision_object_type) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); + const double* size = geom->size; + btScalar a = static_cast(size[0] / 2); + btScalar b = static_cast(size[1] / 2); + btScalar c = static_cast(size[2] / 2); + + return (new btBoxShape(btVector3(a, b, c))); +} + +btCollisionShape* createShapePrimitive(const shapes::Sphere* geom, const CollisionObjectType& collision_object_type) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); + return (new btSphereShape(static_cast(geom->radius))); +} + +btCollisionShape* createShapePrimitive(const shapes::Cylinder* geom, const CollisionObjectType& collision_object_type) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); + btScalar r = static_cast(geom->radius); + btScalar l = static_cast(geom->length / 2); + return (new btCylinderShapeZ(btVector3(r, r, l))); +} + +btCollisionShape* createShapePrimitive(const shapes::Cone* geom, const CollisionObjectType& collision_object_type) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE); + btScalar r = static_cast(geom->radius); + btScalar l = static_cast(geom->length); + return (new btConeShapeZ(r, l)); +} + +btCollisionShape* createShapePrimitive(const shapes::Mesh* geom, const CollisionObjectType& collision_object_type, + CollisionObjectWrapper* cow) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE || + collision_object_type == CollisionObjectType::CONVEX_HULL || + collision_object_type == CollisionObjectType::SDF); + + if (geom->vertex_count > 0 && geom->triangle_count > 0) + { + // convert the mesh to the assigned collision object type + switch (collision_object_type) + { + case CollisionObjectType::CONVEX_HULL: + { + // Create a convex hull shape to approximate Trimesh + collision_detection_bullet::AlignedVector input; + collision_detection_bullet::AlignedVector vertices; + std::vector faces; + + input.reserve(geom->vertex_count); + for (unsigned int i = 0; i < geom->vertex_count; ++i) + input.push_back(Eigen::Vector3d(geom->vertices[3 * i], geom->vertices[3 * i + 1], geom->vertices[3 * i + 2])); + + if (collision_detection_bullet::createConvexHull(vertices, faces, input) < 0) + return nullptr; + + btConvexHullShape* subshape = new btConvexHullShape(); + for (const Eigen::Vector3d& v : vertices) + subshape->addPoint( + btVector3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2]))); + + return subshape; + } + case CollisionObjectType::USE_SHAPE_TYPE: + { + btCompoundShape* compound = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(geom->triangle_count)); + compound->setMargin( + BULLET_MARGIN); // margin: compound seems to have no effect when positive but has an effect when negative + + for (unsigned i = 0; i < geom->triangle_count; ++i) + { + btVector3 v[3]; + for (unsigned x = 0; x < 3; ++x) + { + unsigned idx = geom->triangles[3 * i + x]; + for (unsigned y = 0; y < 3; ++y) + { + v[x][y] = static_cast(geom->vertices[3 * idx + y]); + } + } + + btCollisionShape* subshape = new btTriangleShapeEx(v[0], v[1], v[2]); + if (subshape != nullptr) + { + cow->manage(subshape); + subshape->setMargin(BULLET_MARGIN); + btTransform geom_trans; + geom_trans.setIdentity(); + compound->addChildShape(geom_trans, subshape); + } + } + + return compound; + } + default: + { + ROS_ERROR("This bullet shape type (%d) is not supported for geometry meshs", + static_cast(collision_object_type)); + return nullptr; + } + } + } + ROS_ERROR("The mesh is empty!"); + return nullptr; +} + +btCollisionShape* createShapePrimitive(const shapes::OcTree* geom, const CollisionObjectType& collision_object_type, + CollisionObjectWrapper* cow) +{ + assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE || + collision_object_type == CollisionObjectType::CONVEX_HULL || + collision_object_type == CollisionObjectType::SDF || + collision_object_type == CollisionObjectType::MULTI_SPHERE); + + btCompoundShape* subshape = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(geom->octree->size())); + double occupancy_threshold = geom->octree->getOccupancyThres(); + + // convert the mesh to the assigned collision object type + switch (collision_object_type) + { + case CollisionObjectType::USE_SHAPE_TYPE: + { + for (auto it = geom->octree->begin(static_cast(geom->octree->getTreeDepth())), + end = geom->octree->end(); + it != end; ++it) + { + if (it->getOccupancy() >= occupancy_threshold) + { + double size = it.getSize(); + btTransform geom_trans; + geom_trans.setIdentity(); + geom_trans.setOrigin(btVector3(static_cast(it.getX()), static_cast(it.getY()), + static_cast(it.getZ()))); + btScalar l = static_cast(size / 2); + btBoxShape* childshape = new btBoxShape(btVector3(l, l, l)); + childshape->setMargin(BULLET_MARGIN); + cow->manage(childshape); + + subshape->addChildShape(geom_trans, childshape); + } + } + return subshape; + } + case CollisionObjectType::MULTI_SPHERE: + { + for (auto it = geom->octree->begin(static_cast(geom->octree->getTreeDepth())), + end = geom->octree->end(); + it != end; ++it) + { + if (it->getOccupancy() >= occupancy_threshold) + { + double size = it.getSize(); + btTransform geom_trans; + geom_trans.setIdentity(); + geom_trans.setOrigin(btVector3(static_cast(it.getX()), static_cast(it.getY()), + static_cast(it.getZ()))); + btSphereShape* childshape = + new btSphereShape(static_cast(std::sqrt(2 * ((size / 2) * (size / 2))))); + childshape->setMargin(BULLET_MARGIN); + cow->manage(childshape); + + subshape->addChildShape(geom_trans, childshape); + } + } + return subshape; + } + default: + { + ROS_ERROR("This bullet shape type (%d) is not supported for geometry octree", + static_cast(collision_object_type)); + return nullptr; + } + } +} + +btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom, + const CollisionObjectType& collision_object_type, CollisionObjectWrapper* cow) +{ + switch (geom->type) + { + case shapes::BOX: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type); + } + case shapes::SPHERE: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type); + } + case shapes::CYLINDER: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type); + } + case shapes::CONE: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type); + } + case shapes::MESH: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type, cow); + } + case shapes::OCTREE: + { + return createShapePrimitive(static_cast(geom.get()), collision_object_type, cow); + } + default: + { + ROS_ERROR("This geometric shape type (%d) is not supported using BULLET yet", static_cast(geom->type)); + return nullptr; + } + } +} + +CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + bool active) + : m_name(name) + , m_type_id(type_id) + , m_shapes(shapes) + , m_shape_poses(shape_poses) + , m_collision_object_types(collision_object_types) +{ + if (shapes.empty() || shape_poses.empty() || + (shapes.size() != shape_poses.size() || collision_object_types.empty() || + shapes.size() != collision_object_types.size())) + { + throw std::exception(); + } + + this->setContactProcessingThreshold(BULLET_DEFAULT_CONTACT_DISTANCE); + assert(!name.empty()); + + if (active) + { + m_collisionFilterGroup = btBroadphaseProxy::KinematicFilter; + m_collisionFilterMask = btBroadphaseProxy::KinematicFilter | btBroadphaseProxy::StaticFilter; + } + else + { + m_collisionFilterGroup = btBroadphaseProxy::StaticFilter; + m_collisionFilterMask = btBroadphaseProxy::KinematicFilter; + } + + if (shapes.size() == 1) + { + btCollisionShape* shape = createShapePrimitive(m_shapes[0], collision_object_types[0], this); + shape->setMargin(BULLET_MARGIN); + manage(shape); + setCollisionShape(shape); + setWorldTransform(convertEigenToBt(m_shape_poses[0])); + } + else + { + btCompoundShape* compound = + new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(m_shapes.size())); + manage(compound); + // margin on compound seems to have no effect when positive but has an effect when negative + compound->setMargin(BULLET_MARGIN); + setCollisionShape(compound); + + setWorldTransform(convertEigenToBt(m_shape_poses[0])); + Eigen::Isometry3d inv_world = m_shape_poses[0].inverse(); + + for (std::size_t j = 0; j < m_shapes.size(); ++j) + { + btCollisionShape* subshape = createShapePrimitive(m_shapes[j], collision_object_types[j], this); + if (subshape != nullptr) + { + manage(subshape); + subshape->setMargin(BULLET_MARGIN); + btTransform geom_trans = convertEigenToBt(inv_world * m_shape_poses[j]); + compound->addChildShape(geom_trans, subshape); + } + } + } +} + +CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::set& touch_links) + : CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types, true) +{ + m_touch_links = touch_links; +} + +CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::vector>& data) + : m_name(name) + , m_type_id(type_id) + , m_shapes(shapes) + , m_shape_poses(shape_poses) + , m_collision_object_types(collision_object_types) + , m_data(data) +{ +} +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp b/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp new file mode 100644 index 0000000000..2caee9406e --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp @@ -0,0 +1,49 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +namespace collision_detection +{ +bool CollisionDetectorBtPluginLoader::initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const +{ + scene->setActiveCollisionDetector(CollisionDetectorAllocatorBullet::create(), exclusive); + return true; +} +} // namespace collision_detection + +PLUGINLIB_EXPORT_CLASS(collision_detection::CollisionDetectorBtPluginLoader, collision_detection::CollisionPlugin) diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp new file mode 100644 index 0000000000..0be13bcfbc --- /dev/null +++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp @@ -0,0 +1,432 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include +#include +#include + +namespace collision_detection +{ +const std::string CollisionDetectorAllocatorBullet::NAME("Bullet"); +const double MAX_DISTANCE_MARGIN = 99; +constexpr char LOGNAME[] = "collision_detection.bullet"; + +CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, double padding, double scale) + : CollisionEnv(model, padding, scale) +{ + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + + for (const std::pair& link : robot_model_->getURDF()->links_) + { + addLinkAsCollisionObject(link.second); + } +} + +CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, + double padding, double scale) + : CollisionEnv(model, world, padding, scale) +{ + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + + for (const std::pair& link : robot_model_->getURDF()->links_) + { + addLinkAsCollisionObject(link.second); + } + + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +CollisionEnvBullet::CollisionEnvBullet(const CollisionEnvBullet& other, const WorldPtr& world) + : CollisionEnv(other, world) +{ + // TODO(j-petit): Verify this constructor + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + + for (const std::pair& link : other.robot_model_->getURDF()->links_) + { + addLinkAsCollisionObject(link.second); + } +} + +CollisionEnvBullet::~CollisionEnvBullet() +{ + getWorld()->removeObserver(observer_handle_); +} + +void CollisionEnvBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const +{ + checkSelfCollisionHelper(req, res, state, nullptr); +} + +void CollisionEnvBullet::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + checkSelfCollisionHelper(req, res, state, &acm); +} + +void CollisionEnvBullet::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + std::vector cows; + addAttachedOjects(state, cows); + + if (req.distance) + { + manager_->setContactDistanceThreshold(MAX_DISTANCE_MARGIN); + } + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : cows) + { + manager_->addCollisionObject(cow); + manager_->setCollisionObjectsTransform( + cow->getName(), state.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0]); + } + + // updating link positions with the current robot state + for (const std::string& link : active_) + { + manager_->setCollisionObjectsTransform(link, state.getCollisionBodyTransform(link, 0)); + } + + manager_->contactTest(res, req, acm, true); + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : cows) + { + manager_->removeCollisionObject(cow->getName()); + } +} + +void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const +{ + checkRobotCollisionHelper(req, res, state, nullptr); +} + +void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + checkRobotCollisionHelper(req, res, state, &acm); +} + +void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const +{ + checkRobotCollisionHelperCCD(req, res, state1, state2, nullptr); +} + +void CollisionEnvBullet::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, + const AllowedCollisionMatrix& acm) const +{ + checkRobotCollisionHelperCCD(req, res, state1, state2, &acm); +} + +void CollisionEnvBullet::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + if (req.distance) + { + manager_->setContactDistanceThreshold(MAX_DISTANCE_MARGIN); + } + + std::vector attached_cows; + addAttachedOjects(state, attached_cows); + updateTransformsFromState(state, manager_); + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows) + { + manager_->addCollisionObject(cow); + manager_->setCollisionObjectsTransform( + cow->getName(), state.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0]); + } + + manager_->contactTest(res, req, acm, false); + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows) + { + manager_->removeCollisionObject(cow->getName()); + } +} + +void CollisionEnvBullet::checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, + const AllowedCollisionMatrix* acm) const +{ + std::vector attached_cows; + addAttachedOjects(state1, attached_cows); + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows) + { + manager_CCD_->addCollisionObject(cow); + manager_CCD_->setCastCollisionObjectsTransform( + cow->getName(), state1.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0], + state2.getAttachedBody(cow->getName())->getGlobalCollisionBodyTransforms()[0]); + } + + for (const std::string& link : active_) + { + manager_CCD_->setCastCollisionObjectsTransform(link, state1.getCollisionBodyTransform(link, 0), + state2.getCollisionBodyTransform(link, 0)); + } + + manager_CCD_->contactTest(res, req, acm, false); + + for (const collision_detection_bullet::CollisionObjectWrapperPtr& cow : attached_cows) + { + manager_CCD_->removeCollisionObject(cow->getName()); + } +} + +void CollisionEnvBullet::distanceSelf(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const +{ + ROS_INFO_NAMED(LOGNAME, "distanceSelf is not implemented for Bullet."); +} + +void CollisionEnvBullet::distanceRobot(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const +{ + ROS_INFO_NAMED(LOGNAME, "distanceRobot is not implemented for Bullet."); +} + +void CollisionEnvBullet::addToManager(const World::Object* obj) +{ + std::vector collision_object_types; + + for (const shapes::ShapeConstPtr& shape : obj->shapes_) + { + if (shape->type == shapes::MESH) + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); + else + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + } + + collision_detection_bullet::CollisionObjectWrapperPtr cow(new collision_detection_bullet::CollisionObjectWrapper( + obj->id_, collision_detection::BodyType::WORLD_OBJECT, obj->shapes_, obj->shape_poses_, collision_object_types, + false)); + + manager_->addCollisionObject(cow); + manager_CCD_->addCollisionObject(cow->clone()); +} + +void CollisionEnvBullet::updateManagedObject(const std::string& id) +{ + if (getWorld()->hasObject(id)) + { + auto it = getWorld()->find(id); + if (manager_->hasCollisionObject(id)) + { + manager_->removeCollisionObject(id); + manager_CCD_->removeCollisionObject(id); + addToManager(it->second.get()); + } + else + { + addToManager(it->second.get()); + } + } + else + { + if (manager_->hasCollisionObject(id)) + { + manager_->removeCollisionObject(id); + manager_CCD_->removeCollisionObject(id); + } + } +} + +void CollisionEnvBullet::setWorld(const WorldPtr& world) +{ + if (world == getWorld()) + return; + + // turn off notifications about old world + getWorld()->removeObserver(observer_handle_); + + CollisionEnv::setWorld(world); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2)); + + // get notifications any objects already in the new world + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +void CollisionEnvBullet::notifyObjectChange(const ObjectConstPtr& obj, World::Action action) +{ + if (action == World::DESTROY) + { + manager_->removeCollisionObject(obj->id_); + manager_CCD_->removeCollisionObject(obj->id_); + } + else + { + updateManagedObject(obj->id_); + } +} + +void CollisionEnvBullet::addAttachedOjects(const moveit::core::RobotState& state, + std::vector& cows) const +{ + std::vector attached_bodies; + state.getAttachedBodies(attached_bodies); + + for (const moveit::core::AttachedBody*& body : attached_bodies) + { + const EigenSTL::vector_Isometry3d& attached_body_transform = body->getGlobalCollisionBodyTransforms(); + + std::vector collision_object_types( + attached_body_transform.size(), collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + + try + { + collision_detection_bullet::CollisionObjectWrapperPtr cow(new collision_detection_bullet::CollisionObjectWrapper( + body->getName(), collision_detection::BodyType::ROBOT_ATTACHED, body->getShapes(), attached_body_transform, + collision_object_types, body->getTouchLinks())); + cows.push_back(cow); + } + catch (std::exception&) + { + ROS_ERROR_STREAM_NAMED("collision_detetction.bullet", + "Not adding " << body->getName() << " due to bad arguments."); + } + } +} + +void CollisionEnvBullet::updatedPaddingOrScaling(const std::vector& links) +{ + for (const std::string& link : links) + { + if (robot_model_->getURDF()->links_.find(link) != robot_model_->getURDF()->links_.end()) + { + addLinkAsCollisionObject(robot_model_->getURDF()->links_[link]); + } + else + { + ROS_ERROR_NAMED("collision_detection.bullet", "Updating padding or scaling for unknown link: '%s'", link.c_str()); + } + } +} + +void CollisionEnvBullet::updateTransformsFromState( + const moveit::core::RobotState& state, const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const +{ + // updating link positions with the current robot state + for (const std::string& link : active_) + { + // select the first of the transformations for each link (composed of multiple shapes...) + manager->setCollisionObjectsTransform(link, state.getCollisionBodyTransform(link, 0)); + } +} + +void CollisionEnvBullet::addLinkAsCollisionObject(const urdf::LinkSharedPtr& link) +{ + if (!link->collision_array.empty()) + { + const std::vector& col_array = + link->collision_array.empty() ? std::vector(1, link->collision) : + link->collision_array; + + std::vector shapes; + collision_detection_bullet::AlignedVector shape_poses; + std::vector collision_object_types; + + for (const auto& i : col_array) + { + if (i && i->geometry) + { + shapes::ShapePtr shape = collision_detection_bullet::constructShape(i->geometry.get()); + + if (shape) + { + if (fabs(getLinkScale(link->name) - 1.0) >= std::numeric_limits::epsilon() || + fabs(getLinkPadding(link->name)) >= std::numeric_limits::epsilon()) + { + shape->scaleAndPadd(getLinkScale(link->name), getLinkPadding(link->name)); + } + + shapes.push_back(shape); + shape_poses.push_back(collision_detection_bullet::urdfPose2Eigen(i->origin)); + + if (shape->type == shapes::MESH) + { + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::CONVEX_HULL); + } + else + { + collision_object_types.push_back(collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE); + } + } + } + } + + if (manager_->hasCollisionObject(link->name)) + { + manager_->removeCollisionObject(link->name); + manager_CCD_->removeCollisionObject(link->name); + } + + try + { + collision_detection_bullet::CollisionObjectWrapperPtr cow(new collision_detection_bullet::CollisionObjectWrapper( + link->name, collision_detection::BodyType::ROBOT_LINK, shapes, shape_poses, collision_object_types, true)); + manager_->addCollisionObject(cow); + manager_CCD_->addCollisionObject(cow->clone()); + active_.push_back(cow->getName()); + } + catch (std::exception&) + { + ROS_ERROR_STREAM_NAMED("collision_detetction.bullet", "Not adding " << link->name << " due to bad arguments."); + } + } +} + +} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp new file mode 100644 index 0000000000..339cd15c3b --- /dev/null +++ b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp @@ -0,0 +1,47 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +INSTANTIATE_TYPED_TEST_CASE_P(BulletCollisionCheckPanda, CollisionDetectorPandaTest, + collision_detection::CollisionDetectorAllocatorBullet); + +int main(int argc, char* argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp new file mode 100644 index 0000000000..e522c6d67e --- /dev/null +++ b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp @@ -0,0 +1,47 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +INSTANTIATE_TYPED_TEST_CASE_P(BulletCollisionCheck, CollisionDetectorTest, + collision_detection::CollisionDetectorAllocatorBullet); + +int main(int argc, char* argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp new file mode 100644 index 0000000000..8af1398cc6 --- /dev/null +++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp @@ -0,0 +1,361 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +namespace cb = collision_detection_bullet; + +/** \brief Brings the panda robot in user defined home position */ +inline void setToHome(moveit::core::RobotState& panda_state) +{ + panda_state.setToDefaultValues(); + double joint2 = -0.785; + double joint4 = -2.356; + double joint6 = 1.571; + double joint7 = 0.785; + panda_state.setJointPositions("panda_joint2", &joint2); + panda_state.setJointPositions("panda_joint4", &joint4); + panda_state.setJointPositions("panda_joint6", &joint6); + panda_state.setJointPositions("panda_joint7", &joint7); + panda_state.update(); +} + +class BulletCollisionDetectionTester : public testing::Test +{ +protected: + void SetUp() override + { + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + robot_model_ok_ = static_cast(robot_model_); + + acm_.reset(new collision_detection::AllowedCollisionMatrix()); + // Use default collision operations in the SRDF to setup the acm + const std::vector& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry(); + acm_->setEntry(collision_links, collision_links, false); + + // allow collisions for pairs that have been disabled + const std::vector& dc = robot_model_->getSRDF()->getDisabledCollisionPairs(); + for (const srdf::Model::DisabledCollision& it : dc) + acm_->setEntry(it.link1_, it.link2_, true); + + cenv_.reset(new collision_detection::CollisionEnvBullet(robot_model_)); + + robot_state_.reset(new moveit::core::RobotState(robot_model_)); + + setToHome(*robot_state_); + } + + void TearDown() override + { + } + +protected: + bool robot_model_ok_; + + moveit::core::RobotModelPtr robot_model_; + + collision_detection::CollisionEnvPtr cenv_; + + collision_detection::AllowedCollisionMatrixPtr acm_; + + moveit::core::RobotStatePtr robot_state_; +}; + +void addCollisionObjects(cb::BulletCastBVHManager& checker) +{ + //////////////////////////// + // Add static box to checker + //////////////////////////// + shapes::ShapePtr static_box(new shapes::Box(1, 1, 1)); + Eigen::Isometry3d static_box_pose; + static_box_pose.setIdentity(); + + std::vector obj1_shapes; + cb::AlignedVector obj1_poses; + std::vector obj1_types; + obj1_shapes.push_back(static_box); + obj1_poses.push_back(static_box_pose); + obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE); + + cb::CollisionObjectWrapperPtr cow_1(new cb::CollisionObjectWrapper( + "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types)); + checker.addCollisionObject(cow_1); + + //////////////////////////// + // Add moving box to checker + //////////////////////////// + shapes::ShapePtr moving_box(new shapes::Box(0.2, 0.2, 0.2)); + Eigen::Isometry3d moving_box_pose; + + moving_box_pose.setIdentity(); + moving_box_pose.translation() = Eigen::Vector3d(0.5, -0.5, 0); + + std::vector obj2_shapes; + cb::AlignedVector obj2_poses; + std::vector obj2_types; + obj2_shapes.push_back(moving_box); + obj2_poses.push_back(moving_box_pose); + obj2_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE); + + cb::CollisionObjectWrapperPtr cow_2(new cb::CollisionObjectWrapper( + "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types)); + checker.addCollisionObject(cow_2); +} + +void addCollisionObjectsMesh(cb::BulletCastBVHManager& checker) +{ + //////////////////////////// + // Add static box to checker + //////////////////////////// + shapes::ShapePtr static_box(new shapes::Box(0.3, 0.3, 0.3)); + Eigen::Isometry3d static_box_pose; + static_box_pose.setIdentity(); + + std::vector obj1_shapes; + cb::AlignedVector obj1_poses; + std::vector obj1_types; + obj1_shapes.push_back(static_box); + obj1_poses.push_back(static_box_pose); + obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE); + + cb::CollisionObjectWrapperPtr cow_1(new cb::CollisionObjectWrapper( + "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types)); + checker.addCollisionObject(cow_1); + //////////////////////////// + // Add moving mesh to checker + //////////////////////////// + + std::vector obj2_shapes; + cb::AlignedVector obj2_poses; + std::vector obj2_types; + + obj1_poses.push_back(static_box_pose); + obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE); + + Eigen::Isometry3d s_pose; + s_pose.setIdentity(); + + std::string kinect = "package://moveit_resources_panda_description/meshes/collision/hand.stl"; + shapes::ShapeConstPtr s; + s.reset(shapes::createMeshFromResource(kinect)); + obj2_shapes.push_back(s); + obj2_types.push_back(cb::CollisionObjectType::CONVEX_HULL); + obj2_poses.push_back(s_pose); + + cb::CollisionObjectWrapperPtr cow_2(new cb::CollisionObjectWrapper( + "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types)); + checker.addCollisionObject(cow_2); +} + +void runTest(cb::BulletCastBVHManager& checker, collision_detection::CollisionResult& result, + std::vector& result_vector, Eigen::Isometry3d& start_pos, + Eigen::Isometry3d& end_pos) +{ + ////////////////////////////////////// + // Test when object is inside another + ////////////////////////////////////// + checker.setActiveCollisionObjects({ "moving_box_link" }); + checker.setContactDistanceThreshold(0.1); + + // Set the collision object transforms + checker.setCollisionObjectsTransform("static_box_link", Eigen::Isometry3d::Identity()); + checker.setCastCollisionObjectsTransform("moving_box_link", start_pos, end_pos); + + // Perform collision check + collision_detection::CollisionRequest request; + request.contacts = true; + // cb::ContactResultMap result; + checker.contactTest(result, request, nullptr, false); + + for (const auto& contacts_all : result.contacts) + { + for (const auto& contact : contacts_all.second) + { + result_vector.push_back(contact); + } + } +} + +// TODO(j-petit): Add continuous to continuous collision checking +/** \brief Continuous self collision checks are not supported yet by the bullet integration */ +TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + moveit::core::RobotState state1(robot_model_); + moveit::core::RobotState state2(robot_model_); + + setToHome(state1); + double joint2 = 0.15; + double joint4 = -3.0; + double joint5 = -0.8; + double joint7 = -0.785; + state1.setJointPositions("panda_joint2", &joint2); + state1.setJointPositions("panda_joint4", &joint4); + state1.setJointPositions("panda_joint5", &joint5); + state1.setJointPositions("panda_joint7", &joint7); + state1.update(); + + cenv_->checkSelfCollision(req, res, state1, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + setToHome(state2); + double joint_5_other = 0.8; + state2.setJointPositions("panda_joint2", &joint2); + state2.setJointPositions("panda_joint4", &joint4); + state2.setJointPositions("panda_joint5", &joint_5_other); + state2.setJointPositions("panda_joint7", &joint7); + state2.update(); + + cenv_->checkSelfCollision(req, res, state2, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + ROS_INFO_STREAM("Continous to continous collisions are not supported yet, therefore fail here."); + ASSERT_TRUE(res.collision); + res.clear(); +} + +/** \brief Two similar robot poses are used as start and end pose of a continuous collision check. */ +TEST_F(BulletCollisionDetectionTester, ContinuousCollisionWorld) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 10; + collision_detection::CollisionResult res; + + moveit::core::RobotState state1(robot_model_); + moveit::core::RobotState state2(robot_model_); + + setToHome(state1); + state1.update(); + + setToHome(state2); + double joint_2{ 0.05 }; + double joint_4{ -1.6 }; + state2.setJointPositions("panda_joint2", &joint_2); + state2.setJointPositions("panda_joint4", &joint_4); + state2.update(); + + cenv_->checkRobotCollision(req, res, state1, state2, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + // Adding the box which is not in collision with the individual states but with the casted one. + shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + pos.translation().x() = 0.43; + pos.translation().y() = 0; + pos.translation().z() = 0.55; + cenv_->getWorld()->addToObject("box", shape_ptr, pos); + + cenv_->checkRobotCollision(req, res, state1, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + cenv_->checkRobotCollision(req, res, state2, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + cenv_->checkRobotCollision(req, res, state1, state2, *acm_); + ASSERT_TRUE(res.collision); + ASSERT_EQ(res.contact_count, 4u); + res.clear(); +} + +TEST(ContinuousCollisionUnit, BulletCastBVHCollisionBoxBoxUnit) +{ + collision_detection::CollisionResult result; + std::vector result_vector; + + Eigen::Isometry3d start_pos, end_pos; + start_pos.setIdentity(); + start_pos.translation().x() = -2; + end_pos.setIdentity(); + end_pos.translation().x() = 2; + + cb::BulletCastBVHManager checker; + addCollisionObjects(checker); + runTest(checker, result, result_vector, start_pos, end_pos); + + ASSERT_TRUE(result.collision); + EXPECT_NEAR(result_vector[0].depth, -0.6, 0.001); + EXPECT_NEAR(result_vector[0].percent_interpolation, 0.6, 0.001); +} + +TEST(ContinuousCollisionUnit, BulletCastMeshVsBox) +{ + cb::BulletCastBVHManager checker; + addCollisionObjectsMesh(checker); + + Eigen::Isometry3d start_pos, end_pos; + start_pos.setIdentity(); + start_pos.translation().x() = -1.9; + end_pos.setIdentity(); + end_pos.translation().x() = 1.9; + + collision_detection::CollisionResult result; + std::vector result_vector; + + runTest(checker, result, result_vector, start_pos, end_pos); + + ASSERT_TRUE(result.collision); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index 8e73a17709..48bb00e6e6 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -2,8 +2,7 @@ set(MOVEIT_LIB_NAME moveit_collision_detection_fcl) add_library(${MOVEIT_LIB_NAME} src/collision_common.cpp - src/collision_robot_fcl.cpp - src/collision_world_fcl.cpp + src/collision_env_fcl.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -17,16 +16,21 @@ target_link_libraries(collision_detector_fcl_plugin ${catkin_LIBRARIES} ${MOVEIT install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_fcl_plugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(FILES ../collision_detector_fcl_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - - catkin_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection.cpp) + catkin_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection_pr2.cpp) target_link_libraries(test_fcl_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_fcl_collision_detection PRIVATE -Wno-deprecated-declarations) + + catkin_add_gtest(test_fcl_collision_detection_panda test/test_fcl_collision_detection_panda.cpp) + target_link_libraries(test_fcl_collision_detection_panda moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES}) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_fcl_collision_detection_panda PRIVATE -Wno-deprecated-declarations) endif() diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index f9fce66b21..2359081e9a 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -34,13 +34,13 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_COMMON_ -#define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_COMMON_ +#pragma once #include -#include +#include #include #include +#include #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) #include @@ -59,24 +59,30 @@ namespace collision_detection { MOVEIT_STRUCT_FORWARD(CollisionGeometryData); +/** \brief Wrapper around world, link and attached objects' geometry data. */ struct CollisionGeometryData { - CollisionGeometryData(const robot_model::LinkModel* link, int index) : type(BodyTypes::ROBOT_LINK), shape_index(index) + /** \brief Constructor for a robot link collision geometry object. */ + CollisionGeometryData(const moveit::core::LinkModel* link, int index) + : type(BodyTypes::ROBOT_LINK), shape_index(index) { ptr.link = link; } - CollisionGeometryData(const robot_state::AttachedBody* ab, int index) + /** \brief Constructor for a new collision geometry object which is attached to the robot. */ + CollisionGeometryData(const moveit::core::AttachedBody* ab, int index) : type(BodyTypes::ROBOT_ATTACHED), shape_index(index) { ptr.ab = ab; } + /** \brief Constructor for a new world collision geometry. */ CollisionGeometryData(const World::Object* obj, int index) : type(BodyTypes::WORLD_OBJECT), shape_index(index) { ptr.obj = obj; } + /** \brief Returns the name which is saved in the member pointed to in \e ptr. */ const std::string& getID() const { switch (type) @@ -91,6 +97,7 @@ struct CollisionGeometryData return ptr.obj->id_; } + /** \brief Returns a string of the corresponding \e type. */ std::string getTypeString() const { switch (type) @@ -105,31 +112,40 @@ struct CollisionGeometryData return "Object"; } - /** \brief Check if two CollisionGeometryData objects point to the same source object */ + /** \brief Check if two CollisionGeometryData objects point to the same source object. */ bool sameObject(const CollisionGeometryData& other) const { return type == other.type && ptr.raw == other.ptr.raw; } + /** \brief Indicates the body type of the object. */ BodyType type; + + /** \brief Multiple \e CollisionGeometryData objects construct a collision object. The collision object refers to an + * array of coordinate transformations at a certain start index. The index of the transformation of a child \e + * CollisionGeometryData object is then given by adding the parent collision object index and the \e shape_index of a + * geometry data object. */ int shape_index; + + /** \brief Points to the type of body which contains the geometry. */ union { - const robot_model::LinkModel* link; - const robot_state::AttachedBody* ab; + const moveit::core::LinkModel* link; + const moveit::core::AttachedBody* ab; const World::Object* obj; const void* raw; } ptr; }; +/** \brief Data structure which is passed to the collision callback function of the collision manager. */ struct CollisionData { - CollisionData() : req_(NULL), active_components_only_(NULL), res_(NULL), acm_(NULL), done_(false) + CollisionData() : req_(nullptr), active_components_only_(nullptr), res_(nullptr), acm_(nullptr), done_(false) { } CollisionData(const CollisionRequest* req, CollisionResult* res, const AllowedCollisionMatrix* acm) - : req_(req), active_components_only_(NULL), res_(res), acm_(acm), done_(false) + : req_(req), active_components_only_(nullptr), res_(res), acm_(acm), done_(false) { } @@ -137,27 +153,29 @@ struct CollisionData { } - /// Compute \e active_components_only_ based on \e req_ - void enableGroup(const robot_model::RobotModelConstPtr& robot_model); + /** \brief Compute \e active_components_only_ based on the joint group specified in \e req_ */ + void enableGroup(const moveit::core::RobotModelConstPtr& robot_model); - /// The collision request passed by the user + /** \brief The collision request passed by the user */ const CollisionRequest* req_; - /// If the collision request includes a group name, this set contains the pointers to the link models that are - /// considered for collision; - /// If the pointer is NULL, all collisions are considered. - const std::set* active_components_only_; + /** \brief If the collision request includes a group name, this set contains the pointers to the link models that + * are considered for collision. + * + * If the pointer is NULL, all collisions are considered. */ + const std::set* active_components_only_; - /// The user specified response location + /** \brief The user-specified response location. */ CollisionResult* res_; - /// The user specified collision matrix (may be NULL) + /** \brief The user-specified collision matrix (may be NULL). */ const AllowedCollisionMatrix* acm_; - /// Flag indicating whether collision checking is complete + /** \brief Flag indicating whether collision checking is complete. */ bool done_; }; +/** \brief Data structure which is passed to the distance callback function of the collision manager. */ struct DistanceData { DistanceData(const DistanceRequest* req, DistanceResult* res) : req(req), res(res), done(false) @@ -167,42 +185,48 @@ struct DistanceData { } - /// Distance query request information + /** \brief Distance query request information. */ const DistanceRequest* req; - /// Distance query results information + /** \brief Distance query results information. */ DistanceResult* res; - /// Indicates if distance query is finished. + /** \brief Indicates if distance query is finished. */ bool done; }; MOVEIT_STRUCT_FORWARD(FCLGeometry); +/** \brief Bundles the \e CollisionGeometryData and FCL collision geometry representation into a single class. */ struct FCLGeometry { FCLGeometry() { } - FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const robot_model::LinkModel* link, int shape_index) + /** \brief Constructor for a robot link. */ + FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const moveit::core::LinkModel* link, int shape_index) : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(link, shape_index)) { collision_geometry_->setUserData(collision_geometry_data_.get()); } - FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const robot_state::AttachedBody* ab, int shape_index) + /** \brief Constructor for an attached body. */ + FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const moveit::core::AttachedBody* ab, int shape_index) : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(ab, shape_index)) { collision_geometry_->setUserData(collision_geometry_data_.get()); } + /** \brief Constructor for a world object. */ FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const World::Object* obj, int shape_index) : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(obj, shape_index)) { collision_geometry_->setUserData(collision_geometry_data_.get()); } + /** \brief Updates the \e collision_geometry_data_ with new data while also setting the \e collision_geometry_ to the + * new data. */ template void updateCollisionGeometryData(const T* data, int shape_index, bool newType) { @@ -213,13 +237,18 @@ struct FCLGeometry collision_geometry_->setUserData(collision_geometry_data_.get()); } + /** \brief Pointer to FCL collision geometry. */ std::shared_ptr collision_geometry_; + + /** \brief Pointer to the user-defined geometry data. */ CollisionGeometryDataPtr collision_geometry_data_; }; typedef std::shared_ptr FCLCollisionObjectPtr; typedef std::shared_ptr FCLCollisionObjectConstPtr; +/** \brief A general high-level object which consists of multiple \e FCLCollisionObjects. It is the top level data + * structure which is used in the collision checking process. */ struct FCLObject { void registerTo(fcl::BroadPhaseCollisionManagerd* manager); @@ -227,44 +256,78 @@ struct FCLObject void clear(); std::vector collision_objects_; + + /** \brief Geometry data corresponding to \e collision_objects_. */ std::vector collision_geometry_; }; +/** \brief Bundles an \e FCLObject and a broadphase FCL collision manager. */ struct FCLManager { FCLObject object_; std::shared_ptr manager_; }; +/** \brief Callback function used by the FCLManager used for each pair of collision objects to + * calculate object contact information. + * + * \param o1 First FCL collision object + * \param o2 Second FCL collision object + * \data General pointer to arbitrary data which is used during the callback + * \return True terminates the collision check, false continues it to the next pair of objects */ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data); +/** \brief Callback function used by the FCLManager used for each pair of collision objects to + * calculate collisions and distances. + * + * \param o1 First FCL collision object + * \param o2 Second FCL collision object + * \data General pointer to arbitrary data which is used during the callback + * \return True terminates the distance check, false continues it to the next pair of objects */ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& min_dist); -FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link, +/** \brief Create new FCLGeometry object out of robot link model. */ +FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::LinkModel* link, int shape_index); -FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab, + +/** \brief Create new FCLGeometry object out of attached body. */ +FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::AttachedBody* ab, int shape_index); + +/** \brief Create new FCLGeometry object out of a world object. + * + * A world object always consists only of a single shape, therefore we don't need the \e shape_index. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj); +/** \brief Create new scaled and / or padded FCLGeometry object out of robot link model. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, - const robot_model::LinkModel* link, int shape_index); + const moveit::core::LinkModel* link, int shape_index); + +/** \brief Create new scaled and / or padded FCLGeometry object out of an attached body. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, - const robot_state::AttachedBody* ab, int shape_index); + const moveit::core::AttachedBody* ab, int shape_index); + +/** \brief Create new scaled and / or padded FCLGeometry object out of an world object. */ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, const World::Object* obj); + +/** \brief Increases the counter of the caches which can trigger the cleaning of expired entries from them. */ void cleanCollisionGeometryCache(); +/** \brief Transforms an Eigen Isometry3d to FCL coordinate transformation */ inline void transform2fcl(const Eigen::Isometry3d& b, fcl::Transform3d& f) { + ASSERT_ISOMETRY(b); #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) f = b.matrix(); #else - Eigen::Quaterniond q(b.rotation()); + Eigen::Quaterniond q(b.linear()); f.setTranslation(fcl::Vector3d(b.translation().x(), b.translation().y(), b.translation().z())); f.setQuatRotation(fcl::Quaternion3f(q.w(), q.x(), q.y(), q.z())); #endif } +/** \brief Transforms an Eigen Isometry3d to FCL coordinate transformation */ inline fcl::Transform3d transform2fcl(const Eigen::Isometry3d& b) { fcl::Transform3d t; @@ -272,6 +335,7 @@ inline fcl::Transform3d transform2fcl(const Eigen::Isometry3d& b) return t; } +/** \brief Transforms an FCL contact into a MoveIt contact point. */ inline void fcl2contact(const fcl::Contactd& fc, Contact& c) { c.pos = Eigen::Vector3d(fc.pos[0], fc.pos[1], fc.pos[2]); @@ -285,6 +349,7 @@ inline void fcl2contact(const fcl::Contactd& fc, Contact& c) c.body_type_2 = cgd2->type; } +/** \brief Transforms the FCL internal representation to the MoveIt \e CostSource data structure. */ inline void fcl2costsource(const fcl::CostSourced& fcs, CostSource& cs) { cs.aabb_min[0] = fcs.aabb_min[0]; @@ -297,6 +362,4 @@ inline void fcl2costsource(const fcl::CostSourced& fcs, CostSource& cs) cs.cost = fcs.cost_density; } -} - -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h index 6b7ec1ce52..fde03c4cb5 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h @@ -34,22 +34,18 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_FCL_H_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_FCL_H_ +#pragma once #include -#include -#include +#include namespace collision_detection { /** \brief An allocator for FCL collision detectors */ class CollisionDetectorAllocatorFCL - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_world_fcl.cpp + static const std::string NAME; // defined in collision_env_fcl.cpp }; -} - -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h index 18d7e8c40f..307130939c 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h @@ -1,9 +1,40 @@ -/* - * collision_detector_fcl_plugin_loader.h - */ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Bryce Willey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ -#ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_DETECTOR_FCL_PLUGIN_LOADER_H_ -#define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_DETECTOR_FCL_PLUGIN_LOADER_H_ +/* Author: Bryce Willey */ + +#pragma once #include #include @@ -13,7 +44,6 @@ namespace collision_detection class CollisionDetectorFCLPluginLoader : public CollisionPlugin { public: - virtual bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const; + bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const override; }; -} -#endif // MOVEIT_COLLISION_DETECTION_FCL_COLLISION_DETECTOR_FCL_PLUGIN_LOADER_H_ \ No newline at end of file +} // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h new file mode 100644 index 0000000000..c3e81354c3 --- /dev/null +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h @@ -0,0 +1,159 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Ioan Sucan, Jens Petit */ + +#pragma once + +#include +#include + +#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) +#include +#else +#include +#endif + +#include + +namespace collision_detection +{ +/** \brief FCL implementation of the CollisionEnv */ +class CollisionEnvFCL : public CollisionEnv +{ +public: + CollisionEnvFCL() = delete; + + CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); + + CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, + double scale = 1.0); + + CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& world); + + ~CollisionEnvFCL() override; + + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; + + void distanceSelf(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const override; + + void distanceRobot(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const override; + + void setWorld(const WorldPtr& world) override; + +protected: + /** \brief Updates the FCL collision geometry and objects saved in the CollisionRobotFCL members to reflect a new + * padding or scaling of the robot links. + * + * It searches for the link through the pointed-to robot model of the CollisionRobot and then constructs new FCL + * collision objects and geometries depending on the changed robot model. + * + * \param links The names of the links which have been updated in the robot model */ + void updatedPaddingOrScaling(const std::vector& links) override; + + /** \brief Bundles the different checkSelfCollision functions into a single function */ + void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; + + /** \brief Bundles the different checkRobotCollision functions into a single function */ + void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; + + /** \brief Construct an FCL collision object from MoveIt's World::Object. */ + void constructFCLObjectWorld(const World::Object* obj, FCLObject& fcl_obj) const; + + /** \brief Updates the specified object in \m fcl_objs_ and in the manager from new data available in the World. + * + * If it does not exist in world, it is deleted. If it's not existing in \m fcl_objs_ yet, it's added there. */ + void updateFCLObject(const std::string& id); + + /** \brief Out of the current robot state and its attached bodies construct an FCLObject which can then be used to + * check for collision. + * + * The current state is used to recalculate the AABB of the FCL collision objects. However they are not computed from + * scratch (which would require call to computeLocalAABB()) but are only transformed according to the joint states. + * + * \param state The current robot state + * \param fcl_obj The newly filled object */ + void constructFCLObjectRobot(const moveit::core::RobotState& state, FCLObject& fcl_obj) const; + + /** \brief Prepares for the collision check through constructing an FCL collision object out of the current robot + * state and specifying a broadphase collision manager of FCL where the constructed object is registered to. */ + void allocSelfCollisionBroadPhase(const moveit::core::RobotState& state, FCLManager& manager) const; + + /** \brief Converts all shapes which make up an atttached body into a vector of FCLGeometryConstPtr. + * + * When they are converted, they can be added to the FCL representation of the robot for collision checking. + * + * \param ab Pointer to the attached body + * \param geoms Output vector of geometries + */ + void getAttachedBodyObjects(const moveit::core::AttachedBody* ab, std::vector& geoms) const; + + /** \brief Vector of shared pointers to the FCL geometry for the objects in fcl_objs_. */ + std::vector robot_geoms_; + + /** \brief Vector of shared pointers to the FCL collision objects which make up the robot */ + std::vector robot_fcl_objs_; + + /// FCL collision manager which handles the collision checking process + std::unique_ptr manager_; + + std::map fcl_objs_; + +private: + /** \brief Callback function executed for each change to the world environment */ + void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); + + World::ObserverHandle observer_handle_; +}; +} // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h deleted file mode 100644 index a46794e999..0000000000 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h +++ /dev/null @@ -1,100 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_ROBOT_ -#define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_ROBOT_ - -#include - -namespace collision_detection -{ -class CollisionRobotFCL : public CollisionRobot -{ - friend class CollisionWorldFCL; - -public: - CollisionRobotFCL(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); - - CollisionRobotFCL(const CollisionRobotFCL& other); - - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const override; - void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const override; - - void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override; - - void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override; - -protected: - void updatedPaddingOrScaling(const std::vector& links) override; - void constructFCLObject(const robot_state::RobotState& state, FCLObject& fcl_obj) const; - void allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const; - void getAttachedBodyObjects(const robot_state::AttachedBody* ab, std::vector& geoms) const; - - void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const; - void checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, const AllowedCollisionMatrix* acm) const; - - std::vector geoms_; - std::vector fcl_objs_; -}; -} - -#endif diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h deleted file mode 100644 index e6d9557630..0000000000 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h +++ /dev/null @@ -1,101 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_WORLD_FCL_ -#define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_WORLD_FCL_ - -#include -#include - -#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) -#include -#else -#include -#endif - -#include - -namespace collision_detection -{ -class CollisionWorldFCL : public CollisionWorld -{ -public: - CollisionWorldFCL(); - explicit CollisionWorldFCL(const WorldPtr& world); - CollisionWorldFCL(const CollisionWorldFCL& other, const WorldPtr& world); - ~CollisionWorldFCL() override; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const override; - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const override; - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, - const AllowedCollisionMatrix& acm) const override; - - void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override; - - void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override; - - void setWorld(const WorldPtr& world) override; - -protected: - void checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, - const AllowedCollisionMatrix* acm) const; - void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const; - - void constructFCLObject(const World::Object* obj, FCLObject& fcl_obj) const; - void updateFCLObject(const std::string& id); - - std::unique_ptr manager_; - std::map fcl_objs_; - -private: - void initialize(); - void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); - World::ObserverHandle observer_handle_; -}; -} - -#endif diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h index 9eeda077cd..6d5c774a87 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h @@ -34,8 +34,7 @@ /* Author: Benjamin Scholz */ -#ifndef MOVEIT_COLLISION_DETECTION_FCL_COMPAT_ -#define MOVEIT_COLLISION_DETECTION_FCL_COMPAT_ +#pragma once #include @@ -81,7 +80,7 @@ namespace details struct sse_meta_f4; template struct Vec3Data; -} +} // namespace details template class Vec3fX; #if FCL_HAVE_SSE @@ -96,6 +95,5 @@ class OBBRSS; using OBBRSSd = fcl::OBBRSS; class DynamicAABBTreeCollisionManager; using DynamicAABBTreeCollisionManagerd = fcl::DynamicAABBTreeCollisionManager; -} -#endif +} // namespace fcl #endif diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index e977b025a2..4e12a5610e 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -67,11 +67,11 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi // If active components are specified if (cdata->active_components_only_) { - const robot_model::LinkModel* l1 = + const moveit::core::LinkModel* l1 = cd1->type == BodyTypes::ROBOT_LINK ? cd1->ptr.link : (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : nullptr); - const robot_model::LinkModel* l2 = + const moveit::core::LinkModel* l2 = cd2->type == BodyTypes::ROBOT_LINK ? cd2->ptr.link : (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : nullptr); @@ -96,10 +96,11 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi { always_allow_collision = true; if (cdata->req_->verbose) - ROS_DEBUG_NAMED( - "collision_detection.fcl", "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. " - "No contacts are computed.", - cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str()); + ROS_DEBUG_NAMED("collision_detection.fcl", + "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. " + "No contacts are computed.", + cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), + cd2->getTypeString().c_str()); } else if (type == AllowedCollision::CONDITIONAL) { @@ -179,8 +180,9 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi std::size_t num_max_cost_sources = cdata->req_->max_cost_sources; bool enable_contact = true; fcl::CollisionResultd col_result; - int num_contacts = fcl::collide(o1, o2, fcl::CollisionRequestd(std::numeric_limits::max(), enable_contact, - num_max_cost_sources, enable_cost), + int num_contacts = fcl::collide(o1, o2, + fcl::CollisionRequestd(std::numeric_limits::max(), enable_contact, + num_max_cost_sources, enable_cost), col_result); if (num_contacts > 0) { @@ -211,8 +213,9 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi cd1->getID().c_str(), cd2->getID().c_str()); } else if (cdata->req_->verbose) - ROS_INFO_NAMED("collision_detection.fcl", "Found unacceptable contact between '%s' (type '%s') and '%s' " - "(type '%s'). Contact was stored.", + ROS_INFO_NAMED("collision_detection.fcl", + "Found unacceptable contact between '%s' (type '%s') and '%s' " + "(type '%s'). Contact was stored.", cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str()); cdata->res_->collision = true; @@ -247,9 +250,10 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi bool enable_contact = true; fcl::CollisionResultd col_result; - int num_contacts = fcl::collide( - o1, o2, fcl::CollisionRequestd(want_contact_count, enable_contact, num_max_cost_sources, enable_cost), - col_result); + int num_contacts = + fcl::collide(o1, o2, + fcl::CollisionRequestd(want_contact_count, enable_contact, num_max_cost_sources, enable_cost), + col_result); if (num_contacts > 0) { int num_contacts_initial = num_contacts; @@ -264,8 +268,9 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi } if (cdata->req_->verbose) - ROS_INFO_NAMED("collision_detection.fcl", "Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), " - "which constitute a collision. %d contacts will be stored", + ROS_INFO_NAMED("collision_detection.fcl", + "Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), " + "which constitute a collision. %d contacts will be stored", num_contacts_initial, cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str(), num_contacts); @@ -309,9 +314,10 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi { cdata->res_->collision = true; if (cdata->req_->verbose) - ROS_INFO_NAMED("collision_detection.fcl", "Found a contact between '%s' (type '%s') and '%s' (type '%s'), " - "which constitutes a collision. " - "Contact information is not stored.", + ROS_INFO_NAMED("collision_detection.fcl", + "Found a contact between '%s' (type '%s') and '%s' (type '%s'), " + "which constitutes a collision. " + "Contact information is not stored.", cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str()); } @@ -348,14 +354,18 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi { cdata->done_ = cdata->req_->is_done(*cdata->res_); if (cdata->done_ && cdata->req_->verbose) - ROS_INFO_NAMED("collision_detection.fcl", "Collision checking is considered complete due to external callback. " - "%s was found. %u contacts are stored.", + ROS_INFO_NAMED("collision_detection.fcl", + "Collision checking is considered complete due to external callback. " + "%s was found. %u contacts are stored.", cdata->res_->collision ? "Collision" : "No collision", (unsigned int)cdata->res_->contact_count); } return cdata->done_; } +/** \brief Cache for an arbitrary type of shape. It is assigned during the execution of \e createCollisionGeometry(). + * + * Only a single cache per thread and object type is created as it is a quasi-singleton instance. */ struct FCLShapeCache { using ShapeKey = shapes::ShapeConstWeakPtr; @@ -390,7 +400,10 @@ struct FCLShapeCache static const unsigned int MAX_CLEAN_COUNT = 100; // every this many uses of the cache, a cleaning operation is // executed (this is only removal of expired entries) + /** \brief Map of weak pointers to the FCLGeometry. */ ShapeMap map_; + + /** \brief Counts cache usage and triggers clearing of cache when \m MAX_CLEAN_COUNT is exceeded. */ unsigned int clean_count_; }; @@ -408,11 +421,11 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void // If active components are specified if (cdata->req->active_components_only) { - const robot_model::LinkModel* l1 = + const moveit::core::LinkModel* l1 = cd1->type == BodyTypes::ROBOT_LINK ? cd1->ptr.link : (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : nullptr); - const robot_model::LinkModel* l2 = + const moveit::core::LinkModel* l2 = cd2->type == BodyTypes::ROBOT_LINK ? cd2->ptr.link : (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : nullptr); @@ -459,19 +472,16 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void cd1->getID().c_str(), cd2->getID().c_str()); } } - else + else if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED) { - if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED) + const std::set& tl = cd1->ptr.ab->getTouchLinks(); + if (tl.find(cd2->getID()) != tl.end()) { - const std::set& tl = cd1->ptr.ab->getTouchLinks(); - if (tl.find(cd2->getID()) != tl.end()) - { - always_allow_collision = true; - if (cdata->req->verbose) - ROS_DEBUG_NAMED("collision_detection.fcl", - "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.", - cd2->getID().c_str(), cd1->getID().c_str()); - } + always_allow_collision = true; + if (cdata->req->verbose) + ROS_DEBUG_NAMED("collision_detection.fcl", + "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.", + cd2->getID().c_str(), cd1->getID().c_str()); } } @@ -483,8 +493,6 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void ROS_DEBUG_NAMED("collision_detection.fcl", "Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str()); - fcl::DistanceResultd fcl_result; - DistanceResultsData dist_result; double dist_threshold = cdata->req->distance_threshold; const std::pair& pc = cd1->getID() < cd2->getID() ? @@ -513,7 +521,16 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void } } + fcl::DistanceResultd fcl_result; fcl_result.min_distance = dist_threshold; + // fcl::distance segfaults when given an octree with a null root pointer (using FCL 0.6.1) + if ((o1->getObjectType() == fcl::OT_OCTREE && + !std::static_pointer_cast(o1->collisionGeometry())->getRoot()) || + (o2->getObjectType() == fcl::OT_OCTREE && + !std::static_pointer_cast(o2->collisionGeometry())->getRoot())) + { + return false; + } double d = fcl::distance(o1, o2, fcl::DistanceRequestd(cdata->req->enable_nearest_points), fcl_result); // Check if either object is already in the map. If not add it or if present @@ -521,6 +538,7 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void // one and add the new distance information. if (d < dist_threshold) { + DistanceResultsData dist_result; dist_result.distance = fcl_result.min_distance; #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) dist_result.nearest_points[0] = fcl_result.nearest_points[0]; @@ -604,7 +622,7 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void } else if (cdata->req->type == DistanceRequestType::SINGLE) { - if (it->second[0].distance < dist_result.distance) + if (dist_result.distance < it->second[0].distance) it->second[0] = dist_result; } else if (cdata->req->type == DistanceRequestType::LIMITED) @@ -624,7 +642,9 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void return cdata->done; } -/* We template the function so we get a different cache for each of the template arguments combinations */ +/* Templated function to get a different cache for each of the template arguments combinations. + * + * The returned cache is a quasi-singleton for each thread as it is created \e thread_local. */ template FCLShapeCache& GetShapeCache() { @@ -659,6 +679,11 @@ struct IfSameType }; }; +/** \brief Templated helper function creating new collision geometry out of general object using an arbitrary bounding + * volume (BV). + * + * It assigns a thread-local cache for each type of shape and minimizes memory usage and copying through utilizing the + * cache. */ template FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const T* data, int shape_index) { @@ -694,7 +719,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, // attached objects could have previously been World::Object; we try to move them // from their old cache to the new one, if possible. the code is not pretty, but should help // when we attach/detach objects that are in the world - if (IfSameType::value == 1) + if (IfSameType::value == 1) { // get the cache that corresponds to objects; maybe this attached object used to be a world object FCLShapeCache& othercache = GetShapeCache(); @@ -730,7 +755,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, if (IfSameType::value == 1) { // get the cache that corresponds to objects; maybe this attached object used to be a world object - FCLShapeCache& othercache = GetShapeCache(); + FCLShapeCache& othercache = GetShapeCache(); // attached bodies could be just moved from the environment. auto cache_it = othercache.map_.find(wptr); @@ -839,17 +864,16 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, return FCLGeometryConstPtr(); } -///////////////////////////////////////////////////// -FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link, +FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::LinkModel* link, int shape_index) { - return createCollisionGeometry(shape, link, shape_index); + return createCollisionGeometry(shape, link, shape_index); } -FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab, +FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::AttachedBody* ab, int shape_index) { - return createCollisionGeometry(shape, ab, shape_index); + return createCollisionGeometry(shape, ab, shape_index); } FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj) @@ -857,6 +881,8 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, return createCollisionGeometry(shape, obj, 0); } +/** \brief Templated helper function creating new collision geometry out of general object using an arbitrary bounding + * volume (BV). This can include padding and scaling. */ template FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, const T* data, int shape_index) @@ -873,15 +899,15 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, } FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, - const robot_model::LinkModel* link, int shape_index) + const moveit::core::LinkModel* link, int shape_index) { - return createCollisionGeometry(shape, scale, padding, link, shape_index); + return createCollisionGeometry(shape, scale, padding, link, shape_index); } FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, - const robot_state::AttachedBody* ab, int shape_index) + const moveit::core::AttachedBody* ab, int shape_index) { - return createCollisionGeometry(shape, scale, padding, ab, shape_index); + return createCollisionGeometry(shape, scale, padding, ab, shape_index); } FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding, @@ -896,14 +922,13 @@ void cleanCollisionGeometryCache() { cache1.bumpUseCount(true); } - FCLShapeCache& cache2 = GetShapeCache(); + FCLShapeCache& cache2 = GetShapeCache(); { cache2.bumpUseCount(true); } } -} // namespace collision_detection -void collision_detection::CollisionData::enableGroup(const robot_model::RobotModelConstPtr& robot_model) +void CollisionData::enableGroup(const moveit::core::RobotModelConstPtr& robot_model) { if (robot_model->hasJointModelGroup(req_->group_name)) active_components_only_ = &robot_model->getJointModelGroup(req_->group_name)->getUpdatedLinkModelsSet(); @@ -911,7 +936,7 @@ void collision_detection::CollisionData::enableGroup(const robot_model::RobotMod active_components_only_ = nullptr; } -void collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManagerd* manager) +void FCLObject::registerTo(fcl::BroadPhaseCollisionManagerd* manager) { std::vector collision_objects(collision_objects_.size()); for (std::size_t i = 0; i < collision_objects_.size(); ++i) @@ -920,14 +945,15 @@ void collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManagerd manager->registerObjects(collision_objects); } -void collision_detection::FCLObject::unregisterFrom(fcl::BroadPhaseCollisionManagerd* manager) +void FCLObject::unregisterFrom(fcl::BroadPhaseCollisionManagerd* manager) { for (auto& collision_object : collision_objects_) manager->unregisterObject(collision_object.get()); } -void collision_detection::FCLObject::clear() +void FCLObject::clear() { collision_objects_.clear(); collision_geometry_.clear(); } +} // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp b/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp index d2fa9cb71d..9e3fbdf39a 100644 --- a/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp @@ -1,3 +1,39 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Bryce Willey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Bryce Willey */ + #include #include @@ -8,6 +44,6 @@ bool CollisionDetectorFCLPluginLoader::initialize(const planning_scene::Planning scene->setActiveCollisionDetector(CollisionDetectorAllocatorFCL::create(), exclusive); return true; } -} +} // namespace collision_detection PLUGINLIB_EXPORT_CLASS(collision_detection::CollisionDetectorFCLPluginLoader, collision_detection::CollisionPlugin) diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp new file mode 100644 index 0000000000..e99c954100 --- /dev/null +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -0,0 +1,435 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Ioan Sucan, Jens Petit */ + +#include +#include +#include + +#include + +#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) +#include +#endif + +namespace collision_detection +{ +const std::string CollisionDetectorAllocatorFCL::NAME("FCL"); +constexpr char LOGNAME[] = "collision_detection.fcl"; + +CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, double padding, double scale) + : CollisionEnv(model, padding, scale) +{ + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + std::size_t index; + robot_geoms_.resize(robot_model_->getLinkGeometryCount()); + robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount()); + // we keep the same order of objects as what RobotState *::getLinkState() returns + for (auto link : links) + for (std::size_t j = 0; j < link->getShapes().size(); ++j) + { + FCLGeometryConstPtr link_geometry = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()), + getLinkPadding(link->getName()), link, j); + if (link_geometry) + { + index = link->getFirstCollisionBodyTransformIndex() + j; + robot_geoms_[index] = link_geometry; + + // Need to store the FCL object so the AABB does not get recreated every time. + // Every time this object is created, g->computeLocalAABB() is called which is + // very expensive and should only be calculated once. To update the AABB, use the + // collObj->setTransform and then call collObj->computeAABB() to transform the AABB. + robot_fcl_objs_[index] = + FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(link_geometry->collision_geometry_)); + } + else + ROS_ERROR_NAMED(LOGNAME, "Unable to construct collision geometry for link '%s'", link->getName().c_str()); + } + + auto m = new fcl::DynamicAABBTreeCollisionManagerd(); + // m->tree_init_level = 2; + manager_.reset(m); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); +} + +CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding, + double scale) + : CollisionEnv(model, world, padding, scale) +{ + const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); + std::size_t index; + robot_geoms_.resize(robot_model_->getLinkGeometryCount()); + robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount()); + // we keep the same order of objects as what RobotState *::getLinkState() returns + for (auto link : links) + for (std::size_t j = 0; j < link->getShapes().size(); ++j) + { + FCLGeometryConstPtr g = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()), + getLinkPadding(link->getName()), link, j); + if (g) + { + index = link->getFirstCollisionBodyTransformIndex() + j; + robot_geoms_[index] = g; + + // Need to store the FCL object so the AABB does not get recreated every time. + // Every time this object is created, g->computeLocalAABB() is called which is + // very expensive and should only be calculated once. To update the AABB, use the + // collObj->setTransform and then call collObj->computeAABB() to transform the AABB. + robot_fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_)); + } + else + ROS_ERROR_NAMED(LOGNAME, "Unable to construct collision geometry for link '%s'", link->getName().c_str()); + } + + auto m = new fcl::DynamicAABBTreeCollisionManagerd(); + // m->tree_init_level = 2; + manager_.reset(m); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +CollisionEnvFCL::~CollisionEnvFCL() +{ + getWorld()->removeObserver(observer_handle_); +} + +CollisionEnvFCL::CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& world) : CollisionEnv(other, world) +{ + robot_geoms_ = other.robot_geoms_; + robot_fcl_objs_ = other.robot_fcl_objs_; + + auto m = new fcl::DynamicAABBTreeCollisionManagerd(); + // m->tree_init_level = 2; + manager_.reset(m); + + fcl_objs_ = other.fcl_objs_; + for (auto& fcl_obj : fcl_objs_) + fcl_obj.second.registerTo(manager_.get()); + // manager_->update(); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); +} + +void CollisionEnvFCL::getAttachedBodyObjects(const moveit::core::AttachedBody* ab, + std::vector& geoms) const +{ + const std::vector& shapes = ab->getShapes(); + for (std::size_t i = 0; i < shapes.size(); ++i) + { + FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], ab, i); + if (co) + geoms.push_back(co); + } +} + +void CollisionEnvFCL::constructFCLObjectWorld(const World::Object* obj, FCLObject& fcl_obj) const +{ + for (std::size_t i = 0; i < obj->shapes_.size(); ++i) + { + FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj); + if (g) + { + auto co = new fcl::CollisionObjectd(g->collision_geometry_, transform2fcl(obj->shape_poses_[i])); + fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(co)); + fcl_obj.collision_geometry_.push_back(g); + } + } +} + +void CollisionEnvFCL::constructFCLObjectRobot(const moveit::core::RobotState& state, FCLObject& fcl_obj) const +{ + fcl_obj.collision_objects_.reserve(robot_geoms_.size()); + fcl::Transform3d fcl_tf; + + for (std::size_t i = 0; i < robot_geoms_.size(); ++i) + if (robot_geoms_[i] && robot_geoms_[i]->collision_geometry_) + { + transform2fcl(state.getCollisionBodyTransform(robot_geoms_[i]->collision_geometry_data_->ptr.link, + robot_geoms_[i]->collision_geometry_data_->shape_index), + fcl_tf); + auto coll_obj = new fcl::CollisionObjectd(*robot_fcl_objs_[i]); + coll_obj->setTransform(fcl_tf); + coll_obj->computeAABB(); + fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(coll_obj)); + } + + // TODO: Implement a method for caching fcl::CollisionObject's for moveit::core::AttachedBody's + std::vector ab; + state.getAttachedBodies(ab); + for (auto& body : ab) + { + std::vector objs; + getAttachedBodyObjects(body, objs); + const EigenSTL::vector_Isometry3d& ab_t = body->getGlobalCollisionBodyTransforms(); + for (std::size_t k = 0; k < objs.size(); ++k) + if (objs[k]->collision_geometry_) + { + transform2fcl(ab_t[k], fcl_tf); + fcl_obj.collision_objects_.push_back( + FCLCollisionObjectPtr(new fcl::CollisionObjectd(objs[k]->collision_geometry_, fcl_tf))); + // we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself, + // and would be destroyed when objs goes out of scope. + fcl_obj.collision_geometry_.push_back(objs[k]); + } + } +} + +void CollisionEnvFCL::allocSelfCollisionBroadPhase(const moveit::core::RobotState& state, FCLManager& manager) const +{ + auto m = new fcl::DynamicAABBTreeCollisionManagerd(); + // m->tree_init_level = 2; + manager.manager_.reset(m); + constructFCLObjectRobot(state, manager.object_); + manager.object_.registerTo(manager.manager_.get()); + // manager.manager_->update(); +} + +void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const +{ + checkSelfCollisionHelper(req, res, state, nullptr); +} + +void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const +{ + checkSelfCollisionHelper(req, res, state, &acm); +} + +void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + FCLManager manager; + allocSelfCollisionBroadPhase(state, manager); + CollisionData cd(&req, &res, acm); + cd.enableGroup(getRobotModel()); + manager.manager_->collide(&cd, &collisionCallback); + if (req.distance) + { + DistanceRequest dreq; + DistanceResult dres; + + dreq.group_name = req.group_name; + dreq.acm = acm; + dreq.enableGroup(getRobotModel()); + distanceSelf(dreq, dres, state); + res.distance = dres.minimum_distance.distance; + } +} + +void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const +{ + checkRobotCollisionHelper(req, res, state, nullptr); +} + +void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + checkRobotCollisionHelper(req, res, state, &acm); +} + +void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const +{ + ROS_ERROR_NAMED(LOGNAME, "Continuous collision not implemented"); +} + +void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, + const AllowedCollisionMatrix& acm) const +{ + ROS_ERROR_NAMED(LOGNAME, "Not implemented"); +} + +void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm) const +{ + FCLObject fcl_obj; + constructFCLObjectRobot(state, fcl_obj); + + CollisionData cd(&req, &res, acm); + cd.enableGroup(getRobotModel()); + for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i) + manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback); + + if (req.distance) + { + DistanceRequest dreq; + DistanceResult dres; + + dreq.group_name = req.group_name; + dreq.acm = acm; + dreq.enableGroup(getRobotModel()); + distanceRobot(dreq, dres, state); + res.distance = dres.minimum_distance.distance; + } +} + +void CollisionEnvFCL::distanceSelf(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const +{ + FCLManager manager; + allocSelfCollisionBroadPhase(state, manager); + DistanceData drd(&req, &res); + + manager.manager_->distance(&drd, &distanceCallback); +} + +void CollisionEnvFCL::distanceRobot(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const +{ + FCLObject fcl_obj; + constructFCLObjectRobot(state, fcl_obj); + + DistanceData drd(&req, &res); + for (std::size_t i = 0; !drd.done && i < fcl_obj.collision_objects_.size(); ++i) + manager_->distance(fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback); +} + +void CollisionEnvFCL::updateFCLObject(const std::string& id) +{ + // remove FCL objects that correspond to this object + auto jt = fcl_objs_.find(id); + if (jt != fcl_objs_.end()) + { + jt->second.unregisterFrom(manager_.get()); + jt->second.clear(); + } + + // check to see if we have this object + auto it = getWorld()->find(id); + if (it != getWorld()->end()) + { + // construct FCL objects that correspond to this object + if (jt != fcl_objs_.end()) + { + constructFCLObjectWorld(it->second.get(), jt->second); + jt->second.registerTo(manager_.get()); + } + else + { + constructFCLObjectWorld(it->second.get(), fcl_objs_[id]); + fcl_objs_[id].registerTo(manager_.get()); + } + } + else + { + if (jt != fcl_objs_.end()) + fcl_objs_.erase(jt); + } + + // manager_->update(); +} + +void CollisionEnvFCL::setWorld(const WorldPtr& world) +{ + if (world == getWorld()) + return; + + // turn off notifications about old world + getWorld()->removeObserver(observer_handle_); + + // clear out objects from old world + manager_->clear(); + fcl_objs_.clear(); + cleanCollisionGeometryCache(); + + CollisionEnv::setWorld(world); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2)); + + // get notifications any objects already in the new world + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +void CollisionEnvFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Action action) +{ + if (action == World::DESTROY) + { + auto it = fcl_objs_.find(obj->id_); + if (it != fcl_objs_.end()) + { + it->second.unregisterFrom(manager_.get()); + it->second.clear(); + fcl_objs_.erase(it); + } + cleanCollisionGeometryCache(); + } + else + { + updateFCLObject(obj->id_); + if (action & (World::DESTROY | World::REMOVE_SHAPE)) + cleanCollisionGeometryCache(); + } +} + +void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector& links) +{ + std::size_t index; + for (const auto& link : links) + { + const moveit::core::LinkModel* lmodel = robot_model_->getLinkModel(link); + if (lmodel) + { + for (std::size_t j = 0; j < lmodel->getShapes().size(); ++j) + { + FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShapes()[j], getLinkScale(lmodel->getName()), + getLinkPadding(lmodel->getName()), lmodel, j); + if (g) + { + index = lmodel->getFirstCollisionBodyTransformIndex() + j; + robot_geoms_[index] = g; + robot_fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_)); + } + } + } + else + ROS_ERROR_NAMED(LOGNAME, "Updating padding or scaling for unknown link: '%s'", link.c_str()); + } +} + +} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp deleted file mode 100644 index 9b8a4cea68..0000000000 --- a/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp +++ /dev/null @@ -1,307 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include - -#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) -#include -#endif - -namespace collision_detection -{ -CollisionRobotFCL::CollisionRobotFCL(const robot_model::RobotModelConstPtr& model, double padding, double scale) - : CollisionRobot(model, padding, scale) -{ - const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry(); - std::size_t index; - geoms_.resize(robot_model_->getLinkGeometryCount()); - fcl_objs_.resize(robot_model_->getLinkGeometryCount()); - // we keep the same order of objects as what RobotState *::getLinkState() returns - for (auto link : links) - for (std::size_t j = 0; j < link->getShapes().size(); ++j) - { - FCLGeometryConstPtr g = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()), - getLinkPadding(link->getName()), link, j); - if (g) - { - index = link->getFirstCollisionBodyTransformIndex() + j; - geoms_[index] = g; - - // Need to store the FCL object so the AABB does not get recreated every time. - // Every time this object is created, g->computeLocalAABB() is called which is - // very expensive and should only be calculated once. To update the AABB, use the - // collObj->setTransform and then call collObj->computeAABB() to transform the AABB. - fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_)); - } - else - ROS_ERROR_NAMED("collision_detection.fcl", "Unable to construct collision geometry for link '%s'", - link->getName().c_str()); - } -} - -CollisionRobotFCL::CollisionRobotFCL(const CollisionRobotFCL& other) : CollisionRobot(other) -{ - geoms_ = other.geoms_; - fcl_objs_ = other.fcl_objs_; -} - -void CollisionRobotFCL::getAttachedBodyObjects(const robot_state::AttachedBody* ab, - std::vector& geoms) const -{ - const std::vector& shapes = ab->getShapes(); - for (std::size_t i = 0; i < shapes.size(); ++i) - { - FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], ab, i); - if (co) - geoms.push_back(co); - } -} - -void CollisionRobotFCL::constructFCLObject(const robot_state::RobotState& state, FCLObject& fcl_obj) const -{ - fcl_obj.collision_objects_.reserve(geoms_.size()); - fcl::Transform3d fcl_tf; - - for (std::size_t i = 0; i < geoms_.size(); ++i) - if (geoms_[i] && geoms_[i]->collision_geometry_) - { - transform2fcl(state.getCollisionBodyTransform(geoms_[i]->collision_geometry_data_->ptr.link, - geoms_[i]->collision_geometry_data_->shape_index), - fcl_tf); - auto coll_obj = new fcl::CollisionObjectd(*fcl_objs_[i]); - coll_obj->setTransform(fcl_tf); - coll_obj->computeAABB(); - fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(coll_obj)); - } - - // TODO: Implement a method for caching fcl::CollisionObject's for robot_state::AttachedBody's - std::vector ab; - state.getAttachedBodies(ab); - for (auto& body : ab) - { - std::vector objs; - getAttachedBodyObjects(body, objs); - const EigenSTL::vector_Isometry3d& ab_t = body->getGlobalCollisionBodyTransforms(); - for (std::size_t k = 0; k < objs.size(); ++k) - if (objs[k]->collision_geometry_) - { - transform2fcl(ab_t[k], fcl_tf); - fcl_obj.collision_objects_.push_back( - FCLCollisionObjectPtr(new fcl::CollisionObjectd(objs[k]->collision_geometry_, fcl_tf))); - // we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself, - // and would be destroyed when objs goes out of scope. - fcl_obj.collision_geometry_.push_back(objs[k]); - } - } -} - -void CollisionRobotFCL::allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const -{ - auto m = new fcl::DynamicAABBTreeCollisionManagerd(); - // m->tree_init_level = 2; - manager.manager_.reset(m); - constructFCLObject(state, manager.object_); - manager.object_.registerTo(manager.manager_.get()); - // manager.manager_->update(); -} - -void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state) const -{ - checkSelfCollisionHelper(req, res, state, nullptr); -} - -void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - checkSelfCollisionHelper(req, res, state, &acm); -} - -void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const -{ - ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented"); -} - -void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented"); -} - -void CollisionRobotFCL::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const -{ - FCLManager manager; - allocSelfCollisionBroadPhase(state, manager); - CollisionData cd(&req, &res, acm); - cd.enableGroup(getRobotModel()); - manager.manager_->collide(&cd, &collisionCallback); - if (req.distance) - { - DistanceRequest dreq; - DistanceResult dres; - - dreq.group_name = req.group_name; - dreq.acm = acm; - dreq.enableGroup(getRobotModel()); - distanceSelf(dreq, dres, state); - res.distance = dres.minimum_distance.distance; - } -} - -void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - checkOtherCollisionHelper(req, res, state, other_robot, other_state, nullptr); -} - -void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix& acm) const -{ - checkOtherCollisionHelper(req, res, state, other_robot, other_state, &acm); -} - -void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2) const -{ - ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented"); -} - -void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state1, - const robot_state::RobotState& state2, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state1, - const robot_state::RobotState& other_state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented"); -} - -void CollisionRobotFCL::checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const robot_state::RobotState& state, - const CollisionRobot& other_robot, - const robot_state::RobotState& other_state, - const AllowedCollisionMatrix* acm) const -{ - FCLManager manager; - allocSelfCollisionBroadPhase(state, manager); - - const CollisionRobotFCL& fcl_rob = dynamic_cast(other_robot); - FCLObject other_fcl_obj; - fcl_rob.constructFCLObject(other_state, other_fcl_obj); - - CollisionData cd(&req, &res, acm); - cd.enableGroup(getRobotModel()); - for (std::size_t i = 0; !cd.done_ && i < other_fcl_obj.collision_objects_.size(); ++i) - manager.manager_->collide(other_fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback); - - if (req.distance) - { - DistanceRequest dreq; - DistanceResult dres; - - dreq.group_name = req.group_name; - dreq.acm = acm; - dreq.enableGroup(getRobotModel()); - distanceOther(dreq, dres, state, other_robot, other_state); - res.distance = dres.minimum_distance.distance; - } -} - -void CollisionRobotFCL::updatedPaddingOrScaling(const std::vector& links) -{ - std::size_t index; - for (const auto& link : links) - { - const robot_model::LinkModel* lmodel = robot_model_->getLinkModel(link); - if (lmodel) - { - for (std::size_t j = 0; j < lmodel->getShapes().size(); ++j) - { - FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShapes()[j], getLinkScale(lmodel->getName()), - getLinkPadding(lmodel->getName()), lmodel, j); - if (g) - { - index = lmodel->getFirstCollisionBodyTransformIndex() + j; - geoms_[index] = g; - fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_)); - } - } - } - else - ROS_ERROR_NAMED("collision_detection.fcl", "Updating padding or scaling for unknown link: '%s'", link.c_str()); - } -} - -void CollisionRobotFCL::distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const -{ - FCLManager manager; - allocSelfCollisionBroadPhase(state, manager); - DistanceData drd(&req, &res); - - manager.manager_->distance(&drd, &distanceCallback); -} - -void CollisionRobotFCL::distanceOther(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state, const CollisionRobot& other_robot, - const robot_state::RobotState& other_state) const -{ - FCLManager manager; - allocSelfCollisionBroadPhase(state, manager); - - const CollisionRobotFCL& fcl_rob = dynamic_cast(other_robot); - FCLObject other_fcl_obj; - fcl_rob.constructFCLObject(other_state, other_fcl_obj); - - DistanceData drd(&req, &res); - for (std::size_t i = 0; !drd.done && i < other_fcl_obj.collision_objects_.size(); ++i) - manager.manager_->distance(other_fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback); -} - -} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp deleted file mode 100644 index 8d6eb392f9..0000000000 --- a/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp +++ /dev/null @@ -1,299 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include -#include - -#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) -#include -#include -#include -#include -#else -#include -#include -#include -#include -#endif - -#include - -namespace collision_detection -{ -const std::string CollisionDetectorAllocatorFCL::NAME("FCL"); - -CollisionWorldFCL::CollisionWorldFCL() : CollisionWorld() -{ - auto m = new fcl::DynamicAABBTreeCollisionManagerd(); - // m->tree_init_level = 2; - manager_.reset(m); - - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2)); -} - -CollisionWorldFCL::CollisionWorldFCL(const WorldPtr& world) : CollisionWorld(world) -{ - auto m = new fcl::DynamicAABBTreeCollisionManagerd(); - // m->tree_init_level = 2; - manager_.reset(m); - - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2)); - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -CollisionWorldFCL::CollisionWorldFCL(const CollisionWorldFCL& other, const WorldPtr& world) - : CollisionWorld(other, world) -{ - auto m = new fcl::DynamicAABBTreeCollisionManagerd(); - // m->tree_init_level = 2; - manager_.reset(m); - - fcl_objs_ = other.fcl_objs_; - for (auto& fcl_obj : fcl_objs_) - fcl_obj.second.registerTo(manager_.get()); - // manager_->update(); - - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2)); -} - -CollisionWorldFCL::~CollisionWorldFCL() -{ - getWorld()->removeObserver(observer_handle_); -} - -void CollisionWorldFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state) const -{ - checkRobotCollisionHelper(req, res, robot, state, nullptr); -} - -void CollisionWorldFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - checkRobotCollisionHelper(req, res, robot, state, &acm); -} - -void CollisionWorldFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state1, - const robot_state::RobotState& state2) const -{ - ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented"); -} - -void CollisionWorldFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state1, - const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const -{ - ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented"); -} - -void CollisionWorldFCL::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm) const -{ - const CollisionRobotFCL& robot_fcl = dynamic_cast(robot); - FCLObject fcl_obj; - robot_fcl.constructFCLObject(state, fcl_obj); - - CollisionData cd(&req, &res, acm); - cd.enableGroup(robot.getRobotModel()); - for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i) - manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback); - - if (req.distance) - { - DistanceRequest dreq; - DistanceResult dres; - - dreq.group_name = req.group_name; - dreq.acm = acm; - dreq.enableGroup(robot.getRobotModel()); - distanceRobot(dreq, dres, robot, state); - res.distance = dres.minimum_distance.distance; - } -} - -void CollisionWorldFCL::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const -{ - checkWorldCollisionHelper(req, res, other_world, nullptr); -} - -void CollisionWorldFCL::checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world, const AllowedCollisionMatrix& acm) const -{ - checkWorldCollisionHelper(req, res, other_world, &acm); -} - -void CollisionWorldFCL::checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world, - const AllowedCollisionMatrix* acm) const -{ - const CollisionWorldFCL& other_fcl_world = dynamic_cast(other_world); - CollisionData cd(&req, &res, acm); - manager_->collide(other_fcl_world.manager_.get(), &cd, &collisionCallback); - - if (req.distance) - { - DistanceRequest dreq; - DistanceResult dres; - - dreq.group_name = req.group_name; - dreq.acm = acm; - distanceWorld(dreq, dres, other_world); - res.distance = dres.minimum_distance.distance; - } -} - -void CollisionWorldFCL::constructFCLObject(const World::Object* obj, FCLObject& fcl_obj) const -{ - for (std::size_t i = 0; i < obj->shapes_.size(); ++i) - { - FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj); - if (g) - { - auto co = new fcl::CollisionObjectd(g->collision_geometry_, transform2fcl(obj->shape_poses_[i])); - fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(co)); - fcl_obj.collision_geometry_.push_back(g); - } - } -} - -void CollisionWorldFCL::updateFCLObject(const std::string& id) -{ - // remove FCL objects that correspond to this object - auto jt = fcl_objs_.find(id); - if (jt != fcl_objs_.end()) - { - jt->second.unregisterFrom(manager_.get()); - jt->second.clear(); - } - - // check to see if we have this object - auto it = getWorld()->find(id); - if (it != getWorld()->end()) - { - // construct FCL objects that correspond to this object - if (jt != fcl_objs_.end()) - { - constructFCLObject(it->second.get(), jt->second); - jt->second.registerTo(manager_.get()); - } - else - { - constructFCLObject(it->second.get(), fcl_objs_[id]); - fcl_objs_[id].registerTo(manager_.get()); - } - } - else - { - if (jt != fcl_objs_.end()) - fcl_objs_.erase(jt); - } - - // manager_->update(); -} - -void CollisionWorldFCL::setWorld(const WorldPtr& world) -{ - if (world == getWorld()) - return; - - // turn off notifications about old world - getWorld()->removeObserver(observer_handle_); - - // clear out objects from old world - manager_->clear(); - fcl_objs_.clear(); - cleanCollisionGeometryCache(); - - CollisionWorld::setWorld(world); - - // request notifications about changes to new world - observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2)); - - // get notifications any objects already in the new world - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -void CollisionWorldFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Action action) -{ - if (action == World::DESTROY) - { - auto it = fcl_objs_.find(obj->id_); - if (it != fcl_objs_.end()) - { - it->second.unregisterFrom(manager_.get()); - it->second.clear(); - fcl_objs_.erase(it); - } - cleanCollisionGeometryCache(); - } - else - { - updateFCLObject(obj->id_); - if (action & (World::DESTROY | World::REMOVE_SHAPE)) - cleanCollisionGeometryCache(); - } -} - -void CollisionWorldFCL::distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - const CollisionRobotFCL& robot_fcl = dynamic_cast(robot); - FCLObject fcl_obj; - robot_fcl.constructFCLObject(state, fcl_obj); - - DistanceData drd(&req, &res); - for (std::size_t i = 0; !drd.done && i < fcl_obj.collision_objects_.size(); ++i) - manager_->distance(fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback); -} - -void CollisionWorldFCL::distanceWorld(const DistanceRequest& req, DistanceResult& res, - const CollisionWorld& world) const -{ - const CollisionWorldFCL& other_fcl_world = dynamic_cast(world); - DistanceData drd(&req, &res); - manager_->distance(other_fcl_world.manager_.get(), &drd, &distanceCallback); -} - -} // end of namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp new file mode 100644 index 0000000000..1561841df9 --- /dev/null +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp @@ -0,0 +1,50 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +INSTANTIATE_TYPED_TEST_CASE_P(FCLCollisionCheckPanda, CollisionDetectorPandaTest, + collision_detection::CollisionDetectorAllocatorFCL); + +INSTANTIATE_TYPED_TEST_CASE_P(FCLDistanceCheckPanda, DistanceCheckPandaTest, + collision_detection::CollisionDetectorAllocatorFCL); + +int main(int argc, char* argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp new file mode 100644 index 0000000000..efe312cf3b --- /dev/null +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp @@ -0,0 +1,47 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +INSTANTIATE_TYPED_TEST_CASE_P(FCLCollisionCheck, CollisionDetectorTest, + collision_detection::CollisionDetectorAllocatorFCL); + +int main(int argc, char* argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp new file mode 100644 index 0000000000..a2803e223d --- /dev/null +++ b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp @@ -0,0 +1,324 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include +#include + +/** \brief Brings the panda robot in user defined home position */ +inline void setToHome(moveit::core::RobotState& panda_state) +{ + panda_state.setToDefaultValues(); + double joint2 = -0.785; + double joint4 = -2.356; + double joint6 = 1.571; + double joint7 = 0.785; + panda_state.setJointPositions("panda_joint2", &joint2); + panda_state.setJointPositions("panda_joint4", &joint4); + panda_state.setJointPositions("panda_joint6", &joint6); + panda_state.setJointPositions("panda_joint7", &joint7); + panda_state.update(); +} + +class CollisionDetectionEnvTest : public testing::Test +{ +protected: + void SetUp() override + { + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + robot_model_ok_ = static_cast(robot_model_); + + acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false)); + + acm_->setEntry("panda_link0", "panda_link1", true); + acm_->setEntry("panda_link1", "panda_link2", true); + acm_->setEntry("panda_link2", "panda_link3", true); + acm_->setEntry("panda_link3", "panda_link4", true); + acm_->setEntry("panda_link4", "panda_link5", true); + acm_->setEntry("panda_link5", "panda_link6", true); + acm_->setEntry("panda_link6", "panda_link7", true); + acm_->setEntry("panda_link7", "panda_hand", true); + acm_->setEntry("panda_hand", "panda_rightfinger", true); + acm_->setEntry("panda_hand", "panda_leftfinger", true); + acm_->setEntry("panda_rightfinger", "panda_leftfinger", true); + acm_->setEntry("panda_link5", "panda_link7", true); + acm_->setEntry("panda_link6", "panda_hand", true); + + c_env_.reset(new collision_detection::CollisionEnvFCL(robot_model_)); + + robot_state_.reset(new moveit::core::RobotState(robot_model_)); + + setToHome(*robot_state_); + } + + void TearDown() override + { + } + +protected: + bool robot_model_ok_; + + moveit::core::RobotModelPtr robot_model_; + + collision_detection::CollisionEnvPtr c_env_; + + collision_detection::AllowedCollisionMatrixPtr acm_; + + moveit::core::RobotStatePtr robot_state_; +}; + +/** \brief Correct setup testing. */ +TEST_F(CollisionDetectionEnvTest, InitOK) +{ + ASSERT_TRUE(robot_model_ok_); +} + +/** \brief Tests the default values specified in the SRDF if they are collision free. */ +TEST_F(CollisionDetectionEnvTest, DefaultNotInCollision) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + c_env_->checkSelfCollision(req, res, *robot_state_, *acm_); + ASSERT_FALSE(res.collision); +} + +/** \brief A configuration where the robot should collide with itself. */ +TEST_F(CollisionDetectionEnvTest, LinksInCollision) +{ + // Sets the joint values to zero which is a colliding configuration + robot_state_->setToDefaultValues(); + robot_state_->update(); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + c_env_->checkSelfCollision(req, res, *robot_state_, *acm_); + ASSERT_TRUE(res.collision); +} + +/** \brief Adding obstacles to the world which are tested against the robot. Simple cases. */ +TEST_F(CollisionDetectionEnvTest, RobotWorldCollision_1) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + shapes::Shape* shape = new shapes::Box(.1, .1, .1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + pos1.translation().z() = 0.3; + c_env_->getWorld()->addToObject("box", shape_ptr, pos1); + + c_env_->checkSelfCollision(req, res, *robot_state_, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + c_env_->checkRobotCollision(req, res, *robot_state_, *acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + c_env_->getWorld()->moveObject("box", pos1); + c_env_->checkRobotCollision(req, res, *robot_state_, *acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + c_env_->getWorld()->moveObject("box", pos1); + ASSERT_FALSE(res.collision); +} + +/** \brief Adding obstacles to the world which are tested against the robot. */ +TEST_F(CollisionDetectionEnvTest, RobotWorldCollision_2) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + req.max_contacts = 10; + req.contacts = true; + req.verbose = true; + + shapes::Shape* shape = new shapes::Box(.4, .4, .4); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + pos1.translation().z() = 0.3; + c_env_->getWorld()->addToObject("box", shape_ptr, pos1); + c_env_->checkRobotCollision(req, res, *robot_state_, *acm_); + ASSERT_TRUE(res.collision); + ASSERT_GE(res.contact_count, 3u); + res.clear(); +} + +/** \brief Tests the padding through expanding the link geometry in such a way that a collision occurs. */ +TEST_F(CollisionDetectionEnvTest, PaddingTest) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 10; + collision_detection::CollisionResult res; + + c_env_->checkRobotCollision(req, res, *robot_state_, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + // Adding the box right in front of the robot hand + shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + pos.translation().x() = 0.43; + pos.translation().y() = 0; + pos.translation().z() = 0.55; + c_env_->getWorld()->addToObject("box", shape_ptr, pos); + + c_env_->setLinkPadding("panda_hand", 0.08); + c_env_->checkRobotCollision(req, res, *robot_state_, *acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + c_env_->setLinkPadding("panda_hand", 0.0); + c_env_->checkRobotCollision(req, res, *robot_state_, *acm_); + ASSERT_FALSE(res.collision); +} + +/** \brief Continuous self collision checks of the robot. + * + * Functionality not supported yet. */ +TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionSelf) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + moveit::core::RobotState state1(robot_model_); + moveit::core::RobotState state2(robot_model_); + + setToHome(state1); + double joint2 = 0.15; + double joint4 = -3.0; + double joint5 = -0.8; + double joint7 = -0.785; + state1.setJointPositions("panda_joint2", &joint2); + state1.setJointPositions("panda_joint4", &joint4); + state1.setJointPositions("panda_joint5", &joint5); + state1.setJointPositions("panda_joint7", &joint7); + state1.update(); + + c_env_->checkSelfCollision(req, res, state1, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + setToHome(state2); + double joint_5_other = 0.8; + state2.setJointPositions("panda_joint2", &joint2); + state2.setJointPositions("panda_joint4", &joint4); + state2.setJointPositions("panda_joint5", &joint_5_other); + state2.setJointPositions("panda_joint7", &joint7); + state2.update(); + + c_env_->checkSelfCollision(req, res, state2, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + // c_env_->checkSelfCollision(req, res, state1, state2, *acm_); + ROS_INFO_STREAM("Continous to continous collisions are not supported yet, therefore fail here."); + ASSERT_TRUE(res.collision); + res.clear(); +} + +/** \brief Two similar robot poses are used as start and end pose of a continuous collision check. + * + * Functionality not supported yet. */ +TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionWorld) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 10; + collision_detection::CollisionResult res; + + moveit::core::RobotState state1(robot_model_); + moveit::core::RobotState state2(robot_model_); + + setToHome(state1); + state1.update(); + + setToHome(state2); + double joint_2{ 0.05 }; + double joint_4{ -1.6 }; + state2.setJointPositions("panda_joint2", &joint_2); + state2.setJointPositions("panda_joint4", &joint_4); + state2.update(); + + c_env_->checkRobotCollision(req, res, state1, state2, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + // Adding the box which is not in collision with the individual states but sits right between them. + shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1); + shapes::ShapeConstPtr shape_ptr(shape); + + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + pos.translation().x() = 0.43; + pos.translation().y() = 0; + pos.translation().z() = 0.55; + c_env_->getWorld()->addToObject("box", shape_ptr, pos); + + c_env_->checkRobotCollision(req, res, state1, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + c_env_->checkRobotCollision(req, res, state2, *acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + c_env_->checkRobotCollision(req, res, state1, state2, *acm_); + ASSERT_TRUE(res.collision); + ASSERT_EQ(res.contact_count, 4u); + res.clear(); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/collision_detector_bullet_description.xml b/moveit_core/collision_detector_bullet_description.xml new file mode 100644 index 0000000000..4065ee048d --- /dev/null +++ b/moveit_core/collision_detector_bullet_description.xml @@ -0,0 +1,8 @@ + + + + Bullet Collision Detector + + + diff --git a/moveit_core/collision_distance_field/CMakeLists.txt b/moveit_core/collision_distance_field/CMakeLists.txt index 491b47f0e8..2c7696896d 100644 --- a/moveit_core/collision_distance_field/CMakeLists.txt +++ b/moveit_core/collision_distance_field/CMakeLists.txt @@ -8,10 +8,8 @@ endif() add_library(${MOVEIT_LIB_NAME} src/collision_distance_field_types.cpp src/collision_common_distance_field.cpp - src/collision_robot_distance_field.cpp - src/collision_world_distance_field.cpp - src/collision_robot_hybrid.cpp - src/collision_world_hybrid.cpp + src/collision_env_distance_field.cpp + src/collision_env_hybrid.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} @@ -20,17 +18,14 @@ target_link_libraries(${MOVEIT_LIB_NAME} add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_collision_distance_field test/test_collision_distance_field.cpp) - target_link_libraries(test_collision_distance_field ${MOVEIT_LIB_NAME}) + target_link_libraries(test_collision_distance_field ${MOVEIT_LIB_NAME} moveit_test_utils) endif() install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h index fa5910a581..fe4567d561 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h @@ -34,13 +34,12 @@ /* Author: E. Gil Jones */ -#ifndef MOVEIT_COLLISION_DETECTION_DISTANCE_FIELD_COLLISION_COMMON_ -#define MOVEIT_COLLISION_DETECTION_DISTANCE_FIELD_COLLISION_COMMON_ +#pragma once #include #include #include -#include +#include #include namespace collision_detection @@ -115,7 +114,7 @@ struct DistanceFieldCacheEntry /** for checking collisions between this group and other objects */ std::string group_name_; /** RobotState that this cache entry represents */ - robot_state::RobotStatePtr state_; + moveit::core::RobotStatePtr state_; /** list of indices into the state_values_ vector. One index for each joint * variable which is NOT in the group or a child of the group. In other * words, variables which should not change if only joints in the group move. @@ -171,13 +170,12 @@ BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeCons PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const collision_detection::World::Object& obj, double resolution); -PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const robot_state::AttachedBody* att, +PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const moveit::core::AttachedBody* att, double resolution); -PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const robot_state::AttachedBody* att, +PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const moveit::core::AttachedBody* att, double resolution); void getBodySphereVisualizationMarkers(const GroupStateRepresentationPtr& gsr, const std::string& reference_frame, visualization_msgs::MarkerArray& body_marker_array); -} -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h index 53c71b9f35..188be97465 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h @@ -34,23 +34,18 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_DISTANCE_FIELD_H_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_DISTANCE_FIELD_H_ +#pragma once #include -#include -#include +#include namespace collision_detection { /** \brief An allocator for Distance Field collision detectors */ class CollisionDetectorAllocatorDistanceField - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_world_distance_field.cpp + static const std::string NAME; // defined in collision_env_distance_field.cpp }; -} - -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h index 6eb3277ee7..0e35ad38ae 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h @@ -34,23 +34,18 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_HYBRID_H_ -#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_HYBRID_H_ +#pragma once #include -#include -#include +#include namespace collision_detection { /** \brief An allocator for Hybrid collision detectors */ class CollisionDetectorAllocatorHybrid - : public CollisionDetectorAllocatorTemplate + : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_world_hybrid.cpp + static const std::string NAME; // defined in collision_env_hybrid.cpp }; -} - -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index 364386fb34..94256fe799 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -34,13 +34,11 @@ /* Author: E. Gil Jones */ -#ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_DISTANCE_FIELD_TYPES_ -#define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_DISTANCE_FIELD_TYPES_ +#pragma once #include #include #include -#include #include #include @@ -107,7 +105,7 @@ struct GradientInfo }; MOVEIT_CLASS_FORWARD(PosedDistanceField) -MOVEIT_CLASS_FORWARD(BodyDecomposition); +MOVEIT_CLASS_FORWARD(BodyDecomposition); // Defines BodyDecompositionPtr, ConstPtr, WeakPtr... etc MOVEIT_CLASS_FORWARD(PosedBodySphereDecomposition) MOVEIT_CLASS_FORWARD(PosedBodyPointDecomposition) MOVEIT_CLASS_FORWARD(PosedBodySphereDecompositionVector) @@ -447,10 +445,9 @@ class PosedBodyPointDecompositionVector EigenSTL::vector_Vector3d getCollisionPoints() const { EigenSTL::vector_Vector3d ret_points; - for (unsigned int i = 0; i < decomp_vector_.size(); i++) + for (const PosedBodyPointDecompositionPtr& decomp : decomp_vector_) { - ret_points.insert(ret_points.end(), decomp_vector_[i]->getCollisionPoints().begin(), - decomp_vector_[i]->getCollisionPoints().end()); + ret_points.insert(ret_points.end(), decomp->getCollisionPoints().begin(), decomp->getCollisionPoints().end()); } return ret_points; } @@ -523,6 +520,4 @@ void getCollisionMarkers(const std::string& frame_id, const std::string& ns, con const std::vector& posed_decompositions, const std::vector& posed_vector_decompositions, const std::vector& gradients, visualization_msgs::MarkerArray& arr); -} - -#endif +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h similarity index 55% rename from moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_distance_field.h rename to moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h index 2c968dac76..fc395a7463 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h @@ -34,13 +34,12 @@ /* Author: E. Gil Jones */ -#ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_ROBOT_DISTANCE_FIELD_ -#define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_ROBOT_DISTANCE_FIELD_ +#pragma once #include -#include #include #include +#include #include #include @@ -54,32 +53,36 @@ static const double DEFAULT_RESOLUTION = .02; static const double DEFAULT_COLLISION_TOLERANCE = 0.0; static const double DEFAULT_MAX_PROPOGATION_DISTANCE = .25; -MOVEIT_CLASS_FORWARD(CollisionRobotDistanceField); +MOVEIT_CLASS_FORWARD(CollisionEnvDistanceField); // Defines CollisionEnvDistanceFieldPtr, ConstPtr, WeakPtr... etc -class CollisionRobotDistanceField : public CollisionRobot +class CollisionEnvDistanceField : public CollisionEnv { - friend class CollisionWorldDistanceField; - public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - CollisionRobotDistanceField(const robot_model::RobotModelConstPtr& robot_model); - - CollisionRobotDistanceField(const CollisionRobot& col_robot, const Eigen::Vector3d& size, - const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, - double collision_tolerance, double max_propogation_distance, double padding); - - CollisionRobotDistanceField(const robot_model::RobotModelConstPtr& robot_model, - const std::map>& link_body_decompositions, - double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, - double size_z = DEFAULT_SIZE_Z, - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, - double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, - double scale = 1.0); - - CollisionRobotDistanceField(const CollisionRobotDistanceField& other); + CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr& robot_model, + const std::map>& link_body_decompositions = + std::map>(), + double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, + double size_z = DEFAULT_SIZE_Z, const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), + bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, + double resolution = DEFAULT_RESOLUTION, + double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, + double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, + double scale = 1.0); + + CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, + const std::map>& link_body_decompositions = + std::map>(), + double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, + double size_z = DEFAULT_SIZE_Z, const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), + bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, + double resolution = DEFAULT_RESOLUTION, + double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, + double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, + double scale = 1.0); + + CollisionEnvDistanceField(const CollisionEnvDistanceField& other, const WorldPtr& world); void initialize(const std::map>& link_body_decompositions, const Eigen::Vector3d& size, const Eigen::Vector3d& origin, bool use_signed_distance_field, @@ -99,99 +102,119 @@ class CollisionRobotDistanceField : public CollisionRobot const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state1, const moveit::core::RobotState& state2) const override - { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - }; + void createCollisionModelMarker(const moveit::core::RobotState& state, + visualization_msgs::MarkerArray& model_markers) const; - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, - const collision_detection::AllowedCollisionMatrix& acm) const override + virtual double distanceSelf(const moveit::core::RobotState& /* state */) const { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - }; + return 0.0; + } - void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, const CollisionRobot& other_robot, - const moveit::core::RobotState& other_state) const override + virtual double distanceSelf(const moveit::core::RobotState& /* state */, + const collision_detection::AllowedCollisionMatrix& /* acm */) const { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - }; + return 0.0; + } - void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, const CollisionRobot& other_robot, - const moveit::core::RobotState& other_state, - const collision_detection::AllowedCollisionMatrix& acm) const override + void distanceSelf(const DistanceRequest& /* req */, DistanceResult& /* res */, + const moveit::core::RobotState& /* state */) const override { ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - }; + } - void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, - const CollisionRobot& other_robot, const moveit::core::RobotState& other_state1, - const moveit::core::RobotState& other_state2) const override + DistanceFieldCacheEntryConstPtr getLastDistanceFieldEntry() const { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - }; + return distance_field_cache_entry_; + } + + // void getSelfCollisionsGradients(const collision_detection::CollisionRequest + // &req, + // collision_detection::CollisionResult &res, + // const moveit::core::RobotState &state, + // const + // collision_detection::AllowedCollisionMatrix + // &acm) const; - void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, - const CollisionRobot& other_robot, const moveit::core::RobotState& other_state1, - const moveit::core::RobotState& other_state2, - const collision_detection::AllowedCollisionMatrix& acm) const override + MOVEIT_STRUCT_FORWARD(DistanceFieldCacheEntryWorld) + struct DistanceFieldCacheEntryWorld { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); + std::map> posed_body_point_decompositions_; + distance_field::DistanceFieldPtr distance_field_; }; - void createCollisionModelMarker(const moveit::core::RobotState& state, - visualization_msgs::MarkerArray& model_markers) const; + ~CollisionEnvDistanceField() override; - virtual double distanceSelf(const moveit::core::RobotState& state) const - { - return 0.0; - }; - virtual double distanceSelf(const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm) const - { - return 0.0; - }; - virtual double distanceOther(const moveit::core::RobotState& state, const CollisionRobot& other_robot, - const moveit::core::RobotState& other_state) const + void checkCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const; + + void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; + + virtual double distanceRobot(const moveit::core::RobotState& state, bool verbose = false) const { + (void)state; + (void)verbose; return 0.0; - }; - virtual double distanceOther(const moveit::core::RobotState& state, const CollisionRobot& other_robot, - const moveit::core::RobotState& other_state, - const collision_detection::AllowedCollisionMatrix& acm) const + } + + virtual double distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, + bool verbose = false) const { + (void)state; + (void)acm; + (void)verbose; return 0.0; - }; + } - void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const robot_state::RobotState& state) const override + void distanceRobot(const DistanceRequest& /* req */, DistanceResult& /* res */, + const moveit::core::RobotState& /* state */) const override { ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); } - void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state, - const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override + void setWorld(const WorldPtr& world) override; + + distance_field::DistanceFieldConstPtr getDistanceField() const { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); + return distance_field_cache_entry_->distance_field_; } - DistanceFieldCacheEntryConstPtr getLastDistanceFieldEntry() const + collision_detection::GroupStateRepresentationConstPtr getLastGroupStateRepresentation() const { - return distance_field_cache_entry_; + return last_gsr_; } - // void getSelfCollisionsGradients(const collision_detection::CollisionRequest - // &req, - // collision_detection::CollisionResult &res, - // const moveit::core::RobotState &state, - // const - // collision_detection::AllowedCollisionMatrix - // &acm) const; + void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; + + void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; + protected: bool getSelfProximityGradients(GroupStateRepresentationPtr& gsr) const; @@ -243,7 +266,21 @@ class CollisionRobotDistanceField : public CollisionRobot bool compareCacheEntryToAllowedCollisionMatrix(const DistanceFieldCacheEntryConstPtr& dfce, const collision_detection::AllowedCollisionMatrix& acm) const; - void updatedPaddingOrScaling(const std::vector& links) override{}; + void updatedPaddingOrScaling(const std::vector& /*links*/) override{}; + + DistanceFieldCacheEntryWorldPtr generateDistanceFieldCacheEntryWorld(); + + void updateDistanceObject(const std::string& id, CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr& dfce, + EigenSTL::vector_Vector3d& add_points, EigenSTL::vector_Vector3d& subtract_points); + + bool getEnvironmentCollisions(const CollisionRequest& req, CollisionResult& res, + const distance_field::DistanceFieldConstPtr& env_distance_field, + GroupStateRepresentationPtr& gsr) const; + + bool getEnvironmentProximityGradients(const distance_field::DistanceFieldConstPtr& env_distance_field, + GroupStateRepresentationPtr& gsr) const; + + static void notifyObjectChange(CollisionEnvDistanceField* self, const ObjectConstPtr& obj, World::Action action); Eigen::Vector3d size_; Eigen::Vector3d origin_; @@ -261,7 +298,10 @@ class CollisionRobotDistanceField : public CollisionRobot std::map pregenerated_group_state_representation_map_; planning_scene::PlanningScenePtr planning_scene_; -}; -} -#endif + mutable boost::mutex update_cache_lock_world_; + DistanceFieldCacheEntryWorldPtr distance_field_cache_entry_world_; + GroupStateRepresentationPtr last_gsr_; + World::ObserverHandle observer_handle_; +}; +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h new file mode 100644 index 0000000000..14d2a22a18 --- /dev/null +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h @@ -0,0 +1,153 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: E. Gil Jones, Jens Petit */ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace collision_detection +{ +/** \brief This hybrid collision environment combines FCL and a distance field. Both can be used to calculate + * collisions. */ +class CollisionEnvHybrid : public collision_detection::CollisionEnvFCL +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + CollisionEnvHybrid(const moveit::core::RobotModelConstPtr& robot_model, + const std::map>& link_body_decompositions = + std::map>(), + double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, + const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), + bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, + double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, + double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, + double scale = 1.0); + + CollisionEnvHybrid(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, + const std::map>& link_body_decompositions = + std::map>(), + double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, + const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), + bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, + double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, + double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, + double scale = 1.0); + + CollisionEnvHybrid(const CollisionEnvHybrid& other, const WorldPtr& world); + + ~CollisionEnvHybrid() override + { + } + + void initializeRobotDistanceField(const std::map>& link_body_decompositions, + double size_x, double size_y, double size_z, bool use_signed_distance_field, + double resolution, double collision_tolerance, double max_propogation_distance) + { + cenv_distance_->initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), + Eigen::Vector3d(0, 0, 0), use_signed_distance_field, resolution, collision_tolerance, + max_propogation_distance); + } + + void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state) const; + + void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const; + + void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm) const; + + void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + const CollisionEnvDistanceFieldConstPtr getCollisionRobotDistanceField() const + { + return cenv_distance_; + } + + void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const; + + void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; + + void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; + + void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const; + + void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; + + void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + + void setWorld(const WorldPtr& world) override; + + void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; + + void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; + + const CollisionEnvDistanceFieldConstPtr getCollisionWorldDistanceField() const + { + return cenv_distance_; + } + +protected: + CollisionEnvDistanceFieldPtr cenv_distance_; +}; +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_hybrid.h deleted file mode 100644 index 528fe3b226..0000000000 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_hybrid.h +++ /dev/null @@ -1,101 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: E. Gil Jones */ - -#ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_ROBOT_HYBRID_H_ -#define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_ROBOT_HYBRID_H_ - -#include -#include -#include -#include - -#include - -namespace collision_detection -{ -class CollisionRobotHybrid : public collision_detection::CollisionRobotFCL -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - CollisionRobotHybrid(const robot_model::RobotModelConstPtr& robot_model); - - CollisionRobotHybrid(const robot_model::RobotModelConstPtr& robot_model, - const std::map>& link_body_decompositions, - double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, - double scale = 1.0); - - CollisionRobotHybrid(const CollisionRobotHybrid& other); - - void initializeRobotDistanceField(const std::map>& link_body_decompositions, - double size_x, double size_y, double size_z, bool use_signed_distance_field, - double resolution, double collision_tolerance, double max_propogation_distance) - { - crobot_distance_->initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), - Eigen::Vector3d(0, 0, 0), use_signed_distance_field, resolution, collision_tolerance, - max_propogation_distance); - } - - void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const robot_state::RobotState& state) const; - - void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const robot_state::RobotState& state, - GroupStateRepresentationPtr& gsr) const; - - void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const robot_state::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm) const; - - void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const robot_state::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - const CollisionRobotDistanceFieldConstPtr getCollisionRobotDistanceField() const - { - return crobot_distance_; - } - -protected: - CollisionRobotDistanceFieldPtr crobot_distance_; -}; -} - -#endif diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h deleted file mode 100644 index d2a5e950fb..0000000000 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h +++ /dev/null @@ -1,203 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: E. Gil Jones */ - -#ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_DISTANCE_FIELD_ -#define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_DISTANCE_FIELD_ - -#include -#include -#include -#include - -namespace collision_detection -{ -MOVEIT_CLASS_FORWARD(CollisionWorldDistanceField) - -class CollisionWorldDistanceField : public CollisionWorld -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - MOVEIT_STRUCT_FORWARD(DistanceFieldCacheEntry) - struct DistanceFieldCacheEntry - { - std::map> posed_body_point_decompositions_; - distance_field::DistanceFieldPtr distance_field_; - }; - - CollisionWorldDistanceField(Eigen::Vector3d size = Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z), - Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, - double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE); - - explicit CollisionWorldDistanceField( - const WorldPtr& world, Eigen::Vector3d size = Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z), - Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution = DEFAULT_RESOLUTION, - double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE); - - CollisionWorldDistanceField(const CollisionWorldDistanceField& other, const WorldPtr& world); - - ~CollisionWorldDistanceField() override; - - void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override; - - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; - - void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override; - - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override; - - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override; - - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2) const override - { - } - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state1, const robot_state::RobotState& state2, - const AllowedCollisionMatrix& acm) const override - { - } - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionWorld& other_world) const override - { - } - void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world, - const AllowedCollisionMatrix& acm) const override - { - } - - virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state, - bool verbose = false) const - { - return 0.0; - } - virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm, bool verbose = false) const - { - return 0.0; - } - virtual double distanceWorld(const CollisionWorld& world, bool verbose = false) const - { - return 0.0; - } - virtual double distanceWorld(const CollisionWorld& world, const AllowedCollisionMatrix& acm, - bool verbose = false) const - { - return 0.0; - } - - void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const override - { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - } - - void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override - { - ROS_ERROR_NAMED("collision_distance_field", "Not implemented"); - } - - void setWorld(const WorldPtr& world) override; - - distance_field::DistanceFieldConstPtr getDistanceField() const - { - return distance_field_cache_entry_->distance_field_; - } - - collision_detection::GroupStateRepresentationConstPtr getLastGroupStateRepresentation() const - { - return last_gsr_; - } - - void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const; - - void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const; - -protected: - DistanceFieldCacheEntryPtr generateDistanceFieldCacheEntry(); - - void updateDistanceObject(const std::string& id, CollisionWorldDistanceField::DistanceFieldCacheEntryPtr& dfce, - EigenSTL::vector_Vector3d& add_points, EigenSTL::vector_Vector3d& subtract_points); - - bool getEnvironmentCollisions(const CollisionRequest& req, CollisionResult& res, - const distance_field::DistanceFieldConstPtr& env_distance_field, - GroupStateRepresentationPtr& gsr) const; - - bool getEnvironmentProximityGradients(const distance_field::DistanceFieldConstPtr& env_distance_field, - GroupStateRepresentationPtr& gsr) const; - - static void notifyObjectChange(CollisionWorldDistanceField* self, const ObjectConstPtr& obj, World::Action action); - - Eigen::Vector3d size_; - Eigen::Vector3d origin_; - bool use_signed_distance_field_; - double resolution_; - double collision_tolerance_; - double max_propogation_distance_; - - mutable boost::mutex update_cache_lock_; - DistanceFieldCacheEntryPtr distance_field_cache_entry_; - GroupStateRepresentationPtr last_gsr_; - World::ObserverHandle observer_handle_; -}; -} - -#endif diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_hybrid.h deleted file mode 100644 index 9b70e3c6dc..0000000000 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_hybrid.h +++ /dev/null @@ -1,119 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: E. Gil Jones */ - -#ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_HYBRID_H_ -#define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_HYBRID_H_ - -#include -#include -#include -#include - -namespace collision_detection -{ -class CollisionRobotHybrid; - -class CollisionWorldHybrid : public CollisionWorldFCL -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - CollisionWorldHybrid(Eigen::Vector3d size = Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z), - Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE); - - explicit CollisionWorldHybrid(const WorldPtr& world, - Eigen::Vector3d size = Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z), - Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0), - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, - double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE); - - CollisionWorldHybrid(const CollisionWorldHybrid& other, const WorldPtr& world); - - ~CollisionWorldHybrid() override - { - } - - void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const; - - void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; - - void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; - - void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state) const; - - void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const; - - void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - - void setWorld(const WorldPtr& world) override; - - void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const; - - void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot, - const robot_state::RobotState& state, const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const; - - const CollisionWorldDistanceFieldConstPtr getCollisionWorldDistanceField() const - { - return cworld_distance_; - } - -protected: - CollisionWorldDistanceFieldPtr cworld_distance_; -}; -} - -#endif diff --git a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp index 83e8dc4065..0baa38a20b 100644 --- a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp @@ -100,7 +100,7 @@ PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const return ret; } -PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const robot_state::AttachedBody* att, +PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const moveit::core::AttachedBody* att, double resolution) { PosedBodySphereDecompositionVectorPtr ret(new PosedBodySphereDecompositionVector()); @@ -114,7 +114,7 @@ PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const r return ret; } -PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const robot_state::AttachedBody* att, +PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const moveit::core::AttachedBody* att, double resolution) { PosedBodyPointDecompositionVectorPtr ret(new PosedBodyPointDecompositionVector()); diff --git a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp index 72b49f7696..f9be5ccc4a 100644 --- a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp +++ b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp @@ -280,7 +280,7 @@ void collision_detection::BodyDecomposition::init(const std::vectorclone(), poses[i], padding); + bodies_.addBody(shapes[i].get(), poses[i], padding); } // collecting collision spheres diff --git a/moveit_core/collision_distance_field/src/collision_robot_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp similarity index 67% rename from moveit_core/collision_distance_field/src/collision_robot_distance_field.cpp rename to moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index 85cf2246ee..1340a8299e 100644 --- a/moveit_core/collision_distance_field/src/collision_robot_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -35,54 +35,61 @@ /* Author: E. Gil Jones */ #include -#include -#include -#include #include #include #include +#include +#include +#include +#include +#include +#include +#include namespace collision_detection { const double EPSILON = 0.001f; -CollisionRobotDistanceField::CollisionRobotDistanceField(const robot_model::RobotModelConstPtr& robot_model) - : CollisionRobot(robot_model) +const std::string collision_detection::CollisionDetectorAllocatorDistanceField::NAME("DISTANCE_FIELD"); + +CollisionEnvDistanceField::CollisionEnvDistanceField( + const moveit::core::RobotModelConstPtr& robot_model, + const std::map>& link_body_decompositions, double size_x, double size_y, + double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, + double collision_tolerance, double max_propogation_distance, double padding, double scale) + : CollisionEnv(robot_model) { - // planning_scene_.reset(new planning_scene::PlanningScene(robot_model_)); + initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field, + resolution, collision_tolerance, max_propogation_distance); - std::map> link_body_decompositions; - Eigen::Vector3d size(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z); - initialize(link_body_decompositions, size, Eigen::Vector3d(0, 0, 0), DEFAULT_USE_SIGNED_DISTANCE_FIELD, - DEFAULT_RESOLUTION, DEFAULT_COLLISION_TOLERANCE, DEFAULT_MAX_PROPOGATION_DISTANCE); setPadding(0.0); + + distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld(); + + // request notifications about changes to world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); } -CollisionRobotDistanceField::CollisionRobotDistanceField( - const robot_model::RobotModelConstPtr& robot_model, +CollisionEnvDistanceField::CollisionEnvDistanceField( + const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, const std::map>& link_body_decompositions, double size_x, double size_y, - double size_z, bool use_signed_distance_field, double resolution, double collision_tolerance, - double max_propogation_distance, double padding, double scale) - : CollisionRobot(robot_model, padding, scale) + double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, + double collision_tolerance, double max_propogation_distance, double padding, double scale) + : CollisionEnv(robot_model, world, padding, scale) { - initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), Eigen::Vector3d(0, 0, 0), - use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance); -} + initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field, + resolution, collision_tolerance, max_propogation_distance); -CollisionRobotDistanceField::CollisionRobotDistanceField(const CollisionRobot& col_robot, const Eigen::Vector3d& size, - const Eigen::Vector3d& origin, bool use_signed_distance_field, - double resolution, double collision_tolerance, - double max_propogation_distance, double padding) - : CollisionRobot(col_robot) -{ - std::map> link_body_decompositions; - initialize(link_body_decompositions, size, origin, use_signed_distance_field, resolution, collision_tolerance, - max_propogation_distance); - setPadding(padding); + distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld(); + + // request notifications about changes to world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); } -CollisionRobotDistanceField::CollisionRobotDistanceField(const CollisionRobotDistanceField& other) - : CollisionRobot(other) +CollisionEnvDistanceField::CollisionEnvDistanceField(const CollisionEnvDistanceField& other, const WorldPtr& world) + : CollisionEnv(other, world) { size_ = other.size_; origin_ = other.origin_; @@ -94,11 +101,21 @@ CollisionRobotDistanceField::CollisionRobotDistanceField(const CollisionRobotDis link_body_decomposition_vector_ = other.link_body_decomposition_vector_; link_body_decomposition_index_map_ = other.link_body_decomposition_index_map_; in_group_update_map_ = other.in_group_update_map_; + distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld(); pregenerated_group_state_representation_map_ = other.pregenerated_group_state_representation_map_; planning_scene_.reset(new planning_scene::PlanningScene(robot_model_)); + + // request notifications about changes to world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); } -void CollisionRobotDistanceField::initialize( +CollisionEnvDistanceField::~CollisionEnvDistanceField() +{ + getWorld()->removeObserver(observer_handle_); +} + +void CollisionEnvDistanceField::initialize( const std::map>& link_body_decompositions, const Eigen::Vector3d& size, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance) @@ -130,7 +147,7 @@ void CollisionRobotDistanceField::initialize( } } -void CollisionRobotDistanceField::generateCollisionCheckingStructures( +void CollisionEnvDistanceField::generateCollisionCheckingStructures( const std::string& group_name, const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr, bool generate_distance_field) const @@ -143,17 +160,17 @@ void CollisionRobotDistanceField::generateCollisionCheckingStructures( DistanceFieldCacheEntryPtr new_dfce = generateDistanceFieldCacheEntry(group_name, state, acm, generate_distance_field); boost::mutex::scoped_lock slock(update_cache_lock_); - (const_cast(this))->distance_field_cache_entry_ = new_dfce; + (const_cast(this))->distance_field_cache_entry_ = new_dfce; dfce = new_dfce; } getGroupStateRepresentation(dfce, state, gsr); } -void CollisionRobotDistanceField::checkSelfCollisionHelper(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const +void CollisionEnvDistanceField::checkSelfCollisionHelper(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix* acm, + GroupStateRepresentationPtr& gsr) const { if (!gsr) { @@ -174,9 +191,9 @@ void CollisionRobotDistanceField::checkSelfCollisionHelper(const collision_detec } DistanceFieldCacheEntryConstPtr -CollisionRobotDistanceField::getDistanceFieldCacheEntry(const std::string& group_name, - const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix* acm) const +CollisionEnvDistanceField::getDistanceFieldCacheEntry(const std::string& group_name, + const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix* acm) const { DistanceFieldCacheEntryConstPtr ret; if (!distance_field_cache_entry_) @@ -205,36 +222,36 @@ CollisionRobotDistanceField::getDistanceFieldCacheEntry(const std::string& group return cur; } -void CollisionRobotDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const moveit::core::RobotState& state) const +void CollisionEnvDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state) const { GroupStateRepresentationPtr gsr; checkSelfCollisionHelper(req, res, state, nullptr, gsr); } -void CollisionRobotDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, - GroupStateRepresentationPtr& gsr) const +void CollisionEnvDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const { checkSelfCollisionHelper(req, res, state, nullptr, gsr); } -void CollisionRobotDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm) const +void CollisionEnvDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm) const { GroupStateRepresentationPtr gsr; checkSelfCollisionHelper(req, res, state, &acm, gsr); } -void CollisionRobotDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const +void CollisionEnvDistanceField::checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const { if (gsr) { @@ -244,9 +261,9 @@ void CollisionRobotDistanceField::checkSelfCollision(const collision_detection:: checkSelfCollisionHelper(req, res, state, &acm, gsr); } -bool CollisionRobotDistanceField::getSelfCollisions(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - GroupStateRepresentationPtr& gsr) const +bool CollisionEnvDistanceField::getSelfCollisions(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + GroupStateRepresentationPtr& gsr) const { for (unsigned int i = 0; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); i++) { @@ -271,9 +288,10 @@ bool CollisionRobotDistanceField::getSelfCollisions(const collision_detection::C if (req.contacts) { std::vector colls; - bool coll = getCollisionSphereCollision( - gsr->dfce_->distance_field_.get(), *collision_spheres_1, *sphere_centers_1, max_propogation_distance_, - collision_tolerance_, std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls); + bool coll = + getCollisionSphereCollision(gsr->dfce_->distance_field_.get(), *collision_spheres_1, *sphere_centers_1, + max_propogation_distance_, collision_tolerance_, + std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls); if (coll) { res.collision = true; @@ -324,7 +342,7 @@ bool CollisionRobotDistanceField::getSelfCollisions(const collision_detection::C return (res.contact_count >= req.max_contacts); } -bool CollisionRobotDistanceField::getSelfProximityGradients(GroupStateRepresentationPtr& gsr) const +bool CollisionEnvDistanceField::getSelfProximityGradients(GroupStateRepresentationPtr& gsr) const { bool in_collision = false; @@ -399,9 +417,9 @@ bool CollisionRobotDistanceField::getSelfProximityGradients(GroupStateRepresenta return in_collision; } -bool CollisionRobotDistanceField::getIntraGroupCollisions(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - GroupStateRepresentationPtr& gsr) const +bool CollisionEnvDistanceField::getIntraGroupCollisions(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + GroupStateRepresentationPtr& gsr) const { unsigned int num_links = gsr->dfce_->link_names_.size(); unsigned int num_attached_bodies = gsr->dfce_->attached_body_names_.size(); @@ -628,7 +646,7 @@ bool CollisionRobotDistanceField::getIntraGroupCollisions(const collision_detect return false; } -bool CollisionRobotDistanceField::getIntraGroupProximityGradients(GroupStateRepresentationPtr& gsr) const +bool CollisionEnvDistanceField::getIntraGroupProximityGradients(GroupStateRepresentationPtr& gsr) const { bool in_collision = false; unsigned int num_links = gsr->dfce_->link_names_.size(); @@ -694,7 +712,7 @@ bool CollisionRobotDistanceField::getIntraGroupProximityGradients(GroupStateRepr } return in_collision; } -DistanceFieldCacheEntryPtr CollisionRobotDistanceField::generateDistanceFieldCacheEntry( +DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCacheEntry( const std::string& group_name, const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix* acm, bool generate_distance_field) const { @@ -800,8 +818,7 @@ DistanceFieldCacheEntryPtr CollisionRobotDistanceField::generateDistanceFieldCac // << " num " << attached_bodies[j]->getTouchLinks().size() // << std::endl; // touch links take priority - if (link_attached_bodies[j]->getTouchLinks().find(link_name) != - link_attached_bodies[j]->getTouchLinks().end()) + if (link_attached_bodies[j]->getTouchLinks().find(link_name) != link_attached_bodies[j]->getTouchLinks().end()) { dfce->intra_group_collision_enabled_[i][att_count + dfce->link_names_.size()] = false; // std::cerr << "Setting intra group for " << link_name << " and @@ -947,7 +964,7 @@ DistanceFieldCacheEntryPtr CollisionRobotDistanceField::generateDistanceFieldCac return dfce; } -void CollisionRobotDistanceField::addLinkBodyDecompositions(double resolution) +void CollisionEnvDistanceField::addLinkBodyDecompositions(double resolution) { const std::vector& link_models = robot_model_->getLinkModelsWithCollisionGeometry(); for (const moveit::core::LinkModel* link_model : link_models) @@ -967,8 +984,8 @@ void CollisionRobotDistanceField::addLinkBodyDecompositions(double resolution) } } -void CollisionRobotDistanceField::createCollisionModelMarker(const moveit::core::RobotState& state, - visualization_msgs::MarkerArray& model_markers) const +void CollisionEnvDistanceField::createCollisionModelMarker(const moveit::core::RobotState& state, + visualization_msgs::MarkerArray& model_markers) const { // creating colors std_msgs::ColorRGBA robot_color; @@ -1035,7 +1052,7 @@ void CollisionRobotDistanceField::createCollisionModelMarker(const moveit::core: } } -void CollisionRobotDistanceField::addLinkBodyDecompositions( +void CollisionEnvDistanceField::addLinkBodyDecompositions( double resolution, const std::map>& link_spheres) { ROS_ASSERT_MSG(robot_model_, "RobotModelPtr is invalid"); @@ -1065,8 +1082,8 @@ void CollisionRobotDistanceField::addLinkBodyDecompositions( ROS_DEBUG_STREAM(__FUNCTION__ << " Finished "); } -PosedBodySphereDecompositionPtr CollisionRobotDistanceField::getPosedLinkBodySphereDecomposition( - const moveit::core::LinkModel* ls, unsigned int ind) const +PosedBodySphereDecompositionPtr +CollisionEnvDistanceField::getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel* ls, unsigned int ind) const { PosedBodySphereDecompositionPtr ret; ret.reset(new PosedBodySphereDecomposition(link_body_decomposition_vector_[ind])); @@ -1074,7 +1091,7 @@ PosedBodySphereDecompositionPtr CollisionRobotDistanceField::getPosedLinkBodySph } PosedBodyPointDecompositionPtr -CollisionRobotDistanceField::getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel* ls) const +CollisionEnvDistanceField::getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel* ls) const { PosedBodyPointDecompositionPtr ret; std::map::const_iterator it = link_body_decomposition_index_map_.find(ls->getName()); @@ -1087,8 +1104,8 @@ CollisionRobotDistanceField::getPosedLinkBodyPointDecomposition(const moveit::co return ret; } -void CollisionRobotDistanceField::updateGroupStateRepresentationState(const moveit::core::RobotState& state, - GroupStateRepresentationPtr& gsr) const +void CollisionEnvDistanceField::updateGroupStateRepresentationState(const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const { for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++) { @@ -1141,9 +1158,9 @@ void CollisionRobotDistanceField::updateGroupStateRepresentationState(const move } } -void CollisionRobotDistanceField::getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr& dfce, - const moveit::core::RobotState& state, - GroupStateRepresentationPtr& gsr) const +void CollisionEnvDistanceField::getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr& dfce, + const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const { if (!dfce->pregenerated_group_state_representation_) { @@ -1239,8 +1256,8 @@ void CollisionRobotDistanceField::getGroupStateRepresentation(const DistanceFiel } } -bool CollisionRobotDistanceField::compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr& dfce, - const moveit::core::RobotState& state) const +bool CollisionEnvDistanceField::compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr& dfce, + const moveit::core::RobotState& state) const { std::vector new_state_values(state.getVariableCount()); for (unsigned int i = 0; i < new_state_values.size(); i++) @@ -1299,7 +1316,7 @@ bool CollisionRobotDistanceField::compareCacheEntryToState(const DistanceFieldCa return true; } -bool CollisionRobotDistanceField::compareCacheEntryToAllowedCollisionMatrix( +bool CollisionEnvDistanceField::compareCacheEntryToAllowedCollisionMatrix( const DistanceFieldCacheEntryConstPtr& dfce, const collision_detection::AllowedCollisionMatrix& acm) const { if (dfce->acm_.getSize() != acm.getSize()) @@ -1361,7 +1378,7 @@ bool CollisionRobotDistanceField::compareCacheEntryToAllowedCollisionMatrix( } // void -// CollisionRobotDistanceField::generateAllowedCollisionInformation(CollisionRobotDistanceField::DistanceFieldCacheEntryPtr& +// CollisionEnvDistanceField::generateAllowedCollisionInformation(CollisionEnvDistanceField::DistanceFieldCacheEntryPtr& // dfce) // { // for(unsigned int i = 0; i < dfce.link_names_.size(); i++) { @@ -1369,4 +1386,421 @@ bool CollisionRobotDistanceField::compareCacheEntryToAllowedCollisionMatrix( // if(dfce->acm.find // } // } + +void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const +{ + GroupStateRepresentationPtr gsr; + checkCollision(req, res, state, gsr); +} + +void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const +{ + if (!gsr) + { + generateCollisionCheckingStructures(req.group_name, state, nullptr, gsr, true); + } + else + { + updateGroupStateRepresentationState(state, gsr); + } + bool done = getSelfCollisions(req, res, gsr); + if (!done) + { + done = getIntraGroupCollisions(req, res, gsr); + } + if (!done) + { + getEnvironmentCollisions(req, res, distance_field_cache_entry_world_->distance_field_, gsr); + } + + (const_cast(this))->last_gsr_ = gsr; +} + +void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + GroupStateRepresentationPtr gsr; + checkCollision(req, res, state, acm, gsr); +} + +void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const +{ + if (!gsr) + { + generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true); + } + else + { + updateGroupStateRepresentationState(state, gsr); + } + bool done = getSelfCollisions(req, res, gsr); + if (!done) + { + done = getIntraGroupCollisions(req, res, gsr); + } + if (!done) + { + getEnvironmentCollisions(req, res, distance_field_cache_entry_world_->distance_field_, gsr); + } + + (const_cast(this))->last_gsr_ = gsr; +} + +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const +{ + GroupStateRepresentationPtr gsr; + checkRobotCollision(req, res, state, gsr); +} + +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const +{ + distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_; + if (!gsr) + { + generateCollisionCheckingStructures(req.group_name, state, nullptr, gsr, false); + } + else + { + updateGroupStateRepresentationState(state, gsr); + } + getEnvironmentCollisions(req, res, env_distance_field, gsr); + (const_cast(this))->last_gsr_ = gsr; + + // checkRobotCollisionHelper(req, res, robot, state, &acm); +} + +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + GroupStateRepresentationPtr gsr; + checkRobotCollision(req, res, state, acm, gsr); +} + +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const +{ + distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_; + + if (!gsr) + { + generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true); + } + else + { + updateGroupStateRepresentationState(state, gsr); + } + getEnvironmentCollisions(req, res, env_distance_field, gsr); + (const_cast(this))->last_gsr_ = gsr; + + // checkRobotCollisionHelper(req, res, robot, state, &acm); +} + +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, + const AllowedCollisionMatrix& acm) const +{ + ROS_ERROR_NAMED("collision_detection.distance", "Continuous collision checking not implemented"); +} + +void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const +{ + ROS_ERROR_NAMED("collision_detection.distance", "Continuous collision checking not implemented"); +} + +void CollisionEnvDistanceField::getCollisionGradients(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm, + GroupStateRepresentationPtr& gsr) const +{ + distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_; + + if (!gsr) + { + generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true); + } + else + { + updateGroupStateRepresentationState(state, gsr); + } + + getSelfProximityGradients(gsr); + getIntraGroupProximityGradients(gsr); + getEnvironmentProximityGradients(env_distance_field, gsr); + + (const_cast(this))->last_gsr_ = gsr; +} + +void CollisionEnvDistanceField::getAllCollisions(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm, + GroupStateRepresentationPtr& gsr) const +{ + if (!gsr) + { + generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true); + } + else + { + updateGroupStateRepresentationState(state, gsr); + } + getSelfCollisions(req, res, gsr); + getIntraGroupCollisions(req, res, gsr); + distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_; + getEnvironmentCollisions(req, res, env_distance_field, gsr); + + (const_cast(this))->last_gsr_ = gsr; +} + +bool CollisionEnvDistanceField::getEnvironmentCollisions(const CollisionRequest& req, CollisionResult& res, + const distance_field::DistanceFieldConstPtr& env_distance_field, + GroupStateRepresentationPtr& gsr) const +{ + for (unsigned int i = 0; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); i++) + { + bool is_link = i < gsr->dfce_->link_names_.size(); + std::string link_name = i < gsr->dfce_->link_names_.size() ? gsr->dfce_->link_names_[i] : "attached"; + if (is_link && !gsr->dfce_->link_has_geometry_[i]) + { + continue; + } + + const std::vector* collision_spheres_1; + const EigenSTL::vector_Vector3d* sphere_centers_1; + + if (is_link) + { + collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres()); + sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters()); + } + else + { + collision_spheres_1 = + &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres()); + sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()); + } + + if (req.contacts) + { + std::vector colls; + bool coll = + getCollisionSphereCollision(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, + max_propogation_distance_, collision_tolerance_, + std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls); + if (coll) + { + res.collision = true; + for (unsigned int col : colls) + { + Contact con; + if (is_link) + { + con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[col]; + con.body_type_1 = BodyTypes::ROBOT_LINK; + con.body_name_1 = gsr->dfce_->link_names_[i]; + } + else + { + con.pos = gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()[col]; + con.body_type_1 = BodyTypes::ROBOT_ATTACHED; + con.body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()]; + } + + con.body_type_2 = BodyTypes::WORLD_OBJECT; + con.body_name_2 = "environment"; + res.contact_count++; + res.contacts[std::pair(con.body_name_1, con.body_name_2)].push_back(con); + gsr->gradients_[i].types[col] = ENVIRONMENT; + // ROS_DEBUG_STREAM("Link " << dfce->link_names_[i] << " sphere " << + // colls[j] << " in env collision"); + } + + gsr->gradients_[i].collision = true; + if (res.contact_count >= req.max_contacts) + { + return true; + } + } + } + else + { + bool coll = getCollisionSphereCollision(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, + max_propogation_distance_, collision_tolerance_); + if (coll) + { + res.collision = true; + return true; + } + } + } + return (res.contact_count >= req.max_contacts); +} + +bool CollisionEnvDistanceField::getEnvironmentProximityGradients( + const distance_field::DistanceFieldConstPtr& env_distance_field, GroupStateRepresentationPtr& gsr) const +{ + bool in_collision = false; + for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++) + { + bool is_link = i < gsr->dfce_->link_names_.size(); + + if (is_link && !gsr->dfce_->link_has_geometry_[i]) + { + continue; + } + + const std::vector* collision_spheres_1; + const EigenSTL::vector_Vector3d* sphere_centers_1; + if (is_link) + { + collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres()); + sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters()); + } + else + { + collision_spheres_1 = + &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres()); + sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()); + } + + bool coll = getCollisionSphereGradients(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, + gsr->gradients_[i], ENVIRONMENT, collision_tolerance_, false, + max_propogation_distance_, false); + if (coll) + { + in_collision = true; + } + } + return in_collision; +} + +void CollisionEnvDistanceField::setWorld(const WorldPtr& world) +{ + if (world == getWorld()) + return; + + // turn off notifications about old world + getWorld()->removeObserver(observer_handle_); + + // clear out objects from old world + distance_field_cache_entry_world_->distance_field_->reset(); + + CollisionEnv::setWorld(world); + + // request notifications about changes to new world + observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2)); + + // get notifications any objects already in the new world + getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); +} + +void CollisionEnvDistanceField::notifyObjectChange(CollisionEnvDistanceField* self, const ObjectConstPtr& obj, + World::Action action) +{ + ros::WallTime n = ros::WallTime::now(); + + EigenSTL::vector_Vector3d add_points; + EigenSTL::vector_Vector3d subtract_points; + self->updateDistanceObject(obj->id_, self->distance_field_cache_entry_world_, add_points, subtract_points); + + if (action == World::DESTROY) + { + self->distance_field_cache_entry_world_->distance_field_->removePointsFromField(subtract_points); + } + else if (action & (World::MOVE_SHAPE | World::REMOVE_SHAPE)) + { + self->distance_field_cache_entry_world_->distance_field_->removePointsFromField(subtract_points); + self->distance_field_cache_entry_world_->distance_field_->addPointsToField(add_points); + } + else + { + self->distance_field_cache_entry_world_->distance_field_->addPointsToField(add_points); + } + + ROS_DEBUG_NAMED("collision_distance_field", "Modifying object %s took %lf s", obj->id_.c_str(), + (ros::WallTime::now() - n).toSec()); +} + +void CollisionEnvDistanceField::updateDistanceObject(const std::string& id, DistanceFieldCacheEntryWorldPtr& dfce, + EigenSTL::vector_Vector3d& add_points, + EigenSTL::vector_Vector3d& subtract_points) +{ + std::map>::iterator cur_it = + dfce->posed_body_point_decompositions_.find(id); + if (cur_it != dfce->posed_body_point_decompositions_.end()) + { + for (collision_detection::PosedBodyPointDecompositionPtr& posed_body_point_decomposition : cur_it->second) + { + subtract_points.insert(subtract_points.end(), posed_body_point_decomposition->getCollisionPoints().begin(), + posed_body_point_decomposition->getCollisionPoints().end()); + } + } + + World::ObjectConstPtr object = getWorld()->getObject(id); + if (object) + { + ROS_DEBUG_STREAM("Updating/Adding Object '" << object->id_ << "' with " << object->shapes_.size() + << " shapes to CollisionEnvDistanceField"); + std::vector shape_points; + for (unsigned int i = 0; i < object->shapes_.size(); i++) + { + shapes::ShapeConstPtr shape = object->shapes_[i]; + if (shape->type == shapes::OCTREE) + { + const shapes::OcTree* octree_shape = static_cast(shape.get()); + std::shared_ptr octree = octree_shape->octree; + + shape_points.push_back(std::make_shared(octree)); + } + else + { + BodyDecompositionConstPtr bd = getBodyDecompositionCacheEntry(shape, resolution_); + + shape_points.push_back(std::make_shared(bd, object->shape_poses_[i])); + } + + add_points.insert(add_points.end(), shape_points.back()->getCollisionPoints().begin(), + shape_points.back()->getCollisionPoints().end()); + } + + dfce->posed_body_point_decompositions_[id] = shape_points; + } + else + { + ROS_DEBUG_STREAM("Removing Object '" << id << "' from CollisionEnvDistanceField"); + dfce->posed_body_point_decompositions_.erase(id); + } +} + +CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr +CollisionEnvDistanceField::generateDistanceFieldCacheEntryWorld() +{ + DistanceFieldCacheEntryWorldPtr dfce(new DistanceFieldCacheEntryWorld()); + dfce->distance_field_.reset(new distance_field::PropagationDistanceField( + size_.x(), size_.y(), size_.z(), resolution_, origin_.x() - 0.5 * size_.x(), origin_.y() - 0.5 * size_.y(), + origin_.z() - 0.5 * size_.z(), max_propogation_distance_, use_signed_distance_field_)); + + EigenSTL::vector_Vector3d add_points; + EigenSTL::vector_Vector3d subtract_points; + for (const std::pair& object : *getWorld()) + { + updateDistanceObject(object.first, dfce, add_points, subtract_points); + } + dfce->distance_field_->addPointsToField(add_points); + return dfce; +} } // namespace collision_detection diff --git a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp new file mode 100644 index 0000000000..f1abf3fb4c --- /dev/null +++ b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp @@ -0,0 +1,184 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: E. Gil Jones, Jens Petit */ + +#include +#include + +namespace collision_detection +{ +const std::string collision_detection::CollisionDetectorAllocatorHybrid::NAME("HYBRID"); + +CollisionEnvHybrid::CollisionEnvHybrid( + const moveit::core::RobotModelConstPtr& robot_model, + const std::map>& link_body_decompositions, double size_x, double size_y, + double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, + double collision_tolerance, double max_propogation_distance, double padding, double scale) + : CollisionEnvFCL(robot_model) + , cenv_distance_(new collision_detection::CollisionEnvDistanceField( + robot_model, getWorld(), link_body_decompositions, size_x, size_y, size_z, origin, use_signed_distance_field, + resolution, collision_tolerance, max_propogation_distance, padding, scale)) +{ +} + +CollisionEnvHybrid::CollisionEnvHybrid( + const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, + const std::map>& link_body_decompositions, double size_x, double size_y, + double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, + double collision_tolerance, double max_propogation_distance, double padding, double scale) + : CollisionEnvFCL(robot_model, world, padding, scale) + , cenv_distance_(new collision_detection::CollisionEnvDistanceField( + robot_model, getWorld(), link_body_decompositions, size_x, size_y, size_z, origin, use_signed_distance_field, + resolution, collision_tolerance, max_propogation_distance, padding, scale)) +{ +} + +CollisionEnvHybrid::CollisionEnvHybrid(const CollisionEnvHybrid& other, const WorldPtr& world) + : CollisionEnvFCL(other, world) + , cenv_distance_(new collision_detection::CollisionEnvDistanceField(*other.getCollisionWorldDistanceField(), world)) +{ +} + +void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state) const +{ + cenv_distance_->checkSelfCollision(req, res, state); +} + +void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->checkSelfCollision(req, res, state, gsr); +} + +void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm) const +{ + cenv_distance_->checkSelfCollision(req, res, state, acm); +} + +void CollisionEnvHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->checkSelfCollision(req, res, state, acm, gsr); +} + +void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const +{ + cenv_distance_->checkCollision(req, res, state); +} + +void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->checkCollision(req, res, state, gsr); +} + +void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + cenv_distance_->checkCollision(req, res, state, acm); +} + +void CollisionEnvHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->checkCollision(req, res, state, acm, gsr); +} + +void CollisionEnvHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const +{ + cenv_distance_->checkRobotCollision(req, res, state); +} + +void CollisionEnvHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->checkRobotCollision(req, res, state, gsr); +} + +void CollisionEnvHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const +{ + cenv_distance_->checkRobotCollision(req, res, state, acm); +} + +void CollisionEnvHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->checkRobotCollision(req, res, state, acm, gsr); +} + +void CollisionEnvHybrid::setWorld(const WorldPtr& world) +{ + if (world == getWorld()) + return; + + cenv_distance_->setWorld(world); + CollisionEnvFCL::setWorld(world); +} + +void CollisionEnvHybrid::getCollisionGradients(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->getCollisionGradients(req, res, state, acm, gsr); +} + +void CollisionEnvHybrid::getAllCollisions(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm, + GroupStateRepresentationPtr& gsr) const +{ + cenv_distance_->getAllCollisions(req, res, state, acm, gsr); +} +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/src/collision_robot_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_robot_hybrid.cpp deleted file mode 100644 index 45f35aea88..0000000000 --- a/moveit_core/collision_distance_field/src/collision_robot_hybrid.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: E. Gil Jones */ - -#include - -namespace collision_detection -{ -CollisionRobotHybrid::CollisionRobotHybrid(const robot_model::RobotModelConstPtr& robot_model) - : CollisionRobotFCL(robot_model) -{ - crobot_distance_.reset(new collision_detection::CollisionRobotDistanceField(robot_model)); -} - -CollisionRobotHybrid::CollisionRobotHybrid( - const robot_model::RobotModelConstPtr& robot_model, - const std::map>& link_body_decompositions, double size_x, double size_y, - double size_z, bool use_signed_distance_field, double resolution, double collision_tolerance, - double max_propogation_distance, double padding, double scale) - : CollisionRobotFCL(robot_model) -{ - crobot_distance_.reset(new collision_detection::CollisionRobotDistanceField( - robot_model, link_body_decompositions, size_x, size_y, size_z, use_signed_distance_field, resolution, - collision_tolerance, max_propogation_distance, padding, scale)); -} - -CollisionRobotHybrid::CollisionRobotHybrid(const CollisionRobotHybrid& other) : CollisionRobotFCL(other) -{ - crobot_distance_.reset( - new collision_detection::CollisionRobotDistanceField(*other.getCollisionRobotDistanceField().get())); -} - -void CollisionRobotHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const robot_state::RobotState& state) const -{ - crobot_distance_->checkSelfCollision(req, res, state); -} - -void CollisionRobotHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const robot_state::RobotState& state, - GroupStateRepresentationPtr& gsr) const -{ - crobot_distance_->checkSelfCollision(req, res, state, gsr); -} - -void CollisionRobotHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const robot_state::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm) const -{ - crobot_distance_->checkSelfCollision(req, res, state, acm); -} - -void CollisionRobotHybrid::checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const robot_state::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const -{ - crobot_distance_->checkSelfCollision(req, res, state, acm, gsr); -} -} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/src/collision_world_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_world_distance_field.cpp deleted file mode 100644 index f9d0fb515b..0000000000 --- a/moveit_core/collision_distance_field/src/collision_world_distance_field.cpp +++ /dev/null @@ -1,568 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: E. Gil Jones */ - -#include -#include -#include -#include -#include -#include -#include - -namespace collision_detection -{ -CollisionWorldDistanceField::~CollisionWorldDistanceField() -{ - getWorld()->removeObserver(observer_handle_); -} - -CollisionWorldDistanceField::CollisionWorldDistanceField(Eigen::Vector3d size, Eigen::Vector3d origin, - bool use_signed_distance_field, double resolution, - double collision_tolerance, double max_propogation_distance) - : CollisionWorld() - , size_(std::move(size)) - , origin_(std::move(origin)) - , use_signed_distance_field_(use_signed_distance_field) - , resolution_(resolution) - , collision_tolerance_(collision_tolerance) - , max_propogation_distance_(max_propogation_distance) -{ - distance_field_cache_entry_ = generateDistanceFieldCacheEntry(); - - // request notifications about changes to world - observer_handle_ = - getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2)); -} - -CollisionWorldDistanceField::CollisionWorldDistanceField(const WorldPtr& world, Eigen::Vector3d size, - Eigen::Vector3d origin, bool use_signed_distance_field, - double resolution, double collision_tolerance, - double max_propogation_distance) - : CollisionWorld(world) - , size_(std::move(size)) - , origin_(std::move(origin)) - , use_signed_distance_field_(use_signed_distance_field) - , resolution_(resolution) - , collision_tolerance_(collision_tolerance) - , max_propogation_distance_(max_propogation_distance) -{ - distance_field_cache_entry_ = generateDistanceFieldCacheEntry(); - - // request notifications about changes to world - observer_handle_ = - getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2)); - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -CollisionWorldDistanceField::CollisionWorldDistanceField(const CollisionWorldDistanceField& other, - const WorldPtr& world) - : CollisionWorld(other, world) -{ - size_ = other.size_; - origin_ = other.origin_; - use_signed_distance_field_ = other.use_signed_distance_field_; - resolution_ = other.resolution_; - collision_tolerance_ = other.collision_tolerance_; - max_propogation_distance_ = other.max_propogation_distance_; - distance_field_cache_entry_ = generateDistanceFieldCacheEntry(); - - // request notifications about changes to world - observer_handle_ = - getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2)); - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -void CollisionWorldDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - GroupStateRepresentationPtr gsr; - checkCollision(req, res, robot, state, gsr); -} - -void CollisionWorldDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - GroupStateRepresentationPtr& gsr) const -{ - try - { - const CollisionRobotDistanceField& cdr = dynamic_cast(robot); - if (!gsr) - { - cdr.generateCollisionCheckingStructures(req.group_name, state, nullptr, gsr, true); - } - else - { - cdr.updateGroupStateRepresentationState(state, gsr); - } - bool done = cdr.getSelfCollisions(req, res, gsr); - if (!done) - { - done = cdr.getIntraGroupCollisions(req, res, gsr); - } - if (!done) - { - getEnvironmentCollisions(req, res, distance_field_cache_entry_->distance_field_, gsr); - } - } - catch (const std::bad_cast& e) - { - ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what()); - return; - } - - (const_cast(this))->last_gsr_ = gsr; -} - -void CollisionWorldDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - GroupStateRepresentationPtr gsr; - checkCollision(req, res, robot, state, acm, gsr); -} - -void CollisionWorldDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const -{ - try - { - const CollisionRobotDistanceField& cdr = dynamic_cast(robot); - if (!gsr) - { - cdr.generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true); - } - else - { - cdr.updateGroupStateRepresentationState(state, gsr); - } - bool done = cdr.getSelfCollisions(req, res, gsr); - if (!done) - { - done = cdr.getIntraGroupCollisions(req, res, gsr); - } - if (!done) - { - getEnvironmentCollisions(req, res, distance_field_cache_entry_->distance_field_, gsr); - } - } - catch (const std::bad_cast& e) - { - ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what()); - return; - } - - (const_cast(this))->last_gsr_ = gsr; -} - -void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - GroupStateRepresentationPtr gsr; - checkRobotCollision(req, res, robot, state, gsr); -} - -void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - GroupStateRepresentationPtr& gsr) const -{ - distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_->distance_field_; - try - { - const CollisionRobotDistanceField& cdr = dynamic_cast(robot); - DistanceFieldCacheEntryConstPtr dfce; - if (!gsr) - { - cdr.generateCollisionCheckingStructures(req.group_name, state, nullptr, gsr, false); - } - else - { - cdr.updateGroupStateRepresentationState(state, gsr); - } - getEnvironmentCollisions(req, res, env_distance_field, gsr); - (const_cast(this))->last_gsr_ = gsr; - - // checkRobotCollisionHelper(req, res, robot, state, &acm); - } - catch (const std::bad_cast& e) - { - ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what()); - return; - } -} - -void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - GroupStateRepresentationPtr gsr; - checkRobotCollision(req, res, robot, state, acm, gsr); -} - -void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const -{ - distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_->distance_field_; - try - { - const CollisionRobotDistanceField& cdr = dynamic_cast(robot); - DistanceFieldCacheEntryPtr dfce; - if (!gsr) - { - cdr.generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true); - } - else - { - cdr.updateGroupStateRepresentationState(state, gsr); - } - getEnvironmentCollisions(req, res, env_distance_field, gsr); - (const_cast(this))->last_gsr_ = gsr; - - // checkRobotCollisionHelper(req, res, robot, state, &acm); - } - catch (const std::bad_cast& e) - { - ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what()); - return; - } -} - -void CollisionWorldDistanceField::getCollisionGradients(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const -{ - distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_->distance_field_; - try - { - const CollisionRobotDistanceField& cdr = dynamic_cast(robot); - if (!gsr) - { - cdr.generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true); - } - else - { - cdr.updateGroupStateRepresentationState(state, gsr); - } - cdr.getSelfProximityGradients(gsr); - cdr.getIntraGroupProximityGradients(gsr); - getEnvironmentProximityGradients(env_distance_field, gsr); - } - catch (const std::bad_cast& e) - { - ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what()); - return; - } - - (const_cast(this))->last_gsr_ = gsr; -} - -void CollisionWorldDistanceField::getAllCollisions(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const -{ - try - { - const CollisionRobotDistanceField& cdr = dynamic_cast(robot); - if (!gsr) - { - cdr.generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true); - } - else - { - cdr.updateGroupStateRepresentationState(state, gsr); - } - cdr.getSelfCollisions(req, res, gsr); - cdr.getIntraGroupCollisions(req, res, gsr); - distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_->distance_field_; - getEnvironmentCollisions(req, res, env_distance_field, gsr); - } - catch (const std::bad_cast& e) - { - ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what()); - return; - } - - (const_cast(this))->last_gsr_ = gsr; -} - -bool CollisionWorldDistanceField::getEnvironmentCollisions( - const CollisionRequest& req, CollisionResult& res, const distance_field::DistanceFieldConstPtr& env_distance_field, - GroupStateRepresentationPtr& gsr) const -{ - for (unsigned int i = 0; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); i++) - { - bool is_link = i < gsr->dfce_->link_names_.size(); - std::string link_name = i < gsr->dfce_->link_names_.size() ? gsr->dfce_->link_names_[i] : "attached"; - if (is_link && !gsr->dfce_->link_has_geometry_[i]) - { - continue; - } - - const std::vector* collision_spheres_1; - const EigenSTL::vector_Vector3d* sphere_centers_1; - - if (is_link) - { - collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres()); - sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters()); - } - else - { - collision_spheres_1 = - &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres()); - sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()); - } - - if (req.contacts) - { - std::vector colls; - bool coll = getCollisionSphereCollision( - env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, max_propogation_distance_, - collision_tolerance_, std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls); - if (coll) - { - res.collision = true; - for (unsigned int col : colls) - { - Contact con; - if (is_link) - { - con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[col]; - con.body_type_1 = BodyTypes::ROBOT_LINK; - con.body_name_1 = gsr->dfce_->link_names_[i]; - } - else - { - con.pos = gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()[col]; - con.body_type_1 = BodyTypes::ROBOT_ATTACHED; - con.body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()]; - } - - con.body_type_2 = BodyTypes::WORLD_OBJECT; - con.body_name_2 = "environment"; - res.contact_count++; - res.contacts[std::pair(con.body_name_1, con.body_name_2)].push_back(con); - gsr->gradients_[i].types[col] = ENVIRONMENT; - // ROS_DEBUG_STREAM("Link " << dfce->link_names_[i] << " sphere " << - // colls[j] << " in env collision"); - } - - gsr->gradients_[i].collision = true; - if (res.contact_count >= req.max_contacts) - { - return true; - } - } - } - else - { - bool coll = getCollisionSphereCollision(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, - max_propogation_distance_, collision_tolerance_); - if (coll) - { - res.collision = true; - return true; - } - } - } - return (res.contact_count >= req.max_contacts); -} - -bool CollisionWorldDistanceField::getEnvironmentProximityGradients( - const distance_field::DistanceFieldConstPtr& env_distance_field, GroupStateRepresentationPtr& gsr) const -{ - bool in_collision = false; - for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++) - { - bool is_link = i < gsr->dfce_->link_names_.size(); - - if (is_link && !gsr->dfce_->link_has_geometry_[i]) - { - continue; - } - - const std::vector* collision_spheres_1; - const EigenSTL::vector_Vector3d* sphere_centers_1; - if (is_link) - { - collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres()); - sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters()); - } - else - { - collision_spheres_1 = - &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres()); - sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()); - } - - bool coll = getCollisionSphereGradients(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, - gsr->gradients_[i], ENVIRONMENT, collision_tolerance_, false, - max_propogation_distance_, false); - if (coll) - { - in_collision = true; - } - } - return in_collision; -} - -void CollisionWorldDistanceField::setWorld(const WorldPtr& world) -{ - if (world == getWorld()) - return; - - // turn off notifications about old world - getWorld()->removeObserver(observer_handle_); - - // clear out objects from old world - distance_field_cache_entry_->distance_field_->reset(); - - CollisionWorld::setWorld(world); - - // request notifications about changes to new world - observer_handle_ = - getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2)); - - // get notifications any objects already in the new world - getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); -} - -void CollisionWorldDistanceField::notifyObjectChange(CollisionWorldDistanceField* self, const ObjectConstPtr& obj, - World::Action action) -{ - ros::WallTime n = ros::WallTime::now(); - - EigenSTL::vector_Vector3d add_points; - EigenSTL::vector_Vector3d subtract_points; - self->updateDistanceObject(obj->id_, self->distance_field_cache_entry_, add_points, subtract_points); - - if (action == World::DESTROY) - { - self->distance_field_cache_entry_->distance_field_->removePointsFromField(subtract_points); - } - else if (action & (World::MOVE_SHAPE | World::REMOVE_SHAPE)) - { - self->distance_field_cache_entry_->distance_field_->removePointsFromField(subtract_points); - self->distance_field_cache_entry_->distance_field_->addPointsToField(add_points); - } - else - { - self->distance_field_cache_entry_->distance_field_->addPointsToField(add_points); - } - - ROS_DEBUG_NAMED("collision_distance_field", "Modifying object %s took %lf s", obj->id_.c_str(), - (ros::WallTime::now() - n).toSec()); -} - -void CollisionWorldDistanceField::updateDistanceObject(const std::string& id, DistanceFieldCacheEntryPtr& dfce, - EigenSTL::vector_Vector3d& add_points, - EigenSTL::vector_Vector3d& subtract_points) -{ - std::map>::iterator cur_it = - dfce->posed_body_point_decompositions_.find(id); - if (cur_it != dfce->posed_body_point_decompositions_.end()) - { - for (collision_detection::PosedBodyPointDecompositionPtr& posed_body_point_decomposition : cur_it->second) - { - subtract_points.insert(subtract_points.end(), posed_body_point_decomposition->getCollisionPoints().begin(), - posed_body_point_decomposition->getCollisionPoints().end()); - } - } - - World::ObjectConstPtr object = getWorld()->getObject(id); - if (object) - { - ROS_DEBUG_STREAM("Updating/Adding Object '" << object->id_ << "' with " << object->shapes_.size() - << " shapes to CollisionWorldDistanceField"); - std::vector shape_points; - for (unsigned int i = 0; i < object->shapes_.size(); i++) - { - shapes::ShapeConstPtr shape = object->shapes_[i]; - if (shape->type == shapes::OCTREE) - { - const shapes::OcTree* octree_shape = static_cast(shape.get()); - std::shared_ptr octree = octree_shape->octree; - - shape_points.push_back(std::make_shared(octree)); - } - else - { - BodyDecompositionConstPtr bd = getBodyDecompositionCacheEntry(shape, resolution_); - - shape_points.push_back(std::make_shared(bd, object->shape_poses_[i])); - } - - add_points.insert(add_points.end(), shape_points.back()->getCollisionPoints().begin(), - shape_points.back()->getCollisionPoints().end()); - } - - dfce->posed_body_point_decompositions_[id] = shape_points; - } - else - { - ROS_DEBUG_STREAM("Removing Object '" << id << "' from CollisionWorldDistanceField"); - dfce->posed_body_point_decompositions_.erase(id); - } -} - -CollisionWorldDistanceField::DistanceFieldCacheEntryPtr CollisionWorldDistanceField::generateDistanceFieldCacheEntry() -{ - DistanceFieldCacheEntryPtr dfce(new DistanceFieldCacheEntry()); - dfce->distance_field_.reset(new distance_field::PropagationDistanceField( - size_.x(), size_.y(), size_.z(), resolution_, origin_.x() - 0.5 * size_.x(), origin_.y() - 0.5 * size_.y(), - origin_.z() - 0.5 * size_.z(), max_propogation_distance_, use_signed_distance_field_)); - - EigenSTL::vector_Vector3d add_points; - EigenSTL::vector_Vector3d subtract_points; - for (const std::pair& object : *getWorld()) - { - updateDistanceObject(object.first, dfce, add_points, subtract_points); - } - dfce->distance_field_->addPointsToField(add_points); - return dfce; -} -} // namespace collision_detection - -#include -const std::string collision_detection::CollisionDetectorAllocatorDistanceField::NAME("DISTANCE_FIELD"); diff --git a/moveit_core/collision_distance_field/src/collision_world_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_world_hybrid.cpp deleted file mode 100644 index 551937cd6a..0000000000 --- a/moveit_core/collision_distance_field/src/collision_world_hybrid.cpp +++ /dev/null @@ -1,160 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: E. Gil Jones */ - -#include -#include - -namespace collision_detection -{ -CollisionWorldHybrid::CollisionWorldHybrid(Eigen::Vector3d size, Eigen::Vector3d origin, bool use_signed_distance_field, - double resolution, double collision_tolerance, - double max_propogation_distance) - : CollisionWorldFCL() - , cworld_distance_(new collision_detection::CollisionWorldDistanceField( - getWorld(), std::move(size), std::move(origin), use_signed_distance_field, resolution, collision_tolerance, - max_propogation_distance)) - -{ -} - -CollisionWorldHybrid::CollisionWorldHybrid(const WorldPtr& world, Eigen::Vector3d size, Eigen::Vector3d origin, - bool use_signed_distance_field, double resolution, - double collision_tolerance, double max_propogation_distance) - : CollisionWorldFCL(world) - , cworld_distance_(new collision_detection::CollisionWorldDistanceField( - getWorld(), std::move(size), std::move(origin), use_signed_distance_field, resolution, collision_tolerance, - max_propogation_distance)) -{ -} - -CollisionWorldHybrid::CollisionWorldHybrid(const CollisionWorldHybrid& other, const WorldPtr& world) - : CollisionWorldFCL(other, world) - , cworld_distance_( - new collision_detection::CollisionWorldDistanceField(*other.getCollisionWorldDistanceField(), world)) -{ -} - -void CollisionWorldHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - cworld_distance_->checkCollision(req, res, robot, state); -} - -void CollisionWorldHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - GroupStateRepresentationPtr& gsr) const -{ - cworld_distance_->checkCollision(req, res, robot, state, gsr); -} - -void CollisionWorldHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - cworld_distance_->checkCollision(req, res, robot, state, acm); -} - -void CollisionWorldHybrid::checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const -{ - cworld_distance_->checkCollision(req, res, robot, state, acm, gsr); -} - -void CollisionWorldHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state) const -{ - cworld_distance_->checkRobotCollision(req, res, robot, state); -} - -void CollisionWorldHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - GroupStateRepresentationPtr& gsr) const -{ - cworld_distance_->checkRobotCollision(req, res, robot, state, gsr); -} - -void CollisionWorldHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm) const -{ - cworld_distance_->checkRobotCollision(req, res, robot, state, acm); -} - -void CollisionWorldHybrid::checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, - const robot_state::RobotState& state, - const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const -{ - cworld_distance_->checkRobotCollision(req, res, robot, state, acm, gsr); -} - -void CollisionWorldHybrid::setWorld(const WorldPtr& world) -{ - if (world == getWorld()) - return; - - cworld_distance_->setWorld(world); - CollisionWorldFCL::setWorld(world); -} - -void CollisionWorldHybrid::getCollisionGradients(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const -{ - cworld_distance_->getCollisionGradients(req, res, robot, state, acm, gsr); -} - -void CollisionWorldHybrid::getAllCollisions(const CollisionRequest& req, CollisionResult& res, - const CollisionRobot& robot, const robot_state::RobotState& state, - const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const -{ - cworld_distance_->getAllCollisions(req, res, robot, state, acm, gsr); -} -} // namespace collision_detection - -#include -const std::string collision_detection::CollisionDetectorAllocatorHybrid::NAME("HYBRID"); diff --git a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp index 19445964db..affec2291e 100644 --- a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp +++ b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /** \author E. Gil Jones */ @@ -38,8 +38,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -53,42 +53,19 @@ #include -typedef collision_detection::CollisionWorldDistanceField DefaultCWorldType; -typedef collision_detection::CollisionRobotDistanceField DefaultCRobotType; +typedef collision_detection::CollisionEnvDistanceField DefaultCEnvType; class DistanceFieldCollisionDetectionTester : public testing::Test { protected: void SetUp() override { - srdf_model_.reset(new srdf::Model()); - std::string xml_string; - std::fstream xml_file(ros::package::getPath("moveit_resources") + "/pr2_description/urdf/robot.xml", - std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - urdf_model_ = urdf::parseURDF(xml_string); - urdf_ok_ = static_cast(urdf_model_); - } - else - urdf_ok_ = false; - srdf_ok_ = srdf_model_->initFile(*urdf_model_, - ros::package::getPath("moveit_resources") + "/pr2_description/srdf/robot.xml"); - - robot_model_.reset(new robot_model::RobotModel(urdf_model_, srdf_model_)); + robot_model_ = moveit::core::loadTestingRobotModel("pr2"); acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); std::map> link_body_decompositions; - crobot_.reset(new DefaultCRobotType(robot_model_, link_body_decompositions)); - cworld_.reset(new DefaultCWorldType()); + cenv_.reset(new DefaultCEnvType(robot_model_, link_body_decompositions)); } void TearDown() override @@ -96,41 +73,32 @@ class DistanceFieldCollisionDetectionTester : public testing::Test } protected: - bool urdf_ok_; - bool srdf_ok_; + moveit::core::RobotModelPtr robot_model_; - urdf::ModelInterfaceSharedPtr urdf_model_; - srdf::ModelSharedPtr srdf_model_; + moveit::core::TransformsPtr ftf_; + moveit::core::TransformsConstPtr ftf_const_; - robot_model::RobotModelPtr robot_model_; - - robot_state::TransformsPtr ftf_; - robot_state::TransformsConstPtr ftf_const_; - - collision_detection::CollisionRobotPtr crobot_; - collision_detection::CollisionWorldPtr cworld_; + collision_detection::CollisionEnvPtr cenv_; collision_detection::AllowedCollisionMatrixPtr acm_; }; TEST_F(DistanceFieldCollisionDetectionTester, DefaultNotInCollision) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); - ASSERT_TRUE(urdf_ok_ && srdf_ok_); - collision_detection::CollisionRequest req; collision_detection::CollisionResult res; req.group_name = "whole_body"; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_FALSE(res.collision); } TEST_F(DistanceFieldCollisionDetectionTester, ChangeTorsoPosition) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -139,13 +107,13 @@ TEST_F(DistanceFieldCollisionDetectionTester, ChangeTorsoPosition) collision_detection::CollisionResult res2; req.group_name = "right_arm"; - crobot_->checkSelfCollision(req, res1, robot_state, *acm_); + cenv_->checkSelfCollision(req, res1, robot_state, *acm_); std::map torso_val; torso_val["torso_lift_joint"] = .15; robot_state.setVariablePositions(torso_val); robot_state.update(); - crobot_->checkSelfCollision(req, res1, robot_state, *acm_); - crobot_->checkSelfCollision(req, res1, robot_state, *acm_); + cenv_->checkSelfCollision(req, res1, robot_state, *acm_); + cenv_->checkSelfCollision(req, res1, robot_state, *acm_); } TEST_F(DistanceFieldCollisionDetectionTester, LinksInCollision) @@ -158,7 +126,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, LinksInCollision) // req.max_contacts = 100; req.group_name = "whole_body"; - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); Eigen::Isometry3d offset = Eigen::Isometry3d::Identity(); @@ -168,18 +136,18 @@ TEST_F(DistanceFieldCollisionDetectionTester, LinksInCollision) robot_state.updateStateWithLinkAt("base_bellow_link", offset); acm_->setEntry("base_link", "base_bellow_link", false); - crobot_->checkSelfCollision(req, res1, robot_state, *acm_); + cenv_->checkSelfCollision(req, res1, robot_state, *acm_); ASSERT_TRUE(res1.collision); acm_->setEntry("base_link", "base_bellow_link", true); - crobot_->checkSelfCollision(req, res2, robot_state, *acm_); + cenv_->checkSelfCollision(req, res2, robot_state, *acm_); ASSERT_FALSE(res2.collision); robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity()); robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset); acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); - crobot_->checkSelfCollision(req, res3, robot_state, *acm_); + cenv_->checkSelfCollision(req, res3, robot_state, *acm_); ASSERT_TRUE(res3.collision); } @@ -190,7 +158,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactReporting) req.max_contacts = 1; req.group_name = "whole_body"; - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); Eigen::Isometry3d offset = Eigen::Isometry3d::Identity(); @@ -206,7 +174,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactReporting) acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); collision_detection::CollisionResult res; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); EXPECT_EQ(res.contacts.size(), 1u); EXPECT_EQ(res.contacts.begin()->second.size(), 1u); @@ -215,7 +183,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactReporting) req.max_contacts = 2; req.max_contacts_per_pair = 1; // req.verbose = true; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); EXPECT_EQ(res.contact_count, 2u); EXPECT_EQ(res.contacts.begin()->second.size(), 1u); @@ -226,7 +194,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactReporting) req.max_contacts = 10; req.max_contacts_per_pair = 2; acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false)); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); EXPECT_LE(res.contacts.size(), 10u); EXPECT_LE(res.contact_count, 10u); @@ -239,7 +207,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactPositions) req.max_contacts = 1; req.group_name = "whole_body"; - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); @@ -254,7 +222,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactPositions) acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); collision_detection::CollisionResult res; - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); ASSERT_EQ(res.contacts.size(), 1u); ASSERT_EQ(res.contacts.begin()->second.size(), 1u); @@ -272,7 +240,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactPositions) robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); collision_detection::CollisionResult res2; - crobot_->checkSelfCollision(req, res2, robot_state, *acm_); + cenv_->checkSelfCollision(req, res2, robot_state, *acm_); ASSERT_TRUE(res2.collision); ASSERT_EQ(res2.contacts.size(), 1u); ASSERT_EQ(res2.contacts.begin()->second.size(), 1u); @@ -290,7 +258,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, ContactPositions) robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); collision_detection::CollisionResult res3; - crobot_->checkSelfCollision(req, res2, robot_state, *acm_); + cenv_->checkSelfCollision(req, res2, robot_state, *acm_); ASSERT_FALSE(res3.collision); } @@ -303,7 +271,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true)); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); @@ -311,18 +279,18 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) pos1.translation().x() = 1.0; robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_FALSE(res.collision); shapes::Shape* shape = new shapes::Box(.25, .25, .25); - cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1); + cenv_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1); res = collision_detection::CollisionResult(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_); + cenv_->checkRobotCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); // deletes shape - cworld_->getWorld()->removeObject("box"); + cenv_->getWorld()->removeObject("box"); std::vector shapes; EigenSTL::vector_Isometry3d poses; @@ -330,13 +298,13 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) poses.push_back(Eigen::Isometry3d::Identity()); std::set touch_links; trajectory_msgs::JointTrajectory empty_state; - robot_state::AttachedBody* attached_body = new robot_state::AttachedBody( + moveit::core::AttachedBody* attached_body = new moveit::core::AttachedBody( robot_state.getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state); robot_state.attachBody(attached_body); res = collision_detection::CollisionResult(); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); // deletes shape @@ -345,24 +313,24 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) touch_links.insert("r_gripper_palm_link"); shapes[0].reset(new shapes::Box(.1, .1, .1)); - robot_state::AttachedBody* attached_body_1 = new robot_state::AttachedBody( + moveit::core::AttachedBody* attached_body_1 = new moveit::core::AttachedBody( robot_state.getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state); robot_state.attachBody(attached_body_1); res = collision_detection::CollisionResult(); - crobot_->checkSelfCollision(req, res, robot_state, *acm_); + cenv_->checkSelfCollision(req, res, robot_state, *acm_); // ASSERT_FALSE(res.collision); pos1.translation().x() = 1.01; shapes::Shape* coll = new shapes::Box(.1, .1, .1); - cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1); + cenv_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1); res = collision_detection::CollisionResult(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_); + cenv_->checkRobotCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); acm_->setEntry("coll", "r_gripper_palm_link", true); res = collision_detection::CollisionResult(); - cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_); + cenv_->checkRobotCollision(req, res, robot_state, *acm_); ASSERT_TRUE(res.collision); } diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index e988eb2108..1c3d645c85 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -19,7 +19,8 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) @@ -27,9 +28,8 @@ if(CATKIN_ENABLE_TESTING) find_package(orocos_kdl REQUIRED) find_package(angles REQUIRED) find_package(tf2_kdl REQUIRED) - find_package(moveit_resources REQUIRED) - include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS}) + include_directories(SYSTEM ${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS}) catkin_add_gtest(test_constraint_samplers test/test_constraint_samplers.cpp diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h index ce2f2ed073..40821f395e 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_ -#define MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_ +#pragma once #include #include @@ -51,7 +50,7 @@ */ namespace constraint_samplers { -MOVEIT_CLASS_FORWARD(ConstraintSampler); +MOVEIT_CLASS_FORWARD(ConstraintSampler); // Defines ConstraintSamplerPtr, ConstPtr, WeakPtr... etc /** * \brief ConstraintSampler is an abstract base class that allows the @@ -105,7 +104,7 @@ class ConstraintSampler * * @return The joint model group */ - const robot_model::JointModelGroup* getJointModelGroup() const + const moveit::core::JointModelGroup* getJointModelGroup() const { return jmg_; } @@ -141,7 +140,7 @@ class ConstraintSampler * \brief Gets the callback used to determine state validity during sampling. The sampler will attempt * to satisfy this constraint if possible, but there is no guarantee. */ - const robot_state::GroupStateValidityCallbackFn& getGroupStateValidityCallback() const + const moveit::core::GroupStateValidityCallbackFn& getGroupStateValidityCallback() const { return group_state_validity_callback_; } @@ -152,7 +151,7 @@ class ConstraintSampler * * @param callback The callback to set */ - void setGroupStateValidityCallback(const robot_state::GroupStateValidityCallbackFn& callback) + void setGroupStateValidityCallback(const moveit::core::GroupStateValidityCallbackFn& callback) { group_state_validity_callback_ = callback; } @@ -167,7 +166,7 @@ class ConstraintSampler * * @return True if a sample was successfully taken, false otherwise */ - bool sample(robot_state::RobotState& state) + bool sample(moveit::core::RobotState& state) { return sample(state, state, DEFAULT_MAX_SAMPLING_ATTEMPTS); } @@ -183,7 +182,7 @@ class ConstraintSampler * * @return True if a sample was successfully taken, false otherwise */ - bool sample(robot_state::RobotState& state, unsigned int max_attempts) + bool sample(moveit::core::RobotState& state, unsigned int max_attempts) { return sample(state, state, max_attempts); } @@ -198,7 +197,7 @@ class ConstraintSampler * * @return True if a sample was successfully taken, false otherwise */ - bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state) + bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state) { return sample(state, reference_state, DEFAULT_MAX_SAMPLING_ATTEMPTS); } @@ -213,7 +212,7 @@ class ConstraintSampler * * @return True if a sample was successfully projected, false otherwise */ - bool project(robot_state::RobotState& state) + bool project(moveit::core::RobotState& state) { return project(state, DEFAULT_MAX_SAMPLING_ATTEMPTS); } @@ -229,7 +228,7 @@ class ConstraintSampler * * @return True if a sample was successfully taken, false otherwise */ - virtual bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state, + virtual bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts) = 0; /** @@ -243,7 +242,7 @@ class ConstraintSampler * * @return True if a sample was successfully projected, false otherwise */ - virtual bool project(robot_state::RobotState& state, unsigned int max_attempts) = 0; + virtual bool project(moveit::core::RobotState& state, unsigned int max_attempts) = 0; /** * \brief Returns whether or not the constraint sampler is valid or not. @@ -288,13 +287,11 @@ class ConstraintSampler /// Holds the planning scene planning_scene::PlanningSceneConstPtr scene_; /// Holds the joint model group associated with this constraint - const robot_model::JointModelGroup* const jmg_; + const moveit::core::JointModelGroup* const jmg_; /// Holds the set of frames that must exist in the reference state to allow samples to be drawn std::vector frame_depends_; /// Holds the callback for state validity - robot_state::GroupStateValidityCallbackFn group_state_validity_callback_; + moveit::core::GroupStateValidityCallbackFn group_state_validity_callback_; bool verbose_; ///< True if verbosity is on }; -} - -#endif +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h index abc68a0e28..8f992ed9d4 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h @@ -34,15 +34,14 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_ALLOCATOR_ -#define MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_ALLOCATOR_ +#pragma once #include #include namespace constraint_samplers { -MOVEIT_CLASS_FORWARD(ConstraintSamplerAllocator); +MOVEIT_CLASS_FORWARD(ConstraintSamplerAllocator); // Defines ConstraintSamplerAllocatorPtr, ConstPtr, WeakPtr... etc class ConstraintSamplerAllocator { @@ -61,6 +60,4 @@ class ConstraintSamplerAllocator virtual bool canService(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, const moveit_msgs::Constraints& constr) const = 0; }; -} - -#endif +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h index 05c88548e8..24f4334410 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h @@ -34,15 +34,14 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_MANAGER_ -#define MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_MANAGER_ +#pragma once #include #include namespace constraint_samplers { -MOVEIT_CLASS_FORWARD(ConstraintSamplerManager); +MOVEIT_CLASS_FORWARD(ConstraintSamplerManager); // Defines ConstraintSamplerManagerPtr, ConstPtr, WeakPtr... etc /** * \brief This class assists in the generation of a ConstraintSampler for a @@ -143,6 +142,4 @@ class ConstraintSamplerManager std::vector sampler_alloc_; /**< \brief Holds the constraint sampler allocators, which will be tested in order */ }; -} - -#endif +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h index ce405b9d51..54b392da54 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h @@ -34,15 +34,14 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_TOOLS_ -#define MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_TOOLS_ +#pragma once #include #include namespace constraint_samplers { -void visualizeDistribution(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state, +void visualizeDistribution(const ConstraintSamplerPtr& sampler, const moveit::core::RobotState& reference_state, const std::string& link_name, unsigned int sample_count, visualization_msgs::MarkerArray& markers); @@ -50,10 +49,8 @@ void visualizeDistribution(const moveit_msgs::Constraints& constr, const plannin const std::string& group, const std::string& link_name, unsigned int sample_count, visualization_msgs::MarkerArray& markers); -double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state); +double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const moveit::core::RobotState& reference_state); double countSamplesPerSecond(const moveit_msgs::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, const std::string& group); -} - -#endif +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h index 77a048bdb8..062c98587c 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_CONSTRAINT_SAMPLERS_ -#define MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_CONSTRAINT_SAMPLERS_ +#pragma once #include #include @@ -43,7 +42,7 @@ namespace constraint_samplers { -MOVEIT_CLASS_FORWARD(JointConstraintSampler); +MOVEIT_CLASS_FORWARD(JointConstraintSampler); // Defines JointConstraintSamplerPtr, ConstPtr, WeakPtr... etc /** * \brief JointConstraintSampler is a class that allows the sampling @@ -117,9 +116,9 @@ class JointConstraintSampler : public ConstraintSampler */ bool configure(const std::vector& jc); - bool sample(robot_state::RobotState& state, const robot_state::RobotState& ks, unsigned int max_attempts) override; + bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& ks, unsigned int max_attempts) override; - bool project(robot_state::RobotState& state, unsigned int max_attempts) override; + bool project(moveit::core::RobotState& state, unsigned int max_attempts) override; /** * \brief Gets the number of constrained joints - joints that have an @@ -196,7 +195,7 @@ class JointConstraintSampler : public ConstraintSampler std::vector bounds_; /**< \brief The bounds for any joint with bounds that are more restrictive than the joint limits */ - std::vector unbounded_; /**< \brief The joints that are not bounded except by joint + std::vector unbounded_; /**< \brief The joints that are not bounded except by joint limits */ std::vector uindex_; /**< \brief The index of the unbounded joints in the joint state vector */ std::vector values_; /**< \brief Values associated with this group to avoid continuously reallocating */ @@ -280,7 +279,7 @@ struct IKSamplingPose orientation_constraint_; /**< \brief Holds the orientation constraint for sampling */ }; -MOVEIT_CLASS_FORWARD(IKConstraintSampler); +MOVEIT_CLASS_FORWARD(IKConstraintSampler); // Defines IKConstraintSamplerPtr, ConstPtr, WeakPtr... etc /** * \brief A class that allows the sampling of IK constraints. @@ -446,10 +445,10 @@ class IKConstraintSampler : public ConstraintSampler * * @return True if a valid sample pose was produced and valid IK found for that pose. Otherwise false. */ - bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state, + bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts) override; - bool project(robot_state::RobotState& state, unsigned int max_attempts) override; + bool project(moveit::core::RobotState& state, unsigned int max_attempts) override; /** * \brief Returns a pose that falls within the constraint regions. * @@ -466,13 +465,13 @@ class IKConstraintSampler : public ConstraintSampler * Otherwise, a random quaternion is produced. * * @param [out] pos The position component of the sample - * @param [out] quat The orientation component of the sample + * @param [out] quat The orientation component of the sample. It will always be a normalized quaternion. * @param [in] ks The reference state used for performing transforms * @param [in] max_attempts The maximum attempts to try to sample - applies only to the position constraint * * @return True if a sample was successfully produced, otherwise false */ - bool samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const robot_state::RobotState& ks, + bool samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const moveit::core::RobotState& ks, unsigned int max_attempts); /** @@ -509,10 +508,10 @@ class IKConstraintSampler : public ConstraintSampler */ bool callIK(const geometry_msgs::Pose& ik_query, const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, double timeout, - robot_state::RobotState& state, bool use_as_seed); - bool sampleHelper(robot_state::RobotState& state, const robot_state::RobotState& reference_state, + moveit::core::RobotState& state, bool use_as_seed); + bool sampleHelper(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts, bool project); - bool validate(robot_state::RobotState& state) const; + bool validate(moveit::core::RobotState& state) const; random_numbers::RandomNumberGenerator random_number_generator_; /**< \brief Random generator used by the sampler */ IKSamplingPose sampling_pose_; /**< \brief Holder for the pose used for sampling */ @@ -525,6 +524,4 @@ class IKConstraintSampler : public ConstraintSampler frame of the end effector */ Eigen::Isometry3d eef_to_ik_tip_transform_; /**< \brief Holds the transformation from end effector to IK tip frame */ }; -} - -#endif +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h index a50c16298a..9c46104e76 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_UNION_CONSTRAINT_SAMPLER_ -#define MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_UNION_CONSTRAINT_SAMPLER_ +#pragma once #include @@ -149,10 +148,10 @@ class UnionConstraintSampler : public ConstraintSampler * * @return True if all invidual samplers return true */ - bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state, + bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts) override; - bool project(robot_state::RobotState& state, unsigned int max_attempts) override; + bool project(moveit::core::RobotState& state, unsigned int max_attempts) override; /** * \brief Get the name of the constraint sampler, for debugging purposes @@ -168,6 +167,4 @@ class UnionConstraintSampler : public ConstraintSampler protected: std::vector samplers_; /**< \brief Holder for sorted internal list of samplers*/ }; -} - -#endif +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp index 274c6be1da..f444d35054 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp @@ -57,7 +57,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni const std::string& group_name, const moveit_msgs::Constraints& constr) { - const robot_model::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group_name); if (!jmg) return constraint_samplers::ConstraintSamplerPtr(); std::stringstream ss; @@ -70,8 +70,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // if there are joint constraints, we could possibly get a sampler from those if (!constr.joint_constraints.empty()) { - ROS_DEBUG_NAMED("constraint_samplers", "There are joint constraints specified. " - "Attempting to construct a JointConstraintSampler for group '%s'", + ROS_DEBUG_NAMED("constraint_samplers", + "There are joint constraints specified. " + "Attempting to construct a JointConstraintSampler for group '%s'", jmg->getName().c_str()); std::map joint_coverage; @@ -135,15 +136,16 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni samplers.push_back(joint_sampler); // read the ik allocators, if any - const robot_model::JointModelGroup::KinematicsSolver& ik_alloc = jmg->getGroupKinematics().first; - const robot_model::JointModelGroup::KinematicsSolverMap& ik_subgroup_alloc = jmg->getGroupKinematics().second; + const moveit::core::JointModelGroup::KinematicsSolver& ik_alloc = jmg->getGroupKinematics().first; + const moveit::core::JointModelGroup::KinematicsSolverMap& ik_subgroup_alloc = jmg->getGroupKinematics().second; // if we have a means of computing complete states for the group using IK, then we try to see if any IK constraints // should be used if (ik_alloc) { - ROS_DEBUG_NAMED("constraint_samplers", "There is an IK allocator for '%s'. " - "Checking for corresponding position and/or orientation constraints", + ROS_DEBUG_NAMED("constraint_samplers", + "There is an IK allocator for '%s'. " + "Checking for corresponding position and/or orientation constraints", jmg->getName().c_str()); // keep track of which links we constrained @@ -177,8 +179,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni { // assign the link to a new constraint sampler used_l[constr.position_constraints[p].link_name] = iks; - ROS_DEBUG_NAMED("constraint_samplers", "Allocated an IK-based sampler for group '%s' " - "satisfying position and orientation constraints on link '%s'", + ROS_DEBUG_NAMED("constraint_samplers", + "Allocated an IK-based sampler for group '%s' " + "satisfying position and orientation constraints on link '%s'", jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str()); } } @@ -209,8 +212,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni if (use) { used_l[position_constraint.link_name] = iks; - ROS_DEBUG_NAMED("constraint_samplers", "Allocated an IK-based sampler for group '%s' " - "satisfying position constraints on link '%s'", + ROS_DEBUG_NAMED("constraint_samplers", + "Allocated an IK-based sampler for group '%s' " + "satisfying position constraints on link '%s'", jmg->getName().c_str(), position_constraint.link_name.c_str()); } } @@ -238,8 +242,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni if (use) { used_l[orientation_constraint.link_name] = iks; - ROS_DEBUG_NAMED("constraint_samplers", "Allocated an IK-based sampler for group '%s' " - "satisfying orientation constraints on link '%s'", + ROS_DEBUG_NAMED("constraint_samplers", + "Allocated an IK-based sampler for group '%s' " + "satisfying orientation constraints on link '%s'", jmg->getName().c_str(), orientation_constraint.link_name.c_str()); } } @@ -264,8 +269,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // find the sampler with the smallest sampling volume; delete the rest IKConstraintSamplerPtr iks = used_l.begin()->second; double msv = iks->getSamplingVolume(); - for (std::map::const_iterator it = ++used_l.begin(); it != used_l.end(); - ++it) + for (std::map::const_iterator it = ++used_l.begin(); it != used_l.end(); ++it) { double v = it->second->getSamplingVolume(); if (v < msv) @@ -290,14 +294,15 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // we now check to see if we can use samplers from subgroups if (!ik_subgroup_alloc.empty()) { - ROS_DEBUG_NAMED("constraint_samplers", "There are IK allocators for subgroups of group '%s'. " - "Checking for corresponding position and/or orientation constraints", + ROS_DEBUG_NAMED("constraint_samplers", + "There are IK allocators for subgroups of group '%s'. " + "Checking for corresponding position and/or orientation constraints", jmg->getName().c_str()); bool some_sampler_valid = false; std::set used_p, used_o; - for (robot_model::JointModelGroup::KinematicsSolverMap::const_iterator it = ik_subgroup_alloc.begin(); + for (moveit::core::JointModelGroup::KinematicsSolverMap::const_iterator it = ik_subgroup_alloc.begin(); it != ik_subgroup_alloc.end(); ++it) { // construct a sub-set of constraints that operate on the sub-group for which we have an IK allocator @@ -326,8 +331,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni ConstraintSamplerPtr cs = selectDefaultSampler(scene, it->first->getName(), sub_constr); if (cs) { - ROS_DEBUG_NAMED("constraint_samplers", "Constructed a sampler for the joints corresponding to group '%s', " - "but part of group '%s'", + ROS_DEBUG_NAMED("constraint_samplers", + "Constructed a sampler for the joints corresponding to group '%s', " + "but part of group '%s'", it->first->getName().c_str(), jmg->getName().c_str()); some_sampler_valid = true; samplers.push_back(cs); diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index 70cd7af200..5a3e059bc4 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -55,14 +55,14 @@ double constraint_samplers::countSamplesPerSecond(const moveit_msgs::Constraints } double constraint_samplers::countSamplesPerSecond(const ConstraintSamplerPtr& sampler, - const robot_state::RobotState& reference_state) + const moveit::core::RobotState& reference_state) { if (!sampler) { ROS_ERROR_NAMED("constraint_samplers", "No sampler specified for counting samples per second"); return 0.0; } - robot_state::RobotState ks(reference_state); + moveit::core::RobotState ks(reference_state); unsigned long int valid = 0; unsigned long int total = 0; ros::WallTime end = ros::WallTime::now() + ros::WallDuration(1.0); @@ -80,7 +80,7 @@ double constraint_samplers::countSamplesPerSecond(const ConstraintSamplerPtr& sa } void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& sampler, - const robot_state::RobotState& reference_state, + const moveit::core::RobotState& reference_state, const std::string& link_name, unsigned int sample_count, visualization_msgs::MarkerArray& markers) { @@ -89,10 +89,10 @@ void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& samp ROS_ERROR_NAMED("constraint_samplers", "No sampler specified for visualizing distribution of samples"); return; } - const robot_state::LinkModel* lm = reference_state.getLinkModel(link_name); + const moveit::core::LinkModel* lm = reference_state.getLinkModel(link_name); if (!lm) return; - robot_state::RobotState ks(reference_state); + moveit::core::RobotState ks(reference_state); std_msgs::ColorRGBA color; color.r = 1.0f; color.g = 0.0f; diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 2de8cdbad7..abc70021ad 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -35,7 +35,6 @@ /* Author: Ioan Sucan */ #include -#include #include #include @@ -74,13 +73,13 @@ bool JointConstraintSampler::configure(const std::vectorhasJointModel(jm->getName())) continue; some_valid_constraint = true; - const robot_model::VariableBounds& joint_bounds = jm->getVariableBounds(joint_constraint.getJointVariableName()); + const moveit::core::VariableBounds& joint_bounds = jm->getVariableBounds(joint_constraint.getJointVariableName()); JointInfo ji; std::map::iterator it = bound_data.find(joint_constraint.getJointVariableName()); if (it != bound_data.end()) @@ -120,8 +119,8 @@ bool JointConstraintSampler::configure(const std::vector& joints = jmg_->getJointModels(); - for (const robot_model::JointModel* joint : joints) + const std::vector& joints = jmg_->getJointModels(); + for (const moveit::core::JointModel* joint : joints) if (bound_data.find(joint->getName()) == bound_data.end() && joint->getVariableCount() > 0 && joint->getMimic() == nullptr) { @@ -148,8 +147,8 @@ bool JointConstraintSampler::configure(const std::vectorgetBaseFrame(); - transform_ik_ = !robot_state::Transforms::sameFrame(ik_frame_, jmg_->getParentModel().getModelFrame()); + transform_ik_ = !moveit::core::Transforms::sameFrame(ik_frame_, jmg_->getParentModel().getModelFrame()); if (!ik_frame_.empty() && ik_frame_[0] == '/') ik_frame_.erase(ik_frame_.begin()); if (transform_ik_) @@ -374,7 +373,7 @@ bool IKConstraintSampler::loadIKSolver() for (const std::pair& fixed_link : fixed_links) if (moveit::core::Transforms::sameFrame(fixed_link.first->getName(), kb_->getTipFrame())) { - eef_to_ik_tip_transform_ = fixed_link.second; + eef_to_ik_tip_transform_ = fixed_link.second; // valid isometry by contract need_eef_to_ik_tip_transform_ = true; wrong_link = false; break; @@ -385,14 +384,14 @@ bool IKConstraintSampler::loadIKSolver() if (!wrong_link && sampling_pose_.orientation_constraint_) { const moveit::core::LinkModel* lm = sampling_pose_.orientation_constraint_->getLinkModel(); - if (!robot_state::Transforms::sameFrame(kb_->getTipFrame(), lm->getName())) + if (!moveit::core::Transforms::sameFrame(kb_->getTipFrame(), lm->getName())) { wrong_link = true; const moveit::core::LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms(); for (const std::pair& fixed_link : fixed_links) if (moveit::core::Transforms::sameFrame(fixed_link.first->getName(), kb_->getTipFrame())) { - eef_to_ik_tip_transform_ = fixed_link.second; + eef_to_ik_tip_transform_ = fixed_link.second; // valid isometry by contract need_eef_to_ik_tip_transform_ = true; wrong_link = false; break; @@ -413,7 +412,7 @@ bool IKConstraintSampler::loadIKSolver() return true; } -bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const robot_state::RobotState& ks, +bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const moveit::core::RobotState& ks, unsigned int max_attempts) { if (ks.dirtyLinkTransforms()) @@ -459,7 +458,7 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q else { // do FK for rand state - robot_state::RobotState temp_state(ks); + moveit::core::RobotState temp_state(ks); temp_state.setToRandomPositions(jmg_); pos = temp_state.getGlobalLinkTransform(sampling_pose_.orientation_constraint_->getLinkModel()).translation(); } @@ -479,16 +478,21 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q Eigen::Isometry3d diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ())); - Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.rotation()); - quat = Eigen::Quaterniond(reqr.rotation()); + // diff is isometry by construction + // getDesiredRotationMatrix() returns a valid rotation matrix by contract + // reqr has thus to be a valid isometry + Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear()); + quat = Eigen::Quaterniond(reqr.linear()); // reqr is isometry, so quat has to be normalized // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the // model if (sampling_pose_.orientation_constraint_->mobileReferenceFrame()) { + // getFrameTransform() returns a valid isometry by contract const Eigen::Isometry3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame()); - Eigen::Isometry3d rt(t.rotation() * quat); - quat = Eigen::Quaterniond(rt.rotation()); + // rt is isometry by construction + Eigen::Isometry3d rt(t.linear() * quat); + quat = Eigen::Quaterniond(rt.linear()); // rt is isometry, so quat has to be normalized } } else @@ -496,7 +500,7 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q // sample a random orientation double q[4]; random_number_generator_.quaternion(q); - quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]); + quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]); // quat is normalized by contract } // if there is an offset, we need to undo the induced rotation in the sampled transform origin (point) @@ -509,8 +513,8 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q namespace { -void samplingIkCallbackFnAdapter(robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, - const robot_state::GroupStateValidityCallbackFn& constraint, +void samplingIkCallbackFnAdapter(moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, + const moveit::core::GroupStateValidityCallbackFn& constraint, const geometry_msgs::Pose& /*unused*/, const std::vector& ik_sol, moveit_msgs::MoveItErrorCodes& error_code) { @@ -525,13 +529,13 @@ void samplingIkCallbackFnAdapter(robot_state::RobotState* state, const robot_mod } } // namespace -bool IKConstraintSampler::sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state, +bool IKConstraintSampler::sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts) { return sampleHelper(state, reference_state, max_attempts, false); } -bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const robot_state::RobotState& reference_state, +bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts, bool project) { if (!is_valid_) @@ -549,7 +553,7 @@ bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const rob { // sample a point in the constraint region Eigen::Vector3d point; - Eigen::Quaterniond quat; + Eigen::Quaterniond quat; // quat is normalized by contract if (!samplePose(point, quat, reference_state, max_attempts)) { if (verbose_) @@ -562,19 +566,20 @@ bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const rob { // we need to convert this transform to the frame expected by the IK solver // both the planning frame and the frame for the IK are assumed to be robot links - Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); - ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq; + Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); // valid isometry by construction + // getFrameTransform() returns a valid isometry by contract + ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq; // valid isometry * valid isometry point = ikq.translation(); - quat = Eigen::Quaterniond(ikq.rotation()); + quat = Eigen::Quaterniond(ikq.linear()); // ikq is isometry, so quat is normalized } if (need_eef_to_ik_tip_transform_) { // After sampling the pose needs to be transformed to the ik chain tip - Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); - ikq = ikq * eef_to_ik_tip_transform_; + Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); // valid isometry by construction + ikq = ikq * eef_to_ik_tip_transform_; // eef_to_ik_tip_transform_ is valid isometry (checked in loadIKSolver()) point = ikq.translation(); - quat = Eigen::Quaterniond(ikq.rotation()); + quat = Eigen::Quaterniond(ikq.linear()); // ikq is isometry, so quat is normalized } geometry_msgs::Pose ik_query; @@ -592,12 +597,12 @@ bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const rob return false; } -bool IKConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts) +bool IKConstraintSampler::project(moveit::core::RobotState& state, unsigned int max_attempts) { return sampleHelper(state, state, max_attempts, true); } -bool IKConstraintSampler::validate(robot_state::RobotState& state) const +bool IKConstraintSampler::validate(moveit::core::RobotState& state) const { state.update(); return (!sampling_pose_.orientation_constraint_ || @@ -608,7 +613,7 @@ bool IKConstraintSampler::validate(robot_state::RobotState& state) const bool IKConstraintSampler::callIK(const geometry_msgs::Pose& ik_query, const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, - double timeout, robot_state::RobotState& state, bool use_as_seed) + double timeout, moveit::core::RobotState& state, bool use_as_seed) { const std::vector& ik_joint_bijection = jmg_->getKinematicsSolverJointBijection(); std::vector seed(ik_joint_bijection.size(), 0.0); diff --git a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp index 3eec66d7c6..ec0e753408 100644 --- a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp @@ -123,7 +123,7 @@ UnionConstraintSampler::UnionConstraintSampler(const planning_scene::PlanningSce } } -bool UnionConstraintSampler::sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state, +bool UnionConstraintSampler::sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, unsigned int max_attempts) { state = reference_state; @@ -147,7 +147,7 @@ bool UnionConstraintSampler::sample(robot_state::RobotState& state, const robot_ return true; } -bool UnionConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts) +bool UnionConstraintSampler::project(moveit::core::RobotState& state, unsigned int max_attempts) { for (ConstraintSamplerPtr& sampler : samplers_) { diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index d73fafa9a6..f664f95867 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -298,11 +298,9 @@ void PR2ArmIK::computeIKShoulderPan(const Eigen::Isometry3f& g_in, const double& at = sint1 * (shoulder_elbow_offset_ - shoulder_wrist_offset_) * sint2 * sint4; bt = (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cost1 * sint4; - ct = y - - (shoulder_upperarm_offset_ + - cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ + - (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cos(t4))) * - sint1; + ct = y - (shoulder_upperarm_offset_ + cost2 * (-shoulder_upperarm_offset_ + shoulder_elbow_offset_ + + (-shoulder_elbow_offset_ + shoulder_wrist_offset_) * cos(t4))) * + sint1; if (!solveCosineEqn(at, bt, ct, theta3[0], theta3[1])) continue; @@ -731,8 +729,8 @@ void PR2ArmIK::computeIKShoulderRoll(const Eigen::Isometry3f& g_in, const double std::cout << "t5 " << t5 << std::endl; std::cout << "t7 " << t7 << std::endl; #endif -// if(fabs(sin(t6)*sin(t7)-grhs_local(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_local(0,2)) > IK_EPS) -// continue; + // if(fabs(sin(t6)*sin(t7)-grhs_local(0,1)) > IK_EPS || fabs(cos(t7)*sin(t6)-grhs_local(0,2)) > IK_EPS) + // continue; #ifdef DEBUG std::cout << "theta1: " << t1 << std::endl; diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.h b/moveit_core/constraint_samplers/test/pr2_arm_ik.h index 8ba4052ca9..e27552a098 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.h @@ -34,8 +34,7 @@ /* Author: Sachin Chitta */ -#ifndef PR2_ARM_IK_H -#define PR2_ARM_IK_H +#pragma once #include #include @@ -189,5 +188,4 @@ class PR2ArmIK std::vector continuous_joint_; }; -} -#endif // PR2_ARM_IK_H +} // namespace pr2_arm_kinematics diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index 6d55bbc2b8..a8292a4a0a 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -38,7 +38,6 @@ #include #include #include -#include #include #include "pr2_arm_kinematics_plugin.h" @@ -92,10 +91,7 @@ PR2ArmIKSolver::PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const st search_discretization_angle_ = search_discretization_angle; free_angle_ = free_angle; root_frame_name_ = root_frame_name; - if (!pr2_arm_ik_.init(robot_model, root_frame_name, tip_frame_name)) - active_ = false; - else - active_ = true; + active_ = pr2_arm_ik_.init(robot_model, root_frame_name, tip_frame_name); } void PR2ArmIKSolver::updateInternalDataStructures() diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index 94bd16926e..8255cfbd12 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -34,8 +34,7 @@ /* Author: Sachin Chitta */ -#ifndef PR2_ARM_IK_NODE_H -#define PR2_ARM_IK_NODE_H +#pragma once #include #include @@ -43,8 +42,6 @@ #include #include -#include - #include #include #include @@ -152,10 +149,10 @@ class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase * @param ik_seed_state an initial guess solution for the inverse kinematics * @return True if a valid solution was found, false otherwise */ - bool getPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, std::vector& solution, - moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + bool + getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; /** * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. @@ -260,6 +257,4 @@ class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase void jointSolutionCallback(const KDL::JntArray& jnt_array, const KDL::Frame& ik_pose, moveit_msgs::MoveItErrorCodes& error_code) const; }; -} - -#endif +} // namespace pr2_arm_kinematics diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp index bd0d7b956f..cc579534a7 100644 --- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp @@ -56,14 +56,14 @@ class LoadPlanningModelsPr2 : public testing::Test { protected: - kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const robot_model::JointModelGroup* jmg) + kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const moveit::core::JointModelGroup* jmg) { { return pr2_kinematics_plugin_right_arm_; } } - kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const robot_model::JointModelGroup* jmg) + kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const moveit::core::JointModelGroup* jmg) { { return pr2_kinematics_plugin_left_arm_; @@ -85,7 +85,7 @@ class LoadPlanningModelsPr2 : public testing::Test func_right_arm_ = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, _1); func_left_arm_ = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, _1); - std::map allocators; + std::map allocators; allocators["right_arm"] = func_right_arm_; allocators["left_arm"] = func_left_arm_; allocators["whole_body"] = func_right_arm_; @@ -101,20 +101,20 @@ class LoadPlanningModelsPr2 : public testing::Test } protected: - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; planning_scene::PlanningScenePtr ps_; pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_right_arm_; pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_left_arm_; - robot_model::SolverAllocatorFn func_right_arm_; - robot_model::SolverAllocatorFn func_left_arm_; + moveit::core::SolverAllocatorFn func_right_arm_; + moveit::core::SolverAllocatorFn func_left_arm_; }; TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); kinematic_constraints::JointConstraint jc1(robot_model_); @@ -253,7 +253,7 @@ TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple) TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerSimple) { - robot_state::Transforms& tf = ps_->getTransformsNonConst(); + moveit::core::Transforms& tf = ps_->getTransformsNonConst(); kinematic_constraints::PositionConstraint pc(robot_model_); moveit_msgs::PositionConstraint pcm; @@ -316,14 +316,14 @@ TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerSimple) TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSampler) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); - robot_state::Transforms& tf = ps_->getTransformsNonConst(); + moveit::core::Transforms& tf = ps_->getTransformsNonConst(); kinematic_constraints::OrientationConstraint oc(robot_model_); moveit_msgs::OrientationConstraint ocm; @@ -359,14 +359,14 @@ TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSampler) TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerValid) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); - robot_state::Transforms& tf = ps_->getTransformsNonConst(); + moveit::core::Transforms& tf = ps_->getTransformsNonConst(); kinematic_constraints::PositionConstraint pc(robot_model_); moveit_msgs::PositionConstraint pcm; @@ -438,15 +438,15 @@ TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerValid) TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); - robot_state::Transforms& tf = ps_->getTransformsNonConst(); + moveit::core::Transforms& tf = ps_->getTransformsNonConst(); kinematic_constraints::JointConstraint jc1(robot_model_); @@ -606,10 +606,10 @@ TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler) TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); @@ -692,7 +692,7 @@ TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager) TEST_F(LoadPlanningModelsPr2, JointVersusPoseConstraintSamplerManager) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); @@ -861,10 +861,10 @@ TEST_F(LoadPlanningModelsPr2, JointVersusPoseConstraintSamplerManager) TEST_F(LoadPlanningModelsPr2, MixedJointAndIkSamplerManager) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); @@ -937,10 +937,10 @@ TEST_F(LoadPlanningModelsPr2, MixedJointAndIkSamplerManager) TEST_F(LoadPlanningModelsPr2, SubgroupJointConstraintsSamplerManager) { - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); @@ -1078,7 +1078,7 @@ TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler) ocm.weight = 1.0; c.orientation_constraints.push_back(ocm); - robot_state::Transforms& tf = ps_->getTransformsNonConst(); + moveit::core::Transforms& tf = ps_->getTransformsNonConst(); constraint_samplers::ConstraintSamplerPtr s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "arms", c); EXPECT_TRUE(static_cast(s)); @@ -1089,10 +1089,10 @@ TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler) kinematic_constraints::KinematicConstraintSet kset(robot_model_); kset.add(c, tf); - robot_state::RobotState ks(robot_model_); + moveit::core::RobotState ks(robot_model_); ks.setToDefaultValues(); ks.update(); - robot_state::RobotState ks_const(robot_model_); + moveit::core::RobotState ks_const(robot_model_); ks_const.setToDefaultValues(); ks_const.update(); diff --git a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h index da193cb0c2..48abe31adb 100644 --- a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h +++ b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h @@ -34,15 +34,14 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_CONTROLLER_MANAGER_ -#define MOVEIT_MOVEIT_CONTROLLER_MANAGER_ +#pragma once #include #include #include #include -/// Namespace for the base class of a MoveIt! controller manager +/// Namespace for the base class of a MoveIt controller manager namespace moveit_controller_manager { /// The reported execution status @@ -99,9 +98,9 @@ struct ExecutionStatus Value status_; }; -MOVEIT_CLASS_FORWARD(MoveItControllerHandle); +MOVEIT_CLASS_FORWARD(MoveItControllerHandle); // Defines MoveItControllerHandlePtr, ConstPtr, WeakPtr... etc -/** \brief MoveIt! sends commands to a controller via a handle that satisfies this interface. */ +/** \brief MoveIt sends commands to a controller via a handle that satisfies this interface. */ class MoveItControllerHandle { public: @@ -147,9 +146,9 @@ class MoveItControllerHandle std::string name_; }; -MOVEIT_CLASS_FORWARD(MoveItControllerManager); +MOVEIT_CLASS_FORWARD(MoveItControllerManager); // Defines MoveItControllerManagerPtr, ConstPtr, WeakPtr... etc -/** @brief MoveIt! does not enforce how controllers are implemented. +/** @brief MoveIt does not enforce how controllers are implemented. To make your controllers usable by MoveIt, this interface needs to be implemented. The main purpose of this interface is to expose the set of known controllers and potentially to allow activating and deactivating them, if multiple controllers are available. @@ -157,7 +156,7 @@ MOVEIT_CLASS_FORWARD(MoveItControllerManager); class MoveItControllerManager { public: - /** \brief Each controller known to MoveIt! has a state. This + /** \brief Each controller known to MoveIt has a state. This structure describes that controller's state. */ struct ControllerState { @@ -165,12 +164,12 @@ class MoveItControllerManager { } - /** \brief A controller can be active or inactive. This means that MoveIt! could activate the controller when + /** \brief A controller can be active or inactive. This means that MoveIt could activate the controller when needed, and de-activate controllers that overlap (control the same set of joints) */ bool active_; /** \brief It is often the case that multiple controllers could be used to execute a motion. Marking a controller as - default makes MoveIt! prefer this controller when multiple options are available. */ + default makes MoveIt prefer this controller when multiple options are available. */ bool default_; }; @@ -208,6 +207,4 @@ class MoveItControllerManager virtual bool switchControllers(const std::vector& activate, const std::vector& deactivate) = 0; }; -} - -#endif +} // namespace moveit_controller_manager diff --git a/moveit_core/distance_field/CMakeLists.txt b/moveit_core/distance_field/CMakeLists.txt index ac60b52783..3955c94fd3 100644 --- a/moveit_core/distance_field/CMakeLists.txt +++ b/moveit_core/distance_field/CMakeLists.txt @@ -12,7 +12,8 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h index d1164a4935..b5f516c0bf 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h @@ -34,14 +34,10 @@ /* Author: Mrinal Kalakrishnan, Ken Anderson, E. Gil Jones */ -#ifndef MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_H -#define MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_H +#pragma once #include -#include #include -#include -#include #include #include #include @@ -51,7 +47,7 @@ namespace shapes { -MOVEIT_CLASS_FORWARD(Shape); +MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc } namespace octomap { @@ -70,28 +66,28 @@ namespace distance_field /// \brief The plane to visualize enum PlaneVisualizationType { - XYPlane, - XZPlane, - YZPlane + XY_PLANE, + XZ_PLANE, + YZ_PLANE }; -MOVEIT_CLASS_FORWARD(DistanceField); +MOVEIT_CLASS_FORWARD(DistanceField); // Defines DistanceFieldPtr, ConstPtr, WeakPtr... etc /** -* \brief DistanceField is an abstract base class for computing -* distances from sets of 3D obstacle points. The distance assigned to -* a freespace cell should be the distance to the closest obstacle -* cell. Cells that are obstacle cells should either be marked as zero -* distance, or may have a negative distance if a signed version of the -* distance field is being used and an obstacle point is internal to an -* obstacle volume. -* -* Inherited classes must contain methods for holding a dense set of 3D -* voxels as well as methods for computing the required distances. The -* distance field parent class doesn't hold the data or have any way to -* generate distances from that data. -* -*/ + * \brief DistanceField is an abstract base class for computing + * distances from sets of 3D obstacle points. The distance assigned to + * a freespace cell should be the distance to the closest obstacle + * cell. Cells that are obstacle cells should either be marked as zero + * distance, or may have a negative distance if a signed version of the + * distance field is being used and an obstacle point is internal to an + * obstacle volume. + * + * Inherited classes must contain methods for holding a dense set of 3D + * voxels as well as methods for computing the required distances. The + * distance field parent class doesn't hold the data or have any way to + * generate distances from that data. + * + */ class DistanceField { public: @@ -192,7 +188,7 @@ class DistanceField void addShapeToField(const shapes::Shape* shape, const Eigen::Isometry3d& pose); // DEPRECATED form - MOVEIT_DEPRECATED void addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose); + [[deprecated]] void addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose); /** * \brief Adds an octree to the distance field. Cells that are @@ -231,8 +227,8 @@ class DistanceField const Eigen::Isometry3d& new_pose); // DEPRECATED form - MOVEIT_DEPRECATED void moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose, - const geometry_msgs::Pose& new_pose); + [[deprecated]] void moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose, + const geometry_msgs::Pose& new_pose); /** * \brief All points corresponding to the shape are removed from the @@ -246,7 +242,7 @@ class DistanceField void removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose); // DEPRECATED form - MOVEIT_DEPRECATED void removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose); + [[deprecated]] void removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose); /** * \brief Resets all points in the distance field to an uninitialize @@ -627,5 +623,3 @@ class DistanceField }; } // namespace distance_field - -#endif // MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_H diff --git a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h index be419651f9..f03a42d39f 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h +++ b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley */ -#ifndef MOVEIT_DISTANCE_FIELD__FIND_INTERNAL_POINTS_ -#define MOVEIT_DISTANCE_FIELD__FIND_INTERNAL_POINTS_ +#pragma once #include #include @@ -53,6 +52,4 @@ namespace distance_field * vector. */ void findInternalPointsConvex(const bodies::Body& body, double resolution, EigenSTL::vector_Vector3d& points); -} - -#endif +} // namespace distance_field diff --git a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h index 66cbf2daa2..58f8de8e2d 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h @@ -34,13 +34,11 @@ /* Author: Mrinal Kalakrishnan, Ken Anderson */ -#ifndef MOVEIT_DISTANCE_FIELD_PROPAGATION_DISTANCE_FIELD_ -#define MOVEIT_DISTANCE_FIELD_PROPAGATION_DISTANCE_FIELD_ +#pragma once #include #include #include -#include #include #include #include @@ -56,7 +54,7 @@ namespace distance_field * \brief Struct for sorting type Eigen::Vector3i for use in sorted * std containers. Sorts in z order, then y order, then x order. */ -struct compareEigen_Vector3i +struct CompareEigenVector3i { bool operator()(const Eigen::Vector3i& loc_1, const Eigen::Vector3i& loc_2) const { @@ -418,20 +416,20 @@ class PropagationDistanceField : public DistanceField dist = sqrt_table_[cell->distance_square_]; pos = cell->closest_point_; const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z()); - return ncell == cell ? NULL : ncell; + return ncell == cell ? nullptr : ncell; } if (cell->negative_distance_square_ > 0) { dist = -sqrt_table_[cell->negative_distance_square_]; pos = cell->closest_negative_point_; const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z()); - return ncell == cell ? NULL : ncell; + return ncell == cell ? nullptr : ncell; } dist = 0.0; pos.x() = x; pos.y() = y; pos.z() = z; - return NULL; + return nullptr; } /** @@ -449,7 +447,7 @@ class PropagationDistanceField : public DistanceField private: /** Typedef for set of integer indices */ - typedef std::set> VoxelSet; + typedef std::set> VoxelSet; /** * \brief Initializes the field, resetting the voxel grid and * building a sqrt lookup table for efficiency based on @@ -540,16 +538,6 @@ class PropagationDistanceField : public DistanceField */ void print(const EigenSTL::vector_Vector3d& points); - /** - * \brief Computes squared distance between two 3D integer points - * - * @param point1 Point 1 for distance - * @param point2 Point 2 for distance - * - * @return Distance between points squared - */ - static int eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i point2); - bool propagate_negative_; /**< \brief Whether or not to propagate negative distances */ VoxelGrid::Ptr voxel_grid_; /**< \brief Actual container for distance data */ @@ -610,6 +598,4 @@ inline double PropagationDistanceField::getDistance(const PropDistanceFieldVoxel { return sqrt_table_[object.distance_square_] - sqrt_table_[object.negative_distance_square_]; } -} - -#endif +} // namespace distance_field diff --git a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h index f85cfbdcd7..75dc96806f 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h +++ b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan, Acorn Pooley */ -#ifndef MOVEIT_DISTANCE_FIELD_VOXEL_GRID_ -#define MOVEIT_DISTANCE_FIELD_VOXEL_GRID_ +#pragma once #include #include @@ -342,7 +341,7 @@ class VoxelGrid template VoxelGrid::VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z, T default_object) - : data_(NULL) + : data_(nullptr) { resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object); } @@ -369,7 +368,7 @@ void VoxelGrid::resize(double size_x, double size_y, double size_z, double re double origin_y, double origin_z, T default_object) { delete[] data_; - data_ = NULL; + data_ = nullptr; size_[DIM_X] = size_x; size_[DIM_Y] = size_y; @@ -442,7 +441,7 @@ inline double VoxelGrid::getResolution() const } template -inline double VoxelGrid::getResolution(Dimension dim) const +inline double VoxelGrid::getResolution(Dimension /*dim*/) const { return resolution_; } @@ -462,12 +461,12 @@ inline int VoxelGrid::getNumCells(Dimension dim) const template inline const T& VoxelGrid::operator()(double x, double y, double z) const { - int cellX = getCellFromLocation(DIM_X, x); - int cellY = getCellFromLocation(DIM_Y, y); - int cellZ = getCellFromLocation(DIM_Z, z); - if (!isCellValid(cellX, cellY, cellZ)) + int cell_x = getCellFromLocation(DIM_X, x); + int cell_y = getCellFromLocation(DIM_Y, y); + int cell_z = getCellFromLocation(DIM_Z, z); + if (!isCellValid(cell_x, cell_y, cell_z)) return default_object_; - return getCell(cellX, cellY, cellZ); + return getCell(cell_x, cell_y, cell_z); } template @@ -579,4 +578,3 @@ inline bool VoxelGrid::worldToGrid(const Eigen::Vector3d& world, Eigen::Vecto } } // namespace distance_field -#endif diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index 071777706b..dd3b6d1a6d 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -172,10 +172,10 @@ void DistanceField::getGradientMarkers(double min_distance, double max_distance, // double angle = -gradient.angle(unitX); // Eigen::AngleAxisd rotation(angle, axis); - // marker.pose.orientation.x = rotation.rotation().x(); - // marker.pose.orientation.y = rotation.rotation().y(); - // marker.pose.orientation.z = rotation.rotation().z(); - // marker.pose.orientation.w = rotation.rotation().w(); + // marker.pose.orientation.x = rotation.linear().x(); + // marker.pose.orientation.y = rotation.linear().y(); + // marker.pose.orientation.z = rotation.linear().z(); + // marker.pose.orientation.w = rotation.linear().w(); marker.scale.x = getResolution(); marker.scale.y = getResolution(); @@ -208,8 +208,10 @@ bool DistanceField::getShapePoints(const shapes::Shape* shape, const Eigen::Isom } else { - bodies::Body* body = bodies::createBodyFromShape(shape); - body->setPose(pose); + bodies::Body* body = bodies::createEmptyBodyFromShapeType(shape->type); + body->setDimensionsDirty(shape); + body->setPoseDirty(pose); + body->updateInternalData(); findInternalPointsConvex(*body, resolution_, *points); delete body; } @@ -292,8 +294,10 @@ void DistanceField::moveShapeInField(const shapes::Shape* shape, const Eigen::Is ROS_WARN_NAMED("distance_field", "Move shape not supported for Octree"); return; } - bodies::Body* body = bodies::createBodyFromShape(shape); - body->setPose(old_pose); + bodies::Body* body = bodies::createEmptyBodyFromShapeType(shape->type); + body->setDimensionsDirty(shape); + body->setPoseDirty(old_pose); + body->updateInternalData(); EigenSTL::vector_Vector3d old_point_vec; findInternalPointsConvex(*body, resolution_, old_point_vec); body->setPose(new_pose); @@ -315,8 +319,10 @@ void DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_ void DistanceField::removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose) { - bodies::Body* body = bodies::createBodyFromShape(shape); - body->setPose(pose); + bodies::Body* body = bodies::createEmptyBodyFromShapeType(shape->type); + body->setDimensionsDirty(shape); + body->setPoseDirty(pose); + body->updateInternalData(); EigenSTL::vector_Vector3d point_vec; findInternalPointsConvex(*body, resolution_, point_vec); delete body; @@ -358,7 +364,7 @@ void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length, switch (type) { - case XYPlane: + case XY_PLANE: min_z = height; max_z = height; @@ -367,7 +373,7 @@ void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length, min_y = -width / 2.0; max_y = width / 2.0; break; - case XZPlane: + case XZ_PLANE: min_y = height; max_y = height; @@ -376,7 +382,7 @@ void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length, min_z = -width / 2.0; max_z = width / 2.0; break; - case YZPlane: + case YZ_PLANE: min_x = height; max_x = height; diff --git a/moveit_core/distance_field/src/propagation_distance_field.cpp b/moveit_core/distance_field/src/propagation_distance_field.cpp index d656c22ff0..0cb108cd7b 100644 --- a/moveit_core/distance_field/src/propagation_distance_field.cpp +++ b/moveit_core/distance_field/src/propagation_distance_field.cpp @@ -92,14 +92,6 @@ void PropagationDistanceField::initialize() reset(); } -int PropagationDistanceField::eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i point2) -{ - int dx = point1.x() - point2.x(); - int dy = point1.y() - point2.y(); - int dz = point1.z() - point2.z(); - return dx * dx + dy * dy + dz * dz; -} - void PropagationDistanceField::print(const VoxelSet& set) { ROS_DEBUG_NAMED("distance_field", "["); @@ -148,7 +140,7 @@ void PropagationDistanceField::updatePointsInField(const EigenSTL::vector_Vector new_point_set.insert(voxel_loc); } } - compareEigen_Vector3i comp; + CompareEigenVector3i comp; EigenSTL::vector_Vector3i old_not_new; std::set_difference(old_point_set.begin(), old_point_set.end(), new_point_set.begin(), new_point_set.end(), @@ -430,7 +422,7 @@ void PropagationDistanceField::propagatePositive() // the real update code: // calculate the neighbor's new distance based on my closest filled voxel: PropDistanceFieldVoxel* neighbor = &voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z()); - int new_distance_sq = eucDistSq(vptr->closest_point_, nloc); + int new_distance_sq = (vptr->closest_point_ - nloc).squaredNorm(); if (new_distance_sq > max_distance_sq_) continue; @@ -488,7 +480,7 @@ void PropagationDistanceField::propagateNegative() // the real update code: // calculate the neighbor's new distance based on my closest filled voxel: PropDistanceFieldVoxel* neighbor = &voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z()); - int new_distance_sq = eucDistSq(vptr->closest_negative_point_, nloc); + int new_distance_sq = (vptr->closest_negative_point_ - nloc).squaredNorm(); if (new_distance_sq > max_distance_sq_) continue; // std::cout << "Looking at " << nloc.x() << " " << nloc.y() << " " << nloc.z() << " " << new_distance_sq << " " diff --git a/moveit_core/distance_field/test/test_distance_field.cpp b/moveit_core/distance_field/test/test_distance_field.cpp index ad4a557a3b..a834b11de2 100644 --- a/moveit_core/distance_field/test/test_distance_field.cpp +++ b/moveit_core/distance_field/test/test_distance_field.cpp @@ -412,15 +412,15 @@ TEST(TestPropagationDistanceField, TestAddRemovePoints) std::cout << "Grad " << grad.x() << " " << grad.y() << " " << grad.z() << " comp " << comp_x << " " << comp_y << " " << comp_z << std::endl; } - ASSERT_NEAR(comp_x, POINT1.x(), RESOLUTION) << dist << x << " " << y << " " << z << " " << grad.x() << " " - << grad.y() << " " << grad.z() << " " << xscale << " " << yscale - << " " << zscale << std::endl; - ASSERT_NEAR(comp_y, POINT1.y(), RESOLUTION) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() - << " " << grad.z() << " " << xscale << " " << yscale << " " - << zscale << std::endl; - ASSERT_NEAR(comp_z, POINT1.z(), RESOLUTION) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() - << " " << grad.z() << " " << xscale << " " << yscale << " " - << zscale << std::endl; + ASSERT_NEAR(comp_x, POINT1.x(), RESOLUTION) + << dist << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " + << xscale << " " << yscale << " " << zscale << std::endl; + ASSERT_NEAR(comp_y, POINT1.y(), RESOLUTION) + << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale + << " " << yscale << " " << zscale << std::endl; + ASSERT_NEAR(comp_z, POINT1.z(), RESOLUTION) + << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale + << " " << yscale << " " << zscale << std::endl; } } } @@ -588,10 +588,10 @@ TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints) EXPECT_EQ(ncell_pos.z(), cell_z); EXPECT_EQ(ncell, cell); #endif - EXPECT_GE(cell->distance_square_, 1) << dist << " " << x << " " << y << " " << z << " " << grad.x() << " " - << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " " - << zscale << " cell " << comp_x << " " << comp_y << " " << comp_z - << std::endl; + EXPECT_GE(cell->distance_square_, 1) + << dist << " " << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() + << " " << xscale << " " << yscale << " " << zscale << " cell " << comp_x << " " << comp_y << " " << comp_z + << std::endl; } } } diff --git a/moveit_core/dynamics_solver/CMakeLists.txt b/moveit_core/dynamics_solver/CMakeLists.txt index 61324a752d..07028fc9e5 100644 --- a/moveit_core/dynamics_solver/CMakeLists.txt +++ b/moveit_core/dynamics_solver/CMakeLists.txt @@ -8,6 +8,7 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h index 3e73f8b036..a2db344a52 100644 --- a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h +++ b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h @@ -34,8 +34,7 @@ /* Author: Sachin Chitta */ -#ifndef MOVEIT_DYNAMICS_SOLVER_DYNAMICS_SOLVER_ -#define MOVEIT_DYNAMICS_SOLVER_DYNAMICS_SOLVER_ +#pragma once // KDL #include @@ -50,7 +49,7 @@ /** \brief This namespace includes the dynamics_solver library */ namespace dynamics_solver { -MOVEIT_CLASS_FORWARD(DynamicsSolver); +MOVEIT_CLASS_FORWARD(DynamicsSolver); // Defines DynamicsSolverPtr, ConstPtr, WeakPtr... etc /** * This solver currently computes the required torques given a @@ -67,7 +66,7 @@ class DynamicsSolver * @param group_name The name of the group to compute stuff for * @return False if initialization failed */ - DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, + DynamicsSolver(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, const geometry_msgs::Vector3& gravity_vector); /** @@ -125,12 +124,12 @@ class DynamicsSolver * @brief Get the kinematic model * @return kinematic model */ - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } - const robot_model::JointModelGroup* getGroup() const + const moveit::core::JointModelGroup* getGroup() const { return joint_model_group_; } @@ -139,10 +138,10 @@ class DynamicsSolver std::shared_ptr chain_id_solver_; // KDL chain inverse dynamics KDL::Chain kdl_chain_; // KDL chain - robot_model::RobotModelConstPtr robot_model_; - const robot_model::JointModelGroup* joint_model_group_; + moveit::core::RobotModelConstPtr robot_model_; + const moveit::core::JointModelGroup* joint_model_group_; - robot_state::RobotStatePtr state_; // robot state + moveit::core::RobotStatePtr state_; // robot state std::string base_name_, tip_name_; // base name, tip name unsigned int num_joints_, num_segments_; // number of joints in group, number of segments in group @@ -150,5 +149,4 @@ class DynamicsSolver double gravity_; // Norm of the gravity vector passed in initialize() }; -} -#endif +} // namespace dynamics_solver diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index 9a48b0ed2a..9ada138e8d 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -39,10 +39,6 @@ // KDL #include #include -#include -#include -#include -#include #include namespace dynamics_solver @@ -53,7 +49,8 @@ inline geometry_msgs::Vector3 transformVector(const Eigen::Isometry3d& transform { Eigen::Vector3d p; p = Eigen::Vector3d(vector.x, vector.y, vector.z); - p = transform.rotation() * p; + // transform has to be a valid isometry; the caller is responsible for the check + p = transform.linear() * p; geometry_msgs::Vector3 result; result.x = p.x(); @@ -64,7 +61,7 @@ inline geometry_msgs::Vector3 transformVector(const Eigen::Isometry3d& transform } } // namespace -DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, +DynamicsSolver::DynamicsSolver(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, const geometry_msgs::Vector3& gravity_vector) { robot_model_ = robot_model; @@ -88,7 +85,7 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode return; } - const robot_model::JointModel* joint = joint_model_group_->getJointRoots()[0]; + const moveit::core::JointModel* joint = joint_model_group_->getJointRoots()[0]; if (!joint->getParentLinkModel()) { ROS_ERROR_NAMED("dynamics_solver", "Group '%s' does not have a parent link", group_name.c_str()); @@ -120,7 +117,7 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode num_joints_ = kdl_chain_.getNrOfJoints(); num_segments_ = kdl_chain_.getNrOfSegments(); - state_.reset(new robot_state::RobotState(robot_model_)); + state_.reset(new moveit::core::RobotState(robot_model_)); state_->setToDefaultValues(); const std::vector& joint_model_names = joint_model_group_->getJointModelNames(); @@ -243,9 +240,9 @@ bool DynamicsSolver::getMaxPayload(const std::vector& joint_angles, doub } state_->setJointGroupPositions(joint_model_group_, joint_angles); - const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_); - const Eigen::Isometry3d& tip_frame = state_->getFrameTransform(tip_name_); - Eigen::Isometry3d transform = tip_frame.inverse() * base_frame; + const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_); // valid isometry by contract + const Eigen::Isometry3d& tip_frame = state_->getFrameTransform(tip_name_); // valid isometry by contract + Eigen::Isometry3d transform = tip_frame.inverse() * base_frame; // valid isometry by construction wrenches.back().force.z = 1.0; wrenches.back().force = transformVector(transform, wrenches.back().force); wrenches.back().torque = transformVector(transform, wrenches.back().torque); @@ -299,9 +296,9 @@ bool DynamicsSolver::getPayloadTorques(const std::vector& joint_angles, std::vector wrenches(num_segments_); state_->setJointGroupPositions(joint_model_group_, joint_angles); - const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_); - const Eigen::Isometry3d& tip_frame = state_->getFrameTransform(tip_name_); - Eigen::Isometry3d transform = tip_frame.inverse() * base_frame; + const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_); // valid isometry by contract + const Eigen::Isometry3d& tip_frame = state_->getFrameTransform(tip_name_); // valid isometry by contract + Eigen::Isometry3d transform = tip_frame.inverse() * base_frame; // valid isometry by construction wrenches.back().force.z = payload * gravity_; wrenches.back().force = transformVector(transform, wrenches.back().force); wrenches.back().torque = transformVector(transform, wrenches.back().torque); diff --git a/moveit_core/exceptions/CMakeLists.txt b/moveit_core/exceptions/CMakeLists.txt index 6b80cf3c28..70ab423d27 100644 --- a/moveit_core/exceptions/CMakeLists.txt +++ b/moveit_core/exceptions/CMakeLists.txt @@ -7,5 +7,6 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h index 2dd4f93464..1e4d28ddb9 100644 --- a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h +++ b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h @@ -34,12 +34,11 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#ifndef MOVEIT_EXCEPTIONS_ -#define MOVEIT_EXCEPTIONS_ +#pragma once #include -/** \brief Main namespace for MoveIt! */ +/** \brief Main namespace for MoveIt */ namespace moveit { /** \brief This may be thrown during construction of objects if errors occur */ @@ -55,6 +54,4 @@ class Exception : public std::runtime_error public: explicit Exception(const std::string& what_arg); }; -} - -#endif +} // namespace moveit diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index ea45aa72f0..31198de4aa 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -12,14 +12,15 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_constraints test/test_constraints.cpp) target_link_libraries(test_constraints moveit_test_utils ${MOVEIT_LIB_NAME}) + + catkin_add_gtest(test_orientation_constraints test/test_orientation_constraints.cpp) + target_link_libraries(test_orientation_constraints moveit_test_utils ${MOVEIT_LIB_NAME}) endif() diff --git a/moveit_core/kinematic_constraints/dox/constraint_representation.dox b/moveit_core/kinematic_constraints/dox/constraint_representation.dox deleted file mode 100644 index 886a3100e9..0000000000 --- a/moveit_core/kinematic_constraints/dox/constraint_representation.dox +++ /dev/null @@ -1,17 +0,0 @@ -/** -\page constraint_representation Representation and Evaluation of Constraints - -Constraints are integral component of MoveIt! and they are used both to constrain robot motion as well as to define planning goals. There following set of constraints are defined in the kinematic_constraints namespace: -- kinematic_constraints::JointConstraint -- kinematic_constraints::OrientationConstraint -- kinematic_constraints::PositionConstraint -- kinematic_constraints::VisibilityConstraint -. - -All of these constraints inherit from the kinematic_constraints::KinematicConstraint base class and thus more constraint types can be added by the user by providing their own derived classes. The main operation each constraint implements is the KinematicConstraint::decide() function, which decides whether a constraint is satisfied, and optionally returns a distance (an error) when a constraint is not satisfied. - -Often multiple constraints need to be imposed on a particular motion plan or for a particular goal. The class kinematic_constraints::KinematicConstraintSet facilitates operating with sets of constraints. - -A related functionality to representing and evaluating constraints is \ref constraint_sampling "generating samples" that satisfy those constraints. - -*/ diff --git a/moveit_core/kinematic_constraints/dox/img/exact_opposites.png b/moveit_core/kinematic_constraints/dox/img/exact_opposites.png deleted file mode 100644 index 64e2470d9c..0000000000 Binary files a/moveit_core/kinematic_constraints/dox/img/exact_opposites.png and /dev/null differ diff --git a/moveit_core/kinematic_constraints/dox/img/fingertip_collision.png b/moveit_core/kinematic_constraints/dox/img/fingertip_collision.png deleted file mode 100644 index cf55ddbf1f..0000000000 Binary files a/moveit_core/kinematic_constraints/dox/img/fingertip_collision.png and /dev/null differ diff --git a/moveit_core/kinematic_constraints/dox/img/fingertip_cone.jpg b/moveit_core/kinematic_constraints/dox/img/fingertip_cone.jpg deleted file mode 100644 index 4c6e8ea35e..0000000000 Binary files a/moveit_core/kinematic_constraints/dox/img/fingertip_cone.jpg and /dev/null differ diff --git a/moveit_core/kinematic_constraints/dox/img/fingertip_cone.png b/moveit_core/kinematic_constraints/dox/img/fingertip_cone.png deleted file mode 100644 index f4e5e75c02..0000000000 Binary files a/moveit_core/kinematic_constraints/dox/img/fingertip_cone.png and /dev/null differ diff --git a/moveit_core/kinematic_constraints/dox/img/fourty_five.png b/moveit_core/kinematic_constraints/dox/img/fourty_five.png deleted file mode 100644 index 925cb55124..0000000000 Binary files a/moveit_core/kinematic_constraints/dox/img/fourty_five.png and /dev/null differ diff --git a/moveit_core/kinematic_constraints/dox/img/other_side.png b/moveit_core/kinematic_constraints/dox/img/other_side.png deleted file mode 100644 index d99b570498..0000000000 Binary files a/moveit_core/kinematic_constraints/dox/img/other_side.png and /dev/null differ diff --git a/moveit_core/kinematic_constraints/dox/img/perpindicular.png b/moveit_core/kinematic_constraints/dox/img/perpindicular.png deleted file mode 100644 index 8e17874394..0000000000 Binary files a/moveit_core/kinematic_constraints/dox/img/perpindicular.png and /dev/null differ diff --git a/moveit_core/kinematic_constraints/dox/img/range_angle.png b/moveit_core/kinematic_constraints/dox/img/range_angle.png deleted file mode 100644 index 763ba4dbec..0000000000 Binary files a/moveit_core/kinematic_constraints/dox/img/range_angle.png and /dev/null differ diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index 936ebf82fe..cc634c4019 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -34,13 +34,12 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_KINEMATIC_CONSTRAINTS_KINEMATIC_CONSTRAINT_ -#define MOVEIT_KINEMATIC_CONSTRAINTS_KINEMATIC_CONSTRAINT_ +#pragma once #include #include #include -#include +#include #include #include @@ -72,7 +71,7 @@ struct ConstraintEvaluationResult double distance; /**< \brief The distance evaluation from the constraint or constraints */ }; -MOVEIT_CLASS_FORWARD(KinematicConstraint); +MOVEIT_CLASS_FORWARD(KinematicConstraint); // Defines KinematicConstraintPtr, ConstPtr, WeakPtr... etc /// \brief Base class for representing a kinematic constraint class KinematicConstraint @@ -93,7 +92,7 @@ class KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - KinematicConstraint(const robot_model::RobotModelConstPtr& model); + KinematicConstraint(const moveit::core::RobotModelConstPtr& model); virtual ~KinematicConstraint(); /** \brief Clear the stored constraint */ @@ -107,7 +106,7 @@ class KinematicConstraint * * @return */ - virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const = 0; + virtual ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const = 0; /** \brief This function returns true if this constraint is configured and able to decide whether states do meet the @@ -145,7 +144,7 @@ class KinematicConstraint * * @param [in] out The file descriptor for printing */ - virtual void print(std::ostream& = std::cout) const + virtual void print(std::ostream& /*unused*/ = std::cout) const { } @@ -167,19 +166,19 @@ class KinematicConstraint * * @return The kinematic model associated with this constraint */ - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } protected: - ConstraintType type_; /**< \brief The type of the constraint */ - robot_model::RobotModelConstPtr robot_model_; /**< \brief The kinematic model associated with this constraint */ + ConstraintType type_; /**< \brief The type of the constraint */ + moveit::core::RobotModelConstPtr robot_model_; /**< \brief The kinematic model associated with this constraint */ double constraint_weight_; /**< \brief The weight of a constraint is a multiplicative factor associated to the distance computed by the decide() function */ }; -MOVEIT_CLASS_FORWARD(JointConstraint); +MOVEIT_CLASS_FORWARD(JointConstraint); // Defines JointConstraintPtr, ConstPtr, WeakPtr... etc /** * \brief Class for handling single DOF joint constraints. @@ -208,8 +207,8 @@ class JointConstraint : public KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - JointConstraint(const robot_model::RobotModelConstPtr& model) - : KinematicConstraint(model), joint_model_(NULL), joint_variable_index_(-1) + JointConstraint(const moveit::core::RobotModelConstPtr& model) + : KinematicConstraint(model), joint_model_(nullptr), joint_variable_index_(-1) { type_ = JOINT_CONSTRAINT; } @@ -246,7 +245,7 @@ class JointConstraint : public KinematicConstraint */ bool equal(const KinematicConstraint& other, double margin) const override; - ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; bool enabled() const override; void clear() override; void print(std::ostream& out = std::cout) const override; @@ -256,7 +255,7 @@ class JointConstraint : public KinematicConstraint * * @return The relevant joint model if enabled, and otherwise NULL */ - const robot_model::JointModel* getJointModel() const + const moveit::core::JointModel* getJointModel() const { return joint_model_; } @@ -274,7 +273,7 @@ class JointConstraint : public KinematicConstraint } /** - * \brief Gets the joint variable name, as known to the robot_model::RobotModel + * \brief Gets the joint variable name, as known to the moveit::core::RobotModel * * This will include the local variable name if a variable of a multi-DOF joint is constrained. * @@ -324,15 +323,15 @@ class JointConstraint : public KinematicConstraint } protected: - const robot_model::JointModel* joint_model_; /**< \brief The joint from the kinematic model for this constraint */ - bool joint_is_continuous_; /**< \brief Whether or not the joint is continuous */ - std::string local_variable_name_; /**< \brief The local variable name for a multi DOF joint, if any */ - std::string joint_variable_name_; /**< \brief The joint variable name */ + const moveit::core::JointModel* joint_model_; /**< \brief The joint from the kinematic model for this constraint */ + bool joint_is_continuous_; /**< \brief Whether or not the joint is continuous */ + std::string local_variable_name_; /**< \brief The local variable name for a multi DOF joint, if any */ + std::string joint_variable_name_; /**< \brief The joint variable name */ int joint_variable_index_; /**< \brief The index of the joint variable name in the full robot state */ double joint_position_, joint_tolerance_above_, joint_tolerance_below_; /**< \brief Position and tolerance values*/ }; -MOVEIT_CLASS_FORWARD(OrientationConstraint); +MOVEIT_CLASS_FORWARD(OrientationConstraint); // Defines OrientationConstraintPtr, ConstPtr, WeakPtr... etc /** * \brief Class for constraints on the orientation of a link @@ -340,7 +339,7 @@ MOVEIT_CLASS_FORWARD(OrientationConstraint); * This class expresses an orientation constraint on a particular * link. The constraint is specified in terms of a quaternion, with * tolerances on X,Y, and Z axes. The rotation difference is computed - * based on the ZXZ Euler angle formulation. The header on the + * based on the XYZ Euler angle formulation (intrinsic rotations). The header on the * quaternion can be specified in terms of either a fixed frame or a * mobile frame. The type value will return ORIENTATION_CONSTRAINT. * @@ -356,7 +355,8 @@ class OrientationConstraint : public KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - OrientationConstraint(const robot_model::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL) + OrientationConstraint(const moveit::core::RobotModelConstPtr& model) + : KinematicConstraint(model), link_model_(nullptr) { type_ = ORIENTATION_CONSTRAINT; } @@ -374,7 +374,7 @@ class OrientationConstraint : public KinematicConstraint * * @return True if constraint can be configured from oc */ - bool configure(const moveit_msgs::OrientationConstraint& oc, const robot_state::Transforms& tf); + bool configure(const moveit_msgs::OrientationConstraint& oc, const moveit::core::Transforms& tf); /** * \brief Check if two orientation constraints are the same. @@ -395,7 +395,7 @@ class OrientationConstraint : public KinematicConstraint bool equal(const KinematicConstraint& other, double margin) const override; void clear() override; - ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; bool enabled() const override; void print(std::ostream& out = std::cout) const override; @@ -405,7 +405,7 @@ class OrientationConstraint : public KinematicConstraint * * @return Returns the current link model */ - const robot_model::LinkModel* getLinkModel() const + const moveit::core::LinkModel* getLinkModel() const { return link_model_; } @@ -435,10 +435,13 @@ class OrientationConstraint : public KinematicConstraint /** * \brief The rotation target in the reference frame. * - * @return The target rotation + * @return The target rotation. + * + * The returned matrix is always a valid rotation matrix. */ const Eigen::Matrix3d& getDesiredRotationMatrix() const { + // validity of the rotation matrix is enforced in configure() return desired_rotation_matrix_; } @@ -476,18 +479,18 @@ class OrientationConstraint : public KinematicConstraint } protected: - const robot_model::LinkModel* link_model_; /**< \brief The target link model */ - Eigen::Matrix3d desired_rotation_matrix_; /**< \brief The desired rotation matrix in the tf frame */ + const moveit::core::LinkModel* link_model_; /**< \brief The target link model */ + Eigen::Matrix3d desired_rotation_matrix_; /**< \brief The desired rotation matrix in the tf frame. Guaranteed to + * be valid rotation matrix. */ Eigen::Matrix3d desired_rotation_matrix_inv_; /**< \brief The inverse of the desired rotation matrix, precomputed for - * efficiency - */ + * efficiency. Guaranteed to be valid rotation matrix. */ std::string desired_rotation_frame_id_; /**< \brief The target frame of the transform tree */ bool mobile_frame_; /**< \brief Whether or not the header frame is mobile or fixed */ double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_; /**< \brief Storage for the tolerances */ }; -MOVEIT_CLASS_FORWARD(PositionConstraint); +MOVEIT_CLASS_FORWARD(PositionConstraint); // Defines PositionConstraintPtr, ConstPtr, WeakPtr... etc /** * \brief Class for constraints on the XYZ position of a link @@ -513,7 +516,7 @@ class PositionConstraint : public KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - PositionConstraint(const robot_model::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL) + PositionConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(nullptr) { type_ = POSITION_CONSTRAINT; } @@ -534,7 +537,7 @@ class PositionConstraint : public KinematicConstraint * * @return True if constraint can be configured from pc */ - bool configure(const moveit_msgs::PositionConstraint& pc, const robot_state::Transforms& tf); + bool configure(const moveit_msgs::PositionConstraint& pc, const moveit::core::Transforms& tf); /** * \brief Check if two constraints are the same. For position @@ -562,7 +565,7 @@ class PositionConstraint : public KinematicConstraint bool equal(const KinematicConstraint& other, double margin) const override; void clear() override; - ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; bool enabled() const override; void print(std::ostream& out = std::cout) const override; @@ -572,7 +575,7 @@ class PositionConstraint : public KinematicConstraint * * @return The link model */ - const robot_model::LinkModel* getLinkModel() const + const moveit::core::LinkModel* getLinkModel() const { return link_model_; } @@ -642,13 +645,14 @@ class PositionConstraint : public KinematicConstraint Eigen::Vector3d offset_; /**< \brief The target offset */ bool has_offset_; /**< \brief Whether the offset is substantially different than 0.0 */ std::vector constraint_region_; /**< \brief The constraint region vector */ - EigenSTL::vector_Isometry3d constraint_region_pose_; /**< \brief The constraint region pose vector */ - bool mobile_frame_; /**< \brief Whether or not a mobile frame is employed*/ - std::string constraint_frame_id_; /**< \brief The constraint frame id */ - const robot_model::LinkModel* link_model_; /**< \brief The link model constraint subject */ + /** \brief The constraint region pose vector. All isometries are guaranteed to be valid. */ + EigenSTL::vector_Isometry3d constraint_region_pose_; + bool mobile_frame_; /**< \brief Whether or not a mobile frame is employed*/ + std::string constraint_frame_id_; /**< \brief The constraint frame id */ + const moveit::core::LinkModel* link_model_; /**< \brief The link model constraint subject */ }; -MOVEIT_CLASS_FORWARD(VisibilityConstraint); +MOVEIT_CLASS_FORWARD(VisibilityConstraint); // Defines VisibilityConstraintPtr, ConstPtr, WeakPtr... etc /** * \brief Class for constraints on the visibility relationship between @@ -758,7 +762,7 @@ class VisibilityConstraint : public KinematicConstraint * * @param [in] model The kinematic model used for constraint evaluation */ - VisibilityConstraint(const robot_model::RobotModelConstPtr& model); + VisibilityConstraint(const moveit::core::RobotModelConstPtr& model); /** * \brief Configure the constraint based on a @@ -772,7 +776,7 @@ class VisibilityConstraint : public KinematicConstraint * * @return True if constraint can be configured from vc */ - bool configure(const moveit_msgs::VisibilityConstraint& vc, const robot_state::Transforms& tf); + bool configure(const moveit_msgs::VisibilityConstraint& vc, const moveit::core::Transforms& tf); /** * \brief Check if two constraints are the same. @@ -801,7 +805,7 @@ class VisibilityConstraint : public KinematicConstraint * * @return The shape associated with the cone */ - shapes::Mesh* getVisibilityCone(const robot_state::RobotState& state) const; + shapes::Mesh* getVisibilityCone(const moveit::core::RobotState& state) const; /** * \brief Adds markers associated with the visibility cone, sensor @@ -814,10 +818,10 @@ class VisibilityConstraint : public KinematicConstraint * @param [in] state The state from which to produce the markers * @param [out] markers The marker array to which the markers will be added */ - void getMarkers(const robot_state::RobotState& state, visualization_msgs::MarkerArray& markers) const; + void getMarkers(const moveit::core::RobotState& state, visualization_msgs::MarkerArray& markers) const; bool enabled() const override; - ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; void print(std::ostream& out = std::cout) const override; protected: @@ -832,7 +836,7 @@ class VisibilityConstraint : public KinematicConstraint */ bool decideContact(const collision_detection::Contact& contact) const; - collision_detection::CollisionRobotPtr collision_robot_; /**< \brief A copy of the collision robot maintained for + collision_detection::CollisionEnvPtr collision_env_; /**< \brief A copy of the collision robot maintained for collision checking the cone against robot links */ bool mobile_sensor_frame_; /**< \brief True if the sensor is a non-fixed frame relative to the transform frame */ bool mobile_target_frame_; /**< \brief True if the target is a non-fixed frame relative to the transform frame */ @@ -848,7 +852,7 @@ class VisibilityConstraint : public KinematicConstraint double max_range_angle_; /**< \brief Storage for the max range angle */ }; -MOVEIT_CLASS_FORWARD(KinematicConstraintSet); +MOVEIT_CLASS_FORWARD(KinematicConstraintSet); // Defines KinematicConstraintSetPtr, ConstPtr, WeakPtr... etc /** * \brief A class that contains many different constraints, and can @@ -869,7 +873,7 @@ class KinematicConstraintSet * * @param [in] model The kinematic model used for constraint evaluation */ - KinematicConstraintSet(const robot_model::RobotModelConstPtr& model) : robot_model_(model) + KinematicConstraintSet(const moveit::core::RobotModelConstPtr& model) : robot_model_(model) { } @@ -891,7 +895,7 @@ class KinematicConstraintSet * KinematicConstraintSet can still be used even if the addition * returns false. */ - bool add(const moveit_msgs::Constraints& c, const robot_state::Transforms& tf); + bool add(const moveit_msgs::Constraints& c, const moveit::core::Transforms& tf); /** * \brief Add a vector of joint constraints @@ -909,7 +913,7 @@ class KinematicConstraintSet * * @return Will return true only if all constraints are valid, and false otherwise */ - bool add(const std::vector& pc, const robot_state::Transforms& tf); + bool add(const std::vector& pc, const moveit::core::Transforms& tf); /** * \brief Add a vector of orientation constraints @@ -918,7 +922,7 @@ class KinematicConstraintSet * * @return Will return true only if all constraints are valid, and false otherwise */ - bool add(const std::vector& oc, const robot_state::Transforms& tf); + bool add(const std::vector& oc, const moveit::core::Transforms& tf); /** * \brief Add a vector of visibility constraints @@ -927,7 +931,7 @@ class KinematicConstraintSet * * @return Will return true only if all constraints are valid, and false otherwise */ - bool add(const std::vector& vc, const robot_state::Transforms& tf); + bool add(const std::vector& vc, const moveit::core::Transforms& tf); /** * \brief Determines whether all constraints are satisfied by state, @@ -940,7 +944,7 @@ class KinematicConstraintSet * report satisfied only if all constraints are satisfied, and with * a distance that is the sum of all individual distances. */ - ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const; /** * @@ -960,7 +964,7 @@ class KinematicConstraintSet * report satisfied only if all constraints are satisfied, and with * a distance that is the sum of all individual distances. */ - ConstraintEvaluationResult decide(const robot_state::RobotState& state, + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, std::vector& results, bool verbose = false) const; /** @@ -1054,7 +1058,7 @@ class KinematicConstraintSet } protected: - robot_model::RobotModelConstPtr robot_model_; /**< \brief The kinematic model used for by the Set */ + moveit::core::RobotModelConstPtr robot_model_; /**< \brief The kinematic model used for by the Set */ std::vector kinematic_constraints_; /**< \brief Shared pointers to all the member constraints */ @@ -1068,6 +1072,4 @@ class KinematicConstraintSet internal visibility constraints */ moveit_msgs::Constraints all_constraints_; /**< \brief Messages corresponding to all internal constraints */ }; -} - -#endif +} // namespace kinematic_constraints diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h index e5e9e5f74f..7f25af5a36 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h @@ -34,10 +34,9 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_ -#define MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_ +#pragma once -#include +#include #include #include #include @@ -64,11 +63,10 @@ namespace kinematic_constraints * * @return The merged set of constraints */ -moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints& first, - const moveit_msgs::Constraints& second); +moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints& first, const moveit_msgs::Constraints& second); /** \brief Check if any constraints were specified */ -bool isEmpty(const moveit_msgs::Constraints& constr); +[[deprecated("Use moveit/utils/message_checks.h instead")]] bool isEmpty(const moveit_msgs::Constraints& constr); std::size_t countIndividualConstraints(const moveit_msgs::Constraints& constr); @@ -84,8 +82,8 @@ std::size_t countIndividualConstraints(const moveit_msgs::Constraints& constr); * * @return A full constraint message containing all the joint constraints */ -moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state, - const robot_model::JointModelGroup* jmg, double tolerance_below, +moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg, double tolerance_below, double tolerance_above); /** @@ -99,8 +97,8 @@ moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& * * @return A full constraint message containing all the joint constraints */ -moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state, - const robot_model::JointModelGroup* jmg, +moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg, double tolerance = std::numeric_limits::epsilon()); /** @@ -214,6 +212,5 @@ bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::Constraints& * @param [in] state The RobotState used to resolve frames. * @param [in] constraints The constraint to resolve. */ -bool resolveConstraintFrames(const robot_state::RobotState& state, moveit_msgs::Constraints& constraints); -} -#endif +bool resolveConstraintFrames(const moveit::core::RobotState& state, moveit_msgs::Constraints& constraints); +} // namespace kinematic_constraints diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 526fe4e3d3..9a63c5c3b4 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -38,8 +38,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -58,7 +58,59 @@ static double normalizeAngle(double angle) return v; } -KinematicConstraint::KinematicConstraint(const robot_model::RobotModelConstPtr& model) +// Normalizes an angle to the interval [-pi, +pi] and then take the absolute value +// The returned values will be in the following range [0, +pi] +static double normalizeAbsoluteAngle(const double angle) +{ + const double normalized_angle = std::fmod(std::abs(angle), 2 * M_PI); + return std::min(2 * M_PI - normalized_angle, normalized_angle); +} + +/** + * This's copied from + * https://gitlab.com/libeigen/eigen/-/blob/master/unsupported/Eigen/src/EulerAngles/EulerSystem.h#L187 + * Return the intrinsic Roll-Pitch-Yaw euler angles given the input rotation matrix and boolean indicating whether the + * there's a singularity in the input rotation matrix (true: the input rotation matrix don't have a singularity, false: + * the input rotation matrix have a singularity) The returned angles are in the ranges [-pi:pi]x[-pi/2:pi/2]x[-pi:pi] + */ +template +std::tuple::Scalar, 3, 1>, bool> +CalcEulerAngles(const Eigen::MatrixBase& R) +{ + using std::atan2; + using std::sqrt; + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) + using Index = EIGEN_DEFAULT_DENSE_INDEX_TYPE; + using Scalar = typename Eigen::MatrixBase::Scalar; + const Index i = 0; + const Index j = 1; + const Index k = 2; + Eigen::Matrix res; + const Scalar rsum = sqrt((R(i, i) * R(i, i) + R(i, j) * R(i, j) + R(j, k) * R(j, k) + R(k, k) * R(k, k)) / 2); + res[1] = atan2(R(i, k), rsum); + // There is a singularity when cos(beta) == 0 + if (rsum > 4 * Eigen::NumTraits::epsilon()) + { // cos(beta) != 0 + res[0] = atan2(-R(j, k), R(k, k)); + res[2] = atan2(-R(i, j), R(i, i)); + return { res, true }; + } + else if (R(i, k) > 0) + { // cos(beta) == 0 and sin(beta) == 1 + const Scalar spos = R(j, i) + R(k, j); // 2*sin(alpha + gamma) + const Scalar cpos = R(j, j) - R(k, i); // 2*cos(alpha + gamma) + res[0] = atan2(spos, cpos); + res[2] = 0; + return { res, false }; + } // cos(beta) == 0 and sin(beta) == -1 + const Scalar sneg = R(k, j) - R(j, i); // 2*sin(alpha + gamma) + const Scalar cneg = R(j, j) + R(k, i); // 2*cos(alpha + gamma) + res[0] = atan2(sneg, cneg); + res[2] = 0; + return { res, false }; +} + +KinematicConstraint::KinematicConstraint(const moveit::core::RobotModelConstPtr& model) : type_(UNKNOWN_CONSTRAINT), robot_model_(model), constraint_weight_(std::numeric_limits::epsilon()) { } @@ -107,8 +159,9 @@ bool JointConstraint::configure(const moveit_msgs::JointConstraint& jc) } else if (joint_model_->getVariableCount() > 1) { - ROS_ERROR_NAMED("kinematic_constraints", "Joint '%s' has more than one parameter to constrain. " - "This type of constraint is not supported.", + ROS_ERROR_NAMED("kinematic_constraints", + "Joint '%s' has more than one parameter to constrain. " + "This type of constraint is not supported.", jc.joint_name.c_str()); joint_model_ = nullptr; } @@ -141,13 +194,14 @@ bool JointConstraint::configure(const moveit_msgs::JointConstraint& jc) // check if we have to wrap angles when computing distances joint_is_continuous_ = false; - if (joint_model_->getType() == robot_model::JointModel::REVOLUTE) + if (joint_model_->getType() == moveit::core::JointModel::REVOLUTE) { - const robot_model::RevoluteJointModel* rjoint = static_cast(joint_model_); + const moveit::core::RevoluteJointModel* rjoint = + static_cast(joint_model_); if (rjoint->isContinuous()) joint_is_continuous_ = true; } - else if (joint_model_->getType() == robot_model::JointModel::PLANAR) + else if (joint_model_->getType() == moveit::core::JointModel::PLANAR) { if (local_variable_name_ == "theta") joint_is_continuous_ = true; @@ -160,22 +214,24 @@ bool JointConstraint::configure(const moveit_msgs::JointConstraint& jc) else { joint_position_ = jc.position; - const robot_model::VariableBounds& bounds = joint_model_->getVariableBounds(joint_variable_name_); + const moveit::core::VariableBounds& bounds = joint_model_->getVariableBounds(joint_variable_name_); if (bounds.min_position_ > joint_position_ + joint_tolerance_above_) { joint_position_ = bounds.min_position_; joint_tolerance_above_ = std::numeric_limits::epsilon(); - ROS_WARN_NAMED("kinematic_constraints", "Joint %s is constrained to be below the minimum bounds. " - "Assuming minimum bounds instead.", + ROS_WARN_NAMED("kinematic_constraints", + "Joint %s is constrained to be below the minimum bounds. " + "Assuming minimum bounds instead.", jc.joint_name.c_str()); } else if (bounds.max_position_ < joint_position_ - joint_tolerance_below_) { joint_position_ = bounds.max_position_; joint_tolerance_below_ = std::numeric_limits::epsilon(); - ROS_WARN_NAMED("kinematic_constraints", "Joint %s is constrained to be above the maximum bounds. " - "Assuming maximum bounds instead.", + ROS_WARN_NAMED("kinematic_constraints", + "Joint %s is constrained to be above the maximum bounds. " + "Assuming maximum bounds instead.", jc.joint_name.c_str()); } } @@ -205,7 +261,7 @@ bool JointConstraint::equal(const KinematicConstraint& other, double margin) con return false; } -ConstraintEvaluationResult JointConstraint::decide(const robot_state::RobotState& state, bool verbose) const +ConstraintEvaluationResult JointConstraint::decide(const moveit::core::RobotState& state, bool verbose) const { if (!joint_model_) return ConstraintEvaluationResult(true, 0.0); @@ -230,8 +286,9 @@ ConstraintEvaluationResult JointConstraint::decide(const robot_state::RobotState bool result = dif <= (joint_tolerance_above_ + 2.0 * std::numeric_limits::epsilon()) && dif >= (-joint_tolerance_below_ - 2.0 * std::numeric_limits::epsilon()); if (verbose) - ROS_INFO_NAMED("kinematic_constraints", "Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, " - "tolerance_above: %f, tolerance_below: %f", + ROS_INFO_NAMED("kinematic_constraints", + "Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, " + "tolerance_above: %f, tolerance_below: %f", result ? "satisfied" : "violated", joint_variable_name_.c_str(), current_joint_position, joint_position_, joint_tolerance_above_, joint_tolerance_below_); return ConstraintEvaluationResult(result, constraint_weight_ * fabs(dif)); @@ -269,7 +326,7 @@ void JointConstraint::print(std::ostream& out) const out << "No constraint" << std::endl; } -bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, const robot_state::Transforms& tf) +bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, const moveit::core::Transforms& tf) { // clearing before we configure to get rid of any old data clear(); @@ -315,17 +372,18 @@ bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, co ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses"); continue; } - constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get()))); Eigen::Isometry3d t; tf2::fromMsg(pc.constraint_region.primitive_poses[i], t); + ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry constraint_region_pose_.push_back(t); - if (mobile_frame_) - constraint_region_.back()->setPose(constraint_region_pose_.back()); - else - { + if (!mobile_frame_) tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back()); - constraint_region_.back()->setPose(constraint_region_pose_.back()); - } + + const bodies::BodyPtr body(bodies::createEmptyBodyFromShapeType(shape->type)); + body->setDimensionsDirty(shape.get()); + body->setPoseDirty(constraint_region_pose_.back()); + body->updateInternalData(); + constraint_region_.push_back(body); } else ROS_WARN_NAMED("kinematic_constraints", "Could not construct primitive shape %zu", i); @@ -342,17 +400,17 @@ bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, co ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses"); continue; } - constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get()))); Eigen::Isometry3d t; tf2::fromMsg(pc.constraint_region.mesh_poses[i], t); + ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry constraint_region_pose_.push_back(t); - if (mobile_frame_) - constraint_region_.back()->setPose(constraint_region_pose_.back()); - else - { + if (!mobile_frame_) tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back()); - constraint_region_.back()->setPose(constraint_region_pose_.back()); - } + const bodies::BodyPtr body(bodies::createEmptyBodyFromShapeType(shape->type)); + body->setDimensionsDirty(shape.get()); + body->setPoseDirty(constraint_region_pose_.back()); + body->updateInternalData(); + constraint_region_.push_back(body); } else { @@ -379,7 +437,7 @@ bool PositionConstraint::equal(const KinematicConstraint& other, double margin) return false; const PositionConstraint& o = static_cast(other); - if (link_model_ == o.link_model_ && robot_state::Transforms::sameFrame(constraint_frame_id_, o.constraint_frame_id_)) + if (link_model_ == o.link_model_ && moveit::core::Transforms::sameFrame(constraint_frame_id_, o.constraint_frame_id_)) { if ((offset_ - o.offset_).norm() > margin) return false; @@ -390,8 +448,9 @@ bool PositionConstraint::equal(const KinematicConstraint& other, double margin) // need to check against all other regions for (std::size_t j = 0; j < o.constraint_region_.size(); ++j) { + // constraint_region_pose_ contain only valid isometries, so diff is also a valid isometry Eigen::Isometry3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[j]; - if (diff.translation().norm() < margin && diff.rotation().isIdentity(margin) && + if (diff.translation().norm() < margin && diff.linear().isIdentity(margin) && constraint_region_[i]->getType() == o.constraint_region_[j]->getType() && fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[j]->computeVolume()) < margin) { @@ -422,15 +481,16 @@ static inline ConstraintEvaluationResult finishPositionConstraintDecision(const double dz = desired.z() - pt.z(); if (verbose) { - ROS_INFO_NAMED( - "kinematic_constraints", "Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f", - result ? "satisfied" : "violated", name.c_str(), desired.x(), desired.y(), desired.z(), pt.x(), pt.y(), pt.z()); + ROS_INFO_NAMED("kinematic_constraints", + "Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f", + result ? "satisfied" : "violated", name.c_str(), desired.x(), desired.y(), desired.z(), pt.x(), + pt.y(), pt.z()); ROS_INFO_NAMED("kinematic_constraints", "Differences %g %g %g", dx, dy, dz); } return ConstraintEvaluationResult(result, weight * sqrt(dx * dx + dy * dy + dz * dz)); } -ConstraintEvaluationResult PositionConstraint::decide(const robot_state::RobotState& state, bool verbose) const +ConstraintEvaluationResult PositionConstraint::decide(const moveit::core::RobotState& state, bool verbose) const { if (!link_model_ || constraint_region_.empty()) return ConstraintEvaluationResult(true, 0.0); @@ -490,7 +550,7 @@ bool PositionConstraint::enabled() const return link_model_ && !constraint_region_.empty(); } -bool OrientationConstraint::configure(const moveit_msgs::OrientationConstraint& oc, const robot_state::Transforms& tf) +bool OrientationConstraint::configure(const moveit_msgs::OrientationConstraint& oc, const moveit::core::Transforms& tf) { // clearing out any old data clear(); @@ -505,8 +565,9 @@ bool OrientationConstraint::configure(const moveit_msgs::OrientationConstraint& tf2::fromMsg(oc.orientation, q); if (fabs(q.norm() - 1.0) > 1e-3) { - ROS_WARN_NAMED("kinematic_constraints", "Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, " - "%f. Assuming identity instead.", + ROS_WARN_NAMED("kinematic_constraints", + "Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, " + "%f. Assuming identity instead.", oc.link_name.c_str(), oc.orientation.x, oc.orientation.y, oc.orientation.z, oc.orientation.w); q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); } @@ -563,7 +624,7 @@ bool OrientationConstraint::equal(const KinematicConstraint& other, double margi const OrientationConstraint& o = static_cast(other); if (o.link_model_ == link_model_ && - robot_state::Transforms::sameFrame(desired_rotation_frame_id_, o.desired_rotation_frame_id_)) + moveit::core::Transforms::sameFrame(desired_rotation_frame_id_, o.desired_rotation_frame_id_)) { if (!desired_rotation_matrix_.isApprox(o.desired_rotation_matrix_)) return false; @@ -589,36 +650,53 @@ bool OrientationConstraint::enabled() const return link_model_; } -ConstraintEvaluationResult OrientationConstraint::decide(const robot_state::RobotState& state, bool verbose) const +ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::RobotState& state, bool verbose) const { if (!link_model_) return ConstraintEvaluationResult(true, 0.0); - Eigen::Vector3d xyz; + std::tuple euler_angles_error; if (mobile_frame_) { - Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).rotation() * desired_rotation_matrix_; - Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).rotation()); - xyz = diff.rotation().eulerAngles(0, 1, 2); - // 0,1,2 corresponds to XYZ, the convention used in sampling constraints + // getFrameTransform() returns a valid isometry by contract + Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).linear() * desired_rotation_matrix_; + // getGlobalLinkTransform() returns a valid isometry by contract + Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).linear()); // valid isometry + euler_angles_error = CalcEulerAngles(diff.linear()); } else { - Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).rotation()); - xyz = - diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints + // diff is valid isometry by construction + Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).linear()); + euler_angles_error = CalcEulerAngles(diff.linear()); + } + + // Converting from a rotation matrix to an intrinsic XYZ euler angles have 2 singularities: + // pitch ~= pi/2 ==> roll + yaw = theta + // pitch ~= -pi/2 ==> roll - yaw = theta + // in those cases CalcEulerAngles will set roll (xyz(0)) to theta and yaw (xyz(2)) to zero, so for us to be able to + // capture yaw tolerance violation we do the following, if theta violate the absolute yaw tolerance we think of it as + // pure yaw rotation and set roll to zero + auto& xyz = std::get(euler_angles_error); + if (!std::get(euler_angles_error)) + { + if (normalizeAbsoluteAngle(xyz(0)) > absolute_z_axis_tolerance_ + std::numeric_limits::epsilon()) + { + xyz(2) = xyz(0); + xyz(0) = 0; + } } + // Account for angle wrapping + xyz = xyz.unaryExpr(&normalizeAbsoluteAngle); - xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi() - fabs(xyz(0))); - xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi() - fabs(xyz(1))); - xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi() - fabs(xyz(2))); + // 0,1,2 corresponds to XYZ, the convention used in sampling constraints bool result = xyz(2) < absolute_z_axis_tolerance_ + std::numeric_limits::epsilon() && xyz(1) < absolute_y_axis_tolerance_ + std::numeric_limits::epsilon() && xyz(0) < absolute_x_axis_tolerance_ + std::numeric_limits::epsilon(); if (verbose) { - Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).rotation()); + Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).linear()); Eigen::Quaterniond q_des(desired_rotation_matrix_); ROS_INFO_NAMED("kinematic_constraints", "Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion " @@ -643,8 +721,8 @@ void OrientationConstraint::print(std::ostream& out) const out << "No constraint" << std::endl; } -VisibilityConstraint::VisibilityConstraint(const robot_model::RobotModelConstPtr& model) - : KinematicConstraint(model), collision_robot_(new collision_detection::CollisionRobotFCL(model)) +VisibilityConstraint::VisibilityConstraint(const moveit::core::RobotModelConstPtr& model) + : KinematicConstraint(model), collision_env_(new collision_detection::CollisionEnvFCL(model)) { type_ = VISIBILITY_CONSTRAINT; } @@ -665,7 +743,7 @@ void VisibilityConstraint::clear() max_range_angle_ = 0.0; } -bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc, const robot_state::Transforms& tf) +bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc, const moveit::core::Transforms& tf) { clear(); target_radius_ = fabs(vc.target_radius); @@ -676,8 +754,9 @@ bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc if (vc.cone_sides < 3) { - ROS_WARN_NAMED("kinematic_constraints", "The number of sides for the visibility region must be 3 or more. " - "Assuming 3 sides instead of the specified %d", + ROS_WARN_NAMED("kinematic_constraints", + "The number of sides for the visibility region must be 3 or more. " + "Assuming 3 sides instead of the specified %d", vc.cone_sides); cone_sides_ = 3; } @@ -696,6 +775,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc } tf2::fromMsg(vc.target_pose.pose, target_pose_); + ASSERT_ISOMETRY(target_pose_) // unsanitized input, could contain a non-isometry if (tf.isFixedFrame(vc.target_pose.header.frame_id)) { @@ -713,6 +793,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc } tf2::fromMsg(vc.sensor_pose.pose, sensor_pose_); + ASSERT_ISOMETRY(sensor_pose_) // unsanitized input, could contain a non-isometry if (tf.isFixedFrame(vc.sensor_pose.header.frame_id)) { @@ -747,21 +828,23 @@ bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin return false; const VisibilityConstraint& o = static_cast(other); - if (robot_state::Transforms::sameFrame(target_frame_id_, o.target_frame_id_) && - robot_state::Transforms::sameFrame(sensor_frame_id_, o.sensor_frame_id_) && cone_sides_ == o.cone_sides_ && + if (moveit::core::Transforms::sameFrame(target_frame_id_, o.target_frame_id_) && + moveit::core::Transforms::sameFrame(sensor_frame_id_, o.sensor_frame_id_) && cone_sides_ == o.cone_sides_ && sensor_view_direction_ == o.sensor_view_direction_) { if (fabs(max_view_angle_ - o.max_view_angle_) > margin || fabs(target_radius_ - o.target_radius_) > margin) return false; - Eigen::Isometry3d diff = sensor_pose_.inverse() * o.sensor_pose_; + // sensor_pose_ is valid isometry, checked in configure() + Eigen::Isometry3d diff = sensor_pose_.inverse() * o.sensor_pose_; // valid isometry if (diff.translation().norm() > margin) return false; - if (!diff.rotation().isIdentity(margin)) + if (!diff.linear().isIdentity(margin)) return false; - diff = target_pose_.inverse() * o.target_pose_; + // target_pose_ is valid isometry, checked in configure() + diff = target_pose_.inverse() * o.target_pose_; // valid isometry if (diff.translation().norm() > margin) return false; - if (!diff.rotation().isIdentity(margin)) + if (!diff.linear().isIdentity(margin)) return false; return true; } @@ -773,7 +856,7 @@ bool VisibilityConstraint::enabled() const return target_radius_ > std::numeric_limits::epsilon(); } -shapes::Mesh* VisibilityConstraint::getVisibilityCone(const robot_state::RobotState& state) const +shapes::Mesh* VisibilityConstraint::getVisibilityCone(const moveit::core::RobotState& state) const { // the current pose of the sensor @@ -847,7 +930,7 @@ shapes::Mesh* VisibilityConstraint::getVisibilityCone(const robot_state::RobotSt return m; } -void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, +void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state, visualization_msgs::MarkerArray& markers) const { shapes::Mesh* m = getVisibilityCone(state); @@ -876,8 +959,11 @@ void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, markers.markers.push_back(mk); + // getFrameTransform() returns a valid isometry by contract + // sensor_pose_ is valid isometry (checked in configure()) const Eigen::Isometry3d& sp = mobile_sensor_frame_ ? state.getFrameTransform(sensor_frame_id_) * sensor_pose_ : sensor_pose_; + // target_pose_ is valid isometry (checked in configure()) const Eigen::Isometry3d& tp = mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_; @@ -895,7 +981,7 @@ void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, mka.scale.y = .15; mka.scale.z = 0.0; mka.points.resize(2); - Eigen::Vector3d d = tp.translation() + tp.rotation().col(2) * 0.5; + Eigen::Vector3d d = tp.translation() + tp.linear().col(2) * 0.5; mka.points[0].x = tp.translation().x(); mka.points[0].y = tp.translation().y(); mka.points[0].z = tp.translation().z(); @@ -908,7 +994,7 @@ void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, mka.color.b = 1.0; mka.color.r = 0.0; - d = sp.translation() + sp.rotation().col(2 - sensor_view_direction_) * 0.5; + d = sp.translation() + sp.linear().col(2 - sensor_view_direction_) * 0.5; mka.points[0].x = sp.translation().x(); mka.points[0].y = sp.translation().y(); mka.points[0].z = sp.translation().z(); @@ -919,24 +1005,27 @@ void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, markers.markers.push_back(mka); } -ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::RobotState& state, bool verbose) const +ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::RobotState& state, bool verbose) const { if (target_radius_ <= std::numeric_limits::epsilon()) return ConstraintEvaluationResult(true, 0.0); if (max_view_angle_ > 0.0 || max_range_angle_ > 0.0) { + // getFrameTransform() returns a valid isometry by contract + // sensor_pose_ is valid isometry (checked in configure()) const Eigen::Isometry3d& sp = mobile_sensor_frame_ ? state.getFrameTransform(sensor_frame_id_) * sensor_pose_ : sensor_pose_; + // target_pose_ is valid isometry (checked in configure()) const Eigen::Isometry3d& tp = mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_; // necessary to do subtraction as SENSOR_Z is 0 and SENSOR_X is 2 - const Eigen::Vector3d& normal2 = sp.rotation().col(2 - sensor_view_direction_); + const Eigen::Vector3d& normal2 = sp.linear().col(2 - sensor_view_direction_); if (max_view_angle_ > 0.0) { - const Eigen::Vector3d& normal1 = tp.rotation().col(2) * -1.0; // along Z axis and inverted + const Eigen::Vector3d& normal1 = tp.linear().col(2) * -1.0; // along Z axis and inverted double dp = normal2.dot(normal1); double ang = acos(dp); if (dp < 0.0) @@ -949,8 +1038,9 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot if (max_view_angle_ < ang) { if (verbose) - ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the view angle is %lf " - "(above the maximum allowed of %lf)", + ROS_INFO_NAMED("kinematic_constraints", + "Visibility constraint is violated because the view angle is %lf " + "(above the maximum allowed of %lf)", ang, max_view_angle_); return ConstraintEvaluationResult(false, 0.0); } @@ -971,8 +1061,9 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot if (max_range_angle_ < ang) { if (verbose) - ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the range angle is %lf " - "(above the maximum allowed of %lf)", + ROS_INFO_NAMED("kinematic_constraints", + "Visibility constraint is violated because the range angle is %lf " + "(above the maximum allowed of %lf)", ang, max_range_angle_); return ConstraintEvaluationResult(false, 0.0); } @@ -984,8 +1075,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot return ConstraintEvaluationResult(false, 0.0); // add the visibility cone as an object - collision_detection::CollisionWorldFCL collision_world; - collision_world.getWorld()->addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Isometry3d::Identity()); + collision_env_->getWorld()->addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Isometry3d::Identity()); // check for collisions between the robot and the cone collision_detection::CollisionRequest req; @@ -995,7 +1085,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot req.contacts = true; req.verbose = verbose; req.max_contacts = 1; - collision_world.checkRobotCollision(req, res, *collision_robot_, state, acm); + collision_env_->checkRobotCollision(req, res, state, acm); if (verbose) { @@ -1005,6 +1095,8 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot res.collision ? "not " : "", ss.str().c_str()); } + collision_env_->getWorld()->removeObject("cone"); + return ConstraintEvaluationResult(!res.collision, res.collision ? res.contacts.begin()->second.front().depth : 0.0); } @@ -1015,16 +1107,16 @@ bool VisibilityConstraint::decideContact(const collision_detection::Contact& con return true; if (contact.body_type_1 == collision_detection::BodyTypes::ROBOT_LINK && contact.body_type_2 == collision_detection::BodyTypes::WORLD_OBJECT && - (robot_state::Transforms::sameFrame(contact.body_name_1, sensor_frame_id_) || - robot_state::Transforms::sameFrame(contact.body_name_1, target_frame_id_))) + (moveit::core::Transforms::sameFrame(contact.body_name_1, sensor_frame_id_) || + moveit::core::Transforms::sameFrame(contact.body_name_1, target_frame_id_))) { ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target"); return true; } if (contact.body_type_2 == collision_detection::BodyTypes::ROBOT_LINK && contact.body_type_1 == collision_detection::BodyTypes::WORLD_OBJECT && - (robot_state::Transforms::sameFrame(contact.body_name_2, sensor_frame_id_) || - robot_state::Transforms::sameFrame(contact.body_name_2, target_frame_id_))) + (moveit::core::Transforms::sameFrame(contact.body_name_2, sensor_frame_id_) || + moveit::core::Transforms::sameFrame(contact.body_name_2, target_frame_id_))) { ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target"); return true; @@ -1070,7 +1162,7 @@ bool KinematicConstraintSet::add(const std::vector } bool KinematicConstraintSet::add(const std::vector& pc, - const robot_state::Transforms& tf) + const moveit::core::Transforms& tf) { bool result = true; for (const moveit_msgs::PositionConstraint& position_constraint : pc) @@ -1086,7 +1178,7 @@ bool KinematicConstraintSet::add(const std::vector& oc, - const robot_state::Transforms& tf) + const moveit::core::Transforms& tf) { bool result = true; for (const moveit_msgs::OrientationConstraint& orientation_constraint : oc) @@ -1102,7 +1194,7 @@ bool KinematicConstraintSet::add(const std::vector& vc, - const robot_state::Transforms& tf) + const moveit::core::Transforms& tf) { bool result = true; for (const moveit_msgs::VisibilityConstraint& visibility_constraint : vc) @@ -1117,7 +1209,7 @@ bool KinematicConstraintSet::add(const std::vector& results, bool verbose) const { diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 568efbb95b..11ef55a56c 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -38,6 +38,8 @@ #include #include #include +#include +#include using namespace moveit::core; @@ -116,8 +118,7 @@ moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints& first, bool isEmpty(const moveit_msgs::Constraints& constr) { - return constr.position_constraints.empty() && constr.orientation_constraints.empty() && - constr.visibility_constraints.empty() && constr.joint_constraints.empty(); + return moveit::core::isEmpty(constr); } std::size_t countIndividualConstraints(const moveit_msgs::Constraints& constr) @@ -126,14 +127,14 @@ std::size_t countIndividualConstraints(const moveit_msgs::Constraints& constr) constr.visibility_constraints.size() + constr.joint_constraints.size(); } -moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state, - const robot_model::JointModelGroup* jmg, double tolerance) +moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg, double tolerance) { return constructGoalConstraints(state, jmg, tolerance, tolerance); } -moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state, - const robot_model::JointModelGroup* jmg, double tolerance_below, +moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg, double tolerance_below, double tolerance_above) { moveit_msgs::Constraints goal; @@ -166,7 +167,7 @@ moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name, pcm.constraint_region.primitives.resize(1); shape_msgs::SolidPrimitive& bv = pcm.constraint_region.primitives[0]; bv.type = shape_msgs::SolidPrimitive::SPHERE; - bv.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); + bv.dimensions.resize(geometric_shapes::solidPrimitiveDimCount()); bv.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance_pos; pcm.header = pose.header; @@ -202,7 +203,7 @@ moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name, { shape_msgs::SolidPrimitive& bv = goal.position_constraints[0].constraint_region.primitives[0]; bv.type = shape_msgs::SolidPrimitive::BOX; - bv.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); + bv.dimensions.resize(geometric_shapes::solidPrimitiveDimCount()); bv.dimensions[shape_msgs::SolidPrimitive::BOX_X] = tolerance_pos[0]; bv.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = tolerance_pos[1]; bv.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = tolerance_pos[2]; @@ -257,7 +258,7 @@ moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name, pcm.constraint_region.primitives.resize(1); pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE; pcm.constraint_region.primitives[0].dimensions.resize( - geometric_shapes::SolidPrimitiveDimCount::value); + geometric_shapes::solidPrimitiveDimCount()); pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance; pcm.header = goal_point.header; @@ -529,7 +530,7 @@ bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::Constraints& } } // namespace kinematic_constraints -bool kinematic_constraints::resolveConstraintFrames(const robot_state::RobotState& state, +bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotState& state, moveit_msgs::Constraints& constraints) { for (auto& c : constraints.position_constraints) @@ -540,43 +541,38 @@ bool kinematic_constraints::resolveConstraintFrames(const robot_state::RobotStat if (!frame_found) return false; - // If the frame of the constraint is not part of the robot link model (but is an - // attached body or subframe instead), the constraint needs to be expressed in - // the frame of a robot link. + // If the frame of the constraint is not part of the robot link model (but an attached body or subframe), + // the constraint needs to be expressed in the frame of a robot link. if (c.link_name != robot_link->getName()) { - Eigen::Vector3d pos_in_link_frame, - pos_in_original_frame(c.target_point_offset.x, c.target_point_offset.y, c.target_point_offset.z); + Eigen::Isometry3d robot_link_to_link_name = state.getGlobalLinkTransform(robot_link).inverse() * transform; + Eigen::Vector3d offset_link_name(c.target_point_offset.x, c.target_point_offset.y, c.target_point_offset.z); + Eigen::Vector3d offset_robot_link = robot_link_to_link_name * offset_link_name; - pos_in_link_frame = transform * pos_in_original_frame; c.link_name = robot_link->getName(); - c.target_point_offset.x = pos_in_link_frame[0]; - c.target_point_offset.y = pos_in_link_frame[1]; - c.target_point_offset.z = pos_in_link_frame[2]; + tf::vectorEigenToMsg(offset_robot_link, c.target_point_offset); } } + for (auto& c : constraints.orientation_constraints) { bool frame_found; const moveit::core::LinkModel* robot_link; + // getFrameInfo() returns a valid isometry by contract const Eigen::Isometry3d& transform = state.getFrameInfo(c.link_name, robot_link, frame_found); if (!frame_found) return false; - // If the frame of the constraint is not part of the robot link model (but is an - // attached body or subframe instead), the constraint needs to be expressed in - // the frame of a robot link. + // If the frame of the constraint is not part of the robot link model (but an attached body or subframe), + // the constraint needs to be expressed in the frame of a robot link. if (c.link_name != robot_link->getName()) { - Eigen::Quaterniond q_body_to_link(transform.inverse().rotation()); - Eigen::Quaterniond q_target(c.orientation.w, c.orientation.x, c.orientation.y, c.orientation.z); - Eigen::Quaterniond q_in_link = q_body_to_link * q_target; - c.link_name = robot_link->getName(); - c.orientation.x = q_in_link.x(); - c.orientation.y = q_in_link.y(); - c.orientation.z = q_in_link.z(); - c.orientation.w = q_in_link.w(); + Eigen::Quaterniond link_name_to_robot_link(transform.linear().transpose() * + state.getGlobalLinkTransform(robot_link).linear()); + Eigen::Quaterniond quat_target; + tf::quaternionMsgToEigen(c.orientation, quat_target); + tf::quaternionEigenToMsg(quat_target * link_name_to_robot_link, c.orientation); } } return true; diff --git a/moveit_core/kinematic_constraints/test/test_constraints.cpp b/moveit_core/kinematic_constraints/test/test_constraints.cpp index 4c6cd14eba..2dcb7acbf9 100644 --- a/moveit_core/kinematic_constraints/test/test_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_constraints.cpp @@ -40,6 +40,7 @@ #include #include #include +#include class LoadPlanningModelsPr2 : public testing::Test { @@ -54,14 +55,14 @@ class LoadPlanningModelsPr2 : public testing::Test } protected: - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; }; TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::JointConstraint jc(robot_model_); moveit_msgs::JointConstraint jcm; @@ -174,10 +175,10 @@ TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple) TEST_F(LoadPlanningModelsPr2, JointConstraintsCont) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::JointConstraint jc(robot_model_); moveit_msgs::JointConstraint jcm; @@ -305,7 +306,7 @@ TEST_F(LoadPlanningModelsPr2, JointConstraintsCont) TEST_F(LoadPlanningModelsPr2, JointConstraintsMultiDOF) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); kinematic_constraints::JointConstraint jc(robot_model_); @@ -356,10 +357,10 @@ TEST_F(LoadPlanningModelsPr2, JointConstraintsMultiDOF) TEST_F(LoadPlanningModelsPr2, PositionConstraintsFixed) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(true); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::PositionConstraint pc(robot_model_); moveit_msgs::PositionConstraint pcm; @@ -432,9 +433,9 @@ TEST_F(LoadPlanningModelsPr2, PositionConstraintsFixed) TEST_F(LoadPlanningModelsPr2, PositionConstraintsMobile) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); robot_state.update(); kinematic_constraints::PositionConstraint pc(robot_model_); @@ -509,9 +510,9 @@ TEST_F(LoadPlanningModelsPr2, PositionConstraintsMobile) TEST_F(LoadPlanningModelsPr2, PositionConstraintsEquality) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::PositionConstraint pc(robot_model_); kinematic_constraints::PositionConstraint pc2(robot_model_); @@ -595,10 +596,10 @@ TEST_F(LoadPlanningModelsPr2, PositionConstraintsEquality) TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::OrientationConstraint oc(robot_model_); @@ -652,14 +653,20 @@ TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple) robot_state.setVariablePositions(jvals); robot_state.update(); EXPECT_FALSE(oc.decide(robot_state).satisfied); + + // rotation by pi does not wrap to zero + jvals["r_wrist_roll_joint"] = boost::math::constants::pi(); + robot_state.setVariablePositions(jvals); + robot_state.update(); + EXPECT_FALSE(oc.decide(robot_state).satisfied); } TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::VisibilityConstraint vc(robot_model_); moveit_msgs::VisibilityConstraint vcm; @@ -694,23 +701,23 @@ TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple) // very slight angle, so still ok vcm.target_pose.pose.orientation.y = 0.03; - vcm.target_pose.pose.orientation.w = .9995; + vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2)); EXPECT_TRUE(vc.configure(vcm, tf)); EXPECT_TRUE(vc.decide(robot_state, true).satisfied); // a little bit more puts it over vcm.target_pose.pose.orientation.y = 0.06; - vcm.target_pose.pose.orientation.w = .9981; + vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2)); EXPECT_TRUE(vc.configure(vcm, tf)); EXPECT_FALSE(vc.decide(robot_state, true).satisfied); } TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsPR2) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); robot_state.update(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::VisibilityConstraint vc(robot_model_); moveit_msgs::VisibilityConstraint vcm; @@ -781,9 +788,9 @@ TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsPR2) TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSet) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::KinematicConstraintSet kcs(robot_model_); EXPECT_TRUE(kcs.empty()); @@ -847,9 +854,9 @@ TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSet) TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSetEquality) { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); - robot_state::Transforms tf(robot_model_->getModelFrame()); + moveit::core::Transforms tf(robot_model_->getModelFrame()); kinematic_constraints::KinematicConstraintSet kcs(robot_model_); kinematic_constraints::KinematicConstraintSet kcs2(robot_model_); diff --git a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp new file mode 100644 index 0000000000..dc4eb8a37e --- /dev/null +++ b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp @@ -0,0 +1,244 @@ +#include +#include +#include +#include +#include +#include + +class SphericalRobot : public testing::Test +{ +protected: + void SetUp() override + { + moveit::core::RobotModelBuilder builder("robot", "base_link"); + geometry_msgs::Pose origin; + origin.orientation.w = 1; + builder.addChain("base_link->roll", "revolute", { origin }, urdf::Vector3(1, 0, 0)); + builder.addChain("roll->pitch", "revolute", { origin }, urdf::Vector3(0, 1, 0)); + builder.addChain("pitch->yaw", "revolute", { origin }, urdf::Vector3(0, 0, 1)); + ASSERT_TRUE(builder.isValid()); + robot_model_ = builder.build(); + } + + std::map getJointValues(const double roll, const double pitch, const double yaw) + { + std::map jvals; + jvals["base_link-roll-joint"] = roll; + jvals["roll-pitch-joint"] = pitch; + jvals["pitch-yaw-joint"] = yaw; + return jvals; + } + + void TearDown() override + { + } + +protected: + moveit::core::RobotModelPtr robot_model_; +}; + +TEST_F(SphericalRobot, Test1) +{ + kinematic_constraints::OrientationConstraint oc(robot_model_); + + moveit::core::Transforms tf(robot_model_->getModelFrame()); + moveit_msgs::OrientationConstraint ocm; + + ocm.link_name = "yaw"; + ocm.header.frame_id = robot_model_->getModelFrame(); + ocm.orientation.x = 0.0; + ocm.orientation.y = 0.0; + ocm.orientation.z = 0.0; + ocm.orientation.w = 1.0; + ocm.absolute_x_axis_tolerance = 0.8; + ocm.absolute_y_axis_tolerance = 0.8; + ocm.absolute_z_axis_tolerance = 3.15; + ocm.weight = 1.0; + + moveit::core::RobotState robot_state(robot_model_); + // This's identical to a -1.57rad rotation around Z-axis + robot_state.setVariablePositions(getJointValues(3.140208, 3.141588, 1.570821)); + robot_state.update(); + + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_TRUE(oc.decide(robot_state).satisfied); +} + +TEST_F(SphericalRobot, Test2) +{ + kinematic_constraints::OrientationConstraint oc(robot_model_); + + moveit::core::Transforms tf(robot_model_->getModelFrame()); + moveit_msgs::OrientationConstraint ocm; + + ocm.link_name = "yaw"; + ocm.header.frame_id = robot_model_->getModelFrame(); + ocm.orientation.x = 0.0; + ocm.orientation.y = 0.0; + ocm.orientation.z = 0.0; + ocm.orientation.w = 1.0; + ocm.absolute_x_axis_tolerance = 0.2; + ocm.absolute_y_axis_tolerance = 2.0; + ocm.absolute_z_axis_tolerance = 0.2; + ocm.weight = 1.0; + + moveit::core::RobotState robot_state(robot_model_); + // Singularity: roll + yaw = theta + // These violate either absolute_x_axis_tolerance or absolute_z_axis_tolerance + robot_state.setVariablePositions(getJointValues(0.15, boost::math::constants::half_pi(), 0.15)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.21, boost::math::constants::half_pi(), 0.0)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi(), 0.21)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + // Singularity: roll - yaw = theta + // This's identical to -pi/2 pitch rotation + robot_state.setVariablePositions(getJointValues(0.15, -boost::math::constants::half_pi(), 0.15)); + robot_state.update(); + + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_TRUE(oc.decide(robot_state).satisfied); +} + +TEST_F(SphericalRobot, Test3) +{ + kinematic_constraints::OrientationConstraint oc(robot_model_); + + moveit::core::Transforms tf(robot_model_->getModelFrame()); + moveit_msgs::OrientationConstraint ocm; + + ocm.link_name = "yaw"; + ocm.header.frame_id = robot_model_->getModelFrame(); + ocm.orientation.x = 0.0; + ocm.orientation.y = 0.0; + ocm.orientation.z = 0.0; + ocm.orientation.w = 1.0; + ocm.absolute_x_axis_tolerance = 0.2; + ocm.absolute_y_axis_tolerance = 2.0; + ocm.absolute_z_axis_tolerance = 0.3; + ocm.weight = 1.0; + + moveit::core::RobotState robot_state(robot_model_); + // Singularity: roll + yaw = theta + + // These tests violate absolute_x_axis_tolerance + robot_state.setVariablePositions(getJointValues(0.21, boost::math::constants::half_pi(), 0.0)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi(), 0.21)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + ocm.absolute_x_axis_tolerance = 0.3; + ocm.absolute_y_axis_tolerance = 2.0; + ocm.absolute_z_axis_tolerance = 0.2; + + // These tests violate absolute_z_axis_tolerance + robot_state.setVariablePositions(getJointValues(0.21, boost::math::constants::half_pi(), 0.0)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi(), 0.21)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); +} + +TEST_F(SphericalRobot, Test4) +{ + kinematic_constraints::OrientationConstraint oc(robot_model_); + + moveit::core::Transforms tf(robot_model_->getModelFrame()); + moveit_msgs::OrientationConstraint ocm; + + ocm.link_name = "yaw"; + ocm.header.frame_id = robot_model_->getModelFrame(); + ocm.orientation.x = 0.0; + ocm.orientation.y = 0.0; + ocm.orientation.z = 0.0; + ocm.orientation.w = 1.0; + ocm.absolute_x_axis_tolerance = 0.2; + ocm.absolute_y_axis_tolerance = 2.0; + ocm.absolute_z_axis_tolerance = 0.3; + ocm.weight = 1.0; + + moveit::core::RobotState robot_state(robot_model_); + // Singularity: roll + yaw = theta + + // These tests violate absolute_x_axis_tolerance + robot_state.setVariablePositions(getJointValues(0.21, -boost::math::constants::half_pi(), 0.0)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.0, -boost::math::constants::half_pi(), 0.21)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + ocm.absolute_x_axis_tolerance = 0.3; + ocm.absolute_y_axis_tolerance = 2.0; + ocm.absolute_z_axis_tolerance = 0.2; + + // These tests violate absolute_z_axis_tolerance + robot_state.setVariablePositions(getJointValues(0.21, -boost::math::constants::half_pi(), 0.0)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.0, -boost::math::constants::half_pi(), 0.21)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); + + robot_state.setVariablePositions(getJointValues(0.5, -boost::math::constants::half_pi(), 0.21)); + robot_state.update(); + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_FALSE(oc.decide(robot_state).satisfied); +} + +// Both the current and the desired orientations are in singularities +TEST_F(SphericalRobot, Test5) +{ + kinematic_constraints::OrientationConstraint oc(robot_model_); + + moveit::core::Transforms tf(robot_model_->getModelFrame()); + moveit_msgs::OrientationConstraint ocm; + + moveit::core::RobotState robot_state(robot_model_); + robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi(), 0.0)); + robot_state.update(); + + ocm.link_name = "yaw"; + ocm.header.frame_id = robot_model_->getModelFrame(); + tf2::convert(Eigen::Quaterniond(robot_state.getGlobalLinkTransform(ocm.link_name).linear()), ocm.orientation); + ocm.absolute_x_axis_tolerance = 0.0; + ocm.absolute_y_axis_tolerance = 0.0; + ocm.absolute_z_axis_tolerance = 1.0; + ocm.weight = 1.0; + + robot_state.setVariablePositions(getJointValues(0.2, boost::math::constants::half_pi(), 0.3)); + robot_state.update(); + + EXPECT_TRUE(oc.configure(ocm, tf)); + EXPECT_TRUE(oc.decide(robot_state, true).satisfied); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/kinematics_base/CMakeLists.txt b/moveit_core/kinematics_base/CMakeLists.txt index f4d98357ef..5e0931e401 100644 --- a/moveit_core/kinematics_base/CMakeLists.txt +++ b/moveit_core/kinematics_base/CMakeLists.txt @@ -2,9 +2,12 @@ set(MOVEIT_LIB_NAME moveit_kinematics_base) add_library(${MOVEIT_LIB_NAME} src/kinematics_base.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -# Avoid warnings about deprecated members of KinematicsBase when building KinematicsBase itself. -# TODO: remove when deperecated variables (tip_frame_, search_discretization_) are finally removed. -target_compile_options(${MOVEIT_LIB_NAME} PRIVATE -Wno-deprecated-declarations) + +if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") + # Avoid warnings about deprecated members of KinematicsBase when building KinematicsBase itself. + # TODO: remove when deperecated variables (tip_frame_, search_discretization_) are finally removed. + target_compile_options(${MOVEIT_LIB_NAME} PRIVATE -Wno-deprecated-declarations) +endif() # This line is needed to ensure that messages are done being built before this is built add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) @@ -13,6 +16,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index c6536f4805..0c62197dbd 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -34,13 +34,11 @@ /* Author: Sachin Chitta, Dave Coleman */ -#ifndef MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_ -#define MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_ +#pragma once -#include +#include #include #include -#include #include #include @@ -53,8 +51,8 @@ namespace core MOVEIT_CLASS_FORWARD(JointModelGroup) MOVEIT_CLASS_FORWARD(RobotState) MOVEIT_CLASS_FORWARD(RobotModel) -} -} +} // namespace core +} // namespace moveit /** @brief API for forward and inverse kinematics */ namespace kinematics @@ -77,8 +75,8 @@ enum DiscretizationMethod SOME_RANDOM_SAMPLED /**< the discretization for some redundant joint will be randomly generated. The unused redundant joints will be fixed at their current value. */ }; -} -typedef DiscretizationMethods::DiscretizationMethod DiscretizationMethod; +} // namespace DiscretizationMethods +using DiscretizationMethod = DiscretizationMethods::DiscretizationMethod; /* * @enum KinematicErrors @@ -99,8 +97,8 @@ enum KinematicError NO_SOLUTION /**< A valid joint solution that can reach this pose(s) could not be found */ }; -} -typedef KinematicErrors::KinematicError KinematicError; +} // namespace KinematicErrors +using KinematicError = KinematicErrors::KinematicError; /** * @struct KinematicsQueryOptions @@ -137,7 +135,7 @@ struct KinematicsResult of solutions explored. */ }; -MOVEIT_CLASS_FORWARD(KinematicsBase); +MOVEIT_CLASS_FORWARD(KinematicsBase); // Defines KinematicsBasePtr, ConstPtr, WeakPtr... etc /** * @class KinematicsBase @@ -150,9 +148,8 @@ class KinematicsBase static const double DEFAULT_TIMEOUT; /* = 1.0 */ /** @brief Signature for a callback to validate an IK solution. Typically used for collision checking. */ - typedef boost::function& ik_solution, - moveit_msgs::MoveItErrorCodes& error_code)> - IKCallbackFn; + using IKCallbackFn = + boost::function&, moveit_msgs::MoveItErrorCodes&)>; /** * @brief Given a desired pose of the end-effector, compute the joint angles to reach it @@ -174,12 +171,18 @@ class KinematicsBase /** * @brief Given the desired poses of all end-effectors, compute joint angles that are able to reach it. * - * This is a default implementation that returns only one solution and so its result is equivalent to calling + * The default implementation returns only one solution and so its result is equivalent to calling * 'getPositionIK(...)' with a zero initialized seed. * + * Some planners (e.g. IKFast) support getting multiple joint solutions for a single pose. + * This can be enabled using the |DiscretizationMethods| enum and choosing an option that is not |NO_DISCRETIZATION|. + * * @param ik_poses The desired pose of each tip link * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param solutions A vector of vectors where each entry is a valid joint solution + * @param solutions A vector of valid joint vectors. This return has two variant behaviors: + * 1) Return a joint solution for every input |ik_poses|, e.g. multi-arm support + * 2) Return multiple joint solutions for a single |ik_poses| input, e.g. underconstrained IK + * TODO(dave): This dual behavior is confusing and should be changed in a future refactor of this API * @param result A struct that reports the results of the query * @param options An option struct which contains the type of redundancy discretization used. This default * implementation only supports the KinematicSearches::NO_DISCRETIZATION method; requesting any @@ -293,8 +296,9 @@ class KinematicsBase double timeout, const std::vector& consistency_limits, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const moveit::core::RobotState* context_state = NULL) const + const moveit::core::RobotState* context_state = nullptr) const { + (void)context_state; // For IK solvers that do not support multiple poses, fall back to single pose call if (ik_poses.size() == 1) { @@ -336,9 +340,9 @@ class KinematicsBase * @param search_discretization The discretization of the search when the solver steps through the redundancy */ /* Replace by tip_frames-based method! */ - MOVEIT_DEPRECATED virtual void setValues(const std::string& robot_description, const std::string& group_name, - const std::string& base_frame, const std::string& tip_frame, - double search_discretization); + [[deprecated]] virtual void setValues(const std::string& robot_description, const std::string& group_name, + const std::string& base_frame, const std::string& tip_frame, + double search_discretization); /** * @brief Set the parameters for the solver, for use with non-chain IK solvers @@ -368,9 +372,9 @@ class KinematicsBase * Instead of this method, use the method passing in a RobotModel! * Default implementation returns false, indicating that this API is not supported. */ - MOVEIT_DEPRECATED virtual bool initialize(const std::string& robot_description, const std::string& group_name, - const std::string& base_frame, const std::string& tip_frame, - double search_discretization); + [[deprecated]] virtual bool initialize(const std::string& robot_description, const std::string& group_name, + const std::string& base_frame, const std::string& tip_frame, + double search_discretization); /** * @brief Initialization function for the kinematics, for use with non-chain IK solvers @@ -503,7 +507,7 @@ class KinematicsBase * supported. * \return True if the group is supported, false if not. */ - virtual bool supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out = NULL) const; + virtual bool supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out = nullptr) const; /** * @brief Set the search discretization value for all the redundant joints @@ -526,10 +530,10 @@ class KinematicsBase { redundant_joint_discretization_.clear(); redundant_joint_indices_.clear(); - for (std::map::const_iterator i = discretization.begin(); i != discretization.end(); i++) + for (const auto& pair : discretization) { - redundant_joint_discretization_.insert(*i); - redundant_joint_indices_.push_back(i->first); + redundant_joint_discretization_.insert(pair); + redundant_joint_indices_.push_back(pair.first); } } @@ -588,8 +592,8 @@ class KinematicsBase // The next two variables still exists for backwards compatibility // with previously generated custom ik solvers like IKFast // Replace tip_frame_ -> tip_frames_[0], search_discretization_ -> redundant_joint_discretization_ - MOVEIT_DEPRECATED std::string tip_frame_; - MOVEIT_DEPRECATED double search_discretization_; + [[deprecated]] std::string tip_frame_; + [[deprecated]] double search_discretization_; double default_timeout_; std::vector redundant_joint_indices_; @@ -658,6 +662,4 @@ class KinematicsBase private: std::string removeSlash(const std::string& str) const; }; -}; - -#endif +} // namespace kinematics diff --git a/moveit_core/kinematics_base/src/kinematics_base.cpp b/moveit_core/kinematics_base/src/kinematics_base.cpp index 5454d3d469..fe26075128 100644 --- a/moveit_core/kinematics_base/src/kinematics_base.cpp +++ b/moveit_core/kinematics_base/src/kinematics_base.cpp @@ -120,8 +120,9 @@ bool KinematicsBase::initialize(const moveit::core::RobotModel& robot_model, con const std::string& base_frame, const std::vector& tip_frames, double search_discretization) { - ROS_WARN_NAMED(LOGNAME, "IK plugin for group '%s' relies on deprecated API. " - "Please implement initialize(RobotModel, ...).", + ROS_WARN_NAMED(LOGNAME, + "IK plugin for group '%s' relies on deprecated API. " + "Please implement initialize(RobotModel, ...).", group_name.c_str()); return false; } diff --git a/moveit_core/kinematics_metrics/CMakeLists.txt b/moveit_core/kinematics_metrics/CMakeLists.txt index 8438c8641d..6a43296df8 100644 --- a/moveit_core/kinematics_metrics/CMakeLists.txt +++ b/moveit_core/kinematics_metrics/CMakeLists.txt @@ -8,6 +8,7 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h index c92a7d5063..ab532a166f 100644 --- a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h +++ b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h @@ -34,16 +34,14 @@ /* Author: Sachin Chitta */ -#ifndef MOVEIT_KINEMATICS_METRICS_KINEMATICS_METRICS_ -#define MOVEIT_KINEMATICS_METRICS_KINEMATICS_METRICS_ +#pragma once #include -#include /** @brief Namespace for kinematics metrics */ namespace kinematics_metrics { -MOVEIT_CLASS_FORWARD(KinematicsMetrics); +MOVEIT_CLASS_FORWARD(KinematicsMetrics); // Defines KinematicsMetricsPtr, ConstPtr, WeakPtr... etc /** * \brief Compute different kinds of metrics for kinematics evaluation. Currently includes @@ -53,7 +51,7 @@ class KinematicsMetrics { public: /** \brief Construct a KinematicsMetricss from a RobotModel */ - KinematicsMetrics(const robot_model::RobotModelConstPtr& robot_model) + KinematicsMetrics(const moveit::core::RobotModelConstPtr& robot_model) : robot_model_(robot_model), penalty_multiplier_(0.0) { } @@ -65,7 +63,7 @@ class KinematicsMetrics * @param manipulability_index The computed manipulability = sqrt(det(JJ^T)) * @return False if the group was not found */ - bool getManipulabilityIndex(const robot_state::RobotState& state, const std::string& group_name, + bool getManipulabilityIndex(const moveit::core::RobotState& state, const std::string& group_name, double& manipulability_index, bool translation = false) const; /** @@ -75,8 +73,8 @@ class KinematicsMetrics * @param manipulability_index The computed manipulability = sqrt(det(JJ^T)) * @return False if the group was not found */ - bool getManipulabilityIndex(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group, double& manipulability_index, + bool getManipulabilityIndex(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group, double& manipulability_index, bool translation = false) const; /** @@ -87,7 +85,7 @@ class KinematicsMetrics * @param eigen_vectors The eigen vectors for the translation part of JJ^T * @return False if the group was not found */ - bool getManipulabilityEllipsoid(const robot_state::RobotState& state, const std::string& group_name, + bool getManipulabilityEllipsoid(const moveit::core::RobotState& state, const std::string& group_name, Eigen::MatrixXcd& eigen_values, Eigen::MatrixXcd& eigen_vectors) const; /** @@ -98,9 +96,9 @@ class KinematicsMetrics * @param eigen_vectors The eigen vectors for the translation part of JJ^T * @return False if the group was not found */ - bool getManipulabilityEllipsoid(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group, Eigen::MatrixXcd& eigen_values, - Eigen::MatrixXcd& eigen_vectors) const; + bool getManipulabilityEllipsoid(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group, + Eigen::MatrixXcd& eigen_values, Eigen::MatrixXcd& eigen_vectors) const; /** * @brief Get the manipulability = sigma_min/sigma_max @@ -111,7 +109,7 @@ class KinematicsMetrics * @param condition_number Condition number for JJ^T * @return False if the group was not found */ - bool getManipulability(const robot_state::RobotState& state, const std::string& group_name, double& condition_number, + bool getManipulability(const moveit::core::RobotState& state, const std::string& group_name, double& condition_number, bool translation = false) const; /** @@ -123,7 +121,7 @@ class KinematicsMetrics * @param condition_number Condition number for JJ^T * @return False if the group was not found */ - bool getManipulability(const robot_state::RobotState& state, const robot_model::JointModelGroup* joint_model_group, + bool getManipulability(const moveit::core::RobotState& state, const moveit::core::JointModelGroup* joint_model_group, double& condition_number, bool translation = false) const; void setPenaltyMultiplier(double multiplier) @@ -137,26 +135,24 @@ class KinematicsMetrics } protected: - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; private: /** - * @brief Defines a multiplier for the manipulabilty - * = 1 - exp ( -penalty_multipler_ * product_{i=1}{n} (distance_to_lower_limit * - * distance_to_higher_limit/(joint_range*joint_range))) - * where n is the number of joints in the group. Floating joints are ignored in this computation. Planar joints with - * finite bounds - * are considered. - * Set penalty_multiplier_ to 0 if you don't want this multiplier to have any effect on the manipulability measures. - * See "Workspace Geometric Characterization and Manipulability of Industrial Robots", Ming-June, Tsia, PhD Thesis, - * Ohio State University, 1986, for more details. - * @return multiplier that is multiplied with every manipulability measure computed here - */ - double getJointLimitsPenalty(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group) const; + * @brief Defines a multiplier for the manipulabilty + * = 1 - exp ( -penalty_multipler_ * product_{i=1}{n} (distance_to_lower_limit * + * distance_to_higher_limit/(joint_range*joint_range))) + * where n is the number of joints in the group. Floating joints are ignored in this computation. Planar joints with + * finite bounds + * are considered. + * Set penalty_multiplier_ to 0 if you don't want this multiplier to have any effect on the manipulability measures. + * See "Workspace Geometric Characterization and Manipulability of Industrial Robots", Ming-June, Tsia, PhD Thesis, + * Ohio State University, 1986, for more details. + * @return multiplier that is multiplied with every manipulability measure computed here + */ + double getJointLimitsPenalty(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group) const; double penalty_multiplier_; }; -} - -#endif +} // namespace kinematics_metrics diff --git a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp index 6776a1e475..1ab07c8a2c 100644 --- a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp +++ b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp @@ -41,25 +41,25 @@ namespace kinematics_metrics { -double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group) const +double KinematicsMetrics::getJointLimitsPenalty(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group) const { if (fabs(penalty_multiplier_) <= boost::math::tools::epsilon()) return 1.0; double joint_limits_multiplier(1.0); - const std::vector& joint_model_vector = joint_model_group->getJointModels(); - for (const robot_model::JointModel* joint_model : joint_model_vector) + const std::vector& joint_model_vector = joint_model_group->getJointModels(); + for (const moveit::core::JointModel* joint_model : joint_model_vector) { - if (joint_model->getType() == robot_model::JointModel::REVOLUTE) + if (joint_model->getType() == moveit::core::JointModel::REVOLUTE) { - const robot_model::RevoluteJointModel* revolute_model = - static_cast(joint_model); + const moveit::core::RevoluteJointModel* revolute_model = + static_cast(joint_model); if (revolute_model->isContinuous()) continue; } - if (joint_model->getType() == robot_model::JointModel::PLANAR) + if (joint_model->getType() == moveit::core::JointModel::PLANAR) { - const robot_model::JointModel::Bounds& bounds = joint_model->getVariableBounds(); + const moveit::core::JointModel::Bounds& bounds = joint_model->getVariableBounds(); if (bounds[0].min_position_ == -std::numeric_limits::max() || bounds[0].max_position_ == std::numeric_limits::max() || bounds[1].min_position_ == -std::numeric_limits::max() || @@ -68,13 +68,13 @@ double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState& s bounds[2].max_position_ == boost::math::constants::pi()) continue; } - if (joint_model->getType() == robot_model::JointModel::FLOATING) + if (joint_model->getType() == moveit::core::JointModel::FLOATING) { // Joint limits are not well-defined for floating joints continue; } const double* joint_values = state.getJointPositions(joint_model); - const robot_model::JointModel::Bounds& bounds = joint_model->getVariableBounds(); + const moveit::core::JointModel::Bounds& bounds = joint_model->getVariableBounds(); std::vector lower_bounds, upper_bounds; for (const moveit::core::VariableBounds& bound : bounds) { @@ -91,18 +91,18 @@ double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState& s return (1.0 - exp(-penalty_multiplier_ * joint_limits_multiplier)); } -bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& state, const std::string& group_name, +bool KinematicsMetrics::getManipulabilityIndex(const moveit::core::RobotState& state, const std::string& group_name, double& manipulability_index, bool translation) const { - const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); if (joint_model_group) return getManipulabilityIndex(state, joint_model_group, manipulability_index, translation); else return false; } -bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group, +bool KinematicsMetrics::getManipulabilityIndex(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group, double& manipulability_index, bool translation) const { // state.getJacobian() only works for chain groups. @@ -162,19 +162,19 @@ bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& st return true; } -bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState& state, const std::string& group_name, +bool KinematicsMetrics::getManipulabilityEllipsoid(const moveit::core::RobotState& state, const std::string& group_name, Eigen::MatrixXcd& eigen_values, Eigen::MatrixXcd& eigen_vectors) const { - const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); if (joint_model_group) return getManipulabilityEllipsoid(state, joint_model_group, eigen_values, eigen_vectors); else return false; } -bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group, +bool KinematicsMetrics::getManipulabilityEllipsoid(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group, Eigen::MatrixXcd& eigen_values, Eigen::MatrixXcd& eigen_vectors) const { @@ -192,19 +192,19 @@ bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState return true; } -bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state, const std::string& group_name, +bool KinematicsMetrics::getManipulability(const moveit::core::RobotState& state, const std::string& group_name, double& manipulability, bool translation) const { - const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); if (joint_model_group) return getManipulability(state, joint_model_group, manipulability, translation); else return false; } -bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state, - const robot_model::JointModelGroup* joint_model_group, double& manipulability, - bool translation) const +bool KinematicsMetrics::getManipulability(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group, + double& manipulability, bool translation) const { // state.getJacobian() only works for chain groups. if (!joint_model_group->isChain()) diff --git a/moveit_core/macros/include/moveit/macros/class_forward.h b/moveit_core/macros/include/moveit/macros/class_forward.h index 57bb5ce56b..be4e6918b2 100644 --- a/moveit_core/macros/include/moveit/macros/class_forward.h +++ b/moveit_core/macros/include/moveit/macros/class_forward.h @@ -34,16 +34,13 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MACROS_CLASS_FORWARD_ -#define MOVEIT_MACROS_CLASS_FORWARD_ +#pragma once #include /** * \def MOVEIT_CLASS_FORWARD - * Macro that forward declares a class and defines two shared ptrs types: - * - ${Class}Ptr = shared_ptr<${Class}> - * - ${Class}ConstPtr = shared_ptr + * Macro that forward declares a class and defines the respective smartpointers through MOVEIT_DECLARE_PTR. */ #define MOVEIT_CLASS_FORWARD(C) \ @@ -58,5 +55,3 @@ #define MOVEIT_STRUCT_FORWARD(C) \ struct C; \ MOVEIT_DECLARE_PTR(C, C); - -#endif diff --git a/moveit_core/macros/include/moveit/macros/console_colors.h b/moveit_core/macros/include/moveit/macros/console_colors.h index 4ea7b65d39..0da6ae80de 100644 --- a/moveit_core/macros/include/moveit/macros/console_colors.h +++ b/moveit_core/macros/include/moveit/macros/console_colors.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSOLE_COLORS_ -#define MOVEIT_CONSOLE_COLORS_ +#pragma once #define MOVEIT_CONSOLE_COLOR_RESET "\033[0m" #define MOVEIT_CONSOLE_COLOR_GRAY "\033[90m" @@ -46,5 +45,3 @@ #define MOVEIT_CONSOLE_COLOR_BLUE "\033[94m" #define MOVEIT_CONSOLE_COLOR_PURPLE "\033[95m" #define MOVEIT_CONSOLE_COLOR_CYAN "\033[96m" - -#endif diff --git a/moveit_core/macros/include/moveit/macros/declare_ptr.h b/moveit_core/macros/include/moveit/macros/declare_ptr.h index 36506e618a..1da6dcd7b5 100644 --- a/moveit_core/macros/include/moveit/macros/declare_ptr.h +++ b/moveit_core/macros/include/moveit/macros/declare_ptr.h @@ -32,16 +32,19 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_MACROS_DECLARE_PTR_ -#define MOVEIT_MACROS_DECLARE_PTR_ +#pragma once #include /** * \def MOVEIT_DELCARE_PTR * Macro that given a Name and a Type declares the following types: - * - ${Name}Ptr = shared_ptr<${Type}> - * - ${Name}ConstPtr = shared_ptr + * - ${Name}Ptr = shared_ptr<${Type}> + * - ${Name}ConstPtr = shared_ptr + * - ${Name}WeakPtr = weak_ptr<${Type}> + * - ${Name}ConstWeakPtr = weak_ptr + * - ${Name}UniquePtr = unique_ptr<${Type}> + * - ${Name}ConstUniquePtr = unique_ptr * * For best portability the exact type of shared_ptr declared by the macro * should be considered to be an implementation detail, liable to change in @@ -52,23 +55,22 @@ typedef std::shared_ptr Name##Ptr; \ typedef std::shared_ptr Name##ConstPtr; \ typedef std::weak_ptr Name##WeakPtr; \ - typedef std::weak_ptr Name##ConstWeakPtr; + typedef std::weak_ptr Name##ConstWeakPtr; \ + typedef std::unique_ptr Name##UniquePtr; \ + typedef std::unique_ptr Name##ConstUniquePtr; /** * \def MOVEIT_DELCARE_PTR_MEMBER - * Macro that given a Type declares the following types: - * - Ptr = shared_ptr<${Type}> - * - ConstPtr = shared_ptr + * The macro defines the same typedefs as MOVEIT_DECLARE_PTR, but shortens the new names to their suffix. * - * This macro is intended for declaring the pointer typedefs as members of a - * class template. In other situations, MOVEIT_CLASS_FORWARD and - * MOVEIT_DECLARE_PTR should be preferred. + * This can be used to create `Classname::Ptr` style names, but in most situations in MoveIt's codebase, + * MOVEIT_CLASS_FORWARD and MOVEIT_DECLARE_PTR should be preferred. */ #define MOVEIT_DECLARE_PTR_MEMBER(Type) \ typedef std::shared_ptr Ptr; \ typedef std::shared_ptr ConstPtr; \ typedef std::weak_ptr WeakPtr; \ - typedef std::weak_ptr ConstWeakPtr; - -#endif + typedef std::weak_ptr ConstWeakPtr; \ + typedef std::unique_ptr UniquePtr; \ + typedef std::unique_ptr ConstUniquePtr; diff --git a/moveit_core/macros/include/moveit/macros/deprecation.h b/moveit_core/macros/include/moveit/macros/deprecation.h index 1a69e998f0..2162988004 100644 --- a/moveit_core/macros/include/moveit/macros/deprecation.h +++ b/moveit_core/macros/include/moveit/macros/deprecation.h @@ -32,12 +32,12 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_MACROS_DEPRECATION_ -#define MOVEIT_MACROS_DEPRECATION_ +#pragma once /** \def MOVEIT_DEPRECATED - Macro that marks functions as deprecated */ + Deprecated macro that marks functions as deprecated (TODO: Remove for Noetic) */ +#warning "The usage of MOVEIT_DEPRECATED is deprecated. Use the CPP14 [[deprecated]] instead." #ifdef __GNUC__ #define MOVEIT_DEPRECATED __attribute__((deprecated)) #elif defined(_MSC_VER) @@ -47,5 +47,3 @@ #else #define MOVEIT_DEPRECATED /* Nothing */ #endif - -#endif diff --git a/moveit_core/package.xml b/moveit_core/package.xml index b12b48f6f3..7c3fadcb8c 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -1,7 +1,8 @@ - + + moveit_core - 1.0.1 - Core libraries used by MoveIt! + 1.1.1 + Core libraries used by MoveIt Ioan Sucan Sachin Chitta Acorn Pooley @@ -9,7 +10,7 @@ Dave Coleman Michael GΓΆrner Michael Ferguson - MoveIt! Release Team + MoveIt Release Team BSD http://moveit.ros.org @@ -22,8 +23,11 @@ assimp boost eigen + bullet eigen_stl_containers - libfcl-dev + eigen_conversions + libfcl-dev + fcl geometric_shapes geometry_msgs kdl_parser @@ -48,14 +52,16 @@ visualization_msgs xmlrpcpp - moveit_resources + moveit_resources_panda_moveit_config + moveit_resources_pr2_description angles tf2_kdl - orocos_kdl + orocos_kdl + liborocos-kdl-dev rosunit - code_coverage + diff --git a/moveit_core/planning_interface/CMakeLists.txt b/moveit_core/planning_interface/CMakeLists.txt index 33c0e1a789..8a876192b5 100644 --- a/moveit_core/planning_interface/CMakeLists.txt +++ b/moveit_core/planning_interface/CMakeLists.txt @@ -11,6 +11,7 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index 68214f4a92..b180d653ed 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_INTERFACE_PLANNING_INTERFACE_ -#define MOVEIT_PLANNING_INTERFACE_PLANNING_INTERFACE_ +#pragma once #include #include @@ -45,10 +44,10 @@ namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene); +MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc } -/** \brief This namespace includes the base class for MoveIt! planners */ +/** \brief This namespace includes the base class for MoveIt planners */ namespace planning_interface { /** @@ -74,7 +73,7 @@ struct PlannerConfigurationSettings /** \brief Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings */ typedef std::map PlannerConfigurationMap; -MOVEIT_CLASS_FORWARD(PlanningContext); +MOVEIT_CLASS_FORWARD(PlanningContext); // Defines PlanningContextPtr, ConstPtr, WeakPtr... etc /** \brief Representation of a particular planning context -- the planning scene and the request are known, solution is not yet computed. */ @@ -145,9 +144,9 @@ class PlanningContext MotionPlanRequest request_; }; -MOVEIT_CLASS_FORWARD(PlannerManager); +MOVEIT_CLASS_FORWARD(PlannerManager); // Defines PlannerManagerPtr, ConstPtr, WeakPtr... etc -/** \brief Base class for a MoveIt! planner */ +/** \brief Base class for a MoveIt planner */ class PlannerManager { public: @@ -164,7 +163,7 @@ class PlannerManager /// It is assumed that motion plans will be computed for the robot described by \e model and that any exposed ROS /// functionality /// or required ROS parameters are namespaced by \e ns - virtual bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns); + virtual bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns); /// Get \brief a short string that identifies the planning interface virtual std::string getDescription() const; @@ -212,6 +211,4 @@ class PlannerManager PlannerConfigurationMap config_settings_; }; -} // planning_interface - -#endif +} // namespace planning_interface diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h index 6138283043..62214d71f8 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_INTERFACE_PLANNING_REQUEST_ -#define MOVEIT_PLANNING_INTERFACE_PLANNING_REQUEST_ +#pragma once #include @@ -45,6 +44,4 @@ namespace planning_interface typedef moveit_msgs::MotionPlanRequest MotionPlanRequest; -} // planning_interface - -#endif +} // namespace planning_interface diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h index c902ffd741..6347a3d70f 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_INTERFACE_PLANNING_RESPONSE_ -#define MOVEIT_PLANNING_INTERFACE_PLANNING_RESPONSE_ +#pragma once #include #include @@ -67,6 +66,4 @@ struct MotionPlanDetailedResponse moveit_msgs::MoveItErrorCodes error_code_; }; -} // planning_interface - -#endif +} // namespace planning_interface diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp index 6fedbb22a2..0f3b2ac243 100644 --- a/moveit_core/planning_interface/src/planning_interface.cpp +++ b/moveit_core/planning_interface/src/planning_interface.cpp @@ -91,7 +91,7 @@ void PlanningContext::setMotionPlanRequest(const MotionPlanRequest& request) request_.num_planning_attempts = std::max(1, request_.num_planning_attempts); } -bool PlannerManager::initialize(const robot_model::RobotModelConstPtr& /*unused*/, const std::string& /*unused*/) +bool PlannerManager::initialize(const moveit::core::RobotModelConstPtr& /*unused*/, const std::string& /*unused*/) { return true; } diff --git a/moveit_core/planning_interface/src/planning_response.cpp b/moveit_core/planning_interface/src/planning_response.cpp index fad5558909..7824cd75a3 100644 --- a/moveit_core/planning_interface/src/planning_response.cpp +++ b/moveit_core/planning_interface/src/planning_response.cpp @@ -43,7 +43,7 @@ void planning_interface::MotionPlanResponse::getMessage(moveit_msgs::MotionPlanR msg.planning_time = planning_time_; if (trajectory_ && !trajectory_->empty()) { - robot_state::robotStateToRobotStateMsg(trajectory_->getFirstWayPoint(), msg.trajectory_start); + moveit::core::robotStateToRobotStateMsg(trajectory_->getFirstWayPoint(), msg.trajectory_start); trajectory_->getRobotTrajectoryMsg(msg.trajectory); msg.group_name = trajectory_->getGroupName(); } @@ -66,7 +66,7 @@ void planning_interface::MotionPlanDetailedResponse::getMessage(moveit_msgs::Mot if (first) { first = false; - robot_state::robotStateToRobotStateMsg(trajectory_[i]->getFirstWayPoint(), msg.trajectory_start); + moveit::core::robotStateToRobotStateMsg(trajectory_[i]->getFirstWayPoint(), msg.trajectory_start); msg.group_name = trajectory_[i]->getGroupName(); } msg.trajectory.resize(msg.trajectory.size() + 1); diff --git a/moveit_core/planning_request_adapter/CMakeLists.txt b/moveit_core/planning_request_adapter/CMakeLists.txt index 87038c1d31..401e955ded 100644 --- a/moveit_core/planning_request_adapter/CMakeLists.txt +++ b/moveit_core/planning_request_adapter/CMakeLists.txt @@ -8,6 +8,7 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h index e950bfba11..c721b45134 100644 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h @@ -34,27 +34,24 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_REQUEST_ADAPTER_PLANNING_REQUEST_ADAPTER_ -#define MOVEIT_PLANNING_REQUEST_ADAPTER_PLANNING_REQUEST_ADAPTER_ +#pragma once #include #include #include #include -#include /** \brief Generic interface to adapting motion planning requests */ namespace planning_request_adapter { -MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); +MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); // Defines PlanningRequestAdapterPtr, ConstPtr, WeakPtr... etc class PlanningRequestAdapter { public: - typedef boost::function - PlannerFn; + using PlannerFn = + boost::function; PlanningRequestAdapter() { @@ -64,21 +61,9 @@ class PlanningRequestAdapter { } - /// Initialize parameters using the passed NodeHandle - // TODO - Make initialize() a pure virtual function in O-turtle - virtual void initialize(const ros::NodeHandle& node_handle) - { - // Get name of derived adapter - std::string adapter_name = typeid(*this).name(); - // Try to demangle the name - int status; - std::string demangled_name = abi::__cxa_demangle(adapter_name.c_str(), NULL, NULL, &status); - if (status == 0) - adapter_name = demangled_name; - ROS_WARN_NAMED("planning_request_adapter", "Implementation of function initialize() is missing from '%s'." - "Any parameters should be loaded from the passed NodeHandle.", - adapter_name.c_str()); - } + /** \brief Initialize parameters using the passed NodeHandle + if no initialization is needed, simply implement as empty */ + virtual void initialize(const ros::NodeHandle& node_handle) = 0; /// Get a short string that identifies the planning request adapter virtual std::string getDescription() const @@ -133,6 +118,4 @@ class PlanningRequestAdapterChain private: std::vector adapters_; }; -} - -#endif +} // namespace planning_request_adapter diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt index 13174d3f1d..3f233fb98e 100644 --- a/moveit_core/planning_scene/CMakeLists.txt +++ b/moveit_core/planning_scene/CMakeLists.txt @@ -18,13 +18,14 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_planning_scene test/test_planning_scene.cpp) - target_link_libraries(test_planning_scene ${MOVEIT_LIB_NAME}) + target_link_libraries(test_planning_scene ${MOVEIT_LIB_NAME} moveit_test_utils) + + catkin_add_gtest(test_multi_threaded test/test_multi_threaded.cpp) + target_link_libraries(test_multi_threaded ${MOVEIT_LIB_NAME} moveit_test_utils) endif() diff --git a/moveit_core/planning_scene/dox/planning_scene.dox b/moveit_core/planning_scene/dox/planning_scene.dox index 447b8f52b7..8bd31301b4 100644 --- a/moveit_core/planning_scene/dox/planning_scene.dox +++ b/moveit_core/planning_scene/dox/planning_scene.dox @@ -2,7 +2,7 @@ \page planning_scene_overview Planning Scene -The planning scene (planning_scene::PlanningScene) is the central class for motion planning in MoveIt!. A planning scene represents all the information needed to compute motion plans: The robot's current state, its representation (geometric, kinematic, dynamic) and the world representation. Using this information, things like forward kinematics, inverse kinematics, evaluation of constraints, collision checking, are all possible. +The planning scene (planning_scene::PlanningScene) is the central class for motion planning in MoveIt. A planning scene represents all the information needed to compute motion plans: The robot's current state, its representation (geometric, kinematic, dynamic) and the world representation. Using this information, things like forward kinematics, inverse kinematics, evaluation of constraints, collision checking, are all possible. The planning_scene::PlanningScene class is tightly connected to the planning_scene_monitor::PlannningSceneMonitor class, which maintains a planning scene using information from the ROS Parameter Server and subscription to topics. diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 6503f7793a..7e85755e51 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -34,19 +34,18 @@ /* Author: Ioan Sucan, Acorn Pooley */ -#ifndef MOVEIT_PLANNING_SCENE_PLANNING_SCENE_ -#define MOVEIT_PLANNING_SCENE_PLANNING_SCENE_ +#pragma once #include #include #include #include #include +#include #include #include #include #include -#include #include #include #include @@ -60,26 +59,27 @@ /** \brief This namespace includes the central class for representing planning contexts */ namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene); +MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc /** \brief This is the function signature for additional feasibility checks to be imposed on states (in addition to respecting constraints and collision avoidance). The first argument is the state to check the feasibility for, the second one is whether the check should be verbose or not. */ -typedef boost::function StateFeasibilityFn; +typedef boost::function StateFeasibilityFn; /** \brief This is the function signature for additional feasibility checks to be imposed on motions segments between states (in addition to respecting constraints and collision avoidance). The order of the arguments matters: the notion of feasibility is to be checked for motion segments that start at the first state and end at the second state. The third argument indicates whether the check should be verbose or not. */ -typedef boost::function MotionFeasibilityFn; +using MotionFeasibilityFn = + boost::function; /** \brief A map from object names (e.g., attached bodies, collision objects) to their colors */ -typedef std::map ObjectColorMap; +using ObjectColorMap = std::map; /** \brief A map from object names (e.g., attached bodies, collision objects) to their types */ -typedef std::map ObjectTypeMap; +using ObjectTypeMap = std::map; /** \brief This class maintains the representation of the environment as seen by a planning instance. The environment @@ -89,7 +89,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from public: /** \brief construct using an existing RobotModel */ PlanningScene( - const robot_model::RobotModelConstPtr& robot_model, + const moveit::core::RobotModelConstPtr& robot_model, const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World())); /** \brief construct using a urdf and srdf. @@ -138,23 +138,23 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from } /** \brief Get the kinematic model for which the planning scene is maintained */ - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { // the kinematic model does not change return robot_model_; } /** \brief Get the state at which the robot is assumed to be. */ - const robot_state::RobotState& getCurrentState() const + const moveit::core::RobotState& getCurrentState() const { // if we have an updated state, return it; otherwise, return the parent one return robot_state_ ? *robot_state_ : parent_->getCurrentState(); } /** \brief Get the state at which the robot is assumed to be. */ - robot_state::RobotState& getCurrentStateNonConst(); + moveit::core::RobotState& getCurrentStateNonConst(); /** \brief Get a copy of the current state with components overwritten by the state message \e update */ - robot_state::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState& update) const; + moveit::core::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState& update) const; /** * \name Reasoning about frames @@ -169,7 +169,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from } /** \brief Get the set of fixed transforms from known frames to the planning frame */ - const robot_state::Transforms& getTransforms() const + const moveit::core::Transforms& getTransforms() const { if (scene_transforms_ || !parent_) { @@ -182,10 +182,10 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Get the set of fixed transforms from known frames to the planning frame. This variant is non-const and also * updates the current state */ - const robot_state::Transforms& getTransforms(); + const moveit::core::Transforms& getTransforms(); /** \brief Get the set of fixed transforms from known frames to the planning frame */ - robot_state::Transforms& getTransformsNonConst(); + moveit::core::Transforms& getTransformsNonConst(); /** \brief Get the transform corresponding to the frame \e id. This will be known if \e id is a link name, an attached body id or a collision object. @@ -205,17 +205,17 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be successful or not. This function also updates the link transforms of \e state. */ - const Eigen::Isometry3d& getFrameTransform(robot_state::RobotState& state, const std::string& id) const + const Eigen::Isometry3d& getFrameTransform(moveit::core::RobotState& state, const std::string& id) const { state.updateLinkTransforms(); - return getFrameTransform(static_cast(state), id); + return getFrameTransform(static_cast(state), id); } /** \brief Get the transform corresponding to the frame \e id. This will be known if \e id is a link name, an attached body id or a collision object. Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be successful or not. */ - const Eigen::Isometry3d& getFrameTransform(const robot_state::RobotState& state, const std::string& id) const; + const Eigen::Isometry3d& getFrameTransform(const moveit::core::RobotState& state, const std::string& id) const; /** \brief Check if a transform to the frame \e id is known. This will be known if \e id is a link name, an attached * body id or a collision object */ @@ -223,7 +223,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check if a transform to the frame \e id is known. This will be known if \e id is a link name, an attached * body id or a collision object */ - bool knowsFrameTransform(const robot_state::RobotState& state, const std::string& id) const; + bool knowsFrameTransform(const moveit::core::RobotState& state, const std::string& id) const; /**@}*/ @@ -294,43 +294,31 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from return world_; } - /** \brief Get the active collision detector for the world */ - const collision_detection::CollisionWorldConstPtr& getCollisionWorld() const + /** \brief Get the active collision environment */ + const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const { - // we always have a world representation after configure is called. - return active_collision_->cworld_const_; + return active_collision_->getCollisionEnv(); } /** \brief Get the active collision detector for the robot */ - const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const + const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const { - return active_collision_->getCollisionRobot(); - } - - /** \brief Get the active collision detector for the robot */ - const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const - { - return active_collision_->getCollisionRobotUnpadded(); + return active_collision_->getCollisionEnvUnpadded(); } /** \brief Get a specific collision detector for the world. If not found return active CollisionWorld. */ - const collision_detection::CollisionWorldConstPtr& - getCollisionWorld(const std::string& collision_detector_name) const; - - /** \brief Get a specific collision detector for the padded robot. If no found return active CollisionRobot. */ - const collision_detection::CollisionRobotConstPtr& - getCollisionRobot(const std::string& collision_detector_name) const; + const collision_detection::CollisionEnvConstPtr& getCollisionEnv(const std::string& collision_detector_name) const; /** \brief Get a specific collision detector for the unpadded robot. If no found return active unpadded * CollisionRobot. */ - const collision_detection::CollisionRobotConstPtr& - getCollisionRobotUnpadded(const std::string& collision_detector_name) const; + const collision_detection::CollisionEnvConstPtr& + getCollisionEnvUnpadded(const std::string& collision_detector_name) const; /** \brief Get the representation of the collision robot * This can be used to set padding and link scale on the active collision_robot. * NOTE: After modifying padding and scale on the active robot call * propogateRobotPadding() to copy it to all the other collision detectors. */ - const collision_detection::CollisionRobotPtr& getCollisionRobotNonConst(); + const collision_detection::CollisionEnvPtr& getCollisionEnvNonConst(); /** \brief Copy scale and padding from active CollisionRobot to other CollisionRobots. * This should be called after any changes are made to the scale or padding of the active @@ -369,22 +357,21 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from specified, collision checking is done for that group only. The link transforms for \e state are updated before the collision check. */ - bool isStateColliding(robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const + bool isStateColliding(moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const { state.updateCollisionBodyTransforms(); - return isStateColliding(static_cast(state), group, verbose); + return isStateColliding(static_cast(state), group, verbose); } /** \brief Check if a given state is in collision (with the environment or self collision) If a group name is specified, collision checking is done for that group only. It is expected that the link transforms of \e state are up to date. */ - bool isStateColliding(const robot_state::RobotState& state, const std::string& group = "", + bool isStateColliding(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is in collision (with the environment or self collision) If a group name is specified, collision checking is done for that group only. */ - bool isStateColliding(const moveit_msgs::RobotState& state, const std::string& group = "", - bool verbose = false) const; + bool isStateColliding(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const; /** \brief Check whether the current state is in collision, and if needed, updates the collision transforms of the * current state before the computation. */ @@ -399,33 +386,33 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check whether a specified state (\e robot_state) is in collision. This variant of the function takes a non-const \e robot_state and calls updateCollisionBodyTransforms() on it. */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - robot_state::RobotState& robot_state) const + moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - checkCollision(req, res, static_cast(robot_state)); + checkCollision(req, res, static_cast(robot_state)); } /** \brief Check whether a specified state (\e robot_state) is in collision. The collision transforms of \e * robot_state are * expected to be up to date. */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state) const; + const moveit::core::RobotState& robot_state) const; /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given allowed collision matrix (\e acm). This variant of the function takes a non-const \e robot_state and updates its link transforms if needed. */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - robot_state::RobotState& robot_state, + moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - checkCollision(req, res, static_cast(robot_state), acm); + checkCollision(req, res, static_cast(robot_state), acm); } /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given allowed collision matrix (\e acm). */ void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Check whether the current state is in collision, @@ -446,7 +433,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from but use a collision_detection::CollisionRobot instance that has no padding. */ void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state) const + const moveit::core::RobotState& robot_state) const { checkCollisionUnpadded(req, res, robot_state, getAllowedCollisionMatrix()); } @@ -455,10 +442,10 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from but use a collision_detection::CollisionRobot instance that has no padding. Update the link transforms of \e robot_state if needed. */ void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, robot_state::RobotState& robot_state) const + collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - checkCollisionUnpadded(req, res, static_cast(robot_state), + checkCollisionUnpadded(req, res, static_cast(robot_state), getAllowedCollisionMatrix()); } @@ -466,17 +453,17 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding. This variant of the function takes a non-const \e robot_state and calls updates the link transforms if needed. */ void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, robot_state::RobotState& robot_state, + collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - checkCollisionUnpadded(req, res, static_cast(robot_state), acm); + checkCollisionUnpadded(req, res, static_cast(robot_state), acm); } /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding. */ void checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const robot_state::RobotState& robot_state, + collision_detection::CollisionResult& res, const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Check whether the current state is in self collision */ @@ -491,38 +478,38 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check whether a specified state (\e robot_state) is in self collision */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - robot_state::RobotState& robot_state) const + moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - checkSelfCollision(req, res, static_cast(robot_state), getAllowedCollisionMatrix()); + checkSelfCollision(req, res, static_cast(robot_state), getAllowedCollisionMatrix()); } /** \brief Check whether a specified state (\e robot_state) is in self collision */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state) const + const moveit::core::RobotState& robot_state) const { // do self-collision checking with the unpadded version of the robot - getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix()); + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix()); } /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given allowed collision matrix (\e acm). The link transforms of \e robot_state are updated if needed. */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - robot_state::RobotState& robot_state, + moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - checkSelfCollision(req, res, static_cast(robot_state), acm); + checkSelfCollision(req, res, static_cast(robot_state), acm); } /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given allowed collision matrix (\e acm) */ void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { // do self-collision checking with the unpadded version of the robot - getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, acm); + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); } /** \brief Get the names of the links that are involved in collisions for the current state */ @@ -536,30 +523,30 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. Update the link transforms for \e robot_state if needed. */ - void getCollidingLinks(std::vector& links, robot_state::RobotState& robot_state) const + void getCollidingLinks(std::vector& links, moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - getCollidingLinks(links, static_cast(robot_state), getAllowedCollisionMatrix()); + getCollidingLinks(links, static_cast(robot_state), getAllowedCollisionMatrix()); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state */ - void getCollidingLinks(std::vector& links, const robot_state::RobotState& robot_state) const + void getCollidingLinks(std::vector& links, const moveit::core::RobotState& robot_state) const { getCollidingLinks(links, robot_state, getAllowedCollisionMatrix()); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the allowed collision matrix (\e acm) */ - void getCollidingLinks(std::vector& links, robot_state::RobotState& robot_state, + void getCollidingLinks(std::vector& links, moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - getCollidingLinks(links, static_cast(robot_state), acm); + getCollidingLinks(links, static_cast(robot_state), acm); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the allowed collision matrix (\e acm) */ - void getCollidingLinks(std::vector& links, const robot_state::RobotState& robot_state, + void getCollidingLinks(std::vector& links, const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const; /** \brief Get the names of the links that are involved in collisions for the current state. @@ -574,7 +561,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Get the names of the links that are involved in collisions for the state \e robot_state */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - const robot_state::RobotState& robot_state) const + const moveit::core::RobotState& robot_state) const { getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix()); } @@ -582,26 +569,26 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. Update the link transforms for \e robot_state if needed. */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - robot_state::RobotState& robot_state) const + moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - getCollidingPairs(contacts, static_cast(robot_state), getAllowedCollisionMatrix()); + getCollidingPairs(contacts, static_cast(robot_state), getAllowedCollisionMatrix()); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - robot_state::RobotState& robot_state, + moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - getCollidingPairs(contacts, static_cast(robot_state), acm); + getCollidingPairs(contacts, static_cast(robot_state), acm); } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the allowed collision matrix (\e acm) */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const; /**@}*/ @@ -614,71 +601,71 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring * self-collisions) */ - double distanceToCollision(robot_state::RobotState& robot_state) const + double distanceToCollision(moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - return distanceToCollision(static_cast(robot_state)); + return distanceToCollision(static_cast(robot_state)); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring * self-collisions) */ - double distanceToCollision(const robot_state::RobotState& robot_state) const + double distanceToCollision(const moveit::core::RobotState& robot_state) const { - return getCollisionWorld()->distanceRobot(*getCollisionRobot(), robot_state, getAllowedCollisionMatrix()); + return getCollisionEnv()->distanceRobot(robot_state, getAllowedCollisionMatrix()); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring * self-collisions), if the robot has no padding */ - double distanceToCollisionUnpadded(robot_state::RobotState& robot_state) const + double distanceToCollisionUnpadded(moveit::core::RobotState& robot_state) const { robot_state.updateCollisionBodyTransforms(); - return distanceToCollisionUnpadded(static_cast(robot_state)); + return distanceToCollisionUnpadded(static_cast(robot_state)); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring * self-collisions), if the robot has no padding */ - double distanceToCollisionUnpadded(const robot_state::RobotState& robot_state) const + double distanceToCollisionUnpadded(const moveit::core::RobotState& robot_state) const { - return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), robot_state, getAllowedCollisionMatrix()); + return getCollisionEnvUnpadded()->distanceRobot(robot_state, getAllowedCollisionMatrix()); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring * self-collisions * and elements that are allowed to collide. */ - double distanceToCollision(robot_state::RobotState& robot_state, + double distanceToCollision(moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - return distanceToCollision(static_cast(robot_state), acm); + return distanceToCollision(static_cast(robot_state), acm); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring * self-collisions * and elements that are allowed to collide. */ - double distanceToCollision(const robot_state::RobotState& robot_state, + double distanceToCollision(const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { - return getCollisionWorld()->distanceRobot(*getCollisionRobot(), robot_state, acm); + return getCollisionEnv()->distanceRobot(robot_state, acm); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring * self-collisions * and elements that are allowed to collide, if the robot has no padding. */ - double distanceToCollisionUnpadded(robot_state::RobotState& robot_state, + double distanceToCollisionUnpadded(moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { robot_state.updateCollisionBodyTransforms(); - return distanceToCollisionUnpadded(static_cast(robot_state), acm); + return distanceToCollisionUnpadded(static_cast(robot_state), acm); } /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring * self-collisions * and elements that always allowed to collide, if the robot has no padding. */ - double distanceToCollisionUnpadded(const robot_state::RobotState& robot_state, + double distanceToCollisionUnpadded(const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { - return getCollisionWorld()->distanceRobot(*getCollisionRobotUnpadded(), robot_state, acm); + return getCollisionEnvUnpadded()->distanceRobot(robot_state, acm); } /**@}*/ @@ -765,10 +752,10 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from void setCurrentState(const moveit_msgs::RobotState& state); /** \brief Set the current robot state */ - void setCurrentState(const robot_state::RobotState& state); + void setCurrentState(const moveit::core::RobotState& state); /** \brief Set the callback to be triggered when changes are made to the current scene state */ - void setAttachedBodyUpdateCallback(const robot_state::AttachedBodyCallback& callback); + void setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback& callback); /** \brief Set the callback to be triggered when changes are made to the current scene world */ void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn& callback); @@ -787,8 +774,10 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from void removeObjectType(const std::string& id); void getKnownObjectTypes(ObjectTypeMap& kc) const; - /** \brief Clear the diffs accumulated for this planning scene, with respect to the parent. This function is a no-op - * if there is no parent specified. */ + /** \brief Clear the diffs accumulated for this planning scene, with respect to: + * the parent PlanningScene (if it exists) + * the parent CollisionDetector (if it exists) + * This function is a no-op if there is no parent planning scene. */ void clearDiffs(); /** \brief If there is a parent specified for this scene, then the diffs with respect to that parent are applied to a @@ -836,14 +825,14 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check if a given state is feasible, in accordance to the feasibility predicate specified by * setStateFeasibilityPredicate(). Returns true if no feasibility predicate was specified. */ - bool isStateFeasible(const robot_state::RobotState& state, bool verbose = false) const; + bool isStateFeasible(const moveit::core::RobotState& state, bool verbose = false) const; /** \brief Check if a given state satisfies a set of constraints */ bool isStateConstrained(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr, bool verbose = false) const; /** \brief Check if a given state satisfies a set of constraints */ - bool isStateConstrained(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr, + bool isStateConstrained(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr, bool verbose = false) const; /** \brief Check if a given state satisfies a set of constraints */ @@ -851,14 +840,14 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const; /** \brief Check if a given state satisfies a set of constraints */ - bool isStateConstrained(const robot_state::RobotState& state, + bool isStateConstrained(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions and feasibility */ bool isStateValid(const moveit_msgs::RobotState& state, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions and feasibility */ - bool isStateValid(const robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const; + bool isStateValid(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user * specified validity conditions hold as well */ @@ -867,25 +856,25 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user * specified validity conditions hold as well */ - bool isStateValid(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr, + bool isStateValid(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user * specified validity conditions hold as well */ - bool isStateValid(const robot_state::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, + bool isStateValid(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility) */ bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory, const std::string& group = "", bool verbose = false, - std::vector* invalid_index = NULL) const; + std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the * passed in trajectory. */ bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = NULL) const; + bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the @@ -893,7 +882,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints, const std::string& group = "", bool verbose = false, - std::vector* invalid_index = NULL) const; + std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the @@ -901,7 +890,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from bool isPathValid(const moveit_msgs::RobotState& start_state, const moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, const std::vector& goal_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = NULL) const; + bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the @@ -909,7 +898,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, const std::vector& goal_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = NULL) const; + bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the @@ -917,17 +906,17 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, const moveit_msgs::Constraints& goal_constraints, const std::string& group = "", bool verbose = false, - std::vector* invalid_index = NULL) const; + std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = NULL) const; + bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility) */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = NULL) const; + bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Get the top \e max_costs cost sources for a specified trajectory. The resulting costs are stored in \e * costs */ @@ -941,12 +930,12 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from double overlap_fraction = 0.9) const; /** \brief Get the top \e max_costs cost sources for a specified state. The resulting costs are stored in \e costs */ - void getCostSources(const robot_state::RobotState& state, std::size_t max_costs, + void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, std::set& costs) const; /** \brief Get the top \e max_costs cost sources for a specified state, but only for group \e group_name. The * resulting costs are stored in \e costs */ - void getCostSources(const robot_state::RobotState& state, std::size_t max_costs, const std::string& group_name, + void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, const std::string& group_name, std::set& costs) const; /** \brief Outputs debug information about the planning scene contents */ @@ -954,14 +943,15 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Check if a message includes any information about a planning scene, or it is just a default, empty message. */ - static bool isEmpty(const moveit_msgs::PlanningScene& msg); + [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool isEmpty(const moveit_msgs::PlanningScene& msg); /** \brief Check if a message includes any information about a planning scene world, or it is just a default, empty * message. */ - static bool isEmpty(const moveit_msgs::PlanningSceneWorld& msg); + [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool + isEmpty(const moveit_msgs::PlanningSceneWorld& msg); /** \brief Check if a message includes any information about a robot state, or it is just a default, empty message. */ - static bool isEmpty(const moveit_msgs::RobotState& msg); + [[deprecated("Use moveit/utils/message_checks.h instead")]] static bool isEmpty(const moveit_msgs::RobotState& msg); /** \brief Clone a planning scene. Even if the scene \e scene depends on a parent, the cloned scene will not. */ static PlanningScenePtr clone(const PlanningSceneConstPtr& scene); @@ -975,8 +965,8 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from void initialize(); /* helper function to create a RobotModel from a urdf/srdf. */ - static robot_model::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, - const srdf::ModelConstSharedPtr& srdf_model); + static moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, + const srdf::ModelConstSharedPtr& srdf_model); /* Helper functions for processing collision objects */ bool processCollisionObjectAdd(const moveit_msgs::CollisionObject& object); @@ -992,31 +982,29 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from struct CollisionDetector { collision_detection::CollisionDetectorAllocatorPtr alloc_; - collision_detection::CollisionRobotPtr crobot_unpadded_; // if NULL use parent's - collision_detection::CollisionRobotConstPtr crobot_unpadded_const_; - collision_detection::CollisionRobotPtr crobot_; // if NULL use parent's - collision_detection::CollisionRobotConstPtr crobot_const_; + collision_detection::CollisionEnvPtr cenv_; // never NULL + collision_detection::CollisionEnvConstPtr cenv_const_; - collision_detection::CollisionWorldPtr cworld_; // never NULL - collision_detection::CollisionWorldConstPtr cworld_const_; + collision_detection::CollisionEnvPtr cenv_unpadded_; + collision_detection::CollisionEnvConstPtr cenv_unpadded_const_; CollisionDetectorConstPtr parent_; // may be NULL - const collision_detection::CollisionRobotConstPtr& getCollisionRobot() const + const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const { - return crobot_const_ ? crobot_const_ : parent_->getCollisionRobot(); + return cenv_const_ ? cenv_const_ : parent_->getCollisionEnv(); } - const collision_detection::CollisionRobotConstPtr& getCollisionRobotUnpadded() const + const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const { - return crobot_unpadded_const_ ? crobot_unpadded_const_ : parent_->getCollisionRobotUnpadded(); + return cenv_unpadded_const_ ? cenv_unpadded_const_ : parent_->getCollisionEnvUnpadded(); } void findParent(const PlanningScene& scene); void copyPadding(const CollisionDetector& src); }; friend struct CollisionDetector; - typedef std::map::iterator CollisionDetectorIterator; - typedef std::map::const_iterator CollisionDetectorConstIterator; + using CollisionDetectorIterator = std::map::iterator; + using CollisionDetectorConstIterator = std::map::const_iterator; void allocateCollisionDetectors(); void allocateCollisionDetectors(CollisionDetector& detector); @@ -1025,16 +1013,16 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from PlanningSceneConstPtr parent_; // Null unless this is a diff scene - robot_model::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent) + moveit::core::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent) - robot_state::RobotStatePtr robot_state_; // if NULL use parent's + moveit::core::RobotStatePtr robot_state_; // if NULL use parent's // Called when changes are made to attached bodies - robot_state::AttachedBodyCallback current_state_attached_body_callback_; + moveit::core::AttachedBodyCallback current_state_attached_body_callback_; // This variable is not necessarily used by child planning scenes - // This Transforms class is actually a SceneTransform class - robot_state::TransformsPtr scene_transforms_; // if NULL use parent's + // This Transforms class is actually a SceneTransforms class + moveit::core::TransformsPtr scene_transforms_; // if NULL use parent's collision_detection::WorldPtr world_; // never NULL, never shared with parent/child collision_detection::WorldConstPtr world_const_; // copy of world_ @@ -1055,6 +1043,4 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from // a map of object types std::unique_ptr object_types_; }; -} - -#endif +} // namespace planning_scene diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 37b62f4d85..969a596820 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -55,7 +56,7 @@ const std::string PlanningScene::DEFAULT_SCENE_NAME = "(noname)"; const std::string LOGNAME = "planning_scene"; -class SceneTransforms : public robot_state::Transforms +class SceneTransforms : public moveit::core::Transforms { public: SceneTransforms(const PlanningScene* scene) : Transforms(scene->getRobotModel()->getModelFrame()), scene_(scene) @@ -96,28 +97,20 @@ class SceneTransforms : public robot_state::Transforms bool PlanningScene::isEmpty(const moveit_msgs::PlanningScene& msg) { - return msg.name.empty() && msg.fixed_frame_transforms.empty() && msg.allowed_collision_matrix.entry_names.empty() && - msg.link_padding.empty() && msg.link_scale.empty() && isEmpty(msg.robot_state) && isEmpty(msg.world); + return moveit::core::isEmpty(msg); } bool PlanningScene::isEmpty(const moveit_msgs::RobotState& msg) { - /* a state is empty if it includes no information and it is a diff; if the state is not a diff, then the implicit - information is - that the set of attached bodies is empty, so they must be cleared from the state to be updated */ - return static_cast(msg.is_diff) && msg.multi_dof_joint_state.joint_names.empty() && - msg.joint_state.name.empty() && msg.attached_collision_objects.empty() && msg.joint_state.position.empty() && - msg.joint_state.velocity.empty() && msg.joint_state.effort.empty() && - msg.multi_dof_joint_state.transforms.empty() && msg.multi_dof_joint_state.twist.empty() && - msg.multi_dof_joint_state.wrench.empty(); + return moveit::core::isEmpty(msg); } bool PlanningScene::isEmpty(const moveit_msgs::PlanningSceneWorld& msg) { - return msg.collision_objects.empty() && msg.octomap.octomap.data.empty(); + return moveit::core::isEmpty(msg); } -PlanningScene::PlanningScene(const robot_model::RobotModelConstPtr& robot_model, +PlanningScene::PlanningScene(const moveit::core::RobotModelConstPtr& robot_model, const collision_detection::WorldPtr& world) : robot_model_(robot_model), world_(world), world_const_(world) { @@ -153,7 +146,7 @@ void PlanningScene::initialize() scene_transforms_.reset(new SceneTransforms(this)); - robot_state_.reset(new robot_state::RobotState(robot_model_)); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); robot_state_->setToDefaultValues(); robot_state_->update(); @@ -171,12 +164,12 @@ void PlanningScene::initialize() } /* return NULL on failure */ -robot_model::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, - const srdf::ModelConstSharedPtr& srdf_model) +moveit::core::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, + const srdf::ModelConstSharedPtr& srdf_model) { - robot_model::RobotModelPtr robot_model(new robot_model::RobotModel(urdf_model, srdf_model)); + moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model, srdf_model)); if (!robot_model || !robot_model->getRootJoint()) - return robot_model::RobotModelPtr(); + return moveit::core::RobotModelPtr(); return robot_model; } @@ -208,15 +201,11 @@ PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(pare detector->alloc_ = parent_detector->alloc_; detector->parent_ = parent_detector; - detector->cworld_ = detector->alloc_->allocateWorld(parent_detector->cworld_, world_); - detector->cworld_const_ = detector->cworld_; + detector->cenv_ = detector->alloc_->allocateEnv(parent_detector->cenv_, world_); + detector->cenv_const_ = detector->cenv_; - // leave these empty and use parent collision_robot_ unless/until a non-const one - // is requested (e.g. to modify link padding or scale) - detector->crobot_.reset(); - detector->crobot_const_.reset(); - detector->crobot_unpadded_.reset(); - detector->crobot_unpadded_const_.reset(); + detector->cenv_unpadded_ = detector->alloc_->allocateEnv(parent_detector->cenv_unpadded_, world_); + detector->cenv_unpadded_const_ = detector->cenv_unpadded_; } setActiveCollisionDetector(parent_->getActiveCollisionDetectorName()); } @@ -243,21 +232,12 @@ PlanningScenePtr PlanningScene::diff(const moveit_msgs::PlanningScene& msg) cons void PlanningScene::CollisionDetector::copyPadding(const PlanningScene::CollisionDetector& src) { - if (!crobot_) - { - alloc_->allocateRobot(parent_->getCollisionRobot()); - crobot_const_ = crobot_; - } - - crobot_->setLinkPadding(src.getCollisionRobot()->getLinkPadding()); - crobot_->setLinkScale(src.getCollisionRobot()->getLinkScale()); + cenv_->setLinkPadding(src.getCollisionEnv()->getLinkPadding()); + cenv_->setLinkScale(src.getCollisionEnv()->getLinkScale()); } void PlanningScene::propogateRobotPadding() { - if (!active_collision_->crobot_) - return; - for (std::pair& it : collision_) { if (it.second != active_collision_) @@ -292,26 +272,20 @@ void PlanningScene::addCollisionDetector(const collision_detection::CollisionDet detector->findParent(*this); - detector->cworld_ = detector->alloc_->allocateWorld(world_); - detector->cworld_const_ = detector->cworld_; + detector->cenv_ = detector->alloc_->allocateEnv(world_, getRobotModel()); + detector->cenv_const_ = detector->cenv_; - // Allocate CollisionRobot unless we can use the parent's crobot_. - // If active_collision_->crobot_ is non-NULL there is local padding and we cannot use the parent's crobot_. - if (!detector->parent_ || active_collision_->crobot_) + // if the current active detector is not the added one, copy its padding to the new one and allocate unpadded + if (detector != active_collision_) { - detector->crobot_ = detector->alloc_->allocateRobot(getRobotModel()); - detector->crobot_const_ = detector->crobot_; + detector->cenv_unpadded_ = detector->alloc_->allocateEnv(world_, getRobotModel()); + detector->cenv_unpadded_const_ = detector->cenv_unpadded_; - if (detector != active_collision_) - detector->copyPadding(*active_collision_); + detector->copyPadding(*active_collision_); } - // Allocate CollisionRobot unless we can use the parent's crobot_unpadded_. - if (!detector->parent_) - { - detector->crobot_unpadded_ = detector->alloc_->allocateRobot(getRobotModel()); - detector->crobot_unpadded_const_ = detector->crobot_unpadded_; - } + detector->cenv_unpadded_ = detector->alloc_->allocateEnv(world_, getRobotModel()); + detector->cenv_unpadded_const_ = detector->cenv_unpadded_; } void PlanningScene::setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator, @@ -349,8 +323,9 @@ bool PlanningScene::setActiveCollisionDetector(const std::string& collision_dete } else { - ROS_ERROR_NAMED(LOGNAME, "Cannot setActiveCollisionDetector to '%s' -- it has been added to PlanningScene. " - "Keeping existing active collision detector '%s'", + ROS_ERROR_NAMED(LOGNAME, + "Cannot setActiveCollisionDetector to '%s' -- it has been added to PlanningScene. " + "Keeping existing active collision detector '%s'", collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); return false; } @@ -364,47 +339,34 @@ void PlanningScene::getCollisionDetectorNames(std::vector& names) c names.push_back(it.first); } -const collision_detection::CollisionWorldConstPtr& -PlanningScene::getCollisionWorld(const std::string& collision_detector_name) const -{ - CollisionDetectorConstIterator it = collision_.find(collision_detector_name); - if (it == collision_.end()) - { - ROS_ERROR_NAMED(LOGNAME, "Could not get CollisionWorld named '%s'. Returning active CollisionWorld '%s' instead", - collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); - return active_collision_->cworld_const_; - } - - return it->second->cworld_const_; -} - -const collision_detection::CollisionRobotConstPtr& -PlanningScene::getCollisionRobot(const std::string& collision_detector_name) const +const collision_detection::CollisionEnvConstPtr& +PlanningScene::getCollisionEnv(const std::string& collision_detector_name) const { CollisionDetectorConstIterator it = collision_.find(collision_detector_name); if (it == collision_.end()) { ROS_ERROR_NAMED(LOGNAME, "Could not get CollisionRobot named '%s'. Returning active CollisionRobot '%s' instead", collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); - return active_collision_->getCollisionRobot(); + return active_collision_->getCollisionEnv(); } - return it->second->getCollisionRobot(); + return it->second->getCollisionEnv(); } -const collision_detection::CollisionRobotConstPtr& -PlanningScene::getCollisionRobotUnpadded(const std::string& collision_detector_name) const +const collision_detection::CollisionEnvConstPtr& +PlanningScene::getCollisionEnvUnpadded(const std::string& collision_detector_name) const { CollisionDetectorConstIterator it = collision_.find(collision_detector_name); if (it == collision_.end()) { - ROS_ERROR_NAMED(LOGNAME, "Could not get CollisionRobotUnpadded named '%s'. " - "Returning active CollisionRobotUnpadded '%s' instead", + ROS_ERROR_NAMED(LOGNAME, + "Could not get CollisionRobotUnpadded named '%s'. " + "Returning active CollisionRobotUnpadded '%s' instead", collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); - return active_collision_->getCollisionRobotUnpadded(); + return active_collision_->getCollisionEnvUnpadded(); } - return it->second->getCollisionRobotUnpadded(); + return it->second->getCollisionEnvUnpadded(); } void PlanningScene::clearDiffs() @@ -427,20 +389,16 @@ void PlanningScene::clearDiffs() if (it.second->parent_) { - it.second->crobot_.reset(); - it.second->crobot_const_.reset(); - it.second->crobot_unpadded_.reset(); - it.second->crobot_unpadded_const_.reset(); + it.second->cenv_unpadded_ = it.second->alloc_->allocateEnv(it.second->parent_->cenv_unpadded_, world_); + it.second->cenv_unpadded_const_ = it.second->cenv_unpadded_; - it.second->cworld_ = it.second->alloc_->allocateWorld(it.second->parent_->cworld_, world_); - it.second->cworld_const_ = it.second->cworld_; + it.second->cenv_ = it.second->alloc_->allocateEnv(it.second->parent_->cenv_, world_); + it.second->cenv_const_ = it.second->cenv_; } - else + else // This is the parent CollisionDetector { it.second->copyPadding(*parent_->active_collision_); - - it.second->cworld_ = it.second->alloc_->allocateWorld(world_); - it.second->cworld_const_ = it.second->cworld_; + it.second->cenv_const_ = it.second->cenv_; } } @@ -465,26 +423,22 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene) // push colors and types for attached objects std::vector attached_objs; robot_state_->getAttachedBodies(attached_objs); - for (std::vector::const_iterator it = attached_objs.begin(); - it != attached_objs.end(); ++it) + for (const moveit::core::AttachedBody* attached_obj : attached_objs) { - if (hasObjectType((*it)->getName())) - scene->setObjectType((*it)->getName(), getObjectType((*it)->getName())); - if (hasObjectColor((*it)->getName())) - scene->setObjectColor((*it)->getName(), getObjectColor((*it)->getName())); + if (hasObjectType(attached_obj->getName())) + scene->setObjectType(attached_obj->getName(), getObjectType(attached_obj->getName())); + if (hasObjectColor(attached_obj->getName())) + scene->setObjectColor(attached_obj->getName(), getObjectColor(attached_obj->getName())); } } if (acm_) scene->getAllowedCollisionMatrixNonConst() = *acm_; - if (active_collision_->crobot_) - { - collision_detection::CollisionRobotPtr active_crobot = scene->getCollisionRobotNonConst(); - active_crobot->setLinkPadding(active_collision_->crobot_->getLinkPadding()); - active_crobot->setLinkScale(active_collision_->crobot_->getLinkScale()); - scene->propogateRobotPadding(); - } + collision_detection::CollisionEnvPtr active_cenv = scene->getCollisionEnvNonConst(); + active_cenv->setLinkPadding(active_collision_->cenv_->getLinkPadding()); + active_cenv->setLinkScale(active_collision_->cenv_->getLinkScale()); + scene->propogateRobotPadding(); if (world_diff_) { @@ -523,15 +477,15 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest& void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state) const + const moveit::core::RobotState& robot_state) const { // check collision with the world using the padded version - getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobot(), robot_state, getAllowedCollisionMatrix()); + getCollisionEnv()->checkRobotCollision(req, res, robot_state, getAllowedCollisionMatrix()); if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) { // do self-collision checking with the unpadded version of the robot - getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix()); + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix()); } } @@ -546,15 +500,15 @@ void PlanningScene::checkSelfCollision(const collision_detection::CollisionReque void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { // check collision with the world using the padded version - getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobot(), robot_state, acm); + getCollisionEnv()->checkRobotCollision(req, res, robot_state, acm); // do self-collision checking with the unpadded version of the robot if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) - getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, acm); + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); } void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req, @@ -568,16 +522,16 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { // check collision with the world using the unpadded version - getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobotUnpadded(), robot_state, acm); + getCollisionEnvUnpadded()->checkRobotCollision(req, res, robot_state, acm); // do self-collision checking with the unpadded version of the robot if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) { - getCollisionRobotUnpadded()->checkSelfCollision(req, res, robot_state, acm); + getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm); } } @@ -590,7 +544,7 @@ void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::Cont } void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { collision_detection::CollisionRequest req; @@ -610,7 +564,7 @@ void PlanningScene::getCollidingLinks(std::vector& links) getCollidingLinks(links, getCurrentState(), getAllowedCollisionMatrix()); } -void PlanningScene::getCollidingLinks(std::vector& links, const robot_state::RobotState& robot_state, +void PlanningScene::getCollidingLinks(std::vector& links, const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm) const { collision_detection::CollisionResult::ContactMap contacts; @@ -627,36 +581,30 @@ void PlanningScene::getCollidingLinks(std::vector& links, const rob } } -const collision_detection::CollisionRobotPtr& PlanningScene::getCollisionRobotNonConst() +const collision_detection::CollisionEnvPtr& PlanningScene::getCollisionEnvNonConst() { - if (!active_collision_->crobot_) - { - active_collision_->crobot_ = - active_collision_->alloc_->allocateRobot(active_collision_->parent_->getCollisionRobot()); - active_collision_->crobot_const_ = active_collision_->crobot_; - } - return active_collision_->crobot_; + return active_collision_->cenv_; } -robot_state::RobotState& PlanningScene::getCurrentStateNonConst() +moveit::core::RobotState& PlanningScene::getCurrentStateNonConst() { if (!robot_state_) { - robot_state_.reset(new robot_state::RobotState(parent_->getCurrentState())); + robot_state_.reset(new moveit::core::RobotState(parent_->getCurrentState())); robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } robot_state_->update(); return *robot_state_; } -robot_state::RobotStatePtr PlanningScene::getCurrentStateUpdated(const moveit_msgs::RobotState& update) const +moveit::core::RobotStatePtr PlanningScene::getCurrentStateUpdated(const moveit_msgs::RobotState& update) const { - robot_state::RobotStatePtr state(new robot_state::RobotState(getCurrentState())); - robot_state::robotStateMsgToRobotState(getTransforms(), update, *state); + moveit::core::RobotStatePtr state(new moveit::core::RobotState(getCurrentState())); + moveit::core::robotStateMsgToRobotState(getTransforms(), update, *state); return state; } -void PlanningScene::setAttachedBodyUpdateCallback(const robot_state::AttachedBodyCallback& callback) +void PlanningScene::setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback& callback) { current_state_attached_body_callback_ = callback; if (robot_state_) @@ -679,14 +627,14 @@ collision_detection::AllowedCollisionMatrix& PlanningScene::getAllowedCollisionM return *acm_; } -const robot_state::Transforms& PlanningScene::getTransforms() +const moveit::core::Transforms& PlanningScene::getTransforms() { // Trigger an update of the robot transforms getCurrentStateNonConst().update(); return static_cast(this)->getTransforms(); } -robot_state::Transforms& PlanningScene::getTransformsNonConst() +moveit::core::Transforms& PlanningScene::getTransformsNonConst() { // Trigger an update of the robot transforms getCurrentStateNonConst().update(); @@ -712,7 +660,7 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene_ms scene_msg.fixed_frame_transforms.clear(); if (robot_state_) - robot_state::robotStateToRobotStateMsg(*robot_state_, scene_msg.robot_state); + moveit::core::robotStateToRobotStateMsg(*robot_state_, scene_msg.robot_state); else { scene_msg.robot_state = moveit_msgs::RobotState(); @@ -724,16 +672,8 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene_ms else scene_msg.allowed_collision_matrix = moveit_msgs::AllowedCollisionMatrix(); - if (active_collision_->crobot_) - { - active_collision_->crobot_->getPadding(scene_msg.link_padding); - active_collision_->crobot_->getScale(scene_msg.link_scale); - } - else - { - scene_msg.link_padding.clear(); - scene_msg.link_scale.clear(); - } + active_collision_->cenv_->getPadding(scene_msg.link_padding); + active_collision_->cenv_->getScale(scene_msg.link_scale); scene_msg.object_colors.clear(); if (object_colors_) @@ -819,7 +759,7 @@ bool PlanningScene::getCollisionObjectMsg(moveit_msgs::CollisionObject& collisio collision_obj.header.frame_id = getPlanningFrame(); collision_obj.id = ns; collision_obj.operation = moveit_msgs::CollisionObject::ADD; - collision_detection::CollisionWorld::ObjectConstPtr obj = world_->getObject(ns); + collision_detection::CollisionEnv::ObjectConstPtr obj = world_->getObject(ns); if (!obj) return false; ShapeVisitorAddToCollisionObject sv(&collision_obj); @@ -839,7 +779,7 @@ bool PlanningScene::getCollisionObjectMsg(moveit_msgs::CollisionObject& collisio if (hasObjectType(collision_obj.id)) collision_obj.type = getObjectType(collision_obj.id); } - for (auto frame_pair : obj->subframe_poses_) + for (const auto& frame_pair : obj->subframe_poses_) { collision_obj.subframe_names.push_back(frame_pair.first); geometry_msgs::Pose p; @@ -891,7 +831,7 @@ bool PlanningScene::getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const octomap.header.frame_id = getPlanningFrame(); octomap.octomap = octomap_msgs::Octomap(); - collision_detection::CollisionWorld::ObjectConstPtr map = world_->getObject(OCTOMAP_NS); + collision_detection::CollisionEnv::ObjectConstPtr map = world_->getObject(OCTOMAP_NS); if (map) { if (map->shapes_.size() == 1) @@ -929,10 +869,10 @@ void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg) c scene_msg.robot_model_name = getRobotModel()->getName(); getTransforms().copyTransforms(scene_msg.fixed_frame_transforms); - robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state); + moveit::core::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state); getAllowedCollisionMatrix().getMessage(scene_msg.allowed_collision_matrix); - getCollisionRobot()->getPadding(scene_msg.link_padding); - getCollisionRobot()->getScale(scene_msg.link_scale); + getCollisionEnv()->getPadding(scene_msg.link_padding); + getCollisionEnv()->getScale(scene_msg.link_scale); getObjectColorMsgs(scene_msg.object_colors); @@ -958,7 +898,7 @@ void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg, if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS) { - robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, true); + moveit::core::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, true); for (moveit_msgs::AttachedCollisionObject& attached_collision_object : scene_msg.robot_state.attached_collision_objects) { @@ -970,7 +910,7 @@ void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg, } else if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE) { - robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, false); + moveit::core::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, false); } if (comp.components & moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX) @@ -978,8 +918,8 @@ void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg, if (comp.components & moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING) { - getCollisionRobot()->getPadding(scene_msg.link_padding); - getCollisionRobot()->getScale(scene_msg.link_scale); + getCollisionEnv()->getPadding(scene_msg.link_padding); + getCollisionEnv()->getScale(scene_msg.link_scale); } if (comp.components & moveit_msgs::PlanningSceneComponents::OBJECT_COLORS) @@ -1016,7 +956,7 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const for (const std::string& id : ids) if (id != OCTOMAP_NS) { - collision_detection::CollisionWorld::ObjectConstPtr obj = world_->getObject(id); + collision_detection::CollisionEnv::ObjectConstPtr obj = world_->getObject(id); if (obj) { out << "* " << id << std::endl; @@ -1024,9 +964,10 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const for (std::size_t j = 0; j < obj->shapes_.size(); ++j) { shapes::saveAsText(obj->shapes_[j].get(), out); + // shape_poses_ is valid isometry by contract out << obj->shape_poses_[j].translation().x() << " " << obj->shape_poses_[j].translation().y() << " " << obj->shape_poses_[j].translation().z() << std::endl; - Eigen::Quaterniond r(obj->shape_poses_[j].rotation()); + Eigen::Quaterniond r(obj->shape_poses_[j].linear()); out << r.x() << " " << r.y() << " " << r.z() << " " << r.w() << std::endl; if (hasObjectColor(id)) { @@ -1077,8 +1018,8 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet in >> shape_count; for (std::size_t i = 0; i < shape_count && in.good() && !in.eof(); ++i) { - shapes::Shape* s = shapes::constructShapeFromText(in); - if (!s) + const auto shape = shapes::ShapeConstPtr(shapes::constructShapeFromText(in)); + if (!shape) { ROS_ERROR_NAMED(LOGNAME, "Failed to load shape from scene file"); return false; @@ -1100,12 +1041,12 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet ROS_ERROR_NAMED(LOGNAME, "Improperly formatted color in scene geometry file"); return false; } - if (s) + if (shape) { Eigen::Isometry3d pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz); // Transform pose by input pose offset pose = offset * pose; - world_->addToObject(ns, shapes::ShapePtr(s), pose); + world_->addToObject(ns, shape, pose); if (r > 0.0f || g > 0.0f || b > 0.0f || a > 0.0f) { std_msgs::ColorRGBA color; @@ -1142,20 +1083,21 @@ void PlanningScene::setCurrentState(const moveit_msgs::RobotState& state) { if (!robot_state_) { - robot_state_.reset(new robot_state::RobotState(parent_->getCurrentState())); + robot_state_.reset(new moveit::core::RobotState(parent_->getCurrentState())); robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } - robot_state::robotStateMsgToRobotState(getTransforms(), state_no_attached, *robot_state_); + moveit::core::robotStateMsgToRobotState(getTransforms(), state_no_attached, *robot_state_); } else - robot_state::robotStateMsgToRobotState(*scene_transforms_, state_no_attached, *robot_state_); + moveit::core::robotStateMsgToRobotState(*scene_transforms_, state_no_attached, *robot_state_); for (std::size_t i = 0; i < state.attached_collision_objects.size(); ++i) { if (!state.is_diff && state.attached_collision_objects[i].object.operation != moveit_msgs::CollisionObject::ADD) { - ROS_ERROR_NAMED(LOGNAME, "The specified RobotState is not marked as is_diff. " - "The request to modify the object '%s' is not supported. Object is ignored.", + ROS_ERROR_NAMED(LOGNAME, + "The specified RobotState is not marked as is_diff. " + "The request to modify the object '%s' is not supported. Object is ignored.", state.attached_collision_objects[i].object.id.c_str()); continue; } @@ -1163,7 +1105,7 @@ void PlanningScene::setCurrentState(const moveit_msgs::RobotState& state) } } -void PlanningScene::setCurrentState(const robot_state::RobotState& state) +void PlanningScene::setCurrentState(const moveit::core::RobotState& state) { getCurrentStateNonConst() = state; } @@ -1182,7 +1124,7 @@ void PlanningScene::decoupleParent() if (!robot_state_) { - robot_state_.reset(new robot_state::RobotState(parent_->getCurrentState())); + robot_state_.reset(new moveit::core::RobotState(parent_->getCurrentState())); robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } @@ -1191,16 +1133,6 @@ void PlanningScene::decoupleParent() for (std::pair& it : collision_) { - if (!it.second->crobot_) - { - it.second->crobot_ = it.second->alloc_->allocateRobot(it.second->parent_->getCollisionRobot()); - it.second->crobot_const_ = it.second->crobot_; - } - if (!it.second->crobot_unpadded_) - { - it.second->crobot_unpadded_ = it.second->alloc_->allocateRobot(it.second->parent_->getCollisionRobotUnpadded()); - it.second->crobot_unpadded_const_ = it.second->crobot_unpadded_; - } it.second->parent_.reset(); } world_diff_.reset(); @@ -1272,13 +1204,8 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& sc { for (std::pair& it : collision_) { - if (!it.second->crobot_) - { - it.second->crobot_ = it.second->alloc_->allocateRobot(it.second->parent_->getCollisionRobot()); - it.second->crobot_const_ = it.second->crobot_; - } - it.second->crobot_->setPadding(scene_msg.link_padding); - it.second->crobot_->setScale(scene_msg.link_scale); + it.second->cenv_->setPadding(scene_msg.link_padding); + it.second->cenv_->setScale(scene_msg.link_scale); } } @@ -1315,13 +1242,8 @@ bool PlanningScene::setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene_ acm_.reset(new collision_detection::AllowedCollisionMatrix(scene_msg.allowed_collision_matrix)); for (std::pair& it : collision_) { - if (!it.second->crobot_) - { - it.second->crobot_ = it.second->alloc_->allocateRobot(it.second->parent_->getCollisionRobot()); - it.second->crobot_const_ = it.second->crobot_; - } - it.second->crobot_->setPadding(scene_msg.link_padding); - it.second->crobot_->setScale(scene_msg.link_scale); + it.second->cenv_->setPadding(scene_msg.link_padding); + it.second->cenv_->setScale(scene_msg.link_scale); } object_colors_.reset(new ObjectColorMap()); for (const moveit_msgs::ObjectColor& object_color : scene_msg.object_colors) @@ -1364,7 +1286,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::Octomap& map) std::shared_ptr om(static_cast(octomap_msgs::msgToMap(map))); if (!map.header.frame_id.empty()) { - const Eigen::Isometry3d& t = getTransforms().getTransform(map.header.frame_id); + const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id); world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t); } else @@ -1400,7 +1322,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose& map) } std::shared_ptr om(static_cast(octomap_msgs::msgToMap(map.octomap))); - const Eigen::Isometry3d& t = getTransforms().getTransform(map.header.frame_id); + const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id); Eigen::Isometry3d p; tf2::fromMsg(map.origin, p); p = t * p; @@ -1409,7 +1331,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose& map) void PlanningScene::processOctomapPtr(const std::shared_ptr& octree, const Eigen::Isometry3d& t) { - collision_detection::CollisionWorld::ObjectConstPtr map = world_->getObject(OCTOMAP_NS); + collision_detection::CollisionEnv::ObjectConstPtr map = world_->getObject(OCTOMAP_NS); if (map) { if (map->shapes_.size() == 1) @@ -1456,7 +1378,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache if (!robot_state_) // there must be a parent in this case { - robot_state_.reset(new robot_state::RobotState(parent_->getCurrentState())); + robot_state_.reset(new moveit::core::RobotState(parent_->getCurrentState())); robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } robot_state_->update(); @@ -1498,7 +1420,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache return false; } - const robot_model::LinkModel* link_model = getRobotModel()->getLinkModel(object.link_name); + const moveit::core::LinkModel* link_model = getRobotModel()->getLinkModel(object.link_name); if (link_model) { // items to build the attached object from (filled from existing world object or message) @@ -1510,7 +1432,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache // TODO(felixvd): This code may be duplicated in robot_state/conversions.cpp // STEP 1.1: Get shapes and poses from existing world object or message. - collision_detection::CollisionWorld::ObjectConstPtr obj_in_world = world_->getObject(object.object.id); + collision_detection::CollisionEnv::ObjectConstPtr obj_in_world = world_->getObject(object.object.id); if (object.object.operation == moveit_msgs::CollisionObject::ADD && object.object.primitives.empty() && object.object.meshes.empty() && object.object.planes.empty()) { @@ -1521,8 +1443,6 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache // Extract the shapes from the world shapes = obj_in_world->shapes_; poses = obj_in_world->shape_poses_; - // Remove the object from the collision world - world_->removeObject(object.object.id); // Transform shape poses to the link frame const Eigen::Isometry3d& inv_transform = robot_state_->getGlobalLinkTransform(link_model).inverse(); @@ -1531,8 +1451,9 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache } else { - ROS_ERROR_NAMED(LOGNAME, "Attempting to attach object '%s' to link '%s' but no geometry specified " - "and such an object does not exist in the collision world", + ROS_ERROR_NAMED(LOGNAME, + "Attempting to attach object '%s' to link '%s' but no geometry specified " + "and such an object does not exist in the collision world", object.object.id.c_str(), object.link_name.c_str()); return false; } @@ -1574,7 +1495,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache if (object.object.header.frame_id != object.link_name) { const Eigen::Isometry3d& transform = robot_state_->getGlobalLinkTransform(link_model).inverse() * - getTransforms().getTransform(object.object.header.frame_id); + getFrameTransform(object.object.header.frame_id); for (Eigen::Isometry3d& pose : poses) pose = transform * pose; } @@ -1614,7 +1535,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache if (object.object.header.frame_id != object.link_name) { const Eigen::Isometry3d& transform = robot_state_->getGlobalLinkTransform(link_model).inverse() * - getTransforms().getTransform(object.object.header.frame_id); + getFrameTransform(object.object.header.frame_id); for (auto& subframe : subframe_poses) subframe.second = transform * subframe.second; } @@ -1627,8 +1548,9 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache ROS_DEBUG_NAMED(LOGNAME, "Removing world object with the same name as newly attached object: '%s'", object.object.id.c_str()); else - ROS_WARN_NAMED(LOGNAME, "You tried to append geometry to an attached object " - "that is actually a world object ('%s'). World geometry is ignored.", + ROS_WARN_NAMED(LOGNAME, + "You tried to append geometry to an attached object " + "that is actually a world object ('%s'). World geometry is ignored.", object.object.id.c_str()); } @@ -1637,8 +1559,9 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache !robot_state_->hasAttachedBody(object.object.id)) { if (robot_state_->clearAttachedBody(object.object.id)) - ROS_DEBUG_NAMED(LOGNAME, "The robot state already had an object named '%s' attached to link '%s'. " - "The object was replaced.", + ROS_DEBUG_NAMED(LOGNAME, + "The robot state already had an object named '%s' attached to link '%s'. " + "The object was replaced.", object.object.id.c_str(), object.link_name.c_str()); robot_state_->attachBody(object.object.id, shapes, poses, object.touch_links, object.link_name, @@ -1648,7 +1571,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache } else // APPEND: augment to existing attached object { - const robot_state::AttachedBody* ab = robot_state_->getAttachedBody(object.object.id); + const moveit::core::AttachedBody* ab = robot_state_->getAttachedBody(object.object.id); shapes.insert(shapes.end(), ab->getShapes().begin(), ab->getShapes().end()); poses.insert(poses.end(), ab->getFixedTransforms().begin(), ab->getFixedTransforms().end()); subframe_poses.insert(ab->getSubframeTransforms().begin(), ab->getSubframeTransforms().end()); @@ -1673,44 +1596,37 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache else if (object.object.operation == moveit_msgs::CollisionObject::REMOVE) // == DETACH { // STEP 1: Get info about the object from the RobotState - std::vector attached_bodies; - if (object.link_name.empty()) + std::vector attached_bodies; + if (object.object.id.empty()) { - if (object.object.id.empty()) - robot_state_->getAttachedBodies(attached_bodies); + const moveit::core::LinkModel* link_model = + object.link_name.empty() ? nullptr : getRobotModel()->getLinkModel(object.link_name); + if (link_model) // if we have a link model specified, only fetch bodies attached to this link + robot_state_->getAttachedBodies(attached_bodies, link_model); else - { - const robot_state::AttachedBody* body = robot_state_->getAttachedBody(object.object.id); - if (body) - attached_bodies.push_back(body); - } + robot_state_->getAttachedBodies(attached_bodies); } - else + else // A specific object id will be removed. { - const robot_model::LinkModel* link_model = getRobotModel()->getLinkModel(object.link_name); - if (link_model) + const moveit::core::AttachedBody* body = robot_state_->getAttachedBody(object.object.id); + if (body) { - // If no specific object id is given, then we remove all objects attached to the link_name. - if (object.object.id.empty()) - { - robot_state_->getAttachedBodies(attached_bodies, link_model); - } - else // A specific object id will be removed. + if (!object.link_name.empty() && (body->getAttachedLinkName() != object.link_name)) { - const robot_state::AttachedBody* body = robot_state_->getAttachedBody(object.object.id); - if (body) - attached_bodies.push_back(body); + ROS_ERROR_STREAM_NAMED(LOGNAME, "The AttachedCollisionObject message states the object is attached to " + << object.link_name << ", but it is actually attached to " + << body->getAttachedLinkName() + << ". Leave the link_name empty or specify the correct link."); + return false; } + attached_bodies.push_back(body); } } // STEP 2+3: Remove the attached object(s) from the RobotState and put them in the world for (const moveit::core::AttachedBody* attached_body : attached_bodies) { - const std::vector& shapes = attached_body->getShapes(); - const EigenSTL::vector_Isometry3d& poses = attached_body->getGlobalCollisionBodyTransforms(); const std::string& name = attached_body->getName(); - if (world_->hasObject(name)) ROS_WARN_NAMED(LOGNAME, "The collision world already has an object with the same name as the body about to be detached. " @@ -1718,7 +1634,8 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache object.object.id.c_str()); else { - world_->addToObject(name, shapes, poses); + world_->addToObject(name, attached_body->getShapes(), attached_body->getGlobalCollisionBodyTransforms()); + world_->setSubframesOfObject(name, attached_body->getSubframeTransforms()); ROS_DEBUG_NAMED(LOGNAME, "Detached object '%s' from link '%s' and added it back in the collision world", name.c_str(), object.link_name.c_str()); } @@ -1769,7 +1686,15 @@ void PlanningScene::poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Isomet { Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z); Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z); - quaternion.normalize(); + if ((quaternion.x() == 0) && (quaternion.y() == 0) && (quaternion.z() == 0) && (quaternion.w() == 0)) + { + ROS_WARN_NAMED(LOGNAME, "Empty quaternion found in pose message. Setting to neutral orientation."); + quaternion.setIdentity(); + } + else + { + quaternion.normalize(); + } out = translation * quaternion; } @@ -1800,7 +1725,7 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject return false; } - if (!getTransforms().canTransform(object.header.frame_id)) + if (!knowsFrameTransform(object.header.frame_id)) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Unknown frame: " << object.header.frame_id); return false; @@ -1810,7 +1735,7 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject if (object.operation == moveit_msgs::CollisionObject::ADD && world_->hasObject(object.id)) world_->removeObject(object.id); - const Eigen::Isometry3d& object_frame_transform = getTransforms().getTransform(object.header.frame_id); + const Eigen::Isometry3d& object_frame_transform = getFrameTransform(object.header.frame_id); for (std::size_t i = 0; i < object.primitives.size(); ++i) { @@ -1845,8 +1770,8 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject if (!object.type.key.empty() || !object.type.db.empty()) setObjectType(object.id, object.type); - // Add subframes - moveit::core::FixedTransformsMap subframes; + // Add subframes to the newly created (or possibly modified) object + moveit::core::FixedTransformsMap subframes = world_->getObject(object.id)->subframe_poses_; Eigen::Isometry3d frame_pose; for (std::size_t i = 0; i < object.subframe_poses.size(); ++i) { @@ -1881,7 +1806,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::CollisionObjec ROS_WARN_NAMED(LOGNAME, "Move operation for object '%s' ignores the geometry specified in the message.", object.id.c_str()); - const Eigen::Isometry3d& t = getTransforms().getTransform(object.header.frame_id); + const Eigen::Isometry3d& t = getFrameTransform(object.header.frame_id); EigenSTL::vector_Isometry3d new_poses; for (const geometry_msgs::Pose& primitive_pose : object.primitive_poses) { @@ -1903,6 +1828,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::CollisionObjec } collision_detection::World::ObjectConstPtr obj = world_->getObject(object.id); + if (obj->shapes_.size() == new_poses.size()) { std::vector shapes = obj->shapes_; @@ -1912,8 +1838,9 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::CollisionObjec } else { - ROS_ERROR_NAMED(LOGNAME, "Number of supplied poses (%zu) for object '%s' does not match number of shapes (%zu). " - "Not moving.", + ROS_ERROR_NAMED(LOGNAME, + "Number of supplied poses (%zu) for object '%s' does not match number of shapes (%zu). " + "Not moving.", new_poses.size(), object.id.c_str(), obj->shapes_.size()); return false; } @@ -1937,7 +1864,7 @@ const Eigen::Isometry3d& PlanningScene::getFrameTransform(const std::string& fra return getFrameTransform(getCurrentState(), frame_id); } -const Eigen::Isometry3d& PlanningScene::getFrameTransform(const robot_state::RobotState& state, +const Eigen::Isometry3d& PlanningScene::getFrameTransform(const moveit::core::RobotState& state, const std::string& frame_id) const { if (!frame_id.empty() && frame_id[0] == '/') @@ -1960,7 +1887,7 @@ bool PlanningScene::knowsFrameTransform(const std::string& frame_id) const return knowsFrameTransform(getCurrentState(), frame_id); } -bool PlanningScene::knowsFrameTransform(const robot_state::RobotState& state, const std::string& frame_id) const +bool PlanningScene::knowsFrameTransform(const moveit::core::RobotState& state, const std::string& frame_id) const { if (!frame_id.empty() && frame_id[0] == '/') return knowsFrameTransform(frame_id.substr(1)); @@ -2073,8 +2000,8 @@ void PlanningScene::removeObjectColor(const std::string& object_id) bool PlanningScene::isStateColliding(const moveit_msgs::RobotState& state, const std::string& group, bool verbose) const { - robot_state::RobotState s(getCurrentState()); - robot_state::robotStateMsgToRobotState(getTransforms(), state, s); + moveit::core::RobotState s(getCurrentState()); + moveit::core::robotStateMsgToRobotState(getTransforms(), state, s); return isStateColliding(s, group, verbose); } @@ -2086,7 +2013,7 @@ bool PlanningScene::isStateColliding(const std::string& group, bool verbose) return isStateColliding(getCurrentState(), group, verbose); } -bool PlanningScene::isStateColliding(const robot_state::RobotState& state, const std::string& group, bool verbose) const +bool PlanningScene::isStateColliding(const moveit::core::RobotState& state, const std::string& group, bool verbose) const { collision_detection::CollisionRequest req; req.verbose = verbose; @@ -2100,14 +2027,14 @@ bool PlanningScene::isStateFeasible(const moveit_msgs::RobotState& state, bool v { if (state_feasibility_) { - robot_state::RobotState s(getCurrentState()); - robot_state::robotStateMsgToRobotState(getTransforms(), state, s); + moveit::core::RobotState s(getCurrentState()); + moveit::core::robotStateMsgToRobotState(getTransforms(), state, s); return state_feasibility_(s, verbose); } return true; } -bool PlanningScene::isStateFeasible(const robot_state::RobotState& state, bool verbose) const +bool PlanningScene::isStateFeasible(const moveit::core::RobotState& state, bool verbose) const { if (state_feasibility_) return state_feasibility_(state, verbose); @@ -2117,12 +2044,12 @@ bool PlanningScene::isStateFeasible(const robot_state::RobotState& state, bool v bool PlanningScene::isStateConstrained(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr, bool verbose) const { - robot_state::RobotState s(getCurrentState()); - robot_state::robotStateMsgToRobotState(getTransforms(), state, s); + moveit::core::RobotState s(getCurrentState()); + moveit::core::robotStateMsgToRobotState(getTransforms(), state, s); return isStateConstrained(s, constr, verbose); } -bool PlanningScene::isStateConstrained(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr, +bool PlanningScene::isStateConstrained(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr, bool verbose) const { kinematic_constraints::KinematicConstraintSetPtr ks( @@ -2137,18 +2064,18 @@ bool PlanningScene::isStateConstrained(const robot_state::RobotState& state, con bool PlanningScene::isStateConstrained(const moveit_msgs::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, bool verbose) const { - robot_state::RobotState s(getCurrentState()); - robot_state::robotStateMsgToRobotState(getTransforms(), state, s); + moveit::core::RobotState s(getCurrentState()); + moveit::core::robotStateMsgToRobotState(getTransforms(), state, s); return isStateConstrained(s, constr, verbose); } -bool PlanningScene::isStateConstrained(const robot_state::RobotState& state, +bool PlanningScene::isStateConstrained(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, bool verbose) const { return constr.decide(state, verbose).satisfied; } -bool PlanningScene::isStateValid(const robot_state::RobotState& state, const std::string& group, bool verbose) const +bool PlanningScene::isStateValid(const moveit::core::RobotState& state, const std::string& group, bool verbose) const { static const moveit_msgs::Constraints EMP_CONSTRAINTS; return isStateValid(state, EMP_CONSTRAINTS, group, verbose); @@ -2163,12 +2090,12 @@ bool PlanningScene::isStateValid(const moveit_msgs::RobotState& state, const std bool PlanningScene::isStateValid(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr, const std::string& group, bool verbose) const { - robot_state::RobotState s(getCurrentState()); - robot_state::robotStateMsgToRobotState(getTransforms(), state, s); + moveit::core::RobotState s(getCurrentState()); + moveit::core::robotStateMsgToRobotState(getTransforms(), state, s); return isStateValid(s, constr, group, verbose); } -bool PlanningScene::isStateValid(const robot_state::RobotState& state, const moveit_msgs::Constraints& constr, +bool PlanningScene::isStateValid(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr, const std::string& group, bool verbose) const { if (isStateColliding(state, group, verbose)) @@ -2178,7 +2105,7 @@ bool PlanningScene::isStateValid(const robot_state::RobotState& state, const mov return isStateConstrained(state, constr, verbose); } -bool PlanningScene::isStateValid(const robot_state::RobotState& state, +bool PlanningScene::isStateValid(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, const std::string& group, bool verbose) const { @@ -2224,8 +2151,8 @@ bool PlanningScene::isPathValid(const moveit_msgs::RobotState& start_state, bool verbose, std::vector* invalid_index) const { robot_trajectory::RobotTrajectory t(getRobotModel(), group); - robot_state::RobotState start(getCurrentState()); - robot_state::robotStateMsgToRobotState(getTransforms(), start_state, start); + moveit::core::RobotState start(getCurrentState()); + moveit::core::robotStateMsgToRobotState(getTransforms(), start_state, start); t.setRobotTrajectoryMsg(start, trajectory); return isPathValid(t, path_constraints, goal_constraints, group, verbose, invalid_index); } @@ -2243,7 +2170,7 @@ bool PlanningScene::isPathValid(const robot_trajectory::RobotTrajectory& traject std::size_t n_wp = trajectory.getWayPointCount(); for (std::size_t i = 0; i < n_wp; ++i) { - const robot_state::RobotState& st = trajectory.getWayPoint(i); + const moveit::core::RobotState& st = trajectory.getWayPoint(i); bool this_state_valid = true; if (isStateColliding(st, group, verbose)) @@ -2352,13 +2279,13 @@ void PlanningScene::getCostSources(const robot_trajectory::RobotTrajectory& traj collision_detection::removeOverlapping(costs, overlap_fraction); } -void PlanningScene::getCostSources(const robot_state::RobotState& state, std::size_t max_costs, +void PlanningScene::getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, std::set& costs) const { getCostSources(state, max_costs, std::string(), costs); } -void PlanningScene::getCostSources(const robot_state::RobotState& state, std::size_t max_costs, +void PlanningScene::getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, const std::string& group_name, std::set& costs) const { @@ -2374,7 +2301,7 @@ void PlanningScene::getCostSources(const robot_state::RobotState& state, std::si void PlanningScene::printKnownObjects(std::ostream& out) const { const std::vector& objects = getWorld()->getObjectIds(); - std::vector attached_bodies; + std::vector attached_bodies; getCurrentState().getAttachedBodies(attached_bodies); // Output @@ -2387,7 +2314,7 @@ void PlanningScene::printKnownObjects(std::ostream& out) const } out << " - Attached Bodies:\n"; - for (const robot_state::AttachedBody* attached_body : attached_bodies) + for (const moveit::core::AttachedBody* attached_body : attached_bodies) { out << "\t- " << attached_body->getName() << "\n"; } diff --git a/moveit_core/planning_scene/test/test_multi_threaded.cpp b/moveit_core/planning_scene/test/test_multi_threaded.cpp new file mode 100644 index 0000000000..2cad1d799d --- /dev/null +++ b/moveit_core/planning_scene/test/test_multi_threaded.cpp @@ -0,0 +1,131 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +const int TRIALS = 1000; +const int THREADS = 2; + +bool runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene* scene, + const moveit::core::RobotState* state) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + for (unsigned int i = 0; i < trials; ++i) + { + res.clear(); + scene->checkCollision(req, res, *state); + } + return res.collision; +} + +void runCollisionDetectionAssert(unsigned int id, unsigned int trials, const planning_scene::PlanningScene* scene, + const moveit::core::RobotState* state, bool expected_result) +{ + ASSERT_EQ(expected_result, runCollisionDetection(id, trials, scene, state)); +} + +class CollisionDetectorThreadedTest : public testing::Test +{ +protected: + void SetUp() override + { + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + ASSERT_TRUE(static_cast(robot_model_)); + + robot_state_.reset(new moveit::core::RobotState(robot_model_)); + planning_scene_.reset(new planning_scene::PlanningScene(robot_model_)); + } + + void TearDown() override + { + } + + bool robot_model_ok_; + + moveit::core::RobotModelPtr robot_model_; + + collision_detection::CollisionEnvPtr cenv_; + + collision_detection::AllowedCollisionMatrixPtr acm_; + + moveit::core::RobotStatePtr robot_state_; + + planning_scene::PlanningScenePtr planning_scene_; +}; + +/** \brief Tests the FCL collision detector in multiple threads. */ +TEST_F(CollisionDetectorThreadedTest, FCLThreaded) +{ + std::vector states; + std::vector threads; + std::vector collisions; + + for (unsigned int i = 0; i < THREADS; ++i) + { + moveit::core::RobotState* state = new moveit::core::RobotState(planning_scene_->getRobotModel()); + collision_detection::CollisionRequest req; + state->setToRandomPositions(); + state->update(); + states.push_back(moveit::core::RobotStatePtr(state)); + collisions.push_back(runCollisionDetection(0, 1, planning_scene_.get(), state)); + } + + for (unsigned int i = 0; i < THREADS; ++i) + threads.push_back(new std::thread( + std::bind(&runCollisionDetectionAssert, i, TRIALS, planning_scene_.get(), states[i].get(), collisions[i]))); + + for (unsigned int i = 0; i < states.size(); ++i) + { + threads[i]->join(); + delete threads[i]; + } +} + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 73b68186e3..28dece6b9a 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -36,6 +36,8 @@ #include #include +#include +#include #include #include #include @@ -43,42 +45,9 @@ #include #include -// This function needs to return void so the gtest FAIL() macro inside -// it works right. -void loadModelFile(std::string filename, std::string& file_content) -{ - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); - std::string xml_string; - std::fstream xml_file((res_path / filename).string().c_str(), std::fstream::in); - EXPECT_TRUE(xml_file.is_open()); - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - file_content = xml_string; -} - -void loadRobotModel(urdf::ModelInterfaceSharedPtr& robot_model_out) -{ - std::string xml_string; - loadModelFile("pr2_description/urdf/robot.xml", xml_string); - robot_model_out = urdf::parseURDF(xml_string); -} -void loadRobotModels(urdf::ModelInterfaceSharedPtr& robot_model_out, srdf::ModelSharedPtr& srdf_model_out) -{ - loadRobotModel(robot_model_out); - std::string xml_string; - loadModelFile("pr2_description/srdf/robot.xml", xml_string); - srdf_model_out->initString(*robot_model_out, xml_string); -} - TEST(PlanningScene, LoadRestore) { - urdf::ModelInterfaceSharedPtr urdf_model; - loadRobotModel(urdf_model); + urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); srdf::ModelSharedPtr srdf_model(new srdf::Model()); planning_scene::PlanningScene ps(urdf_model, srdf_model); moveit_msgs::PlanningScene ps_msg; @@ -92,76 +61,100 @@ TEST(PlanningScene, LoadRestore) TEST(PlanningScene, LoadRestoreDiff) { - urdf::ModelInterfaceSharedPtr urdf_model; - loadRobotModel(urdf_model); + urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); srdf::ModelSharedPtr srdf_model(new srdf::Model()); - - planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); + auto ps = std::make_shared(urdf_model, srdf_model); collision_detection::World& world = *ps->getWorldNonConst(); + + /* add one object to ps's world */ Eigen::Isometry3d id = Eigen::Isometry3d::Identity(); world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id); + /* ps can be written to and set from message */ moveit_msgs::PlanningScene ps_msg; ps_msg.robot_state.is_diff = true; - EXPECT_TRUE(planning_scene::PlanningScene::isEmpty(ps_msg)); + EXPECT_TRUE(moveit::core::isEmpty(ps_msg)); ps->getPlanningSceneMsg(ps_msg); ps->setPlanningSceneMsg(ps_msg); - EXPECT_FALSE(planning_scene::PlanningScene::isEmpty(ps_msg)); + EXPECT_EQ(ps_msg.world.collision_objects.size(), 1u); + EXPECT_EQ(ps_msg.world.collision_objects[0].id, "sphere"); EXPECT_TRUE(world.hasObject("sphere")); + /* test diff scene on top of ps */ planning_scene::PlanningScenePtr next = ps->diff(); + /* world is inherited from ps */ EXPECT_TRUE(next->getWorld()->hasObject("sphere")); - next->getWorldNonConst()->addToObject("sphere2", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id); + + /* object in overlay is only added in overlay */ + next->getWorldNonConst()->addToObject("sphere_in_next_only", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id); EXPECT_EQ(next->getWorld()->size(), 2u); EXPECT_EQ(ps->getWorld()->size(), 1u); + + /* the worlds used for collision detection contain one and two objects, respectively */ + EXPECT_EQ(ps->getCollisionEnv()->getWorld()->size(), 1u); + EXPECT_EQ(ps->getCollisionEnvUnpadded()->getWorld()->size(), 1u); + + EXPECT_EQ(next->getCollisionEnv()->getWorld()->size(), 2u); + EXPECT_EQ(next->getCollisionEnvUnpadded()->getWorld()->size(), 2u); + + /* maintained diff contains only overlay object */ next->getPlanningSceneDiffMsg(ps_msg); EXPECT_EQ(ps_msg.world.collision_objects.size(), 1u); + + /* copy ps to next and apply diff */ next->decoupleParent(); moveit_msgs::PlanningScene ps_msg2; + + /* diff is empty now */ next->getPlanningSceneDiffMsg(ps_msg2); EXPECT_EQ(ps_msg2.world.collision_objects.size(), 0u); + + /* next's world contains both objects */ next->getPlanningSceneMsg(ps_msg); EXPECT_EQ(ps_msg.world.collision_objects.size(), 2u); ps->setPlanningSceneMsg(ps_msg); EXPECT_EQ(ps->getWorld()->size(), 2u); + EXPECT_EQ(ps->getCollisionEnv()->getWorld()->size(), 2u); + EXPECT_EQ(ps->getCollisionEnvUnpadded()->getWorld()->size(), 2u); } TEST(PlanningScene, MakeAttachedDiff) { + urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); srdf::ModelSharedPtr srdf_model(new srdf::Model()); - urdf::ModelInterfaceSharedPtr urdf_model; - loadRobotModel(urdf_model); - - planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); + auto ps = std::make_shared(urdf_model, srdf_model); + /* add a single object to ps's world */ collision_detection::World& world = *ps->getWorldNonConst(); Eigen::Isometry3d id = Eigen::Isometry3d::Identity(); world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id); + /* attach object in diff */ planning_scene::PlanningScenePtr attached_object_diff_scene = ps->diff(); moveit_msgs::AttachedCollisionObject att_obj; att_obj.link_name = "r_wrist_roll_link"; att_obj.object.operation = moveit_msgs::CollisionObject::ADD; att_obj.object.id = "sphere"; + attached_object_diff_scene->processAttachedCollisionObjectMsg(att_obj); + + /* object is not in world anymore */ + EXPECT_EQ(attached_object_diff_scene->getWorld()->size(), 0u); + /* it became part of the robot state though */ + EXPECT_TRUE(attached_object_diff_scene->getCurrentState().hasAttachedBody("sphere")); collision_detection::CollisionRequest req; collision_detection::CollisionResult res; - - attached_object_diff_scene->processAttachedCollisionObjectMsg(att_obj); attached_object_diff_scene->checkCollision(req, res); ps->checkCollision(req, res); } TEST(PlanningScene, isStateValid) { - srdf::ModelSharedPtr srdf_model(new srdf::Model()); - urdf::ModelInterfaceSharedPtr urdf_model; - loadRobotModels(urdf_model, srdf_model); - - planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); - robot_state::RobotState current_state = ps->getCurrentState(); + moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); + auto ps = std::make_shared(robot_model->getURDF(), robot_model->getSRDF()); + moveit::core::RobotState current_state = ps->getCurrentState(); if (ps->isStateColliding(current_state, "left_arm")) { EXPECT_FALSE(ps->isStateValid(current_state, "left_arm")); @@ -170,10 +163,8 @@ TEST(PlanningScene, isStateValid) TEST(PlanningScene, loadGoodSceneGeometry) { - srdf::ModelSharedPtr srdf_model(new srdf::Model()); - urdf::ModelInterfaceSharedPtr urdf_model; - loadRobotModels(urdf_model, srdf_model); - planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); + moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); + auto ps = std::make_shared(robot_model->getURDF(), robot_model->getSRDF()); std::istringstream good_scene_geometry; good_scene_geometry.str("foobar_scene\n" @@ -201,10 +192,8 @@ TEST(PlanningScene, loadGoodSceneGeometry) TEST(PlanningScene, loadBadSceneGeometry) { - srdf::ModelSharedPtr srdf_model(new srdf::Model()); - urdf::ModelInterfaceSharedPtr urdf_model; - loadRobotModels(urdf_model, srdf_model); - planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model)); + moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2"); + auto ps = std::make_shared(robot_model->getURDF(), robot_model->getSRDF()); std::istringstream empty_scene_geometry; // This should fail since there is no planning scene name and no end of geometry marker. diff --git a/moveit_core/profiler/CMakeLists.txt b/moveit_core/profiler/CMakeLists.txt index 92671fd8cd..803b7ebdb8 100644 --- a/moveit_core/profiler/CMakeLists.txt +++ b/moveit_core/profiler/CMakeLists.txt @@ -7,5 +7,6 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/profiler/include/moveit/profiler/profiler.h b/moveit_core/profiler/include/moveit/profiler/profiler.h index b996af16f7..3ea38f58fe 100644 --- a/moveit_core/profiler/include/moveit/profiler/profiler.h +++ b/moveit_core/profiler/include/moveit/profiler/profiler.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PROFILER_ -#define MOVEIT_PROFILER_ +#pragma once #define MOVEIT_ENABLE_PROFILING 1 @@ -84,7 +83,7 @@ class Profiler : private boost::noncopyable prof_.begin(name); } - ~ScopedBlock(void) + ~ScopedBlock() { prof_.end(name_); } @@ -106,7 +105,7 @@ class Profiler : private boost::noncopyable prof_.start(); } - ~ScopedStart(void) + ~ScopedStart() { if (!wasRunning_) prof_.stop(); @@ -118,7 +117,7 @@ class Profiler : private boost::noncopyable }; /** \brief Return an instance of the class */ - static Profiler& instance(void); + static Profiler& instance(); /** \brief Constructor. It is allowed to separately instantiate this class (not only as a singleton) */ @@ -129,41 +128,41 @@ class Profiler : private boost::noncopyable } /** \brief Destructor */ - ~Profiler(void) + ~Profiler() { if (printOnDestroy_ && !data_.empty()) status(); } /** \brief Start counting time */ - static void Start(void) + static void Start() // NOLINT(readability-identifier-naming) { instance().start(); } /** \brief Stop counting time */ - static void Stop(void) + static void Stop() // NOLINT(readability-identifier-naming) { instance().stop(); } /** \brief Clear counted time and events */ - static void Clear(void) + static void Clear() // NOLINT(readability-identifier-naming) { instance().clear(); } /** \brief Start counting time */ - void start(void); + void start(); /** \brief Stop counting time */ - void stop(void); + void stop(); /** \brief Clear counted time and events */ - void clear(void); + void clear(); /** \brief Count a specific event for a number of times */ - static void Event(const std::string& name, const unsigned int times = 1) + static void Event(const std::string& name, const unsigned int times = 1) // NOLINT(readability-identifier-naming) { instance().event(name, times); } @@ -172,7 +171,7 @@ class Profiler : private boost::noncopyable void event(const std::string& name, const unsigned int times = 1); /** \brief Maintain the average of a specific value */ - static void Average(const std::string& name, const double value) + static void Average(const std::string& name, const double value) // NOLINT(readability-identifier-naming) { instance().average(name, value); } @@ -181,13 +180,13 @@ class Profiler : private boost::noncopyable void average(const std::string& name, const double value); /** \brief Begin counting time for a specific chunk of code */ - static void Begin(const std::string& name) + static void Begin(const std::string& name) // NOLINT(readability-identifier-naming) { instance().begin(name); } /** \brief Stop counting time for a specific chunk of code */ - static void End(const std::string& name) + static void End(const std::string& name) // NOLINT(readability-identifier-naming) { instance().end(name); } @@ -201,7 +200,7 @@ class Profiler : private boost::noncopyable /** \brief Print the status of the profiled code chunks and events. Optionally, computation done by different threads can be printed separately. */ - static void Status(std::ostream& out = std::cout, bool merge = true) + static void Status(std::ostream& out = std::cout, bool merge = true) // NOLINT(readability-identifier-naming) { instance().status(out, merge); } @@ -213,23 +212,23 @@ class Profiler : private boost::noncopyable /** \brief Print the status of the profiled code chunks and events to the console (using msg::Console) */ - static void Console(void) + static void Console() // NOLINT(readability-identifier-naming) { instance().console(); } /** \brief Print the status of the profiled code chunks and events to the console (using msg::Console) */ - void console(void); + void console(); /** \brief Check if the profiler is counting time or not */ - bool running(void) const + bool running() const { return running_; } /** \brief Check if the profiler is counting time or not */ - static bool Running(void) + static bool Running() // NOLINT(readability-identifier-naming) { return instance().running(); } @@ -238,7 +237,7 @@ class Profiler : private boost::noncopyable /** \brief Information about time spent in a section of the code */ struct TimeInfo { - TimeInfo(void) + TimeInfo() : total(0, 0, 0, 0), shortest(boost::posix_time::pos_infin), longest(boost::posix_time::neg_infin), parts(0) { } @@ -259,13 +258,13 @@ class Profiler : private boost::noncopyable boost::posix_time::ptime start; /** \brief Begin counting time */ - void set(void) + void set() { start = boost::posix_time::microsec_clock::universal_time(); } /** \brief Add the counted time to the total time */ - void update(void) + void update() { const boost::posix_time::time_duration& dt = boost::posix_time::microsec_clock::universal_time() - start; if (dt > longest) @@ -311,8 +310,8 @@ class Profiler : private boost::noncopyable bool running_; bool printOnDestroy_; }; -} -} +} // namespace tools +} // namespace moveit #else @@ -444,9 +443,7 @@ class Profiler return false; } }; -} -} - -#endif +} // namespace tools +} // namespace moveit #endif diff --git a/moveit_core/profiler/src/profiler.cpp b/moveit_core/profiler/src/profiler.cpp index 04616951fb..c70e0fb27b 100644 --- a/moveit_core/profiler/src/profiler.cpp +++ b/moveit_core/profiler/src/profiler.cpp @@ -40,7 +40,6 @@ #include #include #include -#include namespace moveit { @@ -121,7 +120,7 @@ inline double to_seconds(const boost::posix_time::time_duration& d) { return (double)d.total_microseconds() / 1000000.0; } -} +} // namespace void Profiler::status(std::ostream& out, bool merge) { diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index a3383680b3..fbd4162f04 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -18,14 +18,12 @@ target_link_libraries(${MOVEIT_LIB_NAME} moveit_profiler moveit_exceptions movei add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_robot_model test/test.cpp) target_link_libraries(test_robot_model moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) endif() install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/robot_model/include/moveit/robot_model/aabb.h b/moveit_core/robot_model/include/moveit/robot_model/aabb.h index 2ed8ebf382..ce0315fdd2 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/aabb.h +++ b/moveit_core/robot_model/include/moveit/robot_model/aabb.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Martin Pecka */ -#ifndef MOVEIT_ROBOT_MODEL_AABB_H -#define MOVEIT_ROBOT_MODEL_AABB_H +#pragma once #include @@ -50,7 +49,5 @@ class AABB : public Eigen::AlignedBox3d /** \brief Extend with a box transformed by the given transform. */ void extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box); }; -} -} - -#endif // MOVEIT_ROBOT_MODEL_AABB_H +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h index 265b13576c..855cbec6c7 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_FIXED_JOINT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_FIXED_JOINT_MODEL_ +#pragma once #include @@ -66,7 +65,5 @@ class FixedJointModel : public JointModel void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; }; -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h index a39a8471e5..0d0e0a60cc 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_FLOATING_JOINT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_FLOATING_JOINT_MODEL_ +#pragma once #include @@ -89,7 +88,5 @@ class FloatingJointModel : public JointModel private: double angular_distance_weight_; }; -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index 982ae78bb8..928a19d1ff 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -35,8 +35,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_ +#pragma once #include #include @@ -85,13 +84,13 @@ class JointModel; typedef std::map VariableIndexMap; /** \brief Data type for holding mappings from variable names to their bounds */ -typedef std::map VariableBoundsMap; +using VariableBoundsMap = std::map; /** \brief Map of names to instances for JointModel */ -typedef std::map JointModelMap; +using JointModelMap = std::map; /** \brief Map of names to const instances for JointModel */ -typedef std::map JointModelMapConst; +using JointModelMapConst = std::map; /** \brief A joint from the robot. Models the transform that this joint applies in the kinematic chain. A joint @@ -120,7 +119,7 @@ class JointModel }; /** \brief The datatype for the joint bounds */ - typedef std::vector Bounds; + using Bounds = std::vector; /** \brief Construct a joint named \e name */ JointModel(const std::string& name); @@ -444,10 +443,12 @@ class JointModel /** @name Computing transforms @{ */ - /** \brief Given the joint values for a joint, compute the corresponding transform */ + /** \brief Given the joint values for a joint, compute the corresponding transform. The computed transform is + * guaranteed to be a valid isometry. */ virtual void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const = 0; - /** \brief Given the transform generated by joint, compute the corresponding joint values */ + /** \brief Given the transform generated by joint, compute the corresponding joint values. Make sure the passed + * transform is a valid isometry. */ virtual void computeVariablePositions(const Eigen::Isometry3d& transform, double* joint_values) const = 0; /** @} */ @@ -519,7 +520,5 @@ class JointModel /** \brief Operator overload for printing variable bounds to a stream */ std::ostream& operator<<(std::ostream& out, const VariableBounds& b); -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index d46e1fc748..256370877b 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -1,42 +1,41 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_GROUP_ -#define MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_GROUP_ +#pragma once #include #include @@ -56,15 +55,15 @@ class JointModelGroup; typedef boost::function SolverAllocatorFn; /** \brief Map from group instances to allocator functions & bijections */ -typedef std::map SolverAllocatorMapFn; +using SolverAllocatorMapFn = std::map; /** \brief Map of names to instances for JointModelGroup */ -typedef std::map JointModelGroupMap; +using JointModelGroupMap = std::map; /** \brief Map of names to const instances for JointModelGroup */ -typedef std::map JointModelGroupMapConst; +using JointModelGroupMapConst = std::map; -typedef std::vector JointBoundsVector; +using JointBoundsVector = std::vector; class JointModelGroup { @@ -103,7 +102,7 @@ class JointModelGroup }; /// Map from group instances to allocator functions & bijections - typedef std::map KinematicsSolverMap; + using KinematicsSolverMap = std::map; JointModelGroup(const std::string& name, const srdf::Model::Group& config, const std::vector& joint_vector, const RobotModel* parent_model); @@ -412,6 +411,13 @@ class JointModelGroup return variable_count_; } + /** \brief Get the number of variables that describe the active joints in this joint group. This excludes variables + necessary for mimic joints. */ + unsigned int getActiveVariableCount() const + { + return active_variable_count_; + } + /** \brief Set the names of the subgroups for this group */ void setSubgroupNames(const std::vector& subgroups); @@ -695,6 +701,9 @@ class JointModelGroup /** \brief The number of variables necessary to describe this group of joints */ unsigned int variable_count_; + /** \brief The number of variables necessary to describe the active joints in this group of joints */ + unsigned int active_variable_count_; + /** \brief True if the state of this group is contiguous within the full robot state; this also means that the index values in variable_index_list_ are consecutive integers */ bool is_contiguous_index_list_; @@ -743,7 +752,5 @@ class JointModelGroup /** \brief The names of the default states specified for this group in the SRDF */ std::vector default_states_names_; }; -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.h b/moveit_core/robot_model/include/moveit/robot_model/link_model.h index decebc2a82..3bd2dfbe00 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/link_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_LINK_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_LINK_MODEL_ +#pragma once #include #include @@ -44,10 +43,11 @@ #include #include #include +#include namespace shapes { -MOVEIT_CLASS_FORWARD(Shape); +MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc } namespace moveit @@ -61,12 +61,11 @@ class LinkModel; typedef std::map LinkModelMap; /** \brief Map of names to const instances for LinkModel */ -typedef std::map LinkModelMapConst; +using LinkModelMapConst = std::map; /** \brief Map from link model instances to Eigen transforms */ -typedef std::map, - Eigen::aligned_allocator > > - LinkTransformMap; +using LinkTransformMap = std::map, + Eigen::aligned_allocator > >; /** \brief A link from the robot. Contains the constant transform applied to the link and its geometry */ class LinkModel @@ -138,7 +137,8 @@ class LinkModel /** \brief When transforms are computed for this link, they are usually applied to the link's origin. The joint origin transform acts as an offset -- it is - pre-applied before any other transform */ + pre-applied before any other transform. The + transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getJointOriginTransform() const { return joint_origin_transform_; @@ -158,7 +158,8 @@ class LinkModel /** \brief In addition to the link transform, the geometry of a link that is used for collision checking may have - a different offset itself, with respect to the origin */ + a different offset itself, with respect to the origin. + The transform is guaranteed to be a valid isometry. */ const EigenSTL::vector_Isometry3d& getCollisionOriginTransforms() const { return collision_origin_transform_; @@ -192,7 +193,8 @@ class LinkModel return centered_bounding_box_offset_; } - /** \brief Get the set of links that are attached to this one via fixed transforms */ + /** \brief Get the set of links that are attached to this one via fixed transforms. The returned transforms are + * guaranteed to be valid isometries. */ const LinkTransformMap& getAssociatedFixedTransforms() const { return associated_fixed_transforms_; @@ -201,6 +203,7 @@ class LinkModel /** \brief Remember that \e link_model is attached to this link using a fixed transform */ void addAssociatedFixedTransform(const LinkModel* link_model, const Eigen::Isometry3d& transform) { + ASSERT_ISOMETRY(transform); // unsanitized input, could contain a non-isometry associated_fixed_transforms_[link_model] = transform; } @@ -281,7 +284,5 @@ class LinkModel /** \brief Index of the transform for this link in the full robot frame */ int link_index_; }; -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h index 0900e0b8d9..af71e44f4e 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_PLANAR_JOINT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_PLANAR_JOINT_MODEL_ +#pragma once #include @@ -84,7 +83,5 @@ class PlanarJointModel : public JointModel private: double angular_distance_weight_; }; -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h index f745947e18..ece60ffe9c 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_PRISMATIC_JOINT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_PRISMATIC_JOINT_MODEL_ +#pragma once #include @@ -84,7 +83,5 @@ class PrismaticJointModel : public JointModel /** \brief The axis of the joint */ Eigen::Vector3d axis_; }; -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h index 33adc02839..9d70cf0628 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_REVOLUTE_JOINT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_REVOLUTE_JOINT_MODEL_ +#pragma once #include @@ -95,7 +94,5 @@ class RevoluteJointModel : public JointModel private: double x2_, y2_, z2_, xy_, xz_, yz_; }; -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 2131a6f1a8..a53bdb1af7 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -35,8 +35,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_MODEL_ -#define MOVEIT_CORE_ROBOT_MODEL_ +#pragma once #include #include @@ -54,13 +53,13 @@ #include #include -/** \brief Main namespace for MoveIt! */ +/** \brief Main namespace for MoveIt */ namespace moveit { -/** \brief Core components of MoveIt! */ +/** \brief Core components of MoveIt */ namespace core { -MOVEIT_CLASS_FORWARD(RobotModel); +MOVEIT_CLASS_FORWARD(RobotModel); // Defines RobotModelPtr, ConstPtr, WeakPtr... etc /** \brief Definition of a kinematic model. This class is not thread safe, however multiple instances can be created */ @@ -91,7 +90,7 @@ class RobotModel /** \brief Return true if the model is empty (has no root link, no joints) */ bool isEmpty() const { - return root_link_ == NULL; + return root_link_ == nullptr; } /** \brief Get the parsed URDF model */ @@ -430,8 +429,7 @@ class RobotModel return b; if (!b) return a; - return joint_model_vector_[common_joint_roots_[a->getJointIndex() * joint_model_vector_.size() + - b->getJointIndex()]]; + return joint_model_vector_[common_joint_roots_[a->getJointIndex() * joint_model_vector_.size() + b->getJointIndex()]]; } /// A map of known kinematics solvers (associated to their group name) @@ -616,10 +614,8 @@ class RobotModel /** \brief Given a geometry spec from the URDF and a filename (for a mesh), construct the corresponding shape object*/ shapes::ShapePtr constructShape(const urdf::Geometry* geom); }; -} -} +} // namespace core +} // namespace moveit namespace robot_model = moveit::core; namespace robot_state = moveit::core; - -#endif diff --git a/moveit_core/robot_model/src/aabb.cpp b/moveit_core/robot_model/src/aabb.cpp index fa7955ed37..91552b0fa2 100644 --- a/moveit_core/robot_model/src/aabb.cpp +++ b/moveit_core/robot_model/src/aabb.cpp @@ -1,40 +1,41 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Martin Pecka */ #include +#include void moveit::core::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box) { @@ -44,7 +45,8 @@ void moveit::core::AABB::extendWithTransformedBox(const Eigen::Isometry3d& trans // // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/ - const Eigen::Matrix3d& r = transform.rotation(); + ASSERT_ISOMETRY(transform) // unsanitized input, could contain non-isometry + const Eigen::Matrix3d& r = transform.linear(); const Eigen::Vector3d& t = transform.translation(); double x_range = 0.5 * (fabs(r(0, 0) * box[0]) + fabs(r(0, 1) * box[1]) + fabs(r(0, 2) * box[2])); diff --git a/moveit_core/robot_model/src/fixed_joint_model.cpp b/moveit_core/robot_model/src/fixed_joint_model.cpp index 2b514b503a..a5c8d288c3 100644 --- a/moveit_core/robot_model/src/fixed_joint_model.cpp +++ b/moveit_core/robot_model/src/fixed_joint_model.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 47ed6a87d2..37d5826ee0 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -1,41 +1,42 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -45,6 +46,12 @@ namespace moveit { namespace core { +namespace +{ +constexpr size_t STATE_SPACE_DIMENSION = 7; + +} // namespace + FloatingJointModel::FloatingJointModel(const std::string& name) : JointModel(name), angular_distance_weight_(1.0) { type_ = FLOATING; @@ -55,7 +62,7 @@ FloatingJointModel::FloatingJointModel(const std::string& name) : JointModel(nam local_variable_names_.push_back("rot_y"); local_variable_names_.push_back("rot_z"); local_variable_names_.push_back("rot_w"); - for (int i = 0; i < 7; ++i) + for (size_t i = 0; i < STATE_SPACE_DIMENSION; ++i) { variable_names_.push_back(name_ + "/" + local_variable_names_[i]); variable_index_map_[variable_names_.back()] = i; @@ -192,7 +199,7 @@ bool FloatingJointModel::normalizeRotation(double* values) const unsigned int FloatingJointModel::getStateSpaceDimension() const { - return 6; + return STATE_SPACE_DIMENSION; } bool FloatingJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const @@ -216,8 +223,9 @@ bool FloatingJointModel::enforcePositionBounds(double* values, const Bounds& bou void FloatingJointModel::computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const { - transf = Eigen::Isometry3d(Eigen::Translation3d(joint_values[0], joint_values[1], joint_values[2]) * - Eigen::Quaterniond(joint_values[6], joint_values[3], joint_values[4], joint_values[5])); + transf = Eigen::Isometry3d( + Eigen::Translation3d(joint_values[0], joint_values[1], joint_values[2]) * + Eigen::Quaterniond(joint_values[6], joint_values[3], joint_values[4], joint_values[5]).normalized()); } void FloatingJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const @@ -225,7 +233,8 @@ void FloatingJointModel::computeVariablePositions(const Eigen::Isometry3d& trans joint_values[0] = transf.translation().x(); joint_values[1] = transf.translation().y(); joint_values[2] = transf.translation().z(); - Eigen::Quaterniond q(transf.rotation()); + ASSERT_ISOMETRY(transf) // unsanitized input, could contain non-isometry + Eigen::Quaterniond q(transf.linear()); joint_values[3] = q.x(); joint_values[4] = q.y(); joint_values[5] = q.z(); diff --git a/moveit_core/robot_model/src/joint_model.cpp b/moveit_core/robot_model/src/joint_model.cpp index d93cec59cc..323a9e53af 100644 --- a/moveit_core/robot_model/src/joint_model.cpp +++ b/moveit_core/robot_model/src/joint_model.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 21998d8880..6ee0d03e80 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan, Dave Coleman */ @@ -105,6 +105,7 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode , name_(group_name) , common_root_(nullptr) , variable_count_(0) + , active_variable_count_(0) , is_contiguous_index_list_(true) , is_chain_(false) , is_single_dof_(true) @@ -133,6 +134,7 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode active_joint_model_name_vector_.push_back(joint_model->getName()); active_joint_model_start_index_.push_back(variable_count_); active_joint_models_bounds_.push_back(&joint_model->getVariableBounds()); + active_variable_count_ += vc; } else mimic_joints_.push_back(joint_model); @@ -327,15 +329,16 @@ void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNum { assert(active_joint_bounds.size() == active_joint_model_vector_.size()); for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i) - active_joint_model_vector_[i]->getVariableRandomPositionsNearBy( - rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i], - near + active_joint_model_start_index_[i], distance); + active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(rng, values + active_joint_model_start_index_[i], + *active_joint_bounds[i], + near + active_joint_model_start_index_[i], + distance); updateMimicJoints(values); } -void JointModelGroup::getVariableRandomPositionsNearBy( - random_numbers::RandomNumberGenerator& rng, double* values, const JointBoundsVector& active_joint_bounds, - const double* near, const std::map& distance_map) const +void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, + const JointBoundsVector& active_joint_bounds, const double* near, + const std::map& distance_map) const { assert(active_joint_bounds.size() == active_joint_model_vector_.size()); for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i) @@ -347,9 +350,10 @@ void JointModelGroup::getVariableRandomPositionsNearBy( distance = iter->second; else ROS_WARN_NAMED(LOGNAME, "Did not pass in distance for '%s'", active_joint_model_vector_[i]->getName().c_str()); - active_joint_model_vector_[i]->getVariableRandomPositionsNearBy( - rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i], - near + active_joint_model_start_index_[i], distance); + active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(rng, values + active_joint_model_start_index_[i], + *active_joint_bounds[i], + near + active_joint_model_start_index_[i], + distance); } updateMimicJoints(values); } @@ -360,14 +364,14 @@ void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNum { assert(active_joint_bounds.size() == active_joint_model_vector_.size()); if (distances.size() != active_joint_model_vector_.size()) - throw Exception("When sampling random values nearby for group '" + name_ + - "', distances vector should be of size " + + throw Exception("When sampling random values nearby for group '" + name_ + "', distances vector should be of size " + boost::lexical_cast(active_joint_model_vector_.size()) + ", but it is of size " + boost::lexical_cast(distances.size())); for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i) - active_joint_model_vector_[i]->getVariableRandomPositionsNearBy( - rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i], - near + active_joint_model_start_index_[i], distances[i]); + active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(rng, values + active_joint_model_start_index_[i], + *active_joint_bounds[i], + near + active_joint_model_start_index_[i], + distances[i]); updateMimicJoints(values); } @@ -566,8 +570,9 @@ bool JointModelGroup::computeIKIndexBijection(const std::vector& ik // skip reported fixed joints if (hasJointModel(ik_jname) && getJointModel(ik_jname)->getType() == JointModel::FIXED) continue; - ROS_ERROR_NAMED(LOGNAME, "IK solver computes joint values for joint '%s' " - "but group '%s' does not contain such a joint.", + ROS_ERROR_NAMED(LOGNAME, + "IK solver computes joint values for joint '%s' " + "but group '%s' does not contain such a joint.", ik_jname.c_str(), getName().c_str()); return false; } diff --git a/moveit_core/robot_model/src/link_model.cpp b/moveit_core/robot_model/src/link_model.cpp index a4435e425e..f0d8ceb3b6 100644 --- a/moveit_core/robot_model/src/link_model.cpp +++ b/moveit_core/robot_model/src/link_model.cpp @@ -1,41 +1,42 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ #include #include +#include #include #include @@ -59,9 +60,10 @@ LinkModel::~LinkModel() = default; void LinkModel::setJointOriginTransform(const Eigen::Isometry3d& transform) { + ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry joint_origin_transform_ = transform; joint_origin_transform_is_identity_ = - joint_origin_transform_.rotation().isIdentity() && + joint_origin_transform_.linear().isIdentity() && joint_origin_transform_.translation().norm() < std::numeric_limits::epsilon(); } @@ -71,8 +73,7 @@ void LinkModel::setParentJointModel(const JointModel* joint) is_parent_joint_fixed_ = joint->getType() == JointModel::FIXED; } -void LinkModel::setGeometry(const std::vector& shapes, - const EigenSTL::vector_Isometry3d& origins) +void LinkModel::setGeometry(const std::vector& shapes, const EigenSTL::vector_Isometry3d& origins) { shapes_ = shapes; collision_origin_transform_ = origins; @@ -82,8 +83,9 @@ void LinkModel::setGeometry(const std::vector& shapes, for (std::size_t i = 0; i < shapes_.size(); ++i) { + ASSERT_ISOMETRY(collision_origin_transform_[i]) // unsanitized input, could contain a non-isometry collision_origin_transform_is_identity_[i] = - (collision_origin_transform_[i].rotation().isIdentity() && + (collision_origin_transform_[i].linear().isIdentity() && collision_origin_transform_[i].translation().norm() < std::numeric_limits::epsilon()) ? 1 : 0; diff --git a/moveit_core/robot_model/src/order_robot_model_items.inc b/moveit_core/robot_model/src/order_robot_model_items.inc index 8c6a261773..c28073f078 100644 --- a/moveit_core/robot_model/src/order_robot_model_items.inc +++ b/moveit_core/robot_model/src/order_robot_model_items.inc @@ -65,6 +65,6 @@ struct OrderGroupsByName } }; -} -} -} \ No newline at end of file +} // namespace +} // namespace core +} // namespace moveit \ No newline at end of file diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp index 9be6a69bfa..301edd3d65 100644 --- a/moveit_core/robot_model/src/planar_joint_model.cpp +++ b/moveit_core/robot_model/src/planar_joint_model.cpp @@ -1,41 +1,42 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -224,7 +225,8 @@ void PlanarJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, joint_values[0] = transf.translation().x(); joint_values[1] = transf.translation().y(); - Eigen::Quaterniond q(transf.rotation()); + ASSERT_ISOMETRY(transf) // unsanitized input, could contain a non-isometry + Eigen::Quaterniond q(transf.linear()); // taken from Bullet double s_squared = 1.0 - (q.w() * q.w()); if (s_squared < 10.0 * std::numeric_limits::epsilon()) diff --git a/moveit_core/robot_model/src/prismatic_joint_model.cpp b/moveit_core/robot_model/src/prismatic_joint_model.cpp index fd5897905e..2f2166edb5 100644 --- a/moveit_core/robot_model/src/prismatic_joint_model.cpp +++ b/moveit_core/robot_model/src/prismatic_joint_model.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index a7afaa9b91..f3c066e0ba 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -1,44 +1,44 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ #include +#include #include #include -#include #include namespace moveit @@ -46,15 +46,7 @@ namespace moveit namespace core { RevoluteJointModel::RevoluteJointModel(const std::string& name) - : JointModel(name) - , axis_(0.0, 0.0, 0.0) - , continuous_(false) - , x2_(0.0) - , y2_(0.0) - , z2_(0.0) - , xy_(0.0) - , xz_(0.0) - , yz_(0.0) + : JointModel(name), axis_(0.0, 0.0, 0.0), continuous_(false), x2_(0.0), y2_(0.0), z2_(0.0), xy_(0.0), xz_(0.0), yz_(0.0) { type_ = REVOLUTE; variable_names_.push_back(name_); @@ -268,7 +260,8 @@ void RevoluteJointModel::computeTransform(const double* joint_values, Eigen::Iso void RevoluteJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const { - Eigen::Quaterniond q(transf.rotation()); + ASSERT_ISOMETRY(transf) // unsanitized input, could contain a non-isometry + Eigen::Quaterniond q(transf.linear()); q.normalize(); size_t max_idx; axis_.array().abs().maxCoeff(&max_idx); diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 700613ffe5..b00a1a25e4 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -41,7 +41,6 @@ #include #include #include -#include #include #include #include "order_robot_model_items.inc" @@ -327,6 +326,7 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) if (hasJointModelGroup(group_state.group_)) { JointModelGroup* jmg = getJointModelGroup(group_state.group_); + std::vector remaining_joints = jmg->getActiveJointModels(); std::map state; for (std::map>::const_iterator jt = group_state.joint_values_.begin(); jt != group_state.joint_values_.end(); ++jt) @@ -335,20 +335,39 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) { const JointModel* jm = jmg->getJointModel(jt->first); const std::vector& vn = jm->getVariableNames(); + // Remove current joint name from remaining list. + auto it_found = std::find(remaining_joints.begin(), remaining_joints.end(), jm); + if (it_found != remaining_joints.end()) + remaining_joints.erase(it_found); if (vn.size() == jt->second.size()) for (std::size_t j = 0; j < vn.size(); ++j) state[vn[j]] = jt->second[j]; else - ROS_ERROR_NAMED(LOGNAME, "The model for joint '%s' requires %d variable values, " - "but only %d variable values were supplied in default state '%s' for group '%s'", + ROS_ERROR_NAMED(LOGNAME, + "The model for joint '%s' requires %d variable values, " + "but only %d variable values were supplied in default state '%s' for group '%s'", jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), group_state.name_.c_str(), jmg->getName().c_str()); } else - ROS_ERROR_NAMED(LOGNAME, "Group state '%s' specifies value for joint '%s', " - "but that joint is not part of group '%s'", + ROS_ERROR_NAMED(LOGNAME, + "Group state '%s' specifies value for joint '%s', " + "but that joint is not part of group '%s'", group_state.name_.c_str(), jt->first.c_str(), jmg->getName().c_str()); } + if (!remaining_joints.empty()) + { + std::stringstream missing; + missing << (*remaining_joints.begin())->getName(); + for (auto j = ++remaining_joints.begin(); j != remaining_joints.end(); j++) + { + missing << ", " << (*j)->getName(); + } + ROS_WARN_STREAM_NAMED(LOGNAME, "Group state '" << group_state.name_ + << "' doesn't specify all group joints in group '" + << group_state.group_ << "'. " << missing.str() << " " + << (remaining_joints.size() > 1 ? "are" : "is") << " missing."); + } if (!state.empty()) jmg->addDefaultState(group_state.name_, state); } @@ -603,8 +622,9 @@ void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model) eef.parent_group_.c_str(), eef.name_.c_str()); } else - ROS_ERROR_NAMED(LOGNAME, "Group '%s' was specified as parent group for end-effector '%s' " - "but it does not include the parent link '%s'", + ROS_ERROR_NAMED(LOGNAME, + "Group '%s' was specified as parent group for end-effector '%s' " + "but it does not include the parent link '%s'", eef.parent_group_.c_str(), eef.name_.c_str(), eef.parent_link_.c_str()); } else @@ -844,7 +864,7 @@ static inline VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint) JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const urdf::Link* child_link, const srdf::Model& srdf_model) { - JointModel* result = nullptr; + JointModel* new_joint_model = nullptr; // if urdf_joint exists, must be the root link transform if (urdf_joint) @@ -857,7 +877,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint)); j->setContinuous(false); j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z)); - result = j; + new_joint_model = j; } break; case urdf::Joint::CONTINUOUS: @@ -866,7 +886,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint)); j->setContinuous(true); j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z)); - result = j; + new_joint_model = j; } break; case urdf::Joint::PRISMATIC: @@ -874,17 +894,17 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const PrismaticJointModel* j = new PrismaticJointModel(urdf_joint->name); j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint)); j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z)); - result = j; + new_joint_model = j; } break; case urdf::Joint::FLOATING: - result = new FloatingJointModel(urdf_joint->name); + new_joint_model = new FloatingJointModel(urdf_joint->name); break; case urdf::Joint::PLANAR: - result = new PlanarJointModel(urdf_joint->name); + new_joint_model = new PlanarJointModel(urdf_joint->name); break; case urdf::Joint::FIXED: - result = new FixedJointModel(urdf_joint->name); + new_joint_model = new FixedJointModel(urdf_joint->name); break; default: ROS_ERROR_NAMED(LOGNAME, "Unknown joint type: %d", (int)urdf_joint->type); @@ -898,8 +918,9 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const { if (virtual_joint.child_link_ != child_link->name) { - ROS_WARN_NAMED(LOGNAME, "Skipping virtual joint '%s' because its child frame '%s' " - "does not match the URDF frame '%s'", + ROS_WARN_NAMED(LOGNAME, + "Skipping virtual joint '%s' because its child frame '%s' " + "does not match the URDF frame '%s'", virtual_joint.name_.c_str(), virtual_joint.child_link_.c_str(), child_link->name.c_str()); } else if (virtual_joint.parent_frame_.empty()) @@ -910,12 +931,12 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const else { if (virtual_joint.type_ == "fixed") - result = new FixedJointModel(virtual_joint.name_); + new_joint_model = new FixedJointModel(virtual_joint.name_); else if (virtual_joint.type_ == "planar") - result = new PlanarJointModel(virtual_joint.name_); + new_joint_model = new PlanarJointModel(virtual_joint.name_); else if (virtual_joint.type_ == "floating") - result = new FloatingJointModel(virtual_joint.name_); - if (result) + new_joint_model = new FloatingJointModel(virtual_joint.name_); + if (new_joint_model) { // for fixed frames we still use the robot root link if (virtual_joint.type_ != "fixed") @@ -926,28 +947,28 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const } } } - if (!result) + if (!new_joint_model) { ROS_INFO_NAMED(LOGNAME, "No root/virtual joint specified in SRDF. Assuming fixed joint"); - result = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT"); + new_joint_model = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT"); } } - if (result) + if (new_joint_model) { - result->setDistanceFactor(result->getStateSpaceDimension()); + new_joint_model->setDistanceFactor(new_joint_model->getStateSpaceDimension()); const std::vector& pjoints = srdf_model.getPassiveJoints(); for (const srdf::Model::PassiveJoint& pjoint : pjoints) { - if (result->getName() == pjoint.name_) + if (new_joint_model->getName() == pjoint.name_) { - result->setPassive(true); + new_joint_model->setPassive(true); break; } } } - return result; + return new_joint_model; } namespace @@ -958,11 +979,11 @@ static inline Eigen::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose) Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q); return af; } -} +} // namespace LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) { - LinkModel* result = new LinkModel(urdf_link->name); + LinkModel* new_link_model = new LinkModel(urdf_link->name); const std::vector& col_array = urdf_link->collision_array.empty() ? std::vector(1, urdf_link->collision) : @@ -972,6 +993,7 @@ LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) EigenSTL::vector_Isometry3d poses; for (const urdf::CollisionSharedPtr& col : col_array) + { if (col && col->geometry) { shapes::ShapeConstPtr s = constructShape(col->geometry.get()); @@ -981,24 +1003,30 @@ LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) poses.push_back(urdfPose2Isometry3d(col->origin)); } } + } + + // Should we warn that old (melodic) behaviour has changed, not copying visual to collision geometries anymore? + bool warn_about_missing_collision = false; if (shapes.empty()) { - const std::vector& vis_array = urdf_link->visual_array.empty() ? - std::vector(1, urdf_link->visual) : + const auto& vis_array = urdf_link->visual_array.empty() ? std::vector{ urdf_link->visual } : urdf_link->visual_array; for (const urdf::VisualSharedPtr& vis : vis_array) + { if (vis && vis->geometry) - { - shapes::ShapeConstPtr s = constructShape(vis->geometry.get()); - if (s) - { - shapes.push_back(s); - poses.push_back(urdfPose2Isometry3d(vis->origin)); - } - } + warn_about_missing_collision = true; + } + } + if (warn_about_missing_collision) + { + ROS_WARN_STREAM_NAMED(LOGNAME + ".empty_collision_geometry", + "Link " << urdf_link->name + << " has visual geometry but no collision geometry. " + "Collision geometry will be left empty. " + "Fix your URDF file by explicitly specifying collision geometry."); } - result->setGeometry(shapes, poses); + new_link_model->setGeometry(shapes, poses); // figure out visual mesh (try visual urdf tag first, collision tag otherwise if (urdf_link->visual && urdf_link->visual->geometry) @@ -1007,8 +1035,8 @@ LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) { const urdf::Mesh* mesh = static_cast(urdf_link->visual->geometry.get()); if (!mesh->filename.empty()) - result->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->visual->origin), - Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z)); + new_link_model->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->visual->origin), + Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z)); } } else if (urdf_link->collision && urdf_link->collision->geometry) @@ -1017,36 +1045,37 @@ LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) { const urdf::Mesh* mesh = static_cast(urdf_link->collision->geometry.get()); if (!mesh->filename.empty()) - result->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->collision->origin), - Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z)); + new_link_model->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->collision->origin), + Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z)); } } if (urdf_link->parent_joint) - result->setJointOriginTransform(urdfPose2Isometry3d(urdf_link->parent_joint->parent_to_joint_origin_transform)); + new_link_model->setJointOriginTransform( + urdfPose2Isometry3d(urdf_link->parent_joint->parent_to_joint_origin_transform)); - return result; + return new_link_model; } shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom) { moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::constructShape"); - shapes::Shape* result = nullptr; + shapes::Shape* new_shape = nullptr; switch (geom->type) { case urdf::Geometry::SPHERE: - result = new shapes::Sphere(static_cast(geom)->radius); + new_shape = new shapes::Sphere(static_cast(geom)->radius); break; case urdf::Geometry::BOX: { urdf::Vector3 dim = static_cast(geom)->dim; - result = new shapes::Box(dim.x, dim.y, dim.z); + new_shape = new shapes::Box(dim.x, dim.y, dim.z); } break; case urdf::Geometry::CYLINDER: - result = new shapes::Cylinder(static_cast(geom)->radius, - static_cast(geom)->length); + new_shape = new shapes::Cylinder(static_cast(geom)->radius, + static_cast(geom)->length); break; case urdf::Geometry::MESH: { @@ -1055,7 +1084,7 @@ shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom) { Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale); - result = m; + new_shape = m; } } break; @@ -1064,7 +1093,7 @@ shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom) break; } - return shapes::ShapePtr(result); + return shapes::ShapePtr(new_shape); } bool RobotModel::hasJointModel(const std::string& name) const @@ -1141,10 +1170,10 @@ const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* { if (!link) return link; - const robot_model::LinkModel* parent_link = link->getParentLinkModel(); - const robot_model::JointModel* joint = link->getParentJointModel(); + const moveit::core::LinkModel* parent_link = link->getParentLinkModel(); + const moveit::core::JointModel* joint = link->getParentJointModel(); - while (parent_link && joint->getType() == robot_model::JointModel::FIXED) + while (parent_link && joint->getType() == moveit::core::JointModel::FIXED) { link = parent_link; joint = link->getParentJointModel(); @@ -1277,9 +1306,9 @@ void RobotModel::setKinematicsAllocators(const std::map::const_iterator jt = allocators.find(jmg->getName()); if (jt != allocators.end()) { - std::pair result; - result.first = jt->second; - jmg->setSolverAllocators(result); + std::pair solver_allocator_pair; + solver_allocator_pair.first = jt->second; + jmg->setSolverAllocators(solver_allocator_pair); } } @@ -1287,7 +1316,7 @@ void RobotModel::setKinematicsAllocators(const std::map result; + std::pair solver_allocator_pair; std::map::const_iterator jt = allocators.find(jmg->getName()); if (jt == allocators.end()) { @@ -1311,16 +1340,16 @@ void RobotModel::setKinematicsAllocators(const std::map resultj; + std::set joint_model_set; std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(), - std::inserter(resultj, resultj.end())); + std::inserter(joint_model_set, joint_model_set.end())); // TODO: instead of maintaining disjoint joint sets here, // should we leave that work to JMG's setSolverAllocators() / computeIKIndexBijection()? // There, a disjoint bijection from joints to solvers is computed anyway. // Underlying question: How do we resolve overlaps? Now the first considered sub group "wins" // But, if the overlap only involves fixed joints, we could consider all sub groups subs.push_back(sub); - joints.swap(resultj); + joints.swap(joint_model_set); } } @@ -1331,12 +1360,12 @@ void RobotModel::setKinematicsAllocators(const std::mapgetName() << " "; - result.second[sub] = allocators.find(sub->getName())->second; + solver_allocator_pair.second[sub] = allocators.find(sub->getName())->second; } ROS_DEBUG_NAMED(LOGNAME, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), ss.str().c_str()); } - jmg->setSolverAllocators(result); + jmg->setSolverAllocators(solver_allocator_pair); } } } diff --git a/moveit_core/robot_model/test/test.cpp b/moveit_core/robot_model/test/test.cpp index 45c56e1c63..e170e3ea60 100644 --- a/moveit_core/robot_model/test/test.cpp +++ b/moveit_core/robot_model/test/test.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index d2cffe5f6c..aaef8c7c41 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -19,27 +19,25 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) # Unit tests if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_robot_state test/robot_state_test.cpp) - target_link_libraries(test_robot_state moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_robot_state ${MOVEIT_LIB_NAME} moveit_test_utils) # As an executable, this benchmark is not run as a test by default add_executable(robot_state_benchmark test/robot_state_benchmark.cpp) - target_link_libraries(robot_state_benchmark moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME} ${GTEST_LIBRARIES}) + target_link_libraries(robot_state_benchmark ${MOVEIT_LIB_NAME} moveit_test_utils ${GTEST_LIBRARIES}) catkin_add_gtest(test_cartesian_interpolator test/test_cartesian_interpolator.cpp) - target_link_libraries(test_cartesian_interpolator moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_cartesian_interpolator ${MOVEIT_LIB_NAME} moveit_test_utils) catkin_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp) - target_link_libraries(test_robot_state_complex moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_robot_state_complex ${MOVEIT_LIB_NAME} moveit_test_utils) catkin_add_gtest(test_aabb test/test_aabb.cpp) - target_link_libraries(test_aabb moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_aabb ${MOVEIT_LIB_NAME} moveit_test_utils) endif() diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index 3453d3233d..7cd9bd0dce 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -1,44 +1,44 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_ROBOT_STATE_ATTACHED_BODY_ -#define MOVEIT_ROBOT_STATE_ATTACHED_BODY_ +#pragma once #include #include +#include #include #include #include @@ -51,16 +51,16 @@ namespace core class AttachedBody; typedef boost::function AttachedBodyCallback; -/** @brief Object defining bodies that can be attached to robot - * links. This is useful when handling objects picked up by - * the robot. */ +/** @brief Object defining bodies that can be attached to robot links. + * + * This is useful when handling objects picked up by the robot. */ class AttachedBody { public: - /** \brief Construct an attached body for a specified \e link. The name of this body is \e id and it consists of \e - shapes that - attach to the link by the transforms \e attach_trans. The set of links that are allowed to be touched by this - object is specified by \e touch_links. */ + /** \brief Construct an attached body for a specified \e link. + * + * The name of this body is \e id and it consists of \e shapes that attach to the link by the transforms + * \e attach_trans. The set of links that are allowed to be touched by this object is specified by \e touch_links. */ AttachedBody(const LinkModel* link, const std::string& id, const std::vector& shapes, const EigenSTL::vector_Isometry3d& attach_trans, const std::set& touch_links, const trajectory_msgs::JointTrajectory& attach_posture, @@ -106,35 +106,61 @@ class AttachedBody return detach_posture_; } - /** \brief Get the fixed transforms (the transforms to the shapes associated with this body) */ + /** \brief Get the fixed transforms (the transforms to the shapes of this body, relative to the link). The returned + * transforms are guaranteed to be valid isometries. */ const EigenSTL::vector_Isometry3d& getFixedTransforms() const { return attach_trans_; } - /** \brief Get subframes of this object. */ + /** \brief Get subframes of this object (relative to the link). The returned transforms are guaranteed to be valid + * isometries. */ const moveit::core::FixedTransformsMap& getSubframeTransforms() const { return subframe_poses_; } - /** \brief Set all subframes of this object. */ + /** \brief Get subframes of this object (in the world frame) */ + const moveit::core::FixedTransformsMap& getGlobalSubframeTransforms() const + { + return global_subframe_poses_; + } + + /** \brief Set all subframes of this object. + * + * Use these to define points of interest on the object to plan with + * (e.g. screwdriver/tip, kettle/spout, mug/base). + */ void setSubframeTransforms(const moveit::core::FixedTransformsMap& subframe_poses) { + for (const auto& t : subframe_poses) + { + ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry + } subframe_poses_ = subframe_poses; } - /** \brief Get the fixed transform to a named subframe on this body. + /** \brief Get the fixed transform to a named subframe on this body (relative to the robot link) + * * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's - * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). */ + * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). + * The returned transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; + /** \brief Get the fixed transform to a named subframe on this body. + * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's + * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). + * The returned transform is guaranteed to be a valid isometry. */ + const Eigen::Isometry3d& getGlobalSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; + /** \brief Check whether a subframe of given @frame_name is present in this object. + * * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's * name is "screwdriver"). */ bool hasSubframeTransform(const std::string& frame_name) const; - /** \brief Get the global transforms for the collision bodies */ + /** \brief Get the global transforms for the collision bodies. The returned transforms are guaranteed to be valid + * isometries. */ const EigenSTL::vector_Isometry3d& getGlobalCollisionBodyTransforms() const { return global_collision_body_transforms_; @@ -146,12 +172,8 @@ class AttachedBody /** \brief Set the scale for the shapes of this attached object */ void setScale(double scale); - /** \brief Recompute global_collision_body_transform given the transform of the parent link*/ - void computeTransform(const Eigen::Isometry3d& parent_link_global_transform) - { - for (std::size_t i = 0; i < global_collision_body_transforms_.size(); ++i) - global_collision_body_transforms_[i] = parent_link_global_transform * attach_trans_[i]; - } + /** \brief Recompute global_collision_body_transform given the transform of the parent link */ + void computeTransform(const Eigen::Isometry3d& parent_link_global_transform); private: /** \brief The link that owns this attached body */ @@ -176,13 +198,11 @@ class AttachedBody /** \brief The global transforms for these attached bodies (computed by forward kinematics) */ EigenSTL::vector_Isometry3d global_collision_body_transforms_; - /** \brief Transforms to subframes on the object. Transforms are relative to the link. - * Use these to define points of interest on the object to plan with - * (e.g. screwdriver/tip, kettle/spout, mug/base). - * */ + /** \brief Transforms to subframes on the object. Transforms are relative to the link. */ moveit::core::FixedTransformsMap subframe_poses_; -}; -} -} -#endif + /** \brief Transforms to subframes on the object, relative to the model frame. */ + moveit::core::FixedTransformsMap global_subframe_poses_; +}; +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h index a5447f2f85..2eb2c1aaf1 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h +++ b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h @@ -1,43 +1,42 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* Copyright (c) 2019, PickNik LLC. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * Copyright (c) 2019, PickNik LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan, Mike Lautman */ -#ifndef MOVEIT_CORE_ROBOT_STATE_CARTESIAN_INTERPOLATOR_ -#define MOVEIT_CORE_ROBOT_STATE_CARTESIAN_INTERPOLATOR_ +#pragma once #include @@ -50,7 +49,7 @@ namespace core /** \brief Struct for containing jump_threshold. For the purposes of maintaining API, we support both \e jump_threshold_factor which provides a scaling factor for - detecting joint space jumps and \e revolute_jump_threshold and \e prismatic_jump_threshold which provide abolute + detecting joint space jumps and \e revolute_jump_threshold and \e prismatic_jump_threshold which provide absolute thresholds for detecting joint space jumps. */ struct JumpThreshold { @@ -215,5 +214,3 @@ class CartesianInterpolator } // end of namespace core } // end of namespace moveit - -#endif diff --git a/moveit_core/robot_state/include/moveit/robot_state/conversions.h b/moveit_core/robot_state/include/moveit/robot_state/conversions.h index dadd10adbc..51b79b2a0b 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/conversions.h +++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.h @@ -1,64 +1,62 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan, Dave Coleman */ -#ifndef MOVEIT_ROBOT_STATE_CONVERSIONS_ -#define MOVEIT_ROBOT_STATE_CONVERSIONS_ +#pragma once #include #include #include -#include namespace moveit { namespace core { /** - * @brief Convert a joint state to a MoveIt! robot state + * @brief Convert a joint state to a MoveIt robot state * @param joint_state The input joint state to be converted - * @param state The resultant MoveIt! robot state + * @param state The resultant MoveIt robot state * @return True if successful, false if failed for any reason */ bool jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state); /** - * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt! robot state + * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state * @param tf An instance of a transforms object * @param robot_state The input robot state msg - * @param state The resultant MoveIt! robot state + * @param state The resultant MoveIt robot state * @param copy_attached_bodies Flag to include attached objects in robot state copy * @return True if successful, false if failed for any reason */ @@ -66,9 +64,9 @@ bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::RobotSta bool copy_attached_bodies = true); /** - * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt! robot state + * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state * @param robot_state The input robot state msg - * @param state The resultant MoveIt! robot state + * @param state The resultant MoveIt robot state * @param copy_attached_bodies Flag to include attached objects in robot state copy * @return True if successful, false if failed for any reason */ @@ -76,8 +74,8 @@ bool robotStateMsgToRobotState(const moveit_msgs::RobotState& robot_state, Robot bool copy_attached_bodies = true); /** - * @brief Convert a MoveIt! robot state to a robot state message - * @param state The input MoveIt! robot state object + * @brief Convert a MoveIt robot state to a robot state message + * @param state The input MoveIt robot state object * @param robot_state The resultant RobotState *message * @param copy_attached_bodies Flag to include attached objects in robot state copy */ @@ -86,33 +84,33 @@ void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::RobotState& /** * @brief Convert AttachedBodies to AttachedCollisionObjects - * @param attached_bodies The input MoveIt! attached body objects + * @param attached_bodies The input MoveIt attached body objects * @param attached_collision_objs The resultant AttachedCollisionObject messages */ void attachedBodiesToAttachedCollisionObjectMsgs( const std::vector& attached_bodies, std::vector& attached_collision_objs); /** - * @brief Convert a MoveIt! robot state to a joint state message - * @param state The input MoveIt! robot state object + * @brief Convert a MoveIt robot state to a joint state message + * @param state The input MoveIt robot state object * @param robot_state The resultant JointState message */ void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState& joint_state); /** - * @brief Convert a joint trajectory point to a MoveIt! robot state + * @brief Convert a joint trajectory point to a MoveIt robot state * @param joint_trajectory The input msg * @param point_id The index of the trajectory point in the joint trajectory. - * @param state The resultant MoveIt! robot state + * @param state The resultant MoveIt robot state * @return True if successful, false if failed for any reason */ bool jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory& trajectory, std::size_t point_id, RobotState& state); /** - * @brief Convert a MoveIt! robot state to common separated values (CSV) on a single line that is + * @brief Convert a MoveIt robot state to common separated values (CSV) on a single line that is * outputted to a stream e.g. for file saving - * @param state - The input MoveIt! robot state object + * @param state - The input MoveIt robot state object * @param out - a file stream, or any other stream * @param include_header - flag to prefix the output with a line of joint names. * @param separator - allows to override the comma seperator with any symbol, such as a white space @@ -121,9 +119,9 @@ void robotStateToStream(const RobotState& state, std::ostream& out, bool include const std::string& separator = ","); /** - * @brief Convert a MoveIt! robot state to common separated values (CSV) on a single line that is + * @brief Convert a MoveIt robot state to common separated values (CSV) on a single line that is * outputted to a stream e.g. for file saving. This version can order by joint model groups - * @param state - The input MoveIt! robot state object + * @param state - The input MoveIt robot state object * @param out - a file stream, or any other stream * @param joint_group_ordering - output joints based on ordering of joint groups * @param include_header - flag to prefix the output with a line of joint names. @@ -135,13 +133,11 @@ void robotStateToStream(const RobotState& state, std::ostream& out, /** * \brief Convert a string of joint values from a file (CSV) or input source into a RobotState - * @param state - the output MoveIt! robot state object + * @param state - the output MoveIt robot state object * @param line - the input string of joint values * @param separator - allows to override the comma seperator with any symbol, such as a white space * \return true on success */ void streamToRobotState(RobotState& state, const std::string& line, const std::string& separator = ","); -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 62750a6992..9f05a95602 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1,47 +1,45 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CORE_ROBOT_STATE_ -#define MOVEIT_CORE_ROBOT_STATE_ +#pragma once #include #include #include -#include #include #include #include @@ -50,11 +48,18 @@ #include +/* Terminology + * Model Frame: RobotModel's root frame == PlanningScene's planning frame + If the SRDF defines a virtual, non-fixed (e.g. floating) joint, this is the parent of this virtual joint. + Otherwise, it is the root link of the URDF model. + * Dirty Link Transforms: a caching tool for reducing the frequency of calculating forward kinematics +*/ + namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotState); +MOVEIT_CLASS_FORWARD(RobotState); // Defines RobotStatePtr, ConstPtr, WeakPtr... etc /** \brief Signature for functions that can verify that if the group \e joint_group in \e robot_state is set to \e joint_group_variable_values @@ -241,6 +246,9 @@ class RobotState return velocity_; } + /** \brief Set all velocities to 0.0 */ + void zeroVelocities(); + /** \brief Given an array with velocity values for all variables, set those values as the velocities in this state */ void setVariableVelocities(const double* velocity) { @@ -298,6 +306,9 @@ class RobotState return velocity_[index]; } + /** \brief Remove velocities from this state (this differs from setting them to zero) */ + void dropVelocities(); + /** @} */ /** \name Getting and setting variable acceleration @@ -329,6 +340,9 @@ class RobotState return acceleration_; } + /** \brief Set all accelerations to 0.0 */ + void zeroAccelerations(); + /** \brief Given an array with acceleration values for all variables, set those values as the accelerations in this * state */ void setVariableAccelerations(const double* acceleration) @@ -391,6 +405,9 @@ class RobotState return acceleration_[index]; } + /** \brief Remove accelerations from this state (this differs from setting them to zero) */ + void dropAccelerations(); + /** @} */ /** \name Getting and setting variable effort @@ -421,6 +438,9 @@ class RobotState return effort_; } + /** \brief Set all effort values to 0.0 */ + void zeroEffort(); + /** \brief Given an array with effort values for all variables, set those values as the effort in this state */ void setVariableEffort(const double* effort) { @@ -442,8 +462,7 @@ class RobotState /** \brief Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, \e missing_variables is filled with the names of the variables that are not set. */ - void setVariableEffort(const std::map& variable_map, - std::vector& missing_variables); + void setVariableEffort(const std::map& variable_map, std::vector& missing_variables); /** \brief Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. */ void setVariableEffort(const std::vector& variable_names, @@ -477,12 +496,20 @@ class RobotState return effort_[index]; } + /** \brief Remove effort values from this state (this differs from setting them to zero) */ + void dropEffort(); + + /** \brief Reduce RobotState to kinematic information (remove velocity, acceleration and effort, if present) */ + void dropDynamics(); + /** \brief Invert velocity if present. */ void invertVelocity(); /** @} */ - /** \name Getting and setting joint positions, velocities, accelerations and effort + /** \name Getting and setting joint positions, velocities, accelerations and effort for a single joint + * The joint might be multi-DOF, i.e. require more than one variable to set. + * See setVariablePositions(), setVariableVelocities(), setVariableEffort() to handle multiple joints. * @{ */ void setJointPositions(const std::string& joint_name, const double* position) @@ -574,8 +601,7 @@ class RobotState */ /** \brief Given positions for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const std::string& joint_group_name, const double* gstate) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -584,43 +610,81 @@ as the new values that correspond to the group */ } /** \brief Given positions for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const std::string& joint_group_name, const std::vector& gstate) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); if (jmg) + { + assert(gstate.size() == jmg->getVariableCount()); setJointGroupPositions(jmg, &gstate[0]); + } } /** \brief Given positions for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const JointModelGroup* group, const std::vector& gstate) { + assert(gstate.size() == group->getVariableCount()); setJointGroupPositions(group, &gstate[0]); } /** \brief Given positions for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const JointModelGroup* group, const double* gstate); /** \brief Given positions for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); if (jmg) + { + assert(values.size() == jmg->getVariableCount()); setJointGroupPositions(jmg, values); + } } /** \brief Given positions for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values); + /** \brief Given positions for the variables of active joints that make up a group, + * in the order found in the group (excluding values of mimic joints), set those + * as the new values that correspond to the group */ + void setJointGroupActivePositions(const JointModelGroup* group, const std::vector& gstate); + + /** \brief Given positions for the variables of active joints that make up a group, + * in the order found in the group (excluding values of mimic joints), set those + * as the new values that correspond to the group */ + void setJointGroupActivePositions(const std::string& joint_group_name, const std::vector& gstate) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + { + assert(gstate.size() == jmg->getActiveVariableCount()); + setJointGroupActivePositions(jmg, gstate); + } + } + + /** \brief Given positions for the variables of active joints that make up a group, + * in the order found in the group (excluding values of mimic joints), set those + * as the new values that correspond to the group */ + void setJointGroupActivePositions(const JointModelGroup* group, const Eigen::VectorXd& values); + + /** \brief Given positions for the variables of active joints that make up a group, + * in the order found in the group (excluding values of mimic joints), set those + * as the new values that correspond to the group */ + void setJointGroupActivePositions(const std::string& joint_group_name, const Eigen::VectorXd& values) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + { + assert(values.size() == jmg->getActiveVariableCount()); + setJointGroupActivePositions(jmg, values); + } + } + /** \brief For a given group, copy the position values of the variables that make up the group into another location, * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the * RobotState itself, so we copy instead of returning a pointer.*/ @@ -680,8 +744,7 @@ as the new values that correspond to the group */ */ /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -690,8 +753,7 @@ as the new values that correspond to the group */ } /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupVelocities(const std::string& joint_group_name, const std::vector& gstate) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -700,21 +762,18 @@ as the new values that correspond to the group */ } /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupVelocities(const JointModelGroup* group, const std::vector& gstate) { setJointGroupVelocities(group, &gstate[0]); } /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupVelocities(const JointModelGroup* group, const double* gstate); /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -723,8 +782,7 @@ as the new values that correspond to the group */ } /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values -of mimic joints), set those -as the new values that correspond to the group */ + * of mimic joints), set those as the new values that correspond to the group */ void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values); /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, @@ -786,8 +844,7 @@ as the new values that correspond to the group */ */ /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including -values of mimic joints), set those -as the new values that correspond to the group */ + * values of mimic joints), set those as the new values that correspond to the group */ void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -796,8 +853,7 @@ as the new values that correspond to the group */ } /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including -values of mimic joints), set those -as the new values that correspond to the group */ + * values of mimic joints), set those as the new values that correspond to the group */ void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector& gstate) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -806,21 +862,18 @@ as the new values that correspond to the group */ } /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including -values of mimic joints), set those -as the new values that correspond to the group */ + * values of mimic joints), set those as the new values that correspond to the group */ void setJointGroupAccelerations(const JointModelGroup* group, const std::vector& gstate) { setJointGroupAccelerations(group, &gstate[0]); } /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including -values of mimic joints), set those -as the new values that correspond to the group */ + * values of mimic joints), set those as the new values that correspond to the group */ void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate); /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including -values of mimic joints), set those -as the new values that correspond to the group */ + * values of mimic joints), set those as the new values that correspond to the group */ void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values) { const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); @@ -829,8 +882,7 @@ as the new values that correspond to the group */ } /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including -values of mimic joints), set those -as the new values that correspond to the group */ + * values of mimic joints), set those as the new values that correspond to the group */ void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values); /** \brief For a given group, copy the acceleration values of the variables that make up the group into another @@ -891,22 +943,6 @@ as the new values that correspond to the group */ * @{ */ - /** - * \brief Convert the frame of reference of the pose to that same frame as the IK solver expects - * @param pose - the input to change - * @param solver - a kin solver whose base frame is important to us - * @return true if no error - */ - bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver); - - /** - * \brief Convert the frame of reference of the pose to that same frame as the IK solver expects - * @param pose - the input to change - * @param ik_frame - the name of frame of reference of base of ik solver - * @return true if no error - */ - bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame); - /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. @@ -917,8 +953,8 @@ as the new values that correspond to the group */ const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int attempts, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int /* attempts */, + double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { return setFromIK(group, pose, timeout, constraint, options); @@ -936,7 +972,7 @@ as the new values that correspond to the group */ const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip, - unsigned int attempts, double timeout = 0.0, + unsigned int /* attempts */, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { @@ -953,8 +989,8 @@ as the new values that correspond to the group */ const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int attempts, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int /* attempts */, + double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { return setFromIK(group, pose, timeout, constraint, options); @@ -970,8 +1006,9 @@ as the new values that correspond to the group */ double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool - setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, unsigned int attempts, - double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, + unsigned int /* attempts */, double timeout = 0.0, + const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { return setFromIK(group, pose, tip, timeout, constraint, options); @@ -991,7 +1028,7 @@ as the new values that correspond to the group */ const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, - const std::vector& consistency_limits, unsigned int attempts, double timeout = 0.0, + const std::vector& consistency_limits, unsigned int /* attempts */, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { @@ -1013,7 +1050,7 @@ as the new values that correspond to the group */ const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); [[deprecated("The attempts argument is not supported anymore.")]] bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, unsigned int attempts, double timeout = 0.0, + const std::vector& tips, unsigned int /* attempts */, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { @@ -1037,7 +1074,7 @@ as the new values that correspond to the group */ [[deprecated("The attempts argument is not supported anymore.")]] bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, const std::vector& tips, const std::vector >& consistency_limits, - unsigned int attempts, double timeout = 0.0, + unsigned int /* attempts */, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { @@ -1060,7 +1097,7 @@ as the new values that correspond to the group */ [[deprecated("The attempts argument is not supported anymore.")]] bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, const std::vector& tips, const std::vector >& consistency_limits, - unsigned int attempts, double timeout = 0.0, + unsigned int /* attempts */, double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { @@ -1120,7 +1157,7 @@ as the new values that correspond to the group */ NOTE: As of ROS-Melodic these are deprecated and should not be used */ - MOVEIT_DEPRECATED double + [[deprecated]] double computeCartesianPath(const JointModelGroup* group, std::vector& traj, const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, double max_step, double jump_threshold_factor, @@ -1136,7 +1173,7 @@ as the new values that correspond to the group */ NOTE: As of ROS-Melodic these are deprecated and should not be used */ - MOVEIT_DEPRECATED double + [[deprecated]] double computeCartesianPath(const JointModelGroup* group, std::vector& traj, const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, double max_step, double jump_threshold_factor, @@ -1152,7 +1189,7 @@ as the new values that correspond to the group */ NOTE: As of ROS-Melodic these are deprecated and should not be used */ - MOVEIT_DEPRECATED double + [[deprecated]] double computeCartesianPath(const JointModelGroup* group, std::vector& traj, const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, double max_step, double jump_threshold_factor, @@ -1161,8 +1198,8 @@ as the new values that correspond to the group */ /** \brief Compute the Jacobian with reference to a particular point on a given link, for a specified group. * \param group The group to compute the Jacobian for - * \param link_name The name of the link - * \param reference_point_position The reference point position (with respect to the link specified in link_name) + * \param link The link model to compute the Jacobian for + * \param reference_point_position The reference point position (with respect to the link specified in link) * \param jacobian The resultant jacobian * \param use_quaternion_representation Flag indicating if the Jacobian should use a quaternion representation * (default is false) @@ -1173,8 +1210,8 @@ as the new values that correspond to the group */ /** \brief Compute the Jacobian with reference to a particular point on a given link, for a specified group. * \param group The group to compute the Jacobian for - * \param link_name The name of the link - * \param reference_point_position The reference point position (with respect to the link specified in link_name) + * \param link The link model to compute the Jacobian for + * \param reference_point_position The reference point position (with respect to the link specified in link) * \param jacobian The resultant jacobian * \param use_quaternion_representation Flag indicating if the Jacobian should use a quaternion representation * (default is false) @@ -1312,6 +1349,14 @@ as the new values that correspond to the group */ /** \brief Update the state after setting a particular link to the input global transform pose.*/ void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false); + /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel. + * This is typically the root link of the URDF unless a virtual joint is present. + * Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed. + * A related, more comprehensive function is |getFrameTransform|, which additionally to link frames + * also searches for attached object frames and their subframes. + * + * The returned transformation is always a valid isometry. + */ const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) { return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); @@ -1323,6 +1368,27 @@ as the new values that correspond to the group */ return global_link_transforms_[link->getLinkIndex()]; } + const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const + { + return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); + } + + const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const + { + BOOST_VERIFY(checkLinkTransforms()); + return global_link_transforms_[link->getLinkIndex()]; + } + + /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel. + * This is typically the root link of the URDF unless a virtual joint is present. + * Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed. + * + * As opposed to the visual links in |getGlobalLinkTransform|, this function returns + * the collision link transform used for collision checking. + * + * @param link_name: name of link to lookup + * @param index: specify which collision body to lookup, if more than one exists + */ const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) { return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); @@ -1334,6 +1400,17 @@ as the new values that correspond to the group */ return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; } + const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const + { + return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); + } + + const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const + { + BOOST_VERIFY(checkCollisionTransforms()); + return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; + } + const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) { return getJointTransform(robot_model_->getJointModel(joint_name)); @@ -1351,28 +1428,6 @@ as the new values that correspond to the group */ return variable_joint_transforms_[idx]; } - const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const - { - return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); - } - - const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const - { - BOOST_VERIFY(checkLinkTransforms()); - return global_link_transforms_[link->getLinkIndex()]; - } - - const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const - { - return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); - } - - const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const - { - BOOST_VERIFY(checkCollisionTransforms()); - return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; - } - const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const { return getJointTransform(robot_model_->getJointModel(joint_name)); @@ -1592,10 +1647,10 @@ as the new values that correspond to the group */ void getAttachedBodies(std::vector& attached_bodies) const; /** \brief Get all bodies attached to a particular group the model corresponding to this state */ - void getAttachedBodies(std::vector& attached_bodies, const JointModelGroup* lm) const; + void getAttachedBodies(std::vector& attached_bodies, const JointModelGroup* group) const; /** \brief Get all bodies attached to a particular link in the model corresponding to this state */ - void getAttachedBodies(std::vector& attached_bodies, const LinkModel* lm) const; + void getAttachedBodies(std::vector& attached_bodies, const LinkModel* link_model) const; /** \brief Remove the attached body named \e id. Return false if the object was not found (and thus not removed). * Return true on success. */ @@ -1639,24 +1694,30 @@ as the new values that correspond to the group */ return *rng_; } - /** \brief Get the transformation matrix from the model frame to the frame identified by \e frame_id + /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id + * + * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. * - * If frame_id was not found, \e frame_found is set to false and an identity transform is returned */ + * The returned transformation is always a valid isometry. */ const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr); - /** \brief Get the transformation matrix from the model frame to the frame identified by \e frame_id + /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * - * If frame_id was not found, \e frame_found is set to false and an identity transform is returned */ + * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. + * + * The returned transformation is always a valid isometry. */ const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr) const; - /** \brief Get the transformation matrix from the model frame to the frame identified by \e frame_id + /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * * If this frame is attached to a robot link, the link pointer is returned in \e robot_link. - * If frame_id was not found, \e frame_found is set to false and an identity transform is returned */ + * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. + * + * The returned transformation is always a valid isometry. */ const Eigen::Isometry3d& getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link, bool& frame_found) const; - /** \brief Check if a transformation matrix from the model frame to frame \e frame_id is known */ + /** \brief Check if a transformation matrix from the model frame (root of model) to frame \e frame_id is known */ bool knowsFrameTransform(const std::string& frame_id) const; /** @brief Get a MarkerArray that fully describes the robot markers for a given robot. @@ -1706,8 +1767,7 @@ as the new values that correspond to the group */ void printStatePositions(std::ostream& out = std::cout) const; /** \brief Output to console the current state of the robot's joint limits */ - void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, - std::ostream& out = std::cout) const; + void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out = std::cout) const; void printStateInfo(std::ostream& out = std::cout) const; @@ -1724,19 +1784,34 @@ as the new values that correspond to the group */ void initTransforms(); void copyFrom(const RobotState& other); + /** + * \brief Transform pose from the robot model's base frame to the reference frame of the IK solver + * @param pose - the input to change + * @param solver - a kin solver whose base frame is important to us + * @return true if no error + */ + inline bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver); + + /** + * \brief Transform pose from the robot model's base frame to the reference frame of the IK solver + * @param pose - the input to change + * @param ik_frame - the name of frame of reference of base of ik solver + * @return true if no error + */ + bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame); + void markDirtyJointTransforms(const JointModel* joint) { dirty_joint_transforms_[joint->getJointIndex()] = 1; dirty_link_transforms_ = - dirty_link_transforms_ == NULL ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint); + dirty_link_transforms_ == nullptr ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint); } void markDirtyJointTransforms(const JointModelGroup* group) { - const std::vector& jm = group->getActiveJointModels(); - for (std::size_t i = 0; i < jm.size(); ++i) - dirty_joint_transforms_[jm[i]->getJointIndex()] = 1; - dirty_link_transforms_ = dirty_link_transforms_ == NULL ? + for (const JointModel* jm : group->getActiveJointModels()) + dirty_joint_transforms_[jm->getJointIndex()] = 1; + dirty_link_transforms_ = dirty_link_transforms_ == nullptr ? group->getCommonRoot() : robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot()); } @@ -1747,28 +1822,26 @@ as the new values that correspond to the group */ void updateMimicJoint(const JointModel* joint) { - const std::vector& mim = joint->getMimicRequests(); double v = position_[joint->getFirstVariableIndex()]; - for (std::size_t i = 0; i < mim.size(); ++i) + for (const JointModel* jm : joint->getMimicRequests()) { - position_[mim[i]->getFirstVariableIndex()] = mim[i]->getMimicFactor() * v + mim[i]->getMimicOffset(); - markDirtyJointTransforms(mim[i]); + position_[jm->getFirstVariableIndex()] = jm->getMimicFactor() * v + jm->getMimicOffset(); + markDirtyJointTransforms(jm); } } /** \brief Update a set of joints that are certain to be mimicking other joints */ /* use updateMimicJoints() instead, which also marks joints dirty */ - MOVEIT_DEPRECATED void updateMimicJoint(const std::vector& mim) + [[deprecated]] void updateMimicJoint(const std::vector& mim) { - for (std::size_t i = 0; i < mim.size(); ++i) + for (const JointModel* jm : mim) { - const int fvi = mim[i]->getFirstVariableIndex(); - position_[fvi] = - mim[i]->getMimicFactor() * position_[mim[i]->getMimic()->getFirstVariableIndex()] + mim[i]->getMimicOffset(); + const int fvi = jm->getFirstVariableIndex(); + position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); // Only mark joint transform dirty, but not the associated link transform // as this function is always used in combination of // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group); - dirty_joint_transforms_[mim[i]->getJointIndex()] = 1; + dirty_joint_transforms_[jm->getJointIndex()] = 1; } } @@ -1813,9 +1886,12 @@ as the new values that correspond to the group */ const JointModel* dirty_link_transforms_; const JointModel* dirty_collision_body_transforms_; - Eigen::Isometry3d* variable_joint_transforms_; // this points to an element in transforms_, so it is aligned - Eigen::Isometry3d* global_link_transforms_; // this points to an element in transforms_, so it is aligned - Eigen::Isometry3d* global_collision_body_transforms_; // this points to an element in transforms_, so it is aligned + // All the following transform variables point into aligned memory in memory_ + // They are updated lazily, based on the flags in dirty_joint_transforms_ + // resp. the pointers dirty_link_transforms_ and dirty_collision_body_transforms_ + Eigen::Isometry3d* variable_joint_transforms_; ///< Local transforms of all joints + Eigen::Isometry3d* global_link_transforms_; ///< Transforms from model frame to link frame for each link + Eigen::Isometry3d* global_collision_body_transforms_; ///< Transforms from model frame to collision bodies unsigned char* dirty_joint_transforms_; /** \brief All attached bodies that are part of this state, indexed by their name */ @@ -1835,7 +1911,5 @@ as the new values that correspond to the group */ /** \brief Operator overload for printing variable bounds to a stream */ std::ostream& operator<<(std::ostream& out, const RobotState& s); -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_state/src/attached_body.cpp b/moveit_core/robot_state/src/attached_body.cpp index b54e1fa1cf..c5be22ab27 100644 --- a/moveit_core/robot_state/src/attached_body.cpp +++ b/moveit_core/robot_state/src/attached_body.cpp @@ -1,40 +1,41 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ #include +#include #include #include @@ -54,7 +55,16 @@ AttachedBody::AttachedBody(const LinkModel* parent_link_model, const std::string , touch_links_(touch_links) , detach_posture_(detach_posture) , subframe_poses_(subframe_poses) + , global_subframe_poses_(subframe_poses) { + for (const auto& t : attach_trans_) + { + ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry + } + for (const auto& t : subframe_poses_) + { + ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry + } global_collision_body_transforms_.resize(attach_trans.size()); for (Eigen::Isometry3d& global_collision_body_transform : global_collision_body_transforms_) global_collision_body_transform.setIdentity(); @@ -79,6 +89,20 @@ void AttachedBody::setScale(double scale) } } +void AttachedBody::computeTransform(const Eigen::Isometry3d& parent_link_global_transform) +{ + ASSERT_ISOMETRY(parent_link_global_transform) // unsanitized input, could contain a non-isometry + + // update collision body transforms + for (std::size_t i = 0; i < global_collision_body_transforms_.size(); ++i) + global_collision_body_transforms_[i] = parent_link_global_transform * attach_trans_[i]; // valid isometry + + // update subframe transforms + for (auto global = global_subframe_poses_.begin(), end = global_subframe_poses_.end(), local = subframe_poses_.begin(); + global != end; ++global, ++local) + global->second = parent_link_global_transform * local->second; // valid isometry +} + void AttachedBody::setPadding(double padding) { for (shapes::ShapeConstPtr& shape : shapes_) @@ -114,11 +138,30 @@ const Eigen::Isometry3d& AttachedBody::getSubframeTransform(const std::string& f return IDENTITY_TRANSFORM; } +const Eigen::Isometry3d& AttachedBody::getGlobalSubframeTransform(const std::string& frame_name, bool* found) const +{ + if (boost::starts_with(frame_name, id_) && frame_name[id_.length()] == '/') + { + auto it = global_subframe_poses_.find(frame_name.substr(id_.length() + 1)); + if (it != global_subframe_poses_.end()) + { + if (found) + *found = true; + return it->second; + } + } + static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity(); + if (found) + *found = false; + return IDENTITY_TRANSFORM; +} + bool AttachedBody::hasSubframeTransform(const std::string& frame_name) const { bool found; getSubframeTransform(frame_name, &found); return found; } + } // namespace core } // namespace moveit diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index aff7d5bdf5..3dc237d563 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -1,42 +1,43 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* Copyright (c) 2019, PickNik LLC. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * Copyright (c) 2019, PickNik LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */ #include +#include namespace moveit { @@ -59,13 +60,14 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons const kinematics::KinematicsQueryOptions& options) { // this is the Cartesian pose we start from, and have to move in the direction indicated + // getGlobalLinkTransform() returns a valid isometry by contract const Eigen::Isometry3d& start_pose = start_state->getGlobalLinkTransform(link); // the direction can be in the local reference frame (in which case we rotate it) - const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.rotation() * direction; + const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.linear() * direction; // The target pose is built by applying a translation to the start pose for the desired direction and distance - Eigen::Isometry3d target_pose = start_pose; + Eigen::Isometry3d target_pose = start_pose; // valid isometry target_pose.translation() += rotated_direction * distance; // call computeCartesianPath for the computed target pose in the global reference frame @@ -86,13 +88,16 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons start_state->enforceBounds(joint); // this is the Cartesian pose we start from, and we move in the direction indicated - Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link); + // getGlobalLinkTransform() returns a valid isometry by contract + Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link); // valid isometry + + ASSERT_ISOMETRY(target) // unsanitized input, could contain a non-isometry // the target can be in the local reference frame (in which case we rotate it) - Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; + Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; // valid isometry - Eigen::Quaterniond start_quaternion(start_pose.rotation()); - Eigen::Quaterniond target_quaternion(rotated_target.rotation()); + Eigen::Quaterniond start_quaternion(start_pose.linear()); + Eigen::Quaterniond target_quaternion(rotated_target.linear()); if (max_step.translation <= 0.0 && max_step.rotation <= 0.0) { @@ -143,7 +148,7 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons } traj.clear(); - traj.push_back(RobotStatePtr(new robot_state::RobotState(*start_state))); + traj.push_back(RobotStatePtr(new moveit::core::RobotState(*start_state))); double last_valid_percentage = 0.0; for (std::size_t i = 1; i <= steps; ++i) @@ -156,7 +161,7 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons // Explicitly use a single IK attempt only: We want a smooth trajectory. // Random seeding (of additional attempts) would probably create IK jumps. if (start_state->setFromIK(group, pose, link->getName(), consistency_limits, 0.0, validCallback, options)) - traj.push_back(RobotStatePtr(new robot_state::RobotState(*start_state))); + traj.push_back(RobotStatePtr(new moveit::core::RobotState(*start_state))); else break; @@ -231,8 +236,9 @@ double CartesianInterpolator::checkRelativeJointSpaceJump(const JointModelGroup* { if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH) { - ROS_WARN_NAMED(LOGNAME, "The computed trajectory is too short to detect jumps in joint-space " - "Need at least %zu steps, only got %zu. Try a lower max_step.", + ROS_WARN_NAMED(LOGNAME, + "The computed trajectory is too short to detect jumps in joint-space " + "Need at least %zu steps, only got %zu. Try a lower max_step.", MIN_STEPS_FOR_JUMP_THRESH, traj.size()); } @@ -287,8 +293,9 @@ double CartesianInterpolator::checkAbsoluteJointSpaceJump(const JointModelGroup* joint_threshold = prismatic_threshold; break; default: - ROS_WARN_NAMED(LOGNAME, "Joint %s has not supported type %s. \n" - "checkAbsoluteJointSpaceJump only supports prismatic and revolute joints.", + ROS_WARN_NAMED(LOGNAME, + "Joint %s has not supported type %s. \n" + "checkAbsoluteJointSpaceJump only supports prismatic and revolute joints.", joint->getName().c_str(), joint->getTypeName().c_str()); continue; } diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 9632e2d857..1a2722caf1 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2011-2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2011-2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan, Dave Coleman */ @@ -101,8 +101,9 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::MultiDOFJointState& m error = true; if (error) - ROS_WARN_NAMED(LOGNAME, "The transform for multi-dof joints was specified in frame '%s' " - "but it was not possible to transform that to frame '%s'", + ROS_WARN_NAMED(LOGNAME, + "The transform for multi-dof joints was specified in frame '%s' " + "but it was not possible to transform that to frame '%s'", mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str()); } @@ -218,7 +219,7 @@ static void _attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::A } aco.object.subframe_names.clear(); aco.object.subframe_poses.clear(); - for (auto frame_pair : attached_body.getSubframeTransforms()) + for (const auto& frame_pair : attached_body.getSubframeTransforms()) { aco.object.subframe_names.push_back(frame_pair.first); geometry_msgs::Pose pose; @@ -321,8 +322,9 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::Attached else { t0.setIdentity(); - ROS_ERROR_NAMED(LOGNAME, "Cannot properly transform from frame '%s'. " - "The pose of the attached body may be incorrect", + ROS_ERROR_NAMED(LOGNAME, + "Cannot properly transform from frame '%s'. " + "The pose of the attached body may be incorrect", aco.object.header.frame_id.c_str()); } } @@ -339,8 +341,9 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::Attached else { if (state.clearAttachedBody(aco.object.id)) - ROS_DEBUG_NAMED(LOGNAME, "The robot state already had an object named '%s' attached to link '%s'. " - "The object was replaced.", + ROS_DEBUG_NAMED(LOGNAME, + "The robot state already had an object named '%s' attached to link '%s'. " + "The object was replaced.", aco.object.id.c_str(), aco.link_name.c_str()); state.attachBody(aco.object.id, shapes, poses, aco.touch_links, aco.link_name, aco.detach_posture, subframe_poses); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index cc945513c5..909017f573 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1,43 +1,44 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */ #include #include #include +#include #include #include #include @@ -82,8 +83,7 @@ RobotState::~RobotState() void RobotState::allocMemory() { - static_assert((sizeof(Eigen::Isometry3d) / EIGEN_MAX_ALIGN_BYTES) * EIGEN_MAX_ALIGN_BYTES == - sizeof(Eigen::Isometry3d), + static_assert((sizeof(Eigen::Isometry3d) / EIGEN_MAX_ALIGN_BYTES) * EIGEN_MAX_ALIGN_BYTES == sizeof(Eigen::Isometry3d), "sizeof(Eigen::Isometry3d) should be a multiple of EIGEN_MAX_ALIGN_BYTES"); constexpr unsigned int extra_alignment_bytes = EIGEN_MAX_ALIGN_BYTES - 1; @@ -144,9 +144,9 @@ void RobotState::copyFrom(const RobotState& other) if (dirty_link_transforms_ == robot_model_->getRootJoint()) { // everything is dirty; no point in copying transforms; copy positions, potentially velocity & acceleration - memcpy(position_, other.position_, robot_model_->getVariableCount() * sizeof(double) * - (1 + ((has_velocity_ || has_acceleration_ || has_effort_) ? 1 : 0) + - ((has_acceleration_ || has_effort_) ? 1 : 0))); + memcpy(position_, other.position_, + robot_model_->getVariableCount() * sizeof(double) * + (1 + (has_velocity_ ? 1 : 0) + ((has_acceleration_ || has_effort_) ? 1 : 0))); // and just initialize transforms initTransforms(); } @@ -162,7 +162,7 @@ void RobotState::copyFrom(const RobotState& other) (robot_model_->getVariableCount() * (1 + ((has_velocity_ || has_acceleration_ || has_effort_) ? 1 : 0) + ((has_acceleration_ || has_effort_) ? 1 : 0)) + nr_doubles_for_dirty_joint_transforms); - memcpy(variable_joint_transforms_, other.variable_joint_transforms_, bytes); + memcpy((void*)variable_joint_transforms_, (void*)other.variable_joint_transforms_, bytes); } // copy attached bodies @@ -232,6 +232,46 @@ void RobotState::markEffort() } } +void RobotState::zeroVelocities() +{ + has_velocity_ = false; + markVelocity(); +} + +void RobotState::zeroAccelerations() +{ + has_acceleration_ = false; + markAcceleration(); +} + +void RobotState::zeroEffort() +{ + has_effort_ = false; + markEffort(); +} + +void RobotState::dropVelocities() +{ + has_velocity_ = false; +} + +void RobotState::dropAccelerations() +{ + has_acceleration_ = false; +} + +void RobotState::dropEffort() +{ + has_effort_ = false; +} + +void RobotState::dropDynamics() +{ + dropVelocities(); + dropAccelerations(); + dropEffort(); +} + void RobotState::setToRandomPositions() { random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator(); @@ -471,6 +511,30 @@ void RobotState::setJointGroupPositions(const JointModelGroup* group, const Eige updateMimicJoints(group); } +void RobotState::setJointGroupActivePositions(const JointModelGroup* group, const std::vector& gstate) +{ + assert(gstate.size() == group->getActiveVariableCount()); + std::size_t i = 0; + for (const JointModel* jm : group->getActiveJointModels()) + { + setJointPositions(jm, &gstate[i]); + i += jm->getVariableCount(); + } + updateMimicJoints(group); +} + +void RobotState::setJointGroupActivePositions(const JointModelGroup* group, const Eigen::VectorXd& values) +{ + assert(values.size() == group->getActiveVariableCount()); + std::size_t i = 0; + for (const JointModel* jm : group->getActiveJointModels()) + { + setJointPositions(jm, &values(i)); + i += jm->getVariableCount(); + } + updateMimicJoints(group); +} + void RobotState::copyJointGroupPositions(const JointModelGroup* group, double* gstate) const { const std::vector& il = group->getVariableIndexList(); @@ -631,21 +695,21 @@ void RobotState::updateLinkTransformsInternal(const JointModel* start) if (parent) // root JointModel will not have a parent { int idx_parent = parent->getLinkIndex(); - if (link->parentJointIsFixed()) + if (link->parentJointIsFixed()) // fixed joint global_link_transforms_[idx_link].affine().noalias() = global_link_transforms_[idx_parent].affine() * link->getJointOriginTransform().matrix(); - else + else // non-fixed joint { - if (link->jointOriginTransformIsIdentity()) + if (link->jointOriginTransformIsIdentity()) // Link has identity transform global_link_transforms_[idx_link].affine().noalias() = global_link_transforms_[idx_parent].affine() * getJointTransform(link->getParentJointModel()).matrix(); - else + else // Link has non-identity transform global_link_transforms_[idx_link].affine().noalias() = global_link_transforms_[idx_parent].affine() * link->getJointOriginTransform().matrix() * getJointTransform(link->getParentJointModel()).matrix(); } } - else + else // is the origin / root / 'model frame' { if (link->jointOriginTransformIsIdentity()) global_link_transforms_[idx_link] = getJointTransform(link->getParentJointModel()); @@ -844,8 +908,7 @@ void RobotState::interpolate(const RobotState& to, double t, RobotState& state) state.dirty_link_transforms_ = state.robot_model_->getRootJoint(); } -void RobotState::interpolate(const RobotState& to, double t, RobotState& state, - const JointModelGroup* joint_group) const +void RobotState::interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const { const std::vector& jm = joint_group->getActiveJointModels(); for (const JointModel* joint : jm) @@ -880,6 +943,9 @@ const AttachedBody* RobotState::getAttachedBody(const std::string& id) const void RobotState::attachBody(AttachedBody* attached_body) { + // If an attached body with the same id exists, remove it + clearAttachedBody(attached_body->getName()); + attached_body_map_[attached_body->getName()] = attached_body; attached_body->computeTransform(getGlobalLinkTransform(attached_body->getAttachedLink())); if (attached_body_update_callback_) @@ -903,8 +969,7 @@ void RobotState::getAttachedBodies(std::vector& attached_bo attached_bodies.push_back(it.second); } -void RobotState::getAttachedBodies(std::vector& attached_bodies, - const JointModelGroup* group) const +void RobotState::getAttachedBodies(std::vector& attached_bodies, const JointModelGroup* group) const { attached_bodies.clear(); for (const std::pair& it : attached_body_map_) @@ -912,11 +977,11 @@ void RobotState::getAttachedBodies(std::vector& attached_bo attached_bodies.push_back(it.second); } -void RobotState::getAttachedBodies(std::vector& attached_bodies, const LinkModel* lm) const +void RobotState::getAttachedBodies(std::vector& attached_bodies, const LinkModel* link_model) const { attached_bodies.clear(); for (const std::pair& it : attached_body_map_) - if (it.second->getAttachedLink() == lm) + if (it.second->getAttachedLink() == link_model) attached_bodies.push_back(it.second); } @@ -1016,11 +1081,9 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c frame_found = true; return IDENTITY_TRANSFORM; } - if (robot_model_->hasLinkModel(frame_id)) + if ((robot_link = robot_model_->getLinkModel(frame_id, &frame_found))) { - robot_link = robot_model_->getLinkModel(frame_id); BOOST_VERIFY(checkLinkTransforms()); - frame_found = true; return global_link_transforms_[robot_link->getLinkIndex()]; } robot_link = nullptr; @@ -1039,8 +1102,9 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c return IDENTITY_TRANSFORM; } if (tf.size() > 1) - ROS_DEBUG_NAMED(LOGNAME, "There are multiple geometries associated to attached body '%s'. " - "Returning the transform for the first one.", + ROS_DEBUG_NAMED(LOGNAME, + "There are multiple geometries associated to attached body '%s'. " + "Returning the transform for the first one.", frame_id.c_str()); robot_link = jt->second->getAttachedLink(); frame_found = true; @@ -1049,9 +1113,9 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c } // Check if an AttachedBody has a subframe with name frame_id - for (const std::pair& body : attached_body_map_) + for (const std::pair& body : attached_body_map_) { - const auto& transform = body.second->getSubframeTransform(frame_id, &frame_found); + const auto& transform = body.second->getGlobalSubframeTransform(frame_id, &frame_found); if (frame_found) { robot_link = body.second->getAttachedLink(); @@ -1078,7 +1142,7 @@ bool RobotState::knowsFrameTransform(const std::string& frame_id) const return !it->second->getGlobalCollisionBodyTransforms().empty(); // Check if an AttachedBody has a subframe with name frame_id - for (const std::pair& body : attached_body_map_) + for (const std::pair& body : attached_body_map_) { if (body.second->hasSubframeTransform(frame_id)) return true; @@ -1109,12 +1173,12 @@ void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std for (const std::string& link_name : link_names) { ROS_DEBUG_NAMED(LOGNAME, "Trying to get marker for link '%s'", link_name.c_str()); - const LinkModel* lm = robot_model_->getLinkModel(link_name); - if (!lm) + const LinkModel* link_model = robot_model_->getLinkModel(link_name); + if (!link_model) continue; if (include_attached) for (const std::pair& it : attached_body_map_) - if (it.second->getAttachedLink() == lm) + if (it.second->getAttachedLink() == link_model) { for (std::size_t j = 0; j < it.second->getShapes().size(); ++j) { @@ -1132,37 +1196,38 @@ void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std } } - if (lm->getShapes().empty()) + if (link_model->getShapes().empty()) continue; - for (std::size_t j = 0; j < lm->getShapes().size(); ++j) + for (std::size_t j = 0; j < link_model->getShapes().size(); ++j) { visualization_msgs::Marker mark; mark.header.frame_id = robot_model_->getModelFrame(); mark.header.stamp = tm; // we prefer using the visual mesh, if a mesh is available and we have one body to render - const std::string& mesh_resource = lm->getVisualMeshFilename(); - if (mesh_resource.empty() || lm->getShapes().size() > 1) + const std::string& mesh_resource = link_model->getVisualMeshFilename(); + if (mesh_resource.empty() || link_model->getShapes().size() > 1) { - if (!shapes::constructMarkerFromShape(lm->getShapes()[j].get(), mark)) + if (!shapes::constructMarkerFromShape(link_model->getShapes()[j].get(), mark)) continue; // if the object is invisible (0 volume) we skip it if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits::epsilon()) continue; - mark.pose = tf2::toMsg(global_collision_body_transforms_[lm->getFirstCollisionBodyTransformIndex() + j]); + mark.pose = + tf2::toMsg(global_collision_body_transforms_[link_model->getFirstCollisionBodyTransformIndex() + j]); } else { mark.type = mark.MESH_RESOURCE; mark.mesh_use_embedded_materials = false; mark.mesh_resource = mesh_resource; - const Eigen::Vector3d& mesh_scale = lm->getVisualMeshScale(); + const Eigen::Vector3d& mesh_scale = link_model->getVisualMeshScale(); mark.scale.x = mesh_scale[0]; mark.scale.y = mesh_scale[1]; mark.scale.z = mesh_scale[2]; - mark.pose = tf2::toMsg(global_link_transforms_[lm->getLinkIndex()] * lm->getVisualMeshOrigin()); + mark.pose = tf2::toMsg(global_link_transforms_[link_model->getLinkIndex()] * link_model->getVisualMeshOrigin()); } arr.markers.push_back(mark); @@ -1198,15 +1263,17 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link return false; } - const robot_model::JointModel* root_joint_model = group->getJointModels()[0]; // group->getJointRoots()[0]; - const robot_model::LinkModel* root_link_model = root_joint_model->getParentLinkModel(); + const moveit::core::JointModel* root_joint_model = group->getJointModels()[0]; // group->getJointRoots()[0]; + const moveit::core::LinkModel* root_link_model = root_joint_model->getParentLinkModel(); + // getGlobalLinkTransform() returns a valid isometry by contract Eigen::Isometry3d reference_transform = root_link_model ? getGlobalLinkTransform(root_link_model).inverse() : Eigen::Isometry3d::Identity(); int rows = use_quaternion_representation ? 7 : 6; int columns = group->getVariableCount(); jacobian = Eigen::MatrixXd::Zero(rows, columns); - Eigen::Isometry3d link_transform = reference_transform * getGlobalLinkTransform(link); + // getGlobalLinkTransform() returns a valid isometry by contract + Eigen::Isometry3d link_transform = reference_transform * getGlobalLinkTransform(link); // valid isometry Eigen::Vector3d point_transform = link_transform * reference_point_position; /* @@ -1231,29 +1298,28 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link const JointModel* pjm = link->getParentJointModel(); if (pjm->getVariableCount() > 0) { - if (not group->hasJointModel(pjm->getName())) + if (!group->hasJointModel(pjm->getName())) { link = pjm->getParentLinkModel(); continue; } unsigned int joint_index = group->getVariableGroupIndex(pjm->getName()); - if (pjm->getType() == robot_model::JointModel::REVOLUTE) + // getGlobalLinkTransform() returns a valid isometry by contract + joint_transform = reference_transform * getGlobalLinkTransform(link); // valid isometry + if (pjm->getType() == moveit::core::JointModel::REVOLUTE) { - joint_transform = reference_transform * getGlobalLinkTransform(link); - joint_axis = joint_transform.rotation() * static_cast(pjm)->getAxis(); + joint_axis = joint_transform.linear() * static_cast(pjm)->getAxis(); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation()); jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis; } - else if (pjm->getType() == robot_model::JointModel::PRISMATIC) + else if (pjm->getType() == moveit::core::JointModel::PRISMATIC) { - joint_transform = reference_transform * getGlobalLinkTransform(link); - joint_axis = joint_transform.rotation() * static_cast(pjm)->getAxis(); + joint_axis = joint_transform.linear() * static_cast(pjm)->getAxis(); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis; } - else if (pjm->getType() == robot_model::JointModel::PLANAR) + else if (pjm->getType() == moveit::core::JointModel::PLANAR) { - joint_transform = reference_transform * getGlobalLinkTransform(link); joint_axis = joint_transform * Eigen::Vector3d(1.0, 0.0, 0.0); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis; joint_axis = joint_transform * Eigen::Vector3d(0.0, 1.0, 0.0); @@ -1277,7 +1343,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link // [x] [ w -z y ] [ omega_2 ] // [y] [ z w -x ] [ omega_3 ] // [z] [ -y x w ] - Eigen::Quaterniond q(link_transform.rotation()); + Eigen::Quaterniond q(link_transform.linear()); double w = q.w(), x = q.x(), y = q.y(), z = q.z(); Eigen::MatrixXd quaternion_update_matrix(4, 3); quaternion_update_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w; @@ -1435,13 +1501,14 @@ bool RobotState::setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& // Bring the pose to the frame of the IK solver if (!Transforms::sameFrame(ik_frame, robot_model_->getModelFrame())) { - const LinkModel* lm = getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame); - if (!lm) + const LinkModel* link_model = + getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame); + if (!link_model) { ROS_ERROR_STREAM_NAMED(LOGNAME, "IK frame '" << ik_frame << "' does not exist."); return false; } - pose = getGlobalLinkTransform(lm).inverse() * pose; + pose = getGlobalLinkTransform(link_model).inverse() * pose; } return true; } @@ -1526,8 +1593,9 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is std::vector consistency_limits; if (consistency_limit_sets.size() > 1) { - ROS_ERROR_NAMED(LOGNAME, "Invalid number (%zu) of sets of consistency limits for a setFromIK request " - "that is being solved by a single IK solver", + ROS_ERROR_NAMED(LOGNAME, + "Invalid number (%zu) of sets of consistency limits for a setFromIK request " + "that is being solved by a single IK solver", consistency_limit_sets.size()); return false; } @@ -1592,13 +1660,13 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is } if (pose_frame != solver_tip_frame) { - const robot_model::LinkModel* lm = getLinkModel(pose_frame); - if (!lm) + const moveit::core::LinkModel* link_model = getLinkModel(pose_frame); + if (!link_model) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Pose frame '" << pose_frame << "' does not exist."); return false; } - const robot_model::LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms(); + const moveit::core::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); for (const std::pair& fixed_link : fixed_links) if (Transforms::sameFrame(fixed_link.first->getName(), solver_tip_frame)) { @@ -1772,6 +1840,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: // Each each pose's tip frame naming for (std::size_t i = 0; i < poses_in.size(); ++i) { + ASSERT_ISOMETRY(transformed_poses[i]) // unsanitized input, could contain a non-isometry Eigen::Isometry3d& pose = transformed_poses[i]; std::string& pose_frame = pose_frames[i]; @@ -1792,26 +1861,27 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: if (hasAttachedBody(pose_frame)) { const AttachedBody* body = getAttachedBody(pose_frame); - const EigenSTL::vector_Isometry3d& ab_trans = body->getFixedTransforms(); + const EigenSTL::vector_Isometry3d& ab_trans = body->getFixedTransforms(); // valid isometries by contract if (ab_trans.size() != 1) { ROS_ERROR_NAMED(LOGNAME, "Cannot use an attached body with multiple geometries as a reference frame."); return false; } pose_frame = body->getAttachedLinkName(); - pose = pose * ab_trans[0].inverse(); + pose = pose * ab_trans[0].inverse(); // valid isometry } if (pose_frame != solver_tip_frame) { - const robot_model::LinkModel* lm = getLinkModel(pose_frame); - if (!lm) + const moveit::core::LinkModel* link_model = getLinkModel(pose_frame); + if (!link_model) return false; - const robot_model::LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms(); + // getAssociatedFixedTransforms() returns valid isometries by contract + const moveit::core::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); for (const std::pair& fixed_link : fixed_links) if (fixed_link.first->getName() == solver_tip_frame) { pose_frame = solver_tip_frame; - pose = pose * fixed_link.second; + pose = pose * fixed_link.second; // valid isometry break; } } @@ -1833,7 +1903,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: for (std::size_t i = 0; i < transformed_poses.size(); ++i) { - Eigen::Quaterniond quat(transformed_poses[i].rotation()); + Eigen::Quaterniond quat(transformed_poses[i].linear()); Eigen::Vector3d point(transformed_poses[i].translation()); ik_queries[i].position.x = point.x(); ik_queries[i].position.y = point.y(); @@ -2098,7 +2168,8 @@ void RobotState::printStateInfo(std::ostream& out) const void RobotState::printTransform(const Eigen::Isometry3d& transform, std::ostream& out) const { - Eigen::Quaterniond q(transform.rotation()); + ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry + Eigen::Quaterniond q(transform.linear()); out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", " << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "]" << std::endl; @@ -2125,8 +2196,8 @@ void RobotState::printTransforms(std::ostream& out) const } out << "Link poses:" << std::endl; - const std::vector& lm = robot_model_->getLinkModels(); - for (const LinkModel* link : lm) + const std::vector& link_model = robot_model_->getLinkModels(); + for (const LinkModel* link : link_model) { out << " " << link->getName() << ": "; printTransform(global_link_transforms_[link->getLinkIndex()], out); @@ -2173,19 +2244,19 @@ void RobotState::getStateTreeJointString(std::ostream& ss, const JointModel* jm, ss << pfx << jm->getVariableNames()[i] << std::setw(12) << position_[jm->getFirstVariableIndex() + i] << std::endl; } - const LinkModel* lm = jm->getChildLinkModel(); + const LinkModel* link_model = jm->getChildLinkModel(); - ss << pfx << "Link: " << lm->getName() << std::endl; - getPoseString(ss, lm->getJointOriginTransform(), pfx + "joint_origin:"); + ss << pfx << "Link: " << link_model->getName() << std::endl; + getPoseString(ss, link_model->getJointOriginTransform(), pfx + "joint_origin:"); if (variable_joint_transforms_) { getPoseString(ss, variable_joint_transforms_[jm->getJointIndex()], pfx + "joint_variable:"); - getPoseString(ss, global_link_transforms_[lm->getLinkIndex()], pfx + "link_global:"); + getPoseString(ss, global_link_transforms_[link_model->getLinkIndex()], pfx + "link_global:"); } - for (std::vector::const_iterator it = lm->getChildJointModels().begin(); - it != lm->getChildJointModels().end(); ++it) - getStateTreeJointString(ss, *it, pfx, it + 1 == lm->getChildJointModels().end()); + for (std::vector::const_iterator it = link_model->getChildJointModels().begin(); + it != link_model->getChildJointModels().end(); ++it) + getStateTreeJointString(ss, *it, pfx, it + 1 == link_model->getChildJointModels().end()); } std::ostream& operator<<(std::ostream& out, const RobotState& s) diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index 95ae2c1f62..3a1f8a8b48 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2018, CITEC Bielefeld -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, CITEC Bielefeld + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Robert Haschke */ #include @@ -95,9 +95,9 @@ class Timing : public testing::Test TEST_F(Timing, stateUpdate) { - robot_model::RobotModelPtr model = moveit::core::loadTestingRobotModel("pr2_description"); + moveit::core::RobotModelPtr model = moveit::core::loadTestingRobotModel("pr2_description"); ASSERT_TRUE(bool(model)); - robot_state::RobotState state(model); + moveit::core::RobotState state(model); ScopedTimer t("RobotState updates: "); for (unsigned i = 0; i < 1e5; ++i) { diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 2a1fb259fc..50b703fdaa 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ #include @@ -450,28 +450,28 @@ TEST_F(OneRobot, FK) state.setVariablePositions(joint_values); EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(1, 1, 0)); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).x(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).y(), 1e-5); - EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).z(), 1e-5); - EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).w(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).x(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).y(), 1e-5); + EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).z(), 1e-5); + EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).w(), 1e-5); EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_a").translation(), Eigen::Vector3d(1, 1, 0)); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).x(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).y(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).z(), 1e-5); - EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).w(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).x(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).y(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).z(), 1e-5); + EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).w(), 1e-5); EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_b").translation(), Eigen::Vector3d(1, 1.5, 0)); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).x(), 1e-5); - EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).y(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).z(), 1e-5); - EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).w(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).x(), 1e-5); + EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).y(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).z(), 1e-5); + EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).w(), 1e-5); EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(1.08, 1.4, 0)); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).x(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).y(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).z(), 1e-5); - EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).w(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).x(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).y(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).z(), 1e-5); + EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).w(), 1e-5); EXPECT_TRUE(state.satisfiesBounds()); @@ -535,7 +535,7 @@ TEST_F(OneRobot, FK) TEST_F(OneRobot, testPrintCurrentPositionWithJointLimits) { moveit::core::RobotState state(robot_model_); - const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e"); + const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e"); ASSERT_TRUE(joint_model_group); state.setToDefaultValues(); diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index fa058a2c93..97d89b46a1 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Martin Pecka */ @@ -61,15 +61,15 @@ class TestAABB : public testing::Test protected: void SetUp() override{}; - robot_state::RobotState loadModel(const std::string& robot_name) + moveit::core::RobotState loadModel(const std::string& robot_name) { - robot_model::RobotModelPtr model = moveit::core::loadTestingRobotModel(robot_name); + moveit::core::RobotModelPtr model = moveit::core::loadTestingRobotModel(robot_name); return loadModel(model); } - robot_state::RobotState loadModel(const robot_model::RobotModelPtr& model) + moveit::core::RobotState loadModel(const moveit::core::RobotModelPtr& model) { - robot_state::RobotState robot_state = robot_state::RobotState(model); + moveit::core::RobotState robot_state = moveit::core::RobotState(model); robot_state.setToDefaultValues(); robot_state.update(true); @@ -85,7 +85,7 @@ TEST_F(TestAABB, TestPR2) { // Contains a link with mesh geometry that is not centered - robot_state::RobotState pr2_state = this->loadModel("pr2"); + moveit::core::RobotState pr2_state = this->loadModel("pr2"); const Eigen::Vector3d& extents_base_footprint = pr2_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin(); // values taken from moveit_resources/pr2_description/urdf/robot.xml @@ -211,7 +211,7 @@ TEST_F(TestAABB, TestPR2) msg.scale.z = extents[2]; msg.color.r = 0; msg.color.b = 1; - Eigen::Quaterniond q(transform.rotation()); + Eigen::Quaterniond q(transform.linear()); msg.pose.orientation.x = q.x(); msg.pose.orientation.y = q.y(); msg.pose.orientation.z = q.z(); @@ -257,7 +257,7 @@ TEST_F(TestAABB, TestPR2) msg.scale.z = extents[2]; msg.color.r = 0; msg.color.b = 1; - Eigen::Quaterniond q(transforms[i].rotation()); + Eigen::Quaterniond q(transforms[i].linear()); msg.pose.orientation.x = q.x(); msg.pose.orientation.y = q.y(); msg.pose.orientation.z = q.z(); @@ -278,7 +278,7 @@ TEST_F(TestAABB, TestSimple) builder.addChain("base_footprint->base_link", "fixed", { origin }); tf2::toMsg(tf2::Vector3(0, 0, 0), origin.position); - builder.addCollisionMesh("base_link", "package://moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl", + builder.addCollisionMesh("base_link", "package://moveit_resources_pr2_description/urdf/meshes/base_v0/base_L.stl", origin); tf2::toMsg(tf2::Vector3(0, 0, 0.071), origin.position); @@ -288,7 +288,7 @@ TEST_F(TestAABB, TestSimple) builder.addGroup({}, { "world_joint" }, "base"); ASSERT_TRUE(builder.isValid()); - robot_state::RobotState simple_state = loadModel(builder.build()); + moveit::core::RobotState simple_state = loadModel(builder.build()); std::vector simple_aabb; simple_state.computeAABB(simple_aabb); @@ -323,7 +323,7 @@ TEST_F(TestAABB, TestComplex) builder.addGroup({}, { "world_joint" }, "base"); ASSERT_TRUE(builder.isValid()); - robot_state::RobotState complex_state = this->loadModel(builder.build()); + moveit::core::RobotState complex_state = this->loadModel(builder.build()); EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[0], 0.1, 1e-4); EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[1], 1.0, 1e-4); diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index bba9b70656..57c0ac22c0 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -1,40 +1,39 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2019, PickNik LLC. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Michael Lautman */ -#include #include #include #include @@ -241,21 +240,21 @@ class OneRobot : public testing::Test moveit::core::RobotModelConstPtr robot_model_; }; -std::size_t generateTestTraj(std::vector>& traj, +std::size_t generateTestTraj(std::vector>& traj, const moveit::core::RobotModelConstPtr& robot_model_, const moveit::core::JointModelGroup* joint_model_group) { traj.clear(); - std::shared_ptr robot_state(new robot_state::RobotState(robot_model_)); + std::shared_ptr robot_state(new moveit::core::RobotState(robot_model_)); robot_state->setToDefaultValues(); double ja, jc; // 3 waypoints with default joints for (std::size_t traj_ix = 0; traj_ix < 3; ++traj_ix) { - // robot_state.reset(new robot_state::RobotState(*robot_state)); - traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state))); + // robot_state.reset(new moveit::core::RobotState(*robot_state)); + traj.push_back(moveit::core::RobotStatePtr(new moveit::core::RobotState(*robot_state))); } ja = robot_state->getVariablePosition("panda_joint0"); @@ -266,20 +265,20 @@ std::size_t generateTestTraj(std::vectorsetVariablePosition("panda_joint0", ja); jc = jc - 0.01; robot_state->setVariablePosition("panda_joint1", jc); - traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state))); + traj.push_back(moveit::core::RobotStatePtr(new moveit::core::RobotState(*robot_state))); // 5th waypoint with a large jump of 1.01 in first revolute joint ja = ja + 1.01; robot_state->setVariablePosition("panda_joint0", ja); - traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state))); + traj.push_back(moveit::core::RobotStatePtr(new moveit::core::RobotState(*robot_state))); // 6th waypoint with a large jump of 1.01 in first prismatic joint jc = jc + 1.01; robot_state->setVariablePosition("panda_joint1", jc); - traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state))); + traj.push_back(moveit::core::RobotStatePtr(new moveit::core::RobotState(*robot_state))); // 7th waypoint with no jump - traj.push_back(robot_state::RobotStatePtr(new robot_state::RobotState(*robot_state))); + traj.push_back(moveit::core::RobotStatePtr(new moveit::core::RobotState(*robot_state))); return traj.size(); } @@ -287,7 +286,7 @@ std::size_t generateTestTraj(std::vectorgetJointModelGroup("arm"); - std::vector> traj; + std::vector> traj; // The full trajectory should be of length 7 const std::size_t expected_full_traj_len = 7; @@ -302,7 +301,7 @@ TEST_F(OneRobot, testGenerateTrajectory) TEST_F(OneRobot, checkAbsoluteJointSpaceJump) { const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("arm"); - std::vector> traj; + std::vector> traj; // A revolute joint jumps 1.01 at the 5th waypoint and a prismatic joint jumps 1.01 at the 6th waypoint const std::size_t expected_revolute_jump_traj_len = 4; @@ -353,7 +352,7 @@ TEST_F(OneRobot, checkAbsoluteJointSpaceJump) TEST_F(OneRobot, checkRelativeJointSpaceJump) { const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("arm"); - std::vector> traj; + std::vector> traj; // The first large jump of 1.01 occurs at the 5th waypoint so the test should trim the trajectory to length 4 const std::size_t expected_relative_jump_traj_len = 4; diff --git a/moveit_core/robot_state/test/test_kinematic_complex.cpp b/moveit_core/robot_state/test/test_kinematic_complex.cpp index d0db3f5be9..c09cc6907c 100644 --- a/moveit_core/robot_state/test/test_kinematic_complex.cpp +++ b/moveit_core/robot_state/test/test_kinematic_complex.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -43,6 +43,7 @@ #include #include #include +#include #include class LoadPlanningModelsPr2 : public testing::Test @@ -50,24 +51,10 @@ class LoadPlanningModelsPr2 : public testing::Test protected: void SetUp() override { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); - - srdf_model_.reset(new srdf::Model()); - std::string xml_string; - std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - urdf_model_ = urdf::parseURDF(xml_string); - } - srdf_model_->initFile(*urdf_model_, (res_path / "pr2_description/srdf/robot.xml").string()); - robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_model_)); + std::string robot_name = "pr2"; + urdf_model_ = moveit::core::loadModelInterface(robot_name); + srdf_model_ = moveit::core::loadSRDFModel(robot_name); + robot_model_ = std::make_shared(urdf_model_, srdf_model_); }; void TearDown() override @@ -77,7 +64,7 @@ class LoadPlanningModelsPr2 : public testing::Test protected: urdf::ModelInterfaceSharedPtr urdf_model_; srdf::ModelSharedPtr srdf_model_; - moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; }; TEST_F(LoadPlanningModelsPr2, InitOK) diff --git a/moveit_core/robot_trajectory/CMakeLists.txt b/moveit_core/robot_trajectory/CMakeLists.txt index 9bcfe4ae86..078572677c 100644 --- a/moveit_core/robot_trajectory/CMakeLists.txt +++ b/moveit_core/robot_trajectory/CMakeLists.txt @@ -8,6 +8,12 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + +if(CATKIN_ENABLE_TESTING) + catkin_add_gtest(test_robot_trajectory test/test_robot_trajectory.cpp) + target_link_libraries(test_robot_trajectory moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) +endif() diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 64f4fe309e..f595570b7d 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -34,11 +34,9 @@ /* Author: Ioan Sucan, Adam Leeper */ -#ifndef MOVEIT_ROBOT_TRAJECTORY_KINEMATIC_TRAJECTORY_ -#define MOVEIT_ROBOT_TRAJECTORY_KINEMATIC_TRAJECTORY_ +#pragma once #include -#include #include #include #include @@ -46,23 +44,32 @@ namespace robot_trajectory { -MOVEIT_CLASS_FORWARD(RobotTrajectory); +MOVEIT_CLASS_FORWARD(RobotTrajectory); // Defines RobotTrajectoryPtr, ConstPtr, WeakPtr... etc /** \brief Maintain a sequence of waypoints and the time durations between these waypoints */ class RobotTrajectory { public: - RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const std::string& group); + RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group); - RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const robot_model::JointModelGroup* group); + RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const moveit::core::JointModelGroup* group); - const robot_model::RobotModelConstPtr& getRobotModel() const + /** Assignment operator, performing a shallow copy, i.e. copying waypoints by pointer */ + RobotTrajectory& operator=(const RobotTrajectory&) = default; + + /** @brief Copy constructor allowing a shallow or deep copy of waypoints + * @param other - RobotTrajectory to copy from + * @param deepcopy - copy waypoints by value (true) or by pointer (false)? + */ + RobotTrajectory(const RobotTrajectory& other, bool deepcopy = false); + + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } - const robot_model::JointModelGroup* getGroup() const + const moveit::core::JointModelGroup* getGroup() const { return group_; } @@ -76,32 +83,32 @@ class RobotTrajectory return waypoints_.size(); } - const robot_state::RobotState& getWayPoint(std::size_t index) const + const moveit::core::RobotState& getWayPoint(std::size_t index) const { return *waypoints_[index]; } - const robot_state::RobotState& getLastWayPoint() const + const moveit::core::RobotState& getLastWayPoint() const { return *waypoints_.back(); } - const robot_state::RobotState& getFirstWayPoint() const + const moveit::core::RobotState& getFirstWayPoint() const { return *waypoints_.front(); } - robot_state::RobotStatePtr& getWayPointPtr(std::size_t index) + moveit::core::RobotStatePtr& getWayPointPtr(std::size_t index) { return waypoints_[index]; } - robot_state::RobotStatePtr& getLastWayPointPtr() + moveit::core::RobotStatePtr& getLastWayPointPtr() { return waypoints_.back(); } - robot_state::RobotStatePtr& getFirstWayPointPtr() + moveit::core::RobotStatePtr& getFirstWayPointPtr() { return waypoints_.front(); } @@ -117,7 +124,7 @@ class RobotTrajectory */ double getWayPointDurationFromStart(std::size_t index) const; - MOVEIT_DEPRECATED double getWaypointDurationFromStart(std::size_t index) const; + [[deprecated]] double getWaypointDurationFromStart(std::size_t index) const; double getWayPointDurationFromPrevious(std::size_t index) const { @@ -144,9 +151,9 @@ class RobotTrajectory * \param state - current robot state * \param dt - duration from previous */ - void addSuffixWayPoint(const robot_state::RobotState& state, double dt) + void addSuffixWayPoint(const moveit::core::RobotState& state, double dt) { - addSuffixWayPoint(robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt); + addSuffixWayPoint(moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt); } /** @@ -154,31 +161,31 @@ class RobotTrajectory * \param state - current robot state * \param dt - duration from previous */ - void addSuffixWayPoint(const robot_state::RobotStatePtr& state, double dt) + void addSuffixWayPoint(const moveit::core::RobotStatePtr& state, double dt) { state->update(); waypoints_.push_back(state); duration_from_previous_.push_back(dt); } - void addPrefixWayPoint(const robot_state::RobotState& state, double dt) + void addPrefixWayPoint(const moveit::core::RobotState& state, double dt) { - addPrefixWayPoint(robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt); + addPrefixWayPoint(moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt); } - void addPrefixWayPoint(const robot_state::RobotStatePtr& state, double dt) + void addPrefixWayPoint(const moveit::core::RobotStatePtr& state, double dt) { state->update(); waypoints_.push_front(state); duration_from_previous_.push_front(dt); } - void insertWayPoint(std::size_t index, const robot_state::RobotState& state, double dt) + void insertWayPoint(std::size_t index, const moveit::core::RobotState& state, double dt) { - insertWayPoint(index, robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt); + insertWayPoint(index, moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt); } - void insertWayPoint(std::size_t index, const robot_state::RobotStatePtr& state, double dt) + void insertWayPoint(std::size_t index, const moveit::core::RobotStatePtr& state, double dt) { state->update(); waypoints_.insert(waypoints_.begin() + index, state); @@ -202,9 +209,12 @@ class RobotTrajectory void clear(); + double getDuration() const; + double getAverageSegmentDuration() const; - void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory) const; + void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory, + const std::vector& joint_filter = std::vector()) const; /** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required to contain the values @@ -212,7 +222,7 @@ class RobotTrajectory point in the trajectory to be constructed internally is obtained by copying the reference state and overwriting the content from a trajectory point in \e trajectory. */ - void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, + void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const trajectory_msgs::JointTrajectory& trajectory); /** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required @@ -221,7 +231,7 @@ class RobotTrajectory point in the trajectory to be constructed internally is obtained by copying the reference state and overwriting the content from a trajectory point in \e trajectory. */ - void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, + void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const moveit_msgs::RobotTrajectory& trajectory); /** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required @@ -231,13 +241,13 @@ class RobotTrajectory using \e state. Each point in the trajectory to be constructed internally is obtained by copying the reference state and overwriting the content from a trajectory point in \e trajectory. */ - void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, const moveit_msgs::RobotState& state, + void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const moveit_msgs::RobotState& state, const moveit_msgs::RobotTrajectory& trajectory); void reverse(); void unwind(); - void unwind(const robot_state::RobotState& state); + void unwind(const moveit::core::RobotState& state); /** @brief Finds the waypoint indicies before and after a duration from start. * @param The duration from start. @@ -254,14 +264,12 @@ class RobotTrajectory * @param The resulting robot state. * @return True if state is valid, false otherwise (trajectory is empty). */ - bool getStateAtDurationFromStart(const double request_duration, robot_state::RobotStatePtr& output_state) const; + bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const; private: - robot_model::RobotModelConstPtr robot_model_; - const robot_model::JointModelGroup* group_; - std::deque waypoints_; + moveit::core::RobotModelConstPtr robot_model_; + const moveit::core::JointModelGroup* group_; + std::deque waypoints_; std::deque duration_from_previous_; }; -} - -#endif +} // namespace robot_trajectory diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index b0d5de749f..4130f412be 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -42,17 +42,30 @@ namespace robot_trajectory { -RobotTrajectory::RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const std::string& group) +RobotTrajectory::RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group) : robot_model_(robot_model), group_(group.empty() ? nullptr : robot_model->getJointModelGroup(group)) { } -RobotTrajectory::RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, - const robot_model::JointModelGroup* group) +RobotTrajectory::RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, + const moveit::core::JointModelGroup* group) : robot_model_(robot_model), group_(group) { } +RobotTrajectory::RobotTrajectory(const RobotTrajectory& other, bool deepcopy) +{ + *this = other; // default assignment operator performs a shallow copy + if (deepcopy) + { + this->waypoints_.clear(); + for (const auto& waypoint : other.waypoints_) + { + this->waypoints_.emplace_back(std::make_shared(*waypoint)); + } + } +} + void RobotTrajectory::setGroupName(const std::string& group_name) { group_ = robot_model_->getJointModelGroup(group_name); @@ -66,13 +79,17 @@ const std::string& RobotTrajectory::getGroupName() const return EMPTY; } +double RobotTrajectory::getDuration() const +{ + return std::accumulate(duration_from_previous_.begin(), duration_from_previous_.end(), 0.0); +} + double RobotTrajectory::getAverageSegmentDuration() const { if (duration_from_previous_.empty()) return 0.0; else - return std::accumulate(duration_from_previous_.begin(), duration_from_previous_.end(), 0.0) / - (double)duration_from_previous_.size(); + return getDuration() / static_cast(duration_from_previous_.size()); } void RobotTrajectory::swap(RobotTrajectory& other) @@ -101,7 +118,7 @@ void RobotTrajectory::append(const RobotTrajectory& source, double dt, size_t st void RobotTrajectory::reverse() { std::reverse(waypoints_.begin(), waypoints_.end()); - for (robot_state::RobotStatePtr& waypoint : waypoints_) + for (moveit::core::RobotStatePtr& waypoint : waypoints_) { // reversing the trajectory implies inverting the velocity profile waypoint->invertVelocity(); @@ -119,7 +136,7 @@ void RobotTrajectory::unwind() if (waypoints_.empty()) return; - const std::vector& cont_joints = + const std::vector& cont_joints = group_ ? group_->getContinuousJointModels() : robot_model_->getContinuousJointModels(); for (const moveit::core::JointModel* cont_joint : cont_joints) @@ -149,12 +166,12 @@ void RobotTrajectory::unwind() waypoint->update(); } -void RobotTrajectory::unwind(const robot_state::RobotState& state) +void RobotTrajectory::unwind(const moveit::core::RobotState& state) { if (waypoints_.empty()) return; - const std::vector& cont_joints = + const std::vector& cont_joints = group_ ? group_->getContinuousJointModels() : robot_model_->getContinuousJointModels(); for (const moveit::core::JointModel* cont_joint : cont_joints) @@ -201,20 +218,27 @@ void RobotTrajectory::clear() duration_from_previous_.clear(); } -void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory) const +void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory, + const std::vector& joint_filter) const { trajectory = moveit_msgs::RobotTrajectory(); if (waypoints_.empty()) return; - const std::vector& jnt = + const std::vector& jnts = group_ ? group_->getActiveJointModels() : robot_model_->getActiveJointModels(); - std::vector onedof; - std::vector mdof; + std::vector onedof; + std::vector mdof; trajectory.joint_trajectory.joint_names.clear(); trajectory.multi_dof_joint_trajectory.joint_names.clear(); - for (const moveit::core::JointModel* active_joint : jnt) + for (const moveit::core::JointModel* active_joint : jnts) + { + // only consider joints listed in joint_filter + if (!joint_filter.empty() && + std::find(joint_filter.begin(), joint_filter.end(), active_joint->getName()) == joint_filter.end()) + continue; + if (active_joint->getVariableCount() == 1) { trajectory.joint_trajectory.joint_names.push_back(active_joint->getName()); @@ -225,6 +249,8 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajec trajectory.multi_dof_joint_trajectory.joint_names.push_back(active_joint->getName()); mdof.push_back(active_joint); } + } + if (!onedof.empty()) { trajectory.joint_trajectory.header.frame_id = robot_model_->getModelFrame(); @@ -289,7 +315,7 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajec geometry_msgs::TransformStamped ts = tf2::eigenToTransform(waypoints_[i]->getJointTransform(mdof[j])); trajectory.multi_dof_joint_trajectory.points[i].transforms[j] = ts.transform; // TODO: currently only checking for planar multi DOF joints / need to add check for floating - if (waypoints_[i]->hasVelocities() && (mdof[j]->getType() == robot_model::JointModel::JointType::PLANAR)) + if (waypoints_[i]->hasVelocities() && (mdof[j]->getType() == moveit::core::JointModel::JointType::PLANAR)) { const std::vector names = mdof[j]->getVariableNames(); const double* velocities = waypoints_[i]->getJointVelocities(mdof[j]); @@ -326,11 +352,11 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajec } } -void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, +void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const trajectory_msgs::JointTrajectory& trajectory) { // make a copy just in case the next clear() removes the memory for the reference passed in - const robot_state::RobotState& copy = reference_state; + const moveit::core::RobotState& copy = reference_state; clear(); std::size_t state_count = trajectory.points.size(); ros::Time last_time_stamp = trajectory.header.stamp; @@ -339,7 +365,7 @@ void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& refer for (std::size_t i = 0; i < state_count; ++i) { this_time_stamp = trajectory.header.stamp + trajectory.points[i].time_from_start; - robot_state::RobotStatePtr st(new robot_state::RobotState(copy)); + moveit::core::RobotStatePtr st(new moveit::core::RobotState(copy)); st->setVariablePositions(trajectory.joint_names, trajectory.points[i].positions); if (!trajectory.points[i].velocities.empty()) st->setVariableVelocities(trajectory.joint_names, trajectory.points[i].velocities); @@ -352,11 +378,11 @@ void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& refer } } -void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, +void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const moveit_msgs::RobotTrajectory& trajectory) { // make a copy just in case the next clear() removes the memory for the reference passed in - const robot_state::RobotState& copy = reference_state; + const moveit::core::RobotState& copy = reference_state; clear(); std::size_t state_count = @@ -368,11 +394,10 @@ void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& refer for (std::size_t i = 0; i < state_count; ++i) { - robot_state::RobotStatePtr st(new robot_state::RobotState(copy)); + moveit::core::RobotStatePtr st(new moveit::core::RobotState(copy)); if (trajectory.joint_trajectory.points.size() > i) { - st->setVariablePositions(trajectory.joint_trajectory.joint_names, - trajectory.joint_trajectory.points[i].positions); + st->setVariablePositions(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].positions); if (!trajectory.joint_trajectory.points[i].velocities.empty()) st->setVariableVelocities(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].velocities); @@ -400,12 +425,12 @@ void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& refer } } -void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, +void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const moveit_msgs::RobotState& state, const moveit_msgs::RobotTrajectory& trajectory) { - robot_state::RobotState st(reference_state); - robot_state::robotStateMsgToRobotState(state, st); + moveit::core::RobotState st(reference_state); + moveit::core::robotStateMsgToRobotState(state, st); setRobotTrajectoryMsg(st, trajectory); } @@ -459,7 +484,7 @@ double RobotTrajectory::getWaypointDurationFromStart(std::size_t index) const } bool RobotTrajectory::getStateAtDurationFromStart(const double request_duration, - robot_state::RobotStatePtr& output_state) const + moveit::core::RobotStatePtr& output_state) const { // If there are no waypoints we can't do anything if (getWayPointCount() == 0) diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp new file mode 100644 index 0000000000..092c9a54f4 --- /dev/null +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -0,0 +1,211 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Ioan Sucan */ + +#include +#include +#include +#include +#include + +class RobotTrajectoryTestFixture : public testing::Test +{ +protected: + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; + const std::string robot_model_name_ = "panda"; + const std::string arm_jmg_name_ = "panda_arm"; + +protected: + void SetUp() override + { + robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_); + robot_state_ = std::make_shared(robot_model_); + robot_state_->setToDefaultValues(); + robot_state_->update(); + } + + void TearDown() override + { + } + + void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) + { + // Init a traj + ASSERT_TRUE(robot_model_->hasJointModelGroup(arm_jmg_name_)) + << "Robot model does not have group: " << arm_jmg_name_; + + trajectory = std::make_shared(robot_model_, arm_jmg_name_); + + EXPECT_EQ(trajectory->getGroupName(), arm_jmg_name_) << "Generated trajectory group name does not match"; + EXPECT_TRUE(trajectory->empty()) << "Generated trajectory not empty"; + + double duration_from_previous = 0.1; + std::size_t waypoint_count = 5; + for (std::size_t ix = 0; ix < waypoint_count; ++ix) + trajectory->addSuffixWayPoint(robot_state_, duration_from_previous); + // Quick check that getDuration is working correctly + EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count) + << "Generated trajectory duration incorrect"; + EXPECT_EQ(waypoint_count, trajectory->getWayPointDurations().size()) + << "Generated trajectory has the wrong number of waypoints"; + } + + void copyTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, + robot_trajectory::RobotTrajectoryPtr& trajectory_copy, bool deepcopy) + { + // Copy the trajectory + trajectory_copy = std::make_shared(*trajectory, deepcopy); + // Quick check that the getDuration values match + EXPECT_EQ(trajectory_copy->getDuration(), trajectory->getDuration()); + EXPECT_EQ(trajectory_copy->getWayPointDurations().size(), trajectory->getWayPointDurations().size()); + } + + void modifyFirstWaypointPtrAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) + { + /////////////////////////// + // Get the first waypoint by POINTER, modify it, and check that the value WAS updated in trajectory + /////////////////////////// + // Get the first waypoint by shared pointer + moveit::core::RobotStatePtr trajectory_first_waypoint = trajectory->getWayPointPtr(0); + // Get the first waypoint joint values + std::vector trajectory_first_state; + trajectory_first_waypoint->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state); + + // Modify the first waypoint joint values + trajectory_first_state[0] += 0.01; + trajectory_first_waypoint->setJointGroupPositions(arm_jmg_name_, trajectory_first_state); + + // Check that the trajectory's first waypoint was updated + moveit::core::RobotStatePtr trajectory_first_waypoint_after_update = trajectory->getWayPointPtr(0); + std::vector trajectory_first_state_after_update; + trajectory_first_waypoint_after_update->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); + EXPECT_EQ(trajectory_first_state[0], trajectory_first_state_after_update[0]); + } + + void modifyFirstWaypointAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) + { + /////////////////////////// + // Get the first waypoint by VALUE, modify it, and check that the value WAS NOT updated in trajectory + /////////////////////////// + // Get the first waypoint by shared pointer + moveit::core::RobotState trajectory_first_waypoint = trajectory->getWayPoint(0); + // Get the first waypoint joint values + std::vector trajectory_first_state; + trajectory_first_waypoint.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state); + + // Modify the first waypoint joint values + trajectory_first_state[0] += 0.01; + trajectory_first_waypoint.setJointGroupPositions(arm_jmg_name_, trajectory_first_state); + + // Check that the trajectory's first waypoint was updated + moveit::core::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0); + std::vector trajectory_first_state_after_update; + trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); + EXPECT_NE(trajectory_first_state[0], trajectory_first_state_after_update[0]); + } +}; + +TEST_F(RobotTrajectoryTestFixture, ModifyFirstWaypointByPtr) +{ + robot_trajectory::RobotTrajectoryPtr trajectory; + initTestTrajectory(trajectory); + modifyFirstWaypointPtrAndCheckTrajectory(trajectory); +} + +TEST_F(RobotTrajectoryTestFixture, ModifyFirstWaypointByValue) +{ + robot_trajectory::RobotTrajectoryPtr trajectory; + initTestTrajectory(trajectory); + modifyFirstWaypointAndCheckTrajectory(trajectory); +} + +TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryShallowCopy) +{ + bool deepcopy = false; + + robot_trajectory::RobotTrajectoryPtr trajectory; + robot_trajectory::RobotTrajectoryPtr trajectory_copy; + + initTestTrajectory(trajectory); + copyTrajectory(trajectory, trajectory_copy, deepcopy); + modifyFirstWaypointPtrAndCheckTrajectory(trajectory); + + // Check that modifying the waypoint also modified the trajectory + moveit::core::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0); + std::vector trajectory_first_state_after_update; + trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); + + // Get the first waypoint in the modified trajectory_copy + moveit::core::RobotState trajectory_copy_first_waypoint_after_update = trajectory_copy->getWayPoint(0); + std::vector trajectory_copy_first_state_after_update; + trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, + trajectory_copy_first_state_after_update); + + // Check that we updated the value correctly in the trajectory + EXPECT_EQ(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]); +} + +TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDeepCopy) +{ + bool deepcopy = true; + + robot_trajectory::RobotTrajectoryPtr trajectory; + robot_trajectory::RobotTrajectoryPtr trajectory_copy; + + initTestTrajectory(trajectory); + copyTrajectory(trajectory, trajectory_copy, deepcopy); + modifyFirstWaypointPtrAndCheckTrajectory(trajectory); + + // Check that modifying the waypoint also modified the trajectory + moveit::core::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0); + std::vector trajectory_first_state_after_update; + trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update); + + // Get the first waypoint in the modified trajectory_copy + moveit::core::RobotState trajectory_copy_first_waypoint_after_update = trajectory_copy->getWayPoint(0); + std::vector trajectory_copy_first_state_after_update; + trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, + trajectory_copy_first_state_after_update); + + // Check that we updated the value correctly in the trajectory + EXPECT_NE(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h b/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h index 66c05d4036..99f50e55a1 100644 --- a/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h +++ b/moveit_core/sensor_manager/include/moveit/sensor_manager/sensor_manager.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_SENSOR_MANAGER_ -#define MOVEIT_MOVEIT_SENSOR_MANAGER_ +#pragma once #include #include @@ -43,7 +42,7 @@ #include #include -/// Namespace for the base class of a MoveIt! sensor manager +/// Namespace for the base class of a MoveIt sensor manager namespace moveit_sensor_manager { /** \brief Define the frame of reference and the frustum of a sensor (usually this is a visual sensor) */ @@ -71,7 +70,7 @@ struct SensorInfo double y_angle; }; -MOVEIT_CLASS_FORWARD(MoveItSensorManager); +MOVEIT_CLASS_FORWARD(MoveItSensorManager); // Defines MoveItSensorManagerPtr, ConstPtr, WeakPtr... etc class MoveItSensorManager { @@ -102,6 +101,4 @@ class MoveItSensorManager virtual bool pointSensorTo(const std::string& name, const geometry_msgs::PointStamped& target, moveit_msgs::RobotTrajectory& sensor_trajectory) = 0; }; -} - -#endif +} // namespace moveit_sensor_manager diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index fdd392a586..e595724de0 100644 --- a/moveit_core/trajectory_processing/CMakeLists.txt +++ b/moveit_core/trajectory_processing/CMakeLists.txt @@ -13,13 +13,12 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) catkin_add_gtest(test_time_parameterization test/test_time_parameterization.cpp) target_link_libraries(test_time_parameterization moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) catkin_add_gtest(test_time_optimal_trajectory_generation test/test_time_optimal_trajectory_generation.cpp) diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h index c3cb24cb12..92f14befdb 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h @@ -35,12 +35,8 @@ /* Author: Ken Anderson */ -#ifndef MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_SPLINE_PARAMETERIZATION__ -#define MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_SPLINE_PARAMETERIZATION__ +#pragma once -#include -#include -#include #include namespace trajectory_processing @@ -75,7 +71,7 @@ class IterativeSplineParameterization { public: IterativeSplineParameterization(bool add_points = true); - ~IterativeSplineParameterization(); + ~IterativeSplineParameterization() = default; bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0) const; @@ -84,6 +80,4 @@ class IterativeSplineParameterization bool add_points_; /// @brief If true, add two points to trajectory (first and last segments). /// If false, move the 2nd and 2nd-last points. }; -} - -#endif +} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h index 59b883f089..b142f987a5 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h @@ -34,12 +34,8 @@ /* Author: Ken Anderson */ -#ifndef MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_PARABOLIC_SMOOTHER_ -#define MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_PARABOLIC_SMOOTHER_ +#pragma once -#include -#include -#include #include namespace trajectory_processing @@ -50,7 +46,7 @@ class IterativeParabolicTimeParameterization { public: IterativeParabolicTimeParameterization(unsigned int max_iterations = 100, double max_time_change_per_it = .01); - ~IterativeParabolicTimeParameterization(); + ~IterativeParabolicTimeParameterization() = default; bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0) const; @@ -68,6 +64,4 @@ class IterativeParabolicTimeParameterization double findT1(const double d1, const double d2, double t1, const double t2, const double a_max) const; double findT2(const double d1, const double d2, const double t1, double t2, const double a_max) const; }; -} - -#endif +} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index 21db426fd1..0ee4d13f41 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -36,8 +36,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef MOVEIT_TRAJECTORY_PROCESSING_TIME_OPTIMAL_TRAJECTORY_GENERATION_H -#define MOVEIT_TRAJECTORY_PROCESSING_TIME_OPTIMAL_TRAJECTORY_GENERATION_H +#pragma once #include #include @@ -96,7 +95,7 @@ class Trajectory Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration, double time_step = 0.001); - ~Trajectory(void); + ~Trajectory(); /** @brief Call this method after constructing the object to make sure the trajectory generation succeeded without errors. If this method returns @@ -163,7 +162,8 @@ class Trajectory class TimeOptimalTrajectoryGeneration { public: - TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, const double resample_dt = 0.1); + TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, const double resample_dt = 0.1, + const double min_angle_change = 0.001); ~TimeOptimalTrajectoryGeneration(); bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, @@ -172,7 +172,6 @@ class TimeOptimalTrajectoryGeneration private: const double path_tolerance_; const double resample_dt_; + const double min_angle_change_; }; } // namespace trajectory_processing - -#endif // MOVEIT_TRAJECTORY_PROCESSING_TIME_OPTIMAL_TRAJECTORY_GENERATION_H diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h index 0c7658d39f..bd49dee3c4 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_TRAJECTORY_PROCESSING_TRAJECTORY_TOOLS_ -#define MOVEIT_TRAJECTORY_PROCESSING_TRAJECTORY_TOOLS_ +#pragma once #include @@ -43,6 +42,4 @@ namespace trajectory_processing { bool isTrajectoryEmpty(const moveit_msgs::RobotTrajectory& trajectory); std::size_t trajectoryWaypointCount(const moveit_msgs::RobotTrajectory& trajectory); -} - -#endif +} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp index 1f7816b860..855c3a7929 100644 --- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp @@ -35,7 +35,6 @@ /* Author: Ken Anderson */ #include -#include #include #include @@ -47,8 +46,7 @@ namespace trajectory_processing static void fit_cubic_spline(const int n, const double dt[], const double x[], double x1[], double x2[]); static void adjust_two_positions(const int n, const double dt[], double x[], double x1[], double x2[], const double x2_i, const double x2_f); -static void init_times(const int n, double dt[], const double x[], const double max_velocity, - const double min_velocity); +static void init_times(const int n, double dt[], const double x[], const double max_velocity, const double min_velocity); static int fit_spline_and_adjust_times(const int n, double dt[], const double x[], double x1[], double x2[], const double max_velocity, const double min_velocity, const double max_acceleration, const double min_acceleration, @@ -78,8 +76,6 @@ IterativeSplineParameterization::IterativeSplineParameterization(bool add_points { } -IterativeSplineParameterization::~IterativeSplineParameterization() = default; - bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor) const @@ -87,14 +83,14 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT if (trajectory.empty()) return true; - const robot_model::JointModelGroup* group = trajectory.getGroup(); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization", "It looks like the planner did not set " "the group the plan was computed for"); return false; } - const robot_model::RobotModel& rmodel = group->getParentModel(); + const moveit::core::RobotModel& rmodel = group->getParentModel(); const std::vector& idx = group->getVariableIndexList(); const std::vector& vars = group->getVariableNames(); double velocity_scaling_factor = 1.0; @@ -134,8 +130,8 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT // (required to force acceleration to specified values at endpoints) if (trajectory.getWayPointCount() >= 2) { - robot_state::RobotState point = trajectory.getWayPoint(1); - robot_state::RobotStatePtr p0, p1; + moveit::core::RobotState point = trajectory.getWayPoint(1); + moveit::core::RobotStatePtr p0, p1; // 2nd point is 90% of p0, and 10% of p1 p0 = trajectory.getWayPointPtr(0); @@ -197,7 +193,7 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT t2[j].accelerations_[num_points - 1] = t2[j].final_acceleration_; // Set bounds based on model, or default limits - const robot_model::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); + const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); t2[j].max_velocity_ = VLIMIT; t2[j].min_velocity_ = -VLIMIT; if (bounds.velocity_bounded_) @@ -301,8 +297,7 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT &t2[j].accelerations_[0], t2[j].initial_acceleration_, t2[j].final_acceleration_); } - fit_cubic_spline(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], - &t2[j].accelerations_[0]); + fit_cubic_spline(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], &t2[j].accelerations_[0]); for (unsigned i = 0; i < num_points; i++) { const double acc = t2[j].accelerations_[i]; diff --git a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp index 1b86b56918..afef388e49 100644 --- a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp @@ -50,8 +50,6 @@ IterativeParabolicTimeParameterization::IterativeParabolicTimeParameterization(u { } -IterativeParabolicTimeParameterization::~IterativeParabolicTimeParameterization() = default; - namespace { void printPoint(const trajectory_msgs::JointTrajectoryPoint& point, std::size_t i) @@ -102,10 +100,10 @@ void IterativeParabolicTimeParameterization::applyVelocityConstraints(robot_traj std::vector& time_diff, const double max_velocity_scaling_factor) const { - const robot_model::JointModelGroup* group = rob_trajectory.getGroup(); + const moveit::core::JointModelGroup* group = rob_trajectory.getGroup(); const std::vector& vars = group->getVariableNames(); const std::vector& idx = group->getVariableIndexList(); - const robot_model::RobotModel& rmodel = group->getParentModel(); + const moveit::core::RobotModel& rmodel = group->getParentModel(); const int num_points = rob_trajectory.getWayPointCount(); double velocity_scaling_factor = 1.0; @@ -123,13 +121,13 @@ void IterativeParabolicTimeParameterization::applyVelocityConstraints(robot_traj for (int i = 0; i < num_points - 1; ++i) { - const robot_state::RobotStatePtr& curr_waypoint = rob_trajectory.getWayPointPtr(i); - const robot_state::RobotStatePtr& next_waypoint = rob_trajectory.getWayPointPtr(i + 1); + const moveit::core::RobotStatePtr& curr_waypoint = rob_trajectory.getWayPointPtr(i); + const moveit::core::RobotStatePtr& next_waypoint = rob_trajectory.getWayPointPtr(i + 1); for (std::size_t j = 0; j < vars.size(); ++j) { double v_max = DEFAULT_VEL_MAX; - const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]); + const moveit::core::VariableBounds& b = rmodel.getVariableBounds(vars[j]); if (b.velocity_bounded_) v_max = std::min(fabs(b.max_velocity_ * velocity_scaling_factor), fabs(b.min_velocity_ * velocity_scaling_factor)); @@ -195,11 +193,11 @@ void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const s double time_sum = 0.0; - robot_state::RobotStatePtr prev_waypoint; - robot_state::RobotStatePtr curr_waypoint; - robot_state::RobotStatePtr next_waypoint; + moveit::core::RobotStatePtr prev_waypoint; + moveit::core::RobotStatePtr curr_waypoint; + moveit::core::RobotStatePtr next_waypoint; - const robot_model::JointModelGroup* group = rob_trajectory.getGroup(); + const moveit::core::JointModelGroup* group = rob_trajectory.getGroup(); const std::vector& vars = group->getVariableNames(); const std::vector& idx = group->getVariableIndexList(); @@ -301,14 +299,14 @@ void IterativeParabolicTimeParameterization::applyAccelerationConstraints( robot_trajectory::RobotTrajectory& rob_trajectory, std::vector& time_diff, const double max_acceleration_scaling_factor) const { - robot_state::RobotStatePtr prev_waypoint; - robot_state::RobotStatePtr curr_waypoint; - robot_state::RobotStatePtr next_waypoint; + moveit::core::RobotStatePtr prev_waypoint; + moveit::core::RobotStatePtr curr_waypoint; + moveit::core::RobotStatePtr next_waypoint; - const robot_model::JointModelGroup* group = rob_trajectory.getGroup(); + const moveit::core::JointModelGroup* group = rob_trajectory.getGroup(); const std::vector& vars = group->getVariableNames(); const std::vector& idx = group->getVariableIndexList(); - const robot_model::RobotModel& rmodel = group->getParentModel(); + const moveit::core::RobotModel& rmodel = group->getParentModel(); const int num_points = rob_trajectory.getWayPointCount(); const unsigned int num_joints = group->getVariableCount(); @@ -363,7 +361,7 @@ void IterativeParabolicTimeParameterization::applyAccelerationConstraints( // Get acceleration limits double a_max = DEFAULT_ACCEL_MAX; - const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]); + const moveit::core::VariableBounds& b = rmodel.getVariableBounds(vars[j]); if (b.acceleration_bounded_) a_max = std::min(fabs(b.max_acceleration_ * acceleration_scaling_factor), fabs(b.min_acceleration_ * acceleration_scaling_factor)); @@ -464,7 +462,7 @@ bool IterativeParabolicTimeParameterization::computeTimeStamps(robot_trajectory: if (trajectory.empty()) return true; - const robot_model::JointModelGroup* group = trajectory.getGroup(); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { ROS_ERROR_NAMED("trajectory_processing.iterative_time_parameterization", "It looks like the planner did not set " diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 68fe5a8ccf..a873becbbb 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -39,14 +39,11 @@ #include #include #include -#include #include #include #include #include -#include - namespace trajectory_processing { const std::string LOGNAME = "trajectory_processing.time_optimal_trajectory_generation"; @@ -110,7 +107,6 @@ class CircularPathSegment : public PathSegment const Eigen::VectorXd start_direction = (intersection - start).normalized(); const Eigen::VectorXd end_direction = (end - intersection).normalized(); - // check if directions are divergent if ((start_direction - end_direction).norm() < 0.000001) { length_ = 0.0; @@ -122,7 +118,7 @@ class CircularPathSegment : public PathSegment } // directions must be different at this point so angle is always non-zero - const double angle = acos(start_direction.dot(end_direction)); + const double angle = acos(std::max(-1.0, start_direction.dot(end_direction))); const double start_distance = (start - intersection).norm(); const double end_distance = (end - intersection).norm(); @@ -439,9 +435,8 @@ bool Trajectory::getNextAccelerationSwitchingPoint(double path_pos, TrajectorySt if ((before_path_vel > after_path_vel || getMinMaxPhaseSlope(switching_path_pos - EPS, switching_path_vel, false) > getAccelerationMaxPathVelocityDeriv(switching_path_pos - 2.0 * EPS)) && - (before_path_vel < after_path_vel || - getMinMaxPhaseSlope(switching_path_pos + EPS, switching_path_vel, true) < - getAccelerationMaxPathVelocityDeriv(switching_path_pos + 2.0 * EPS))) + (before_path_vel < after_path_vel || getMinMaxPhaseSlope(switching_path_pos + EPS, switching_path_vel, true) < + getAccelerationMaxPathVelocityDeriv(switching_path_pos + 2.0 * EPS))) { break; } @@ -481,9 +476,8 @@ bool Trajectory::getNextVelocitySwitchingPoint(double path_pos, TrajectoryStep& { start = true; } - } while ((!start || - getMinMaxPhaseSlope(path_pos, getVelocityMaxPathVelocity(path_pos), false) > - getVelocityMaxPathVelocityDeriv(path_pos)) && + } while ((!start || getMinMaxPhaseSlope(path_pos, getVelocityMaxPathVelocity(path_pos), false) > + getVelocityMaxPathVelocityDeriv(path_pos)) && path_pos < path_.getLength()); if (path_pos >= path_.getLength()) @@ -538,6 +532,12 @@ bool Trajectory::integrateForward(std::list& trajectory, double if (next_discontinuity != switching_points.end() && path_pos > next_discontinuity->first) { + // Avoid having a TrajectoryStep with path_pos near a switching point which will cause an almost identical + // TrajectoryStep get added in the next run (https://github.com/ros-planning/moveit/issues/1665) + if (path_pos - next_discontinuity->first < EPS) + { + continue; + } path_vel = old_path_vel + (next_discontinuity->first - old_path_pos) * (path_vel - old_path_vel) / (path_pos - old_path_pos); path_pos = next_discontinuity->first; @@ -859,8 +859,9 @@ Eigen::VectorXd Trajectory::getAcceleration(double time) const return path_acc; } -TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(const double path_tolerance, const double resample_dt) - : path_tolerance_(path_tolerance), resample_dt_(resample_dt) +TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(const double path_tolerance, const double resample_dt, + const double min_angle_change) + : path_tolerance_(path_tolerance), resample_dt_(resample_dt), min_angle_change_(min_angle_change) { } @@ -875,7 +876,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT if (trajectory.empty()) return true; - const robot_model::JointModelGroup* group = trajectory.getGroup(); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { ROS_ERROR_NAMED(LOGNAME, "It looks like the planner did not set the group the plan was computed for"); @@ -921,7 +922,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT // This is pretty much copied from IterativeParabolicTimeParameterization::applyVelocityConstraints const std::vector& vars = group->getVariableNames(); const std::vector& idx = group->getVariableIndexList(); - const robot_model::RobotModel& rmodel = group->getParentModel(); + const moveit::core::RobotModel& rmodel = group->getParentModel(); const unsigned num_joints = group->getVariableCount(); const unsigned num_points = trajectory.getWayPointCount(); @@ -930,7 +931,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT Eigen::VectorXd max_acceleration(num_joints); for (size_t j = 0; j < num_joints; ++j) { - const robot_model::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); + const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); // Limits need to be non-zero, otherwise we never exit max_velocity[j] = 1.0; @@ -954,14 +955,14 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT std::list points; for (size_t p = 0; p < num_points; ++p) { - robot_state::RobotStatePtr waypoint = trajectory.getWayPointPtr(p); + moveit::core::RobotStatePtr waypoint = trajectory.getWayPointPtr(p); Eigen::VectorXd new_point(num_joints); bool diverse_point = (p == 0); for (size_t j = 0; j < num_joints; j++) { new_point[j] = waypoint->getVariablePosition(idx[j]); - if (p > 0 && std::abs(new_point[j] - points.back()[j]) > 0.001) + if (p > 0 && std::abs(new_point[j] - points.back()[j]) > min_angle_change_) diverse_point = true; } @@ -972,8 +973,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT // Return trajectory with only the first waypoint if there are not multiple diverse points if (points.size() == 1) { - ROS_WARN_NAMED(LOGNAME, "Trajectory is not being parameterized since it only contains a single distinct waypoint."); - robot_state::RobotState waypoint = robot_state::RobotState(trajectory.getWayPoint(0)); + ROS_DEBUG_NAMED(LOGNAME, + "Trajectory is parameterized with 0.0 dynamics since it only contains a single distinct waypoint."); + moveit::core::RobotState waypoint = moveit::core::RobotState(trajectory.getWayPoint(0)); + waypoint.zeroVelocities(); + waypoint.zeroAccelerations(); trajectory.clear(); trajectory.addSuffixWayPoint(waypoint, 0.0); return true; @@ -991,7 +995,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt_); // Resample and fill in trajectory - robot_state::RobotState waypoint = robot_state::RobotState(trajectory.getWayPoint(0)); + moveit::core::RobotState waypoint = moveit::core::RobotState(trajectory.getWayPoint(0)); trajectory.clear(); double last_t = 0; for (size_t sample = 0; sample <= sample_count; ++sample) diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp index d05a423ce9..7413afac44 100644 --- a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp @@ -150,6 +150,83 @@ TEST(time_optimal_trajectory_generation, test3) EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]); } +// Test that totg algorithm doesn't give large acceleration +TEST(time_optimal_trajectory_generation, testLargeAccel) +{ + double path_tolerance = 0.1; + double resample_dt = 0.1; + Eigen::VectorXd waypoint(6); + std::list waypoints; + Eigen::VectorXd max_velocities(6); + Eigen::VectorXd max_accelerations(6); + + // Waypoints + // clang-format off + waypoint << 1.6113056281076339, + -0.21400163389235427, + -1.974502599739185, + 9.9653618690354051e-12, + -1.3810916877429624, + 1.5293902838041467; + waypoints.push_back(waypoint); + + waypoint << 1.6088016187976597, + -0.21792862470933924, + -1.9758628799742952, + 0.00010424017303217738, + -1.3835690515335755, + 1.5279972853269816; + waypoints.push_back(waypoint); + + waypoint << 1.5887695443178671, + -0.24934455124521923, + -1.9867451218551782, + 0.00093816147756670078, + -1.4033879618584812, + 1.5168532975096607; + waypoints.push_back(waypoint); + + waypoint << 1.1647412393815282, + -0.91434018564402375, + -2.2170946337498498, + 0.018590164397622583, + -1.8229041212673529, + 1.2809632867583278; + waypoints.push_back(waypoint); + + // Max velocities + max_velocities << 0.89535390627300004, + 0.89535390627300004, + 0.79587013890930003, + 0.92022484811399996, + 0.82074108075029995, + 1.3927727430915; + // Max accelerations + max_accelerations << 0.82673490883799994, + 0.78539816339699997, + 0.60883578557700002, + 3.2074759432319997, + 1.4398966328939999, + 4.7292792634680003; + // clang-format on + + Trajectory parameterized(Path(waypoints, path_tolerance), max_velocities, max_accelerations, 0.001); + + ASSERT_TRUE(parameterized.isValid()); + + size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt); + for (size_t sample = 0; sample <= sample_count; ++sample) + { + // always sample the end of the trajectory as well + double t = std::min(parameterized.getDuration(), sample * resample_dt); + Eigen::VectorXd acceleration = parameterized.getAcceleration(t); + + ASSERT_EQ(acceleration.size(), 6); + for (std::size_t i = 0; i < 6; ++i) + EXPECT_NEAR(acceleration(i), 0.0, 100.0) << "Invalid acceleration at position " << sample_count << "\n"; + } +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp index a27a834141..d3cba00ecb 100644 --- a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp @@ -53,7 +53,7 @@ int initRepeatedPointTrajectory(robot_trajectory::RobotTrajectory& trajectory) const int num = 3; unsigned i; - const robot_model::JointModelGroup* group = trajectory.getGroup(); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { ROS_ERROR_NAMED("trajectory_processing", "Need to set the group"); @@ -83,7 +83,7 @@ int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory, double const double max = 2.0; unsigned i; - const robot_model::JointModelGroup* group = trajectory.getGroup(); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); if (!group) { ROS_ERROR_NAMED("trajectory_processing", "Need to set the group"); @@ -109,7 +109,7 @@ int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory, double void printTrajectory(robot_trajectory::RobotTrajectory& trajectory) { - const robot_model::JointModelGroup* group = trajectory.getGroup(); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); const std::vector& idx = group->getVariableIndexList(); unsigned int count = trajectory.getWayPointCount(); @@ -118,13 +118,13 @@ void printTrajectory(robot_trajectory::RobotTrajectory& trajectory) std::cout << " Trajectory Points" << std::endl; for (unsigned i = 0; i < count; i++) { - robot_state::RobotStatePtr point = trajectory.getWayPointPtr(i); + moveit::core::RobotStatePtr point = trajectory.getWayPointPtr(i); printf(" waypoint %2d time %6.2f pos %6.2f vel %6.2f acc %6.2f ", i, trajectory.getWayPointDurationFromStart(i), point->getVariablePosition(idx[0]), point->getVariableVelocity(idx[0]), point->getVariableAcceleration(idx[0])); if (i > 0) { - robot_state::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1); + moveit::core::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1); printf("jrk %6.2f", (point->getVariableAcceleration(idx[0]) - prev->getVariableAcceleration(idx[0])) / (trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1))); diff --git a/moveit_core/transforms/CMakeLists.txt b/moveit_core/transforms/CMakeLists.txt index 411b925a68..ac2bd777b3 100644 --- a/moveit_core/transforms/CMakeLists.txt +++ b/moveit_core/transforms/CMakeLists.txt @@ -8,7 +8,8 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) # Unit tests diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 949a61341f..e5a26c4c5f 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -34,11 +34,9 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_TRANSFORMS_ -#define MOVEIT_TRANSFORMS_ +#pragma once #include -#include #include #include #include @@ -47,13 +45,12 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(Transforms); +MOVEIT_CLASS_FORWARD(Transforms); // Defines TransformsPtr, ConstPtr, WeakPtr... etc /// @brief Map frame names to the transformation matrix that can transform objects from the frame name to the planning /// frame -typedef std::map, - Eigen::aligned_allocator > > - FixedTransformsMap; +using FixedTransformsMap = std::map, + Eigen::aligned_allocator > >; /** @brief Provides an implementation of a snapshot of a transform tree that can be easily queried for transforming different quantities. Transforms are maintained as a list of transforms to a particular frame. @@ -87,13 +84,14 @@ class Transforms : private boost::noncopyable /** * @brief Return all the transforms - * @return A map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the planning frame) + * @return A map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the planning frame). The + * transforms are guaranteed to be valid isometries. */ const FixedTransformsMap& getAllTransforms() const; /** * @brief Get a vector of all the transforms as ROS messages - * @param transforms The output transforms + * @param transforms The output transforms. They are guaranteed to be valid isometries. */ void copyTransforms(std::vector& transforms) const; @@ -138,40 +136,44 @@ class Transforms : private boost::noncopyable */ void transformVector3(const std::string& from_frame, const Eigen::Vector3d& v_in, Eigen::Vector3d& v_out) const { - v_out = getTransform(from_frame).rotation() * v_in; + // getTransform() returns a valid isometry by contract + v_out = getTransform(from_frame).linear() * v_in; } /** * @brief Transform a quaternion in from_frame to the target_frame * @param from_frame The frame in which the input quaternion is specified - * @param v_in The input quaternion (in from_frame) - * @param v_out The resultant (transformed) quaternion + * @param v_in The input quaternion (in from_frame). Make sure the quaternion is normalized. + * @param v_out The resultant (transformed) quaternion. It is guaranteed to be a valid and normalized quaternion. */ void transformQuaternion(const std::string& from_frame, const Eigen::Quaterniond& q_in, Eigen::Quaterniond& q_out) const { - q_out = getTransform(from_frame).rotation() * q_in; + // getTransform() returns a valid isometry by contract + q_out = getTransform(from_frame).linear() * q_in; } /** * @brief Transform a rotation matrix in from_frame to the target_frame * @param from_frame The frame in which the input rotation matrix is specified - * @param m_in The input rotation matrix (in from_frame) - * @param m_out The resultant (transformed) rotation matrix + * @param m_in The input rotation matrix (in from_frame). Make sure the matrix is a valid rotation matrix. + * @param m_out The resultant (transformed) rotation matrix. It is guaranteed to be a valid rotation matrix. */ void transformRotationMatrix(const std::string& from_frame, const Eigen::Matrix3d& m_in, Eigen::Matrix3d& m_out) const { - m_out = getTransform(from_frame).rotation() * m_in; + // getTransform() returns a valid isometry by contract + m_out = getTransform(from_frame).linear() * m_in; } /** * @brief Transform a pose in from_frame to the target_frame * @param from_frame The frame in which the input pose is specified - * @param t_in The input pose (in from_frame) - * @param t_out The resultant (transformed) pose + * @param t_in The input pose (in from_frame). Make sure the pose is a valid isometry. + * @param t_out The resultant (transformed) pose. It is guaranteed to be a valid isometry. */ void transformPose(const std::string& from_frame, const Eigen::Isometry3d& t_in, Eigen::Isometry3d& t_out) const { + // getTransform() returns a valid isometry by contract t_out = getTransform(from_frame) * t_in; } /**@}*/ @@ -190,7 +192,7 @@ class Transforms : private boost::noncopyable /** * @brief Get transform for from_frame (w.r.t target frame) * @param from_frame The string id of the frame for which the transform is being computed - * @return The required transform + * @return The required transform. It is guaranteed to be a valid isometry. */ virtual const Eigen::Isometry3d& getTransform(const std::string& from_frame) const; @@ -198,7 +200,5 @@ class Transforms : private boost::noncopyable std::string target_frame_; FixedTransformsMap transforms_map_; }; -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index b853319172..005bc2e07f 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -35,6 +35,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -47,7 +48,7 @@ Transforms::Transforms(const std::string& target_frame) : target_frame_(target_f { boost::trim(target_frame_); if (target_frame_.empty()) - ROS_ERROR_NAMED("transforms", "The target frame for MoveIt! Transforms cannot be empty."); + ROS_ERROR_NAMED("transforms", "The target frame for MoveIt Transforms cannot be empty."); else { transforms_map_[target_frame_] = Eigen::Isometry3d::Identity(); @@ -75,6 +76,10 @@ const FixedTransformsMap& Transforms::getAllTransforms() const void Transforms::setAllTransforms(const FixedTransformsMap& transforms) { + for (const auto& t : transforms) + { + ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry + } transforms_map_ = transforms; } @@ -114,6 +119,7 @@ bool Transforms::canTransform(const std::string& from_frame) const void Transforms::setTransform(const Eigen::Isometry3d& t, const std::string& from_frame) { + ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry if (from_frame.empty()) ROS_ERROR_NAMED("transforms", "Cannot record transform with empty name"); else diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index 05f4bb4878..51bc2c8a71 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -3,8 +3,9 @@ set(MOVEIT_LIB_NAME moveit_utils) add_library(${MOVEIT_LIB_NAME} src/lexical_casts.cpp src/xmlrpc_casts.cpp + src/message_checks.cpp ) - +add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -22,6 +23,7 @@ install( ${MOVEIT_LIB_NAME} ${MOVEIT_TEST_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/utils/include/moveit/utils/lexical_casts.h b/moveit_core/utils/include/moveit/utils/lexical_casts.h index ce0fbcb5b5..8b3496b028 100644 --- a/moveit_core/utils/include/moveit/utils/lexical_casts.h +++ b/moveit_core/utils/include/moveit/utils/lexical_casts.h @@ -34,8 +34,7 @@ /* Author: Simon Schmeisser */ -#ifndef MOVEIT_CORE_UTILS_LEXICAL_CASTS_ -#define MOVEIT_CORE_UTILS_LEXICAL_CASTS_ +#pragma once /** \file lexical_casts.h * \brief locale-agnostic conversion functions from floating point numbers to strings @@ -66,7 +65,5 @@ double toDouble(const std::string& s); \throws std::runtime_exception if not a valid number */ float toFloat(const std::string& s); -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/include/moveit/utils/message_checks.h b/moveit_core/utils/include/moveit/utils/message_checks.h new file mode 100644 index 0000000000..ffb12c8292 --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/message_checks.h @@ -0,0 +1,65 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Universitaet Hamburg. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Michael 'v4hn' Goerner */ + +#pragma once + +/** \file empty_msgs.h + * \brief Checks for empty MoveIt-related messages + * + */ + +#include +#include +#include +#include + +namespace moveit +{ +namespace core +{ +/** \brief Check if a message includes any information about a planning scene, or whether it is empty. */ +bool isEmpty(const moveit_msgs::PlanningScene& msg); + +/** \brief Check if a message includes any information about a planning scene world, or whether it is empty. */ +bool isEmpty(const moveit_msgs::PlanningSceneWorld& msg); + +/** \brief Check if a message includes any information about a robot state, or whether it is empty. */ +bool isEmpty(const moveit_msgs::RobotState& msg); + +/** \brief Check if a message specifies constraints */ +bool isEmpty(const moveit_msgs::Constraints& msg); +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h index fa9bbf989c..0667b0408e 100644 --- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h @@ -14,7 +14,7 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * Neither the name of MoveIt! nor the names of its + * * Neither the name of MoveIt nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -35,16 +35,10 @@ /* Author: Bryce Willey */ /** \brief convenience functions and classes used for making simple robot models for testing. */ -#ifndef MOVEIT_CORE_UTILS_TEST_ -#define MOVEIT_CORE_UTILS_TEST_ +#pragma once -#include -#include -#include #include #include -#include -#include #include #include @@ -107,14 +101,16 @@ class RobotModelBuilder * the joints will be given this type. To add multiple types of joints, call this method multiple times * \param[in] joint_origins The "parent to joint" origins for the joints connecting the links. If not used, all * origins will default to the identity transform + * \param[in] joint_axis The joint axis specified in the joint frame defaults to (1,0,0) */ void addChain(const std::string& section, const std::string& type, - const std::vector& joint_origins = {}); + const std::vector& joint_origins = {}, + urdf::Vector3 joint_axis = urdf::Vector3(1.0, 0.0, 0.0)); /** \brief Adds a collision mesh to a specific link. * \param[in] link_name The name of the link to which the mesh will be added. Must already be in the builder * \param[in] filename The path to the mesh file, e.g. - * "package://moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl" + * "package://moveit_resources_pr2_description/urdf/meshes/base_v0/base_L.stl" * \param[in] origin The origin pose of this collision mesh relative to the link origin */ void addCollisionMesh(const std::string& link_name, const std::string& filename, geometry_msgs::Pose origin); @@ -170,6 +166,9 @@ class RobotModelBuilder */ void addGroup(const std::vector& links, const std::vector& joints, const std::string& name); + void addEndEffector(const std::string& name, const std::string& parent_link, const std::string& parent_group = "", + const std::string& component_group = ""); + /** \} */ /** \brief Returns true if the building process so far has been valid. */ @@ -194,7 +193,5 @@ class RobotModelBuilder /// Whether the current builder state is valid. If any 'add' method fails, this becomes false. bool is_valid_ = true; }; -} -} - -#endif +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h b/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h index 2ffa863b3c..5b9c161a4d 100644 --- a/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h +++ b/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h @@ -51,8 +51,7 @@ double parseDouble(XmlRpc::XmlRpcValue& v); * @param name: if non-empty, print a warning message "name is not an array[size]" * @param description: if non-empty, serves as a descriptor for array items */ -bool isArray(XmlRpc::XmlRpcValue& v, size_t size = 0, const std::string& name = "", - const std::string& description = ""); +bool isArray(XmlRpc::XmlRpcValue& v, size_t size = 0, const std::string& name = "", const std::string& description = ""); /** check that v is a struct with given keys * @@ -60,5 +59,5 @@ bool isArray(XmlRpc::XmlRpcValue& v, size_t size = 0, const std::string& name = * @param name: if non-empty, print a warning message "name is not a struct with keys ..." */ bool isStruct(XmlRpc::XmlRpcValue& v, const std::vector& keys = {}, const std::string& name = ""); -} -} +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/src/message_checks.cpp b/moveit_core/utils/src/message_checks.cpp new file mode 100644 index 0000000000..b466e4768b --- /dev/null +++ b/moveit_core/utils/src/message_checks.cpp @@ -0,0 +1,73 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Universitaet Hamburg. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Hamburg University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Michael Goerner */ + +#include + +namespace moveit +{ +namespace core +{ +bool isEmpty(const moveit_msgs::PlanningScene& msg) +{ + return msg.name.empty() && msg.fixed_frame_transforms.empty() && msg.allowed_collision_matrix.entry_names.empty() && + msg.link_padding.empty() && msg.link_scale.empty() && isEmpty(msg.robot_state) && isEmpty(msg.world); +} + +bool isEmpty(const moveit_msgs::PlanningSceneWorld& msg) +{ + return msg.collision_objects.empty() && msg.octomap.octomap.data.empty(); +} + +bool isEmpty(const moveit_msgs::RobotState& msg) +{ + /* a state is empty if it includes no information and it is a diff; if the state is not a diff, then the implicit + information is + that the set of attached bodies is empty, so they must be cleared from the state to be updated */ + return static_cast(msg.is_diff) && msg.multi_dof_joint_state.joint_names.empty() && + msg.joint_state.name.empty() && msg.attached_collision_objects.empty() && msg.joint_state.position.empty() && + msg.joint_state.velocity.empty() && msg.joint_state.effort.empty() && + msg.multi_dof_joint_state.transforms.empty() && msg.multi_dof_joint_state.twist.empty() && + msg.multi_dof_joint_state.wrench.empty(); +} + +bool isEmpty(const moveit_msgs::Constraints& constr) +{ + return constr.position_constraints.empty() && constr.orientation_constraints.empty() && + constr.visibility_constraints.empty() && constr.joint_constraints.empty(); +} + +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index 9a083176a3..3492e12dbd 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -14,7 +14,7 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * Neither the name of MoveIt! nor the names of its + * * Neither the name of MoveIt nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -39,7 +39,8 @@ #include #include -#include "moveit/utils/robot_model_test_utils.h" +#include +#include #include namespace moveit @@ -58,15 +59,15 @@ moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name) urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name) { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); std::string urdf_path; if (robot_name == "pr2") { - urdf_path = (res_path / "pr2_description/urdf/robot.xml").string(); + urdf_path = ros::package::getPath("moveit_resources_pr2_description") + "/urdf/robot.xml"; } else { - urdf_path = (res_path / (robot_name + "_description") / "urdf" / (robot_name + ".urdf")).string(); + urdf_path = + ros::package::getPath("moveit_resources_" + robot_name + "_description") + "/urdf/" + robot_name + ".urdf"; } urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path); if (urdf_model == nullptr) @@ -79,17 +80,17 @@ urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name) srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name) { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); urdf::ModelInterfaceSharedPtr urdf_model = loadModelInterface(robot_name); srdf::ModelSharedPtr srdf_model(new srdf::Model()); std::string srdf_path; if (robot_name == "pr2") { - srdf_path = (res_path / "pr2_description/srdf/robot.xml").string(); + srdf_path = ros::package::getPath("moveit_resources_pr2_description") + "/srdf/robot.xml"; } else { - srdf_path = (res_path / (robot_name + "_moveit_config") / "config" / (robot_name + ".srdf")).string(); + srdf_path = + ros::package::getPath("moveit_resources_" + robot_name + "_moveit_config") + "/config/" + robot_name + ".srdf"; } srdf_model->initFile(*urdf_model, srdf_path); return srdf_model; @@ -109,7 +110,7 @@ RobotModelBuilder::RobotModelBuilder(const std::string& name, const std::string& } void RobotModelBuilder::addChain(const std::string& section, const std::string& type, - const std::vector& joint_origins) + const std::vector& joint_origins, urdf::Vector3 joint_axis) { std::vector link_names; boost::split_regex(link_names, section, boost::regex("->")); @@ -120,14 +121,14 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& return; } // First link should already be added. - if (not urdf_model_->getLink(link_names[0])) + if (!urdf_model_->getLink(link_names[0])) { ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_names[0].c_str()); is_valid_ = false; return; } - if (not joint_origins.empty() && link_names.size() - 1 != joint_origins.size()) + if (!joint_origins.empty() && link_names.size() - 1 != joint_origins.size()) { ROS_ERROR_NAMED(LOGNAME, "There should be one more link (%zu) than there are joint origins (%zu)", link_names.size(), joint_origins.size()); @@ -152,7 +153,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& joint->name = link_names[i - 1] + "-" + link_names[i] + "-joint"; // Default to Identity transform for origins. joint->parent_to_joint_origin_transform.clear(); - if (not joint_origins.empty()) + if (!joint_origins.empty()) { geometry_msgs::Pose o = joint_origins[i - 1]; joint->parent_to_joint_origin_transform.position = urdf::Vector3(o.position.x, o.position.y, o.position.z); @@ -181,7 +182,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& return; } - joint->axis = urdf::Vector3(1.0, 0.0, 0.0); + joint->axis = joint_axis; if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::PRISMATIC) { urdf::JointLimitsSharedPtr limits(new urdf::JointLimits); @@ -197,7 +198,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& void RobotModelBuilder::addInertial(const std::string& link_name, double mass, geometry_msgs::Pose origin, double ixx, double ixy, double ixz, double iyy, double iyz, double izz) { - if (not urdf_model_->getLink(link_name)) + if (!urdf_model_->getLink(link_name)) { ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str()); is_valid_ = false; @@ -260,7 +261,7 @@ void RobotModelBuilder::addCollisionMesh(const std::string& link_name, const std void RobotModelBuilder::addLinkCollision(const std::string& link_name, const urdf::CollisionSharedPtr& collision, geometry_msgs::Pose origin) { - if (not urdf_model_->getLink(link_name)) + if (!urdf_model_->getLink(link_name)) { ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str()); is_valid_ = false; @@ -278,7 +279,7 @@ void RobotModelBuilder::addLinkCollision(const std::string& link_name, const urd void RobotModelBuilder::addLinkVisual(const std::string& link_name, const urdf::VisualSharedPtr& vis, geometry_msgs::Pose origin) { - if (not urdf_model_->getLink(link_name)) + if (!urdf_model_->getLink(link_name)) { ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str()); is_valid_ = false; @@ -290,7 +291,7 @@ void RobotModelBuilder::addLinkVisual(const std::string& link_name, const urdf:: urdf::LinkSharedPtr link; urdf_model_->getLink(link_name, link); - if (not link->visual_array.empty()) + if (!link->visual_array.empty()) { link->visual_array.push_back(vis); } @@ -320,8 +321,7 @@ void RobotModelBuilder::addVirtualJoint(const std::string& parent_frame, const s srdf_writer_->virtual_joints_.push_back(new_virtual_joint); } -void RobotModelBuilder::addGroupChain(const std::string& base_link, const std::string& tip_link, - const std::string& name) +void RobotModelBuilder::addGroupChain(const std::string& base_link, const std::string& tip_link, const std::string& name) { srdf::Model::Group new_group; if (name.empty()) @@ -342,6 +342,17 @@ void RobotModelBuilder::addGroup(const std::vector& links, const st srdf_writer_->groups_.push_back(new_group); } +void RobotModelBuilder::addEndEffector(const std::string& name, const std::string& parent_link, + const std::string& parent_group, const std::string& component_group) +{ + srdf::Model::EndEffector eef; + eef.name_ = name; + eef.parent_link_ = parent_link; + eef.parent_group_ = parent_group; + eef.component_group_ = component_group; + srdf_writer_->end_effectors_.push_back(eef); +} + bool RobotModelBuilder::isValid() { return is_valid_; diff --git a/moveit_experimental/CHANGELOG.rst b/moveit_experimental/CHANGELOG.rst index 813d8a2015..b86a0e191b 100644 --- a/moveit_experimental/CHANGELOG.rst +++ b/moveit_experimental/CHANGELOG.rst @@ -2,7 +2,10 @@ Changelog for package moveit_experimental ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.0.1 (2019-03-08) +1.1.1 (2020-10-13) +------------------ + +1.1.0 (2020-09-04) ------------------ 1.0.0 (2019-02-24) @@ -69,7 +72,7 @@ Changelog for package moveit_experimental 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * Contributors: Dave Coleman 0.9.4 (2017-02-06) diff --git a/moveit_experimental/CMakeLists.txt b/moveit_experimental/CMakeLists.txt deleted file mode 100644 index 085c96e33d..0000000000 --- a/moveit_experimental/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(moveit_experimental) - -add_definitions(-std=c++14) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - -find_package(catkin REQUIRED -COMPONENTS - control_msgs - geometry_msgs - joy_teleop - moveit_ros_planning_interface - rosparam_shortcuts - spacenav_node - tf2_ros -) -find_package(Eigen3 REQUIRED) - -set(THIS_PACKAGE_INCLUDE_DIRS - jog_arm/include -) - -catkin_package( - INCLUDE_DIRS - ${THIS_PACKAGE_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} - LIBRARIES - CATKIN_DEPENDS - control_msgs - geometry_msgs - joy_teleop - moveit_ros_planning_interface - rosparam_shortcuts - spacenav_node - tf2_ros - DEPENDS - EIGEN3 -) - -include_directories( - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${THIS_PACKAGE_INCLUDE_DIRS} -) - -add_subdirectory(jog_arm) diff --git a/moveit_experimental/README.md b/moveit_experimental/README.md index 2d250777f5..7ef8ed18c2 100644 --- a/moveit_experimental/README.md +++ b/moveit_experimental/README.md @@ -1,5 +1,5 @@ -# Experimental Packages for MoveIt! +# Experimental Packages for MoveIt -This repository includes possibly broken/unmaintained/unfinished libraries that are being worked on or have been worked on in the past. They are not part of MoveIt! officially. +This repository includes possibly broken/unmaintained/unfinished libraries that are being worked on or have been worked on in the past. They are not part of MoveIt officially. No support is offered for code in this repository, but you are welcome to use it if you see fit. diff --git a/moveit_experimental/collision_distance_field_ros/CATKIN_IGNORE b/moveit_experimental/collision_distance_field_ros/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_experimental/collision_distance_field_ros/CMakeLists.txt b/moveit_experimental/collision_distance_field_ros/CMakeLists.txt index f8f1c9ccad..9094b77a6b 100644 --- a/moveit_experimental/collision_distance_field_ros/CMakeLists.txt +++ b/moveit_experimental/collision_distance_field_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.4.6) +cmake_minimum_required(VERSION 3.1.3) include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) # Set the build type. Options are: diff --git a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_distance_field_ros_helpers.h b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_distance_field_ros_helpers.h index c90f312dc6..8de0d3970d 100644 --- a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_distance_field_ros_helpers.h +++ b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_distance_field_ros_helpers.h @@ -34,8 +34,7 @@ /** \author E. Gil Jones */ -#ifndef COLLISION_DISTANCE_FIELD_HELPERS_H_ -#define COLLISION_DISTANCE_FIELD_HELPERS_H_ +#pragma once #include #include @@ -125,5 +124,4 @@ static inline bool loadLinkBodySphereDecompositions( } return true; } -} -#endif +} // namespace collision_detection diff --git a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_robot_distance_field_ros.h b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_robot_distance_field_ros.h index 8c9861b953..9167d98d62 100644 --- a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_robot_distance_field_ros.h +++ b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/collision_robot_distance_field_ros.h @@ -34,8 +34,7 @@ /** \author E. Gil Jones */ -#ifndef _COLLISION_ROBOT_DISTANCE_FIELD_ROS_H_ -#define _COLLISION_ROBOT_DISTANCE_FIELD_ROS_H_ +#pragma once #include #include @@ -62,6 +61,4 @@ class CollisionRobotDistanceFieldROS : public CollisionRobotDistanceField max_propogation_distance); } }; -} - -#endif +} // namespace collision_detection diff --git a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/hybrid_collision_robot_ros.h b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/hybrid_collision_robot_ros.h index 193d9debd2..ccedfc2027 100644 --- a/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/hybrid_collision_robot_ros.h +++ b/moveit_experimental/collision_distance_field_ros/include/collision_distance_field_ros/hybrid_collision_robot_ros.h @@ -34,8 +34,7 @@ /** \author E. Gil Jones */ -#ifndef _COLLISION_ROBOT_HYBRID_ROS_H_ -#define _COLLISION_ROBOT_HYBRID_ROS_H_ +#pragma once #include #include @@ -62,6 +61,4 @@ class CollisionRobotHybridROS : public CollisionRobotHybrid collision_tolerance, max_propogation_distance); } }; -} - -#endif +} // namespace collision_detection diff --git a/moveit_experimental/jog_arm/CMakeLists.txt b/moveit_experimental/jog_arm/CMakeLists.txt deleted file mode 100644 index 329a117dc6..0000000000 --- a/moveit_experimental/jog_arm/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -set(JOG_ARM_NAME jog_arm) - -add_executable(${JOG_ARM_NAME}_server - src/jog_arm/collision_check_thread.cpp - src/jog_arm/jog_arm_server.cpp - src/jog_arm/jog_calcs.cpp - src/jog_arm/jog_ros_interface.cpp - src/jog_arm/low_pass_filter.cpp -) -add_dependencies(${JOG_ARM_NAME}_server ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${JOG_ARM_NAME}_server ${catkin_LIBRARIES} ${Eigen_LIBRARIES}) - -add_executable(${JOG_ARM_NAME}_spacenav_to_twist - src/jog_arm/teleop_examples/spacenav_to_twist.cpp -) -add_dependencies(${JOG_ARM_NAME}_spacenav_to_twist ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${JOG_ARM_NAME}_spacenav_to_twist ${catkin_LIBRARIES}) - -install(TARGETS ${JOG_ARM_NAME}_server ${JOG_ARM_NAME}_spacenav_to_twist - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install( - DIRECTORY - include/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" -) - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - find_package(ros_pytest REQUIRED) - add_rostest(test/launch/jog_arm_integration_test.test) -endif() diff --git a/moveit_experimental/jog_arm/README.md b/moveit_experimental/jog_arm/README.md deleted file mode 100644 index 8dfa5dcd09..0000000000 --- a/moveit_experimental/jog_arm/README.md +++ /dev/null @@ -1,5 +0,0 @@ -## Jog Arm - -See the [tutorial](https://ros-planning.github.io/moveit_tutorials/doc/arm_jogging/arm_jogging_tutorial.html) - -Run tests with `catkin run_tests --no-deps --this` diff --git a/moveit_experimental/jog_arm/config/sia5_simulated_config.yaml b/moveit_experimental/jog_arm/config/sia5_simulated_config.yaml deleted file mode 100644 index 0ce6b2614e..0000000000 --- a/moveit_experimental/jog_arm/config/sia5_simulated_config.yaml +++ /dev/null @@ -1,34 +0,0 @@ -use_gazebo: true # Whether the robot is started in a Gazebo simulation environment -check_collisions: true # Check collisions? -command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s -scale: # Only used if command_in_type=="unitless" - linear: 0.375 # Max linear velocity [m/s] - rotational: 0.750 # Max angular velocity [rad/s] - joint: 1.25 # Max joint angular/linear velocity. [m/s] or [rad/s] -cartesian_command_in_topic: jog_arm_server/delta_jog_cmds # Topic for xyz commands -joint_command_in_topic: jog_arm_server/joint_delta_jog_cmds # Topic for angle commands -command_frame: base_link # If incoming cmds have no frame, this is the default -incoming_command_timeout: 5 # Stop jogging if X seconds elapse without a new cmd -joint_topic: joint_states -move_group_name: arm # Often 'manipulator' or 'arm' -lower_singularity_threshold: 15 # Start decelerating when the condition number hits this (close to singularity). Larger --> closer to singularity -hard_stop_singularity_threshold: 30 # Stop when the condition number hits this. Larger --> closer to singularity -collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] -planning_frame: base_link # The MoveIt! planning frame. Often 'base_link' -low_pass_filter_coeff: 2. # Larger-> more smoothing of jog commands, but more lag. -publish_period: 0.008 # 1/Nominal publish rate [seconds] -publish_delay: 0.005 # delay between calculation and execution start of command -collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. -num_halt_msgs_to_publish: 4 # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish -# Publish boolean warnings to this topic -warning_topic: jog_arm_server/warning -joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. -command_out_topic: sia5_controller/command # Find this topic by 'rostopic list' or 'rostopic list | grep command' or 'rqt_graph' -# What type of topic does your robot driver expect? -# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController) -# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) -command_out_type: trajectory_msgs/JointTrajectory -# Can save some bandwidth as most robots only require positions or velocities -publish_joint_positions: true -publish_joint_velocities: true -publish_joint_accelerations: false diff --git a/moveit_experimental/jog_arm/include/jog_arm/collision_check_thread.h b/moveit_experimental/jog_arm/include/jog_arm/collision_check_thread.h deleted file mode 100644 index 1049f26967..0000000000 --- a/moveit_experimental/jog_arm/include/jog_arm/collision_check_thread.h +++ /dev/null @@ -1,58 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : collision_check_thread.h -// Project : jog_arm -// Created : 1/11/2019 -// Author : Brian O'Neil, Andy Zelenak, Blake Anderson -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// -/////////////////////////////////////////////////////////////////////////////// - -#ifndef JOG_ARM_COLLISION_CHECK_THREAD_H -#define JOG_ARM_COLLISION_CHECK_THREAD_H - -#include -#include -#include -#include - -namespace jog_arm -{ -class CollisionCheckThread -{ -public: - CollisionCheckThread(const jog_arm::JogArmParameters parameters, jog_arm::JogArmShared& shared_variables, - pthread_mutex_t& mutex, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); -}; -} - -#endif // JOG_ARM_COLLISION_CHECK_THREAD_H diff --git a/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h b/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h deleted file mode 100644 index 119de4adc9..0000000000 --- a/moveit_experimental/jog_arm/include/jog_arm/jog_arm_data.h +++ /dev/null @@ -1,96 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : jog_arm_data.h -// Project : jog_arm -// Created : 1/11/2019 -// Author : Brian O'Neil, Andy Zelenak, Blake Anderson -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// -/////////////////////////////////////////////////////////////////////////////// - -#ifndef JOG_ARM_JOG_ARM_DATA_H -#define JOG_ARM_JOG_ARM_DATA_H - -#include -#include -#include -#include -#include - -namespace jog_arm -{ -static const std::string LOGNAME = "jog_arm_server"; -static const double WHILE_LOOP_WAIT = 0.001; - -// Variables to share between threads, and their mutexes -struct JogArmShared -{ - geometry_msgs::TwistStamped command_deltas; - - control_msgs::JointJog joint_command_deltas; - - sensor_msgs::JointState joints; - - double collision_velocity_scale = 1; - - // Indicates that an incoming Cartesian command is all zero velocities - bool zero_cartesian_cmd_flag = true; - - // Indicates that an incoming joint angle command is all zero velocities - bool zero_joint_cmd_flag = true; - - // Indicates that we have not received a new command in some time - bool command_is_stale = false; - - // The new command which is calculated - trajectory_msgs::JointTrajectory outgoing_command; - - // Timestamp of incoming commands - ros::Time incoming_cmd_stamp = ros::Time(0.); - - // Indicates no collision, etc, so outgoing commands can be sent - bool ok_to_publish = false; -}; - -// ROS params to be read. See the yaml file in /config for a description of each. -struct JogArmParameters -{ - std::string move_group_name, joint_topic, cartesian_command_in_topic, command_frame, command_out_topic, - planning_frame, warning_topic, joint_command_in_topic, command_in_type, command_out_type; - double linear_scale, rotational_scale, joint_scale, lower_singularity_threshold, hard_stop_singularity_threshold, - collision_proximity_threshold, low_pass_filter_coeff, publish_period, publish_delay, incoming_command_timeout, - joint_limit_margin, collision_check_rate; - int num_halt_msgs_to_publish; - bool use_gazebo, check_collisions, publish_joint_positions, publish_joint_velocities, publish_joint_accelerations; -}; -} -#endif // JOG_ARM_DATA_H diff --git a/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h b/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h deleted file mode 100644 index e610dba2c1..0000000000 --- a/moveit_experimental/jog_arm/include/jog_arm/jog_calcs.h +++ /dev/null @@ -1,141 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : jog_calcs.h -// Project : jog_arm -// Created : 1/11/2019 -// Author : Brian O'Neil, Andy Zelenak, Blake Anderson -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// -/////////////////////////////////////////////////////////////////////////////// - -#ifndef JOG_ARM_JOG_CALCS_H -#define JOG_ARM_JOG_CALCS_H - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace jog_arm -{ -class JogCalcs -{ -public: - JogCalcs(const JogArmParameters parameters, JogArmShared& shared_variables, pthread_mutex_t& mutex, - const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); - -protected: - ros::NodeHandle nh_; - - moveit::planning_interface::MoveGroupInterface move_group_; - - sensor_msgs::JointState incoming_jts_; - - bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables, pthread_mutex_t& mutex); - - bool jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables); - - // Parse the incoming joint msg for the joints of our MoveGroup - bool updateJoints(); - - Eigen::VectorXd scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const; - - Eigen::VectorXd scaleJointCommand(const control_msgs::JointJog& command) const; - - bool addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const; - - // Scale the delta theta to match joint velocity limits. Uniform scaling - void enforceJointVelocityLimits(Eigen::VectorXd& calculated_joint_velocity); - - // Reset the data stored in low-pass filters so the trajectory won't jump when jogging is resumed. - void resetVelocityFilters(); - - // Avoid a singularity or other issue. Is handled differently for position vs. velocity control. - void halt(trajectory_msgs::JointTrajectory& jt_traj); - - void publishWarning(bool active) const; - - bool checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory_>& new_jt_traj); - - // Possibly calculate a velocity scaling factor, due to proximity of - // singularity and direction of motion - double decelerateForSingularity(const Eigen::VectorXd& commanded_velocity, - const Eigen::JacobiSVD& svd); - - // Apply velocity scaling for proximity of collisions and singularities - bool applyVelocityScaling(JogArmShared& shared_variables, pthread_mutex_t& mutex, - trajectory_msgs::JointTrajectory& new_jt_traj, const Eigen::VectorXd& delta_theta, - double singularity_scale); - - trajectory_msgs::JointTrajectory composeOutgoingMessage(sensor_msgs::JointState& joint_state, - const ros::Time& stamp) const; - - void lowPassFilterVelocities(const Eigen::VectorXd& joint_vel); - - void lowPassFilterPositions(); - - void insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& trajectory, int count) const; - - const robot_state::JointModelGroup* joint_model_group_; - - robot_state::RobotStatePtr kinematic_state_; - - sensor_msgs::JointState jt_state_, original_jt_state_; - std::map jt_state_name_map_; - trajectory_msgs::JointTrajectory outgoing_command_; - - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - - std::vector velocity_filters_; - std::vector position_filters_; - - ros::Publisher warning_pub_; - - JogArmParameters parameters_; - - // For jacobian calculations - Eigen::MatrixXd jacobian_, pseudo_inverse_, matrix_s_; - Eigen::JacobiSVD svd_; - Eigen::VectorXd delta_theta_; - - const int gazebo_redundant_message_count_ = 30; - - uint num_joints_; -}; -} // namespace jog_arm - -#endif // JOG_ARM_JOG_CALCS_H diff --git a/moveit_experimental/jog_arm/include/jog_arm/jog_ros_interface.h b/moveit_experimental/jog_arm/include/jog_arm/jog_ros_interface.h deleted file mode 100644 index 636e920afe..0000000000 --- a/moveit_experimental/jog_arm/include/jog_arm/jog_ros_interface.h +++ /dev/null @@ -1,91 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : jog_ros_interface.h -// Project : jog_arm -// Created : 3/9/2017 -// Author : Brian O'Neil, Blake Anderson, Andy Zelenak -// -// BSD 3-Clause License -// -// Copyright (c) 2018, Los Alamos National Security, LLC -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// -/////////////////////////////////////////////////////////////////////////////// - -// Server node for arm jogging with MoveIt. - -#ifndef JOG_ARM_JOG_ROS_INTERFACE_H -#define JOG_ARM_JOG_ROS_INTERFACE_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace jog_arm -{ -/** - * Class JogROSInterface - Instantiated in main(). Handles ROS subs & pubs and creates the worker threads. - */ -class JogROSInterface -{ -public: - JogROSInterface(); - ~JogROSInterface(); - -private: - // ROS subscriber callbacks - void deltaCartesianCmdCB(const geometry_msgs::TwistStampedConstPtr& msg); - void deltaJointCmdCB(const control_msgs::JointJogConstPtr& msg); - void jointsCB(const sensor_msgs::JointStateConstPtr& msg); - - bool readParameters(ros::NodeHandle& n); - - // Jogging calculation thread - bool startJogCalcThread(); - - // Collision checking thread - bool startCollisionCheckThread(); - - // Share data between threads - JogArmShared shared_variables_; - pthread_mutex_t shared_variables_mutex_; - - robot_model_loader::RobotModelLoaderPtr model_loader_ptr_; - - // Store the parameters that were read from ROS server - JogArmParameters ros_parameters_; -}; -} // namespace jog_arm - -#endif // JOG_ARM_JOG_ROS_INTERFACE_H diff --git a/moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h b/moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h deleted file mode 100644 index e9c9a767c7..0000000000 --- a/moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h +++ /dev/null @@ -1,66 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : low_pass_filter.h -// Project : jog_arm -// Created : 1/11/2019 -// Author : Andy Zelenak -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// -/////////////////////////////////////////////////////////////////////////////// - -#ifndef JOG_ARM_LOW_PASS_FILTER_H -#define JOG_ARM_LOW_PASS_FILTER_H - -namespace jog_arm -{ -/** - * Class LowPassFilter - Filter a signal to soften jerks. - * This is a second-order Butterworth low-pass filter. - * See https://ccrma.stanford.edu/~jos/filters/Example_Second_Order_Butterworth_Lowpass.html - */ -class LowPassFilter -{ -public: - explicit LowPassFilter(double low_pass_filter_coeff); - double filter(double new_measurement); - void reset(double data); - -private: - double previous_measurements_[3] = { 0., 0., 0. }; - double previous_filtered_measurements_[2] = { 0., 0. }; - // Larger filter_coeff-> more smoothing of jog commands, but more lag. - // Rough plot, with cutoff frequency on the y-axis: - // https://www.wolframalpha.com/input/?i=plot+arccot(c) - double filter_coeff_ = 10.; -}; -} // namespace jog_arm -#endif // JOG_ARM_LOW_PASS_FILTER_H diff --git a/moveit_experimental/jog_arm/launch/spacenav_cpp.launch b/moveit_experimental/jog_arm/launch/spacenav_cpp.launch deleted file mode 100644 index 22a577bf85..0000000000 --- a/moveit_experimental/jog_arm/launch/spacenav_cpp.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - diff --git a/moveit_experimental/jog_arm/launch/spacenav_teleop_tools.launch b/moveit_experimental/jog_arm/launch/spacenav_teleop_tools.launch deleted file mode 100644 index e49ddbfa54..0000000000 --- a/moveit_experimental/jog_arm/launch/spacenav_teleop_tools.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_experimental/jog_arm/package.xml b/moveit_experimental/jog_arm/package.xml deleted file mode 100644 index 063cf7020e..0000000000 --- a/moveit_experimental/jog_arm/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - jog_arm - 0.0.3 - - Provides real-time manipulator Cartesian jogging. - - Blake Anderson - Andy Zelenak - - BSD 3-Clause - - http://wiki.ros.org/jog_arm - - Brian O'Neil - Andy Zelenak - Blake Anderson - Alexander RΓΆssler - diff --git a/moveit_experimental/jog_arm/src/jog_arm/collision_check_thread.cpp b/moveit_experimental/jog_arm/src/jog_arm/collision_check_thread.cpp deleted file mode 100644 index 47ecd0b10d..0000000000 --- a/moveit_experimental/jog_arm/src/jog_arm/collision_check_thread.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : collision_check_thread.cpp -// Project : jog_arm -// Created : 1/11/2019 -// Author : Brian O'Neil, Andy Zelenak, Blake Anderson -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// -/////////////////////////////////////////////////////////////////////////////// - -#include - -namespace jog_arm -{ -// Constructor for the class that handles collision checking -CollisionCheckThread::CollisionCheckThread(const jog_arm::JogArmParameters parameters, JogArmShared& shared_variables, - pthread_mutex_t& mutex, - const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) -{ - // If user specified true in yaml file - if (parameters.check_collisions) - { - // MoveIt! Setup - while (ros::ok() && !model_loader_ptr) - { - ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); - ros::Duration(WHILE_LOOP_WAIT).sleep(); - } - const robot_model::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); - planning_scene::PlanningScene planning_scene(kinematic_model); - collision_detection::CollisionRequest collision_request; - collision_request.group_name = parameters.move_group_name; - collision_request.distance = true; - collision_detection::CollisionResult collision_result; - robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst(); - - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; - planning_scene_monitor.reset(new planning_scene_monitor::PlanningSceneMonitor(model_loader_ptr)); - planning_scene_monitor->startSceneMonitor(); - planning_scene_monitor->startStateMonitor(); - - if (planning_scene_monitor->getPlanningScene()) - { - planning_scene_monitor->startSceneMonitor("/planning_scene"); - planning_scene_monitor->startWorldGeometryMonitor(); - planning_scene_monitor->startStateMonitor(); - } - else - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } - - double velocity_scale_coefficient = -log(0.001) / parameters.collision_proximity_threshold; - - // Wait for initial messages - ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg."); - ros::topic::waitForMessage(parameters.joint_topic); - ROS_INFO_NAMED(LOGNAME, "Received first joint msg."); - - ROS_INFO_NAMED(LOGNAME, "Waiting for first command msg."); - ros::topic::waitForMessage(parameters.cartesian_command_in_topic); - ROS_INFO_NAMED(LOGNAME, "Received first command msg."); - - ros::Rate collision_rate(parameters.collision_check_rate); - - ///////////////////////////////////////////////// - // Spin while checking collisions - ///////////////////////////////////////////////// - while (ros::ok()) - { - pthread_mutex_lock(&mutex); - sensor_msgs::JointState jts = shared_variables.joints; - pthread_mutex_unlock(&mutex); - - for (std::size_t i = 0; i < jts.position.size(); ++i) - current_state.setJointPositions(jts.name[i], &jts.position[i]); - - collision_result.clear(); - planning_scene_monitor->getPlanningScene()->checkCollision(collision_request, collision_result, current_state); - - // Scale robot velocity according to collision proximity and user-defined thresholds. - // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. - // Proximity decreasing --> decelerate - double velocity_scale = 1; - - // If we are far from a collision, velocity_scale should be 1. - // If we are very close to a collision, velocity_scale should be ~zero. - // When collision_proximity_threshold is breached, start decelerating exponentially. - if (collision_result.distance < parameters.collision_proximity_threshold) - { - // velocity_scale = e ^ k * (collision_distance - threshold) - // k = - ln(0.001) / collision_proximity_threshold - // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. - // velocity_scale should equal 0.001 when collision_distance is at zero. - velocity_scale = - exp(velocity_scale_coefficient * (collision_result.distance - parameters.collision_proximity_threshold)); - } - - pthread_mutex_lock(&mutex); - shared_variables.collision_velocity_scale = velocity_scale; - pthread_mutex_unlock(&mutex); - - collision_rate.sleep(); - } - } -} -} // namespace jog_arm diff --git a/moveit_experimental/jog_arm/src/jog_arm/jog_arm_server.cpp b/moveit_experimental/jog_arm/src/jog_arm/jog_arm_server.cpp deleted file mode 100644 index 2d2c545a70..0000000000 --- a/moveit_experimental/jog_arm/src/jog_arm/jog_arm_server.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : jog_arm_server.cpp -// Project : jog_arm -// Created : 12/31/2018 -// Author : Andy Zelenak -// -// BSD 3-Clause License -// -// Copyright (c) 2018, Los Alamos National Security, LLC -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// -/////////////////////////////////////////////////////////////////////////////// - -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, jog_arm::LOGNAME); - - jog_arm::JogROSInterface ros_interface; - - return 0; -} diff --git a/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp b/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp deleted file mode 100644 index 55c76fb190..0000000000 --- a/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp +++ /dev/null @@ -1,764 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : jog_calcs.h -// Project : jog_arm -// Created : 1/11/2019 -// Author : Brian O'Neil, Andy Zelenak, Blake Anderson -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// -/////////////////////////////////////////////////////////////////////////////// - -#include - -namespace jog_arm -{ -// Constructor for the class that handles jogging calculations -JogCalcs::JogCalcs(const JogArmParameters parameters, JogArmShared& shared_variables, pthread_mutex_t& mutex, - const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) - : move_group_(parameters.move_group_name), tf_listener_(tf_buffer_), parameters_(parameters) -{ - // Publish collision status - warning_pub_ = nh_.advertise(parameters_.warning_topic, 1); - - // MoveIt! Setup - while (ros::ok() && !model_loader_ptr) - { - ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); - ros::Duration(WHILE_LOOP_WAIT).sleep(); - } - const robot_model::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); - kinematic_state_ = std::make_shared(kinematic_model); - kinematic_state_->setToDefaultValues(); - - joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); - - // Wait for initial messages - ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg."); - ros::topic::waitForMessage(parameters_.joint_topic); - ROS_INFO_NAMED(LOGNAME, "Received first joint msg."); - - ROS_INFO_NAMED(LOGNAME, "Waiting for first command msg."); - ros::topic::waitForMessage(parameters_.cartesian_command_in_topic); - ROS_INFO_NAMED(LOGNAME, "Received first command msg."); - - resetVelocityFilters(); - - jt_state_.name = move_group_.getJointNames(); - num_joints_ = jt_state_.name.size(); - jt_state_.position.resize(num_joints_); - jt_state_.velocity.resize(num_joints_); - jt_state_.effort.resize(num_joints_); - // A map for the indices of incoming joint commands - for (std::size_t i = 0; i < jt_state_.name.size(); ++i) - { - jt_state_name_map_[jt_state_.name[i]] = i; - } - - // Low-pass filters for the joint positions & velocities - for (size_t i = 0; i < num_joints_; ++i) - { - velocity_filters_.emplace_back(parameters_.low_pass_filter_coeff); - position_filters_.emplace_back(parameters_.low_pass_filter_coeff); - } - - // Initialize the position filters to initial robot joints - while (!updateJoints() && ros::ok()) - { - pthread_mutex_lock(&mutex); - incoming_jts_ = shared_variables.joints; - pthread_mutex_unlock(&mutex); - ros::Duration(WHILE_LOOP_WAIT).sleep(); - } - for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(jt_state_.position[i]); - - // Wait for the first jogging cmd. - // Store it in a class member for further calcs, then free up the shared variable again. - geometry_msgs::TwistStamped cartesian_deltas; - control_msgs::JointJog joint_deltas; - while (ros::ok() && (cartesian_deltas.header.stamp == ros::Time(0.)) && (joint_deltas.header.stamp == ros::Time(0.))) - { - ros::Duration(WHILE_LOOP_WAIT).sleep(); - - pthread_mutex_lock(&mutex); - cartesian_deltas = shared_variables.command_deltas; - joint_deltas = shared_variables.joint_command_deltas; - pthread_mutex_unlock(&mutex); - } - - // Track the number of cycles during which motion has not occurred. - // Will avoid re-publishing zero velocities endlessly. - int zero_velocity_count = 0; - - ros::Rate loop_rate(1. / parameters_.publish_period); - - // Now do jogging calcs - while (ros::ok()) - { - // If user commands are all zero, reset the low-pass filters when commands resume - pthread_mutex_lock(&mutex); - bool zero_cartesian_cmd_flag = shared_variables.zero_cartesian_cmd_flag; - bool zero_joint_cmd_flag = shared_variables.zero_joint_cmd_flag; - pthread_mutex_unlock(&mutex); - - if (zero_cartesian_cmd_flag && zero_joint_cmd_flag) - // Reset low-pass filters - resetVelocityFilters(); - - // Pull data from the shared variables. - pthread_mutex_lock(&mutex); - incoming_jts_ = shared_variables.joints; - pthread_mutex_unlock(&mutex); - - // Initialize the position filters to initial robot joints - while (!updateJoints() && ros::ok()) - { - pthread_mutex_lock(&mutex); - incoming_jts_ = shared_variables.joints; - pthread_mutex_unlock(&mutex); - ros::Duration(WHILE_LOOP_WAIT).sleep(); - } - - // Assume cartesian jogging command unless the joint command has some nonzero values - if (zero_joint_cmd_flag) - { - pthread_mutex_lock(&mutex); - cartesian_deltas = shared_variables.command_deltas; - pthread_mutex_unlock(&mutex); - - if (!cartesianJogCalcs(cartesian_deltas, shared_variables, mutex)) - continue; - } - // If joint jogging command is not all zeros - else - { - pthread_mutex_lock(&mutex); - joint_deltas = shared_variables.joint_command_deltas; - pthread_mutex_unlock(&mutex); - - if (!jointJogCalcs(joint_deltas, shared_variables)) - continue; - } - - // Halt if the command is stale or inputs are all zero, or commands were zero - pthread_mutex_lock(&mutex); - bool stale_command = shared_variables.command_is_stale; - pthread_mutex_unlock(&mutex); - - if (stale_command || (zero_cartesian_cmd_flag && zero_joint_cmd_flag)) - { - halt(outgoing_command_); - zero_cartesian_cmd_flag = true; - zero_joint_cmd_flag = true; - } - - bool valid_nonzero_command = !zero_cartesian_cmd_flag && !zero_joint_cmd_flag; - - // Send the newest target joints - if (!outgoing_command_.joint_names.empty()) - { - pthread_mutex_lock(&mutex); - // If everything normal, share the new traj to be published - if (valid_nonzero_command) - { - shared_variables.outgoing_command = outgoing_command_; - shared_variables.ok_to_publish = true; - } - // Skip the jogging publication if all inputs have been zero for several cycles in a row. - // num_halt_msgs_to_publish == 0 signifies that we shoud keep republishing forever. - else if ((parameters_.num_halt_msgs_to_publish != 0) && - (zero_velocity_count > parameters_.num_halt_msgs_to_publish)) - { - shared_variables.ok_to_publish = false; - } - else - { - shared_variables.outgoing_command = outgoing_command_; - shared_variables.ok_to_publish = true; - } - pthread_mutex_unlock(&mutex); - - // Store last zero-velocity message flag to prevent superfluous warnings. - // Cartesian and joint commands must both be zero. - if (zero_cartesian_cmd_flag && zero_joint_cmd_flag) - { - // Avoid overflow - if (zero_velocity_count < INT_MAX) - ++zero_velocity_count; - } - else - zero_velocity_count = 0; - } - - loop_rate.sleep(); - } -} - -// Perform the jogging calculations -bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables, - pthread_mutex_t& mutex) -{ - // Check for nan's in the incoming command - if (std::isnan(cmd.twist.linear.x) || std::isnan(cmd.twist.linear.y) || std::isnan(cmd.twist.linear.z) || - std::isnan(cmd.twist.angular.x) || std::isnan(cmd.twist.angular.y) || std::isnan(cmd.twist.angular.z)) - { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "nan in incoming command. Skipping this datapoint."); - return false; - } - - // If incoming commands should be in the range [-1:1], check for |delta|>1 - if (parameters_.command_in_type == "unitless") - { - if ((fabs(cmd.twist.linear.x) > 1) || (fabs(cmd.twist.linear.y) > 1) || (fabs(cmd.twist.linear.z) > 1) || - (fabs(cmd.twist.angular.x) > 1) || (fabs(cmd.twist.angular.y) > 1) || (fabs(cmd.twist.angular.z) > 1)) - { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "Component of incoming command is >1. Skipping this datapoint."); - return false; - } - } - - // Transform the command to the MoveGroup planning frame. - geometry_msgs::TransformStamped command_frame_to_planning_frame; - try - { - command_frame_to_planning_frame = - tf_buffer_.lookupTransform(parameters_.planning_frame, cmd.header.frame_id, ros::Time(0), ros::Duration(1.0)); - } - catch (tf2::TransformException& ex) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, ros::this_node::getName() << ": " << ex.what()); - return false; - } - - geometry_msgs::Vector3 lin_vector = cmd.twist.linear; - try - { - tf2::doTransform(lin_vector, lin_vector, command_frame_to_planning_frame); - } - catch (const tf2::TransformException& ex) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, ros::this_node::getName() << ": " << ex.what()); - return false; - } - - geometry_msgs::Vector3 rot_vector = cmd.twist.angular; - try - { - tf2::doTransform(rot_vector, rot_vector, command_frame_to_planning_frame); - } - catch (const tf2::TransformException& ex) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, ros::this_node::getName() << ": " << ex.what()); - return false; - } - - // Put these components back into a TwistStamped - cmd.header.frame_id = parameters_.planning_frame; - cmd.twist.linear = lin_vector; - cmd.twist.angular = rot_vector; - - const Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); - - kinematic_state_->setVariableValues(jt_state_); - original_jt_state_ = jt_state_; - - // Convert from cartesian commands to joint commands - jacobian_ = kinematic_state_->getJacobian(joint_model_group_); - svd_ = Eigen::JacobiSVD(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV); - matrix_s_ = svd_.singularValues().asDiagonal(); - pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose(); - delta_theta_ = pseudo_inverse_ * delta_x; - - enforceJointVelocityLimits(delta_theta_); - - if (!addJointIncrements(jt_state_, delta_theta_)) - return false; - - // Include a velocity estimate for velocity-controlled robots - Eigen::VectorXd joint_vel(delta_theta_ / parameters_.publish_period); - - lowPassFilterVelocities(joint_vel); - lowPassFilterPositions(); - - const ros::Time next_time = ros::Time::now() + ros::Duration(parameters_.publish_period); - outgoing_command_ = composeOutgoingMessage(jt_state_, next_time); - - // If close to a collision or a singularity, decelerate - applyVelocityScaling(shared_variables, mutex, outgoing_command_, delta_theta_, - decelerateForSingularity(delta_x, svd_)); - - if (!checkIfJointsWithinURDFBounds(outgoing_command_)) - { - halt(outgoing_command_); - publishWarning(true); - } - else - publishWarning(false); - - // If using Gazebo simulator, insert redundant points - if (parameters_.use_gazebo) - { - insertRedundantPointsIntoTrajectory(outgoing_command_, gazebo_redundant_message_count_); - } - - return true; -} - -bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables) -{ - // Check for nan's or |delta|>1 in the incoming command - for (double velocity : cmd.velocities) - { - if (std::isnan(velocity) || (fabs(velocity) > 1)) - { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "nan in incoming command. Skipping this datapoint."); - return false; - } - } - - // Apply user-defined scaling - const Eigen::VectorXd delta = scaleJointCommand(cmd); - - kinematic_state_->setVariableValues(jt_state_); - original_jt_state_ = jt_state_; - - if (!addJointIncrements(jt_state_, delta)) - return false; - - // Include a velocity estimate for velocity-controlled robots - Eigen::VectorXd joint_vel(delta / parameters_.publish_period); - - lowPassFilterVelocities(joint_vel); - lowPassFilterPositions(); - - // update joint state with new values - kinematic_state_->setVariableValues(jt_state_); - - const ros::Time next_time = ros::Time::now() + ros::Duration(parameters_.publish_delay); - outgoing_command_ = composeOutgoingMessage(jt_state_, next_time); - - // check if new joint state is valid - if (!checkIfJointsWithinURDFBounds(outgoing_command_)) - { - halt(outgoing_command_); - publishWarning(true); - } - else - { - publishWarning(false); - } - - // done with calculations - if (parameters_.use_gazebo) - { - insertRedundantPointsIntoTrajectory(outgoing_command_, gazebo_redundant_message_count_); - } - - return true; -} - -// Spam several redundant points into the trajectory. The first few may be skipped if the -// time stamp is in the past when it reaches the client. Needed for gazebo simulation. -// Start from 2 because the first point's timestamp is already 1*parameters_.publish_period -void JogCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& trajectory, int count) const -{ - auto point = trajectory.points[0]; - // Start from 2 because we already have the first point. End at count+1 so (total #) == count - for (int i = 2; i < count + 1; ++i) - { - point.time_from_start = ros::Duration(i * parameters_.publish_period); - trajectory.points.push_back(point); - } -} - -void JogCalcs::lowPassFilterPositions() -{ - for (size_t i = 0; i < num_joints_; ++i) - { - jt_state_.position[i] = position_filters_[i].filter(jt_state_.position[i]); - - // Check for nan's - if (std::isnan(jt_state_.position[i])) - { - jt_state_.position[i] = original_jt_state_.position[i]; - jt_state_.velocity[i] = 0.; - } - } -} - -void JogCalcs::lowPassFilterVelocities(const Eigen::VectorXd& joint_vel) -{ - for (size_t i = 0; i < num_joints_; ++i) - { - jt_state_.velocity[i] = velocity_filters_[i].filter(joint_vel[static_cast(i)]); - - // Check for nan's - if (std::isnan(jt_state_.velocity[static_cast(i)])) - { - jt_state_.position[i] = original_jt_state_.position[i]; - jt_state_.velocity[i] = 0.; - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "nan in velocity filter"); - } - } -} - -trajectory_msgs::JointTrajectory JogCalcs::composeOutgoingMessage(sensor_msgs::JointState& joint_state, - const ros::Time& stamp) const -{ - trajectory_msgs::JointTrajectory new_jt_traj; - new_jt_traj.header.frame_id = parameters_.planning_frame; - new_jt_traj.header.stamp = stamp; - new_jt_traj.joint_names = joint_state.name; - - trajectory_msgs::JointTrajectoryPoint point; - point.time_from_start = ros::Duration(parameters_.publish_period); - if (parameters_.publish_joint_positions) - point.positions = joint_state.position; - if (parameters_.publish_joint_velocities) - point.velocities = joint_state.velocity; - if (parameters_.publish_joint_accelerations) - { - // I do not know of a robot that takes acceleration commands. - // However, some controllers check that this data is non-empty. - // Send all zeros, for now. - std::vector acceleration(num_joints_); - point.accelerations = acceleration; - } - new_jt_traj.points.push_back(point); - - return new_jt_traj; -} - -// Apply velocity scaling for proximity of collisions and singularities. -// Scale for collisions is read from a shared variable. -// Key equation: new_velocity = collision_scale*singularity_scale*previous_velocity -bool JogCalcs::applyVelocityScaling(JogArmShared& shared_variables, pthread_mutex_t& mutex, - trajectory_msgs::JointTrajectory& new_jt_traj, const Eigen::VectorXd& delta_theta, - double singularity_scale) -{ - pthread_mutex_unlock(&mutex); - double collision_scale = shared_variables.collision_velocity_scale; - pthread_mutex_unlock(&mutex); - - for (size_t i = 0; i < num_joints_; ++i) - { - if (parameters_.publish_joint_positions) - { - // If close to a singularity or joint limit, undo any change to the joint angles - new_jt_traj.points[0].positions[i] = - new_jt_traj.points[0].positions[i] - - (1. - singularity_scale * collision_scale) * delta_theta[static_cast(i)]; - } - if (parameters_.publish_joint_velocities) - new_jt_traj.points[0].velocities[i] *= singularity_scale * collision_scale; - } - - return true; -} - -// Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion -double JogCalcs::decelerateForSingularity(const Eigen::VectorXd& commanded_velocity, - const Eigen::JacobiSVD& svd) -{ - double velocity_scale = 1; - - // Find the direction away from nearest singularity. - // The last column of U from the SVD of the Jacobian points directly toward or away from the singularity. - // The sign can flip at any time, so we have to do some extra checking. - // Look ahead to see if the Jacobian's condition will decrease. - Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(5); - - double ini_condition = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); - - // This singular vector tends to flip direction unpredictably. See R. Bro, - // "Resolving the Sign Ambiguity in the Singular Value Decomposition". - // Look ahead to see if the Jacobian's condition will decrease in this - // direction. Start with a scaled version of the singular vector - Eigen::VectorXd delta_x(6); - double scale = 100; - delta_x = vector_toward_singularity / scale; - - // Calculate a small change in joints - Eigen::VectorXd new_theta; - kinematic_state_->copyJointGroupPositions(joint_model_group_, new_theta); - new_theta += pseudo_inverse_ * delta_x; - kinematic_state_->setJointGroupPositions(joint_model_group_, new_theta); - - jacobian_ = kinematic_state_->getJacobian(joint_model_group_); - Eigen::JacobiSVD new_svd = Eigen::JacobiSVD(jacobian_); - double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1); - // If new_condition < ini_condition, the singular vector does point towards a - // singularity. Otherwise, flip its direction. - if (ini_condition >= new_condition) - { - vector_toward_singularity[0] *= -1; - vector_toward_singularity[1] *= -1; - vector_toward_singularity[2] *= -1; - vector_toward_singularity[3] *= -1; - vector_toward_singularity[4] *= -1; - vector_toward_singularity[5] *= -1; - } - - // If this dot product is positive, we're moving toward singularity ==> decelerate - double dot = vector_toward_singularity.dot(commanded_velocity); - if (dot > 0) - { - // Ramp velocity down linearly when the Jacobian condition is between lower_singularity_threshold and - // hard_stop_singularity_threshold, and we're moving towards the singularity - if ((ini_condition > parameters_.lower_singularity_threshold) && - (ini_condition < parameters_.hard_stop_singularity_threshold)) - { - velocity_scale = 1. - - (ini_condition - parameters_.lower_singularity_threshold) / - (parameters_.hard_stop_singularity_threshold - parameters_.lower_singularity_threshold); - } - - // Very close to singularity, so halt. - else if (ini_condition > parameters_.hard_stop_singularity_threshold) - { - velocity_scale = 0; - ROS_WARN_THROTTLE_NAMED(2, LOGNAME, "Close to a singularity. Halting."); - } - } - - return velocity_scale; -} - -void JogCalcs::enforceJointVelocityLimits(Eigen::VectorXd& calculated_joint_change) -{ - double maximum_joint_change = calculated_joint_change.cwiseAbs().maxCoeff(); - if (maximum_joint_change > parameters_.joint_scale * parameters_.publish_period) - { - // Scale the entire joint velocity vector so that each joint velocity is below min, and the output movement is - // scaled uniformly to match expected motion - calculated_joint_change = - calculated_joint_change * parameters_.joint_scale * parameters_.publish_period / maximum_joint_change; - } -} - -bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& new_jt_traj) -{ - bool halting = false; - for (auto joint : joint_model_group_->getJointModels()) - { - if (!kinematic_state_->satisfiesVelocityBounds(joint)) - { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, ros::this_node::getName() << " " << joint->getName() << " " - << " close to a " - " velocity limit. Enforcing limit."); - kinematic_state_->enforceVelocityBounds(joint); - for (std::size_t c = 0; c < num_joints_; ++c) - { - if (new_jt_traj.joint_names[c] == joint->getName()) - { - new_jt_traj.points[0].velocities[c] = kinematic_state_->getJointVelocities(joint)[0]; - break; - } - } - } - - // Halt if we're past a joint margin and joint velocity is moving even farther past - double joint_angle = 0; - for (std::size_t c = 0; c < num_joints_; ++c) - { - if (original_jt_state_.name[c] == joint->getName()) - { - joint_angle = original_jt_state_.position.at(c); - break; - } - } - - if (!kinematic_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin)) - { - const std::vector limits = joint->getVariableBoundsMsg(); - - // Joint limits are not defined for some joints. Skip them. - if (!limits.empty()) - { - if ((kinematic_state_->getJointVelocities(joint)[0] < 0 && - (joint_angle < (limits[0].min_position + parameters_.joint_limit_margin))) || - (kinematic_state_->getJointVelocities(joint)[0] > 0 && - (joint_angle > (limits[0].max_position - parameters_.joint_limit_margin)))) - { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, ros::this_node::getName() << " " << joint->getName() - << " close to a " - " position limit. Halting."); - halting = true; - } - } - } - } - - return !halting; -} - -void JogCalcs::publishWarning(bool active) const -{ - std_msgs::Bool status; - status.data = static_cast(active); - warning_pub_.publish(status); -} - -// Avoid a singularity or other issue. Needs to be handled differently for position vs. velocity control -void JogCalcs::halt(trajectory_msgs::JointTrajectory& jt_traj) -{ - for (std::size_t i = 0; i < num_joints_; ++i) - { - // For position-controlled robots, can reset the joints to a known, good state - if (parameters_.publish_joint_positions) - jt_traj.points[0].positions[i] = original_jt_state_.position[i]; - - // For velocity-controlled robots, stop - if (parameters_.publish_joint_velocities) - jt_traj.points[0].velocities[i] = 0; - } -} - -// Reset the data stored in filters so the trajectory won't jump when jogging is resumed. -void JogCalcs::resetVelocityFilters() -{ - for (std::size_t i = 0; i < num_joints_; ++i) - velocity_filters_[i].reset(0); // Zero velocity -} - -// Parse the incoming joint msg for the joints of our MoveGroup -bool JogCalcs::updateJoints() -{ - // Check that the msg contains enough joints - if (incoming_jts_.name.size() < num_joints_) - return false; - - // Store joints in a member variable - for (std::size_t m = 0; m < incoming_jts_.name.size(); ++m) - { - std::size_t c; - try - { - c = jt_state_name_map_.at(incoming_jts_.name[m]); - } - catch (const std::out_of_range& e) - { - ROS_ERROR_STREAM_THROTTLE_NAMED(5, LOGNAME, "Command joint name unknown."); - continue; - } - - jt_state_.position[c] = incoming_jts_.position[m]; - } - - return true; -} - -// Scale the incoming jog command -Eigen::VectorXd JogCalcs::scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const -{ - Eigen::VectorXd result(6); - - // Apply user-defined scaling if inputs are unitless [-1:1] - if (parameters_.command_in_type == "unitless") - { - result[0] = parameters_.linear_scale * parameters_.publish_period * command.twist.linear.x; - result[1] = parameters_.linear_scale * parameters_.publish_period * command.twist.linear.y; - result[2] = parameters_.linear_scale * parameters_.publish_period * command.twist.linear.z; - result[3] = parameters_.rotational_scale * parameters_.publish_period * command.twist.angular.x; - result[4] = parameters_.rotational_scale * parameters_.publish_period * command.twist.angular.y; - result[5] = parameters_.rotational_scale * parameters_.publish_period * command.twist.angular.z; - } - // Otherwise, commands are in m/s and rad/s - else if (parameters_.command_in_type == "speed_units") - { - result[0] = command.twist.linear.x * parameters_.publish_period; - result[1] = command.twist.linear.y * parameters_.publish_period; - result[2] = command.twist.linear.z * parameters_.publish_period; - result[3] = command.twist.angular.x * parameters_.publish_period; - result[4] = command.twist.angular.y * parameters_.publish_period; - result[5] = command.twist.angular.z * parameters_.publish_period; - } - else - ROS_ERROR_STREAM_NAMED(LOGNAME, "Unexpected command_in_type"); - - return result; -} - -Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& command) const -{ - Eigen::VectorXd result(num_joints_); - - for (std::size_t i = 0; i < num_joints_; ++i) - { - result[i] = 0.0; - } - - std::size_t c; - for (std::size_t m = 0; m < command.joint_names.size(); ++m) - { - try - { - c = jt_state_name_map_.at(command.joint_names[m]); - } - catch (const std::out_of_range& e) - { - ROS_ERROR_STREAM_THROTTLE_NAMED(5, LOGNAME, "Command joint name unknown."); - continue; - } - // Apply user-defined scaling if inputs are unitless [-1:1] - if (parameters_.command_in_type == "unitless") - result[c] = command.velocities[m] * parameters_.joint_scale * parameters_.publish_period; - // Otherwise, commands are in m/s and rad/s - else if (parameters_.command_in_type == "speed_units") - result[c] = command.velocities[m] * parameters_.publish_period; - else - ROS_ERROR_STREAM_NAMED(LOGNAME, "Unexpected command_in_type, check yaml file."); - } - - return result; -} - -// Add the deltas to each joint -bool JogCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const -{ - for (std::size_t i = 0, size = static_cast(increments.size()); i < size; ++i) - { - try - { - output.position[i] += increments[static_cast(i)]; - } - catch (const std::out_of_range& e) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, ros::this_node::getName() << " Lengths of output and " - "increments do not match."); - return false; - } - } - - return true; -} -} // namespace jog_arm diff --git a/moveit_experimental/jog_arm/src/jog_arm/jog_ros_interface.cpp b/moveit_experimental/jog_arm/src/jog_arm/jog_ros_interface.cpp deleted file mode 100644 index 0486a530ef..0000000000 --- a/moveit_experimental/jog_arm/src/jog_arm/jog_ros_interface.cpp +++ /dev/null @@ -1,367 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : jog_ros_interface.cpp -// Project : jog_arm -// Created : 3/9/2017 -// Author : Brian O'Neil, Andy Zelenak, Blake Anderson -// -// BSD 3-Clause License -// -// Copyright (c) 2018, Los Alamos National Security, LLC -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// -/////////////////////////////////////////////////////////////////////////////// - -// Server node for arm jogging with MoveIt. - -#include - -namespace jog_arm -{ -///////////////////////////////////////////////////////////////////////////////// -// JogROSInterface handles ROS subscriptions and instantiates the worker threads. -// One worker thread does the jogging calculations. -// Another worker thread does collision checking. -///////////////////////////////////////////////////////////////////////////////// - -// Constructor for the main ROS interface node -JogROSInterface::JogROSInterface() -{ - ros::NodeHandle nh; - - pthread_mutex_init(&shared_variables_mutex_, nullptr); - - // Read ROS parameters, typically from YAML file - if (!readParameters(nh)) - exit(EXIT_FAILURE); - - // Load the robot model. This is used by the worker threads. - model_loader_ptr_ = std::shared_ptr(new robot_model_loader::RobotModelLoader); - - // Crunch the numbers in this thread - std::thread jogging_thread(&JogROSInterface::startJogCalcThread, this); - - // Check collisions in this thread - std::thread collision_thread(&JogROSInterface::startCollisionCheckThread, this); - - // ROS subscriptions. Share the data with the worker threads - ros::Subscriber cmd_sub = - nh.subscribe(ros_parameters_.cartesian_command_in_topic, 1, &JogROSInterface::deltaCartesianCmdCB, this); - ros::Subscriber joints_sub = nh.subscribe(ros_parameters_.joint_topic, 1, &JogROSInterface::jointsCB, this); - ros::Subscriber joint_jog_cmd_sub = - nh.subscribe(ros_parameters_.joint_command_in_topic, 1, &JogROSInterface::deltaJointCmdCB, this); - - // Publish freshly-calculated joints to the robot. - // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - ros::Publisher outgoing_cmd_pub; - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - outgoing_cmd_pub = nh.advertise(ros_parameters_.command_out_topic, 1); - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") - outgoing_cmd_pub = nh.advertise(ros_parameters_.command_out_topic, 1); - - // Wait for incoming topics to appear - ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); - ros::topic::waitForMessage(ros_parameters_.joint_topic); - ROS_DEBUG_NAMED(LOGNAME, "Waiting for Cartesian command topic"); - ros::topic::waitForMessage(ros_parameters_.cartesian_command_in_topic); - - // Wait for low pass filters to stabilize - ROS_INFO_STREAM_NAMED(LOGNAME, "Waiting for low-pass filters to stabilize."); - ros::Duration(10 * ros_parameters_.publish_period).sleep(); - - ros::Rate main_rate(1. / ros_parameters_.publish_period); - - while (ros::ok()) - { - ros::spinOnce(); - - pthread_mutex_lock(&shared_variables_mutex_); - trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; - - // Check for stale cmds - if ((ros::Time::now() - shared_variables_.incoming_cmd_stamp) < - ros::Duration(ros_parameters_.incoming_command_timeout)) - { - // Mark that incoming commands are not stale - shared_variables_.command_is_stale = false; - } - else - { - shared_variables_.command_is_stale = true; - } - - // Publish the most recent trajectory, unless the jogging calculation thread tells not to - if (shared_variables_.ok_to_publish) - { - // Put the outgoing msg in the right format - // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - { - outgoing_command.header.stamp = ros::Time::now(); - outgoing_cmd_pub.publish(outgoing_command); - } - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") - { - std_msgs::Float64MultiArray joints; - if (ros_parameters_.publish_joint_positions) - joints.data = outgoing_command.points[0].positions; - else if (ros_parameters_.publish_joint_velocities) - joints.data = outgoing_command.points[0].velocities; - outgoing_cmd_pub.publish(joints); - } - } - else if (shared_variables_.command_is_stale) - { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "Stale command. " - "Try a larger 'incoming_command_timeout' parameter?"); - } - else - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(2, LOGNAME, "All-zero command. Doing nothing."); - } - - pthread_mutex_unlock(&shared_variables_mutex_); - - main_rate.sleep(); - } - - jogging_thread.join(); - collision_thread.join(); -} - -JogROSInterface::~JogROSInterface() -{ - pthread_mutex_destroy(&shared_variables_mutex_); -} - -// A separate thread for the heavy jogging calculations. -bool JogROSInterface::startJogCalcThread() -{ - JogCalcs ja(ros_parameters_, shared_variables_, shared_variables_mutex_, model_loader_ptr_); - return true; -} - -// A separate thread for collision checking. -bool JogROSInterface::startCollisionCheckThread() -{ - CollisionCheckThread cc(ros_parameters_, shared_variables_, shared_variables_mutex_, model_loader_ptr_); - return true; -} - -// Listen to cartesian delta commands. Store them in a shared variable. -void JogROSInterface::deltaCartesianCmdCB(const geometry_msgs::TwistStampedConstPtr& msg) -{ - pthread_mutex_lock(&shared_variables_mutex_); - - // Copy everything but the frame name. The frame name is set by yaml file at startup. - // (so it isn't copied over and over) - shared_variables_.command_deltas.twist = msg->twist; - shared_variables_.command_deltas.header = msg->header; - - // Input frame determined by YAML file if not passed with message - if (shared_variables_.command_deltas.header.frame_id.empty()) - { - shared_variables_.command_deltas.header.frame_id = ros_parameters_.command_frame; - } - - // Check if input is all zeros. Flag it if so to skip calculations/publication - shared_variables_.zero_cartesian_cmd_flag = shared_variables_.command_deltas.twist.linear.x == 0.0 && - shared_variables_.command_deltas.twist.linear.y == 0.0 && - shared_variables_.command_deltas.twist.linear.z == 0.0 && - shared_variables_.command_deltas.twist.angular.x == 0.0 && - shared_variables_.command_deltas.twist.angular.y == 0.0 && - shared_variables_.command_deltas.twist.angular.z == 0.0; - - shared_variables_.incoming_cmd_stamp = msg->header.stamp; - pthread_mutex_unlock(&shared_variables_mutex_); -} - -// Listen to joint delta commands. Store them in a shared variable. -void JogROSInterface::deltaJointCmdCB(const control_msgs::JointJogConstPtr& msg) -{ - pthread_mutex_lock(&shared_variables_mutex_); - shared_variables_.joint_command_deltas = *msg; - - // Check if joint inputs is all zeros. Flag it if so to skip calculations/publication - bool all_zeros = true; - for (double delta : shared_variables_.joint_command_deltas.velocities) - { - all_zeros &= (delta == 0.0); - }; - shared_variables_.zero_joint_cmd_flag = all_zeros; - - shared_variables_.incoming_cmd_stamp = msg->header.stamp; - pthread_mutex_unlock(&shared_variables_mutex_); -} - -// Listen to joint angles. Store them in a shared variable. -void JogROSInterface::jointsCB(const sensor_msgs::JointStateConstPtr& msg) -{ - pthread_mutex_lock(&shared_variables_mutex_); - shared_variables_.joints = *msg; - pthread_mutex_unlock(&shared_variables_mutex_); -} - -// Read ROS parameters, typically from YAML file -bool JogROSInterface::readParameters(ros::NodeHandle& n) -{ - std::size_t error = 0; - - // Specified in the launch file. All other parameters will be read from this namespace. - std::string parameter_ns; - ros::param::get("~parameter_ns", parameter_ns); - if (parameter_ns.empty()) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "A namespace must be specified in the launch file, like:"); - ROS_ERROR_STREAM_NAMED(LOGNAME, ""); - return false; - } - - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_period", ros_parameters_.publish_period); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_delay", ros_parameters_.publish_delay); - error += - !rosparam_shortcuts::get("", n, parameter_ns + "/collision_check_rate", ros_parameters_.collision_check_rate); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/num_halt_msgs_to_publish", - ros_parameters_.num_halt_msgs_to_publish); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/scale/linear", ros_parameters_.linear_scale); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/scale/rotational", ros_parameters_.rotational_scale); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/scale/joint", ros_parameters_.joint_scale); - error += - !rosparam_shortcuts::get("", n, parameter_ns + "/low_pass_filter_coeff", ros_parameters_.low_pass_filter_coeff); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/joint_topic", ros_parameters_.joint_topic); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_in_type", ros_parameters_.command_in_type); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/cartesian_command_in_topic", - ros_parameters_.cartesian_command_in_topic); - error += - !rosparam_shortcuts::get("", n, parameter_ns + "/joint_command_in_topic", ros_parameters_.joint_command_in_topic); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_frame", ros_parameters_.command_frame); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/incoming_command_timeout", - ros_parameters_.incoming_command_timeout); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/lower_singularity_threshold", - ros_parameters_.lower_singularity_threshold); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/hard_stop_singularity_threshold", - ros_parameters_.hard_stop_singularity_threshold); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/collision_proximity_threshold", - ros_parameters_.collision_proximity_threshold); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/move_group_name", ros_parameters_.move_group_name); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/planning_frame", ros_parameters_.planning_frame); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/use_gazebo", ros_parameters_.use_gazebo); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/check_collisions", ros_parameters_.check_collisions); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/warning_topic", ros_parameters_.warning_topic); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/joint_limit_margin", ros_parameters_.joint_limit_margin); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_out_topic", ros_parameters_.command_out_topic); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_out_type", ros_parameters_.command_out_type); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_positions", - ros_parameters_.publish_joint_positions); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_velocities", - ros_parameters_.publish_joint_velocities); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_accelerations", - ros_parameters_.publish_joint_accelerations); - - rosparam_shortcuts::shutdownIfError(parameter_ns, error); - - // Input checking - if (ros_parameters_.num_halt_msgs_to_publish < 0) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'num_halt_msgs_to_publish' should be greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.hard_stop_singularity_threshold < ros_parameters_.lower_singularity_threshold) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'hard_stop_singularity_threshold' " - "should be greater than 'lower_singularity_threshold.' " - "Check yaml file."); - return false; - } - if ((ros_parameters_.hard_stop_singularity_threshold < 0.) || (ros_parameters_.lower_singularity_threshold < 0.)) - { - ROS_WARN_NAMED(LOGNAME, "Parameters 'hard_stop_singularity_threshold' " - "and 'lower_singularity_threshold' should be " - "greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.collision_proximity_threshold < 0.) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_proximity_threshold' should be " - "greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.low_pass_filter_coeff < 0.) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'low_pass_filter_coeff' should be " - "greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.joint_limit_margin < 0.) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'joint_limit_margin' should be " - "greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.command_in_type != "unitless" && ros_parameters_.command_in_type != "speed_units") - { - ROS_WARN_NAMED(LOGNAME, "command_in_type should be 'unitless' or " - "'speed_units'. Check yaml file."); - return false; - } - if (ros_parameters_.command_out_type != "trajectory_msgs/JointTrajectory" && - ros_parameters_.command_out_type != "std_msgs/Float64MultiArray") - { - ROS_WARN_NAMED(LOGNAME, "Parameter command_out_type should be " - "'trajectory_msgs/JointTrajectory' or " - "'std_msgs/Float64MultiArray'. Check yaml file."); - return false; - } - if (!ros_parameters_.publish_joint_positions && !ros_parameters_.publish_joint_velocities && - !ros_parameters_.publish_joint_accelerations) - { - ROS_WARN_NAMED(LOGNAME, "At least one of publish_joint_positions / " - "publish_joint_velocities / " - "publish_joint_accelerations must be true. Check " - "yaml file."); - return false; - } - if ((ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") && ros_parameters_.publish_joint_positions && - ros_parameters_.publish_joint_velocities) - { - ROS_WARN_NAMED(LOGNAME, "When publishing a std_msgs/Float64MultiArray, " - "you must select positions OR velocities."); - return false; - } - if (ros_parameters_.collision_check_rate < 0) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_check_rate' should be " - "greater than zero. Check yaml file."); - return false; - } - - return true; -} -} // namespace jog_arm diff --git a/moveit_experimental/jog_arm/src/jog_arm/low_pass_filter.cpp b/moveit_experimental/jog_arm/src/jog_arm/low_pass_filter.cpp deleted file mode 100644 index fcec7bdaa1..0000000000 --- a/moveit_experimental/jog_arm/src/jog_arm/low_pass_filter.cpp +++ /dev/null @@ -1,78 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// Title : low_pass_filter.cpp -// Project : jog_arm -// Created : 1/11/2019 -// Author : Andy Zelenak -// -// BSD 3-Clause License -// -// Copyright (c) 2019, Los Alamos National Security, LLC -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -// -/////////////////////////////////////////////////////////////////////////////// - -#include - -namespace jog_arm -{ -LowPassFilter::LowPassFilter(double low_pass_filter_coeff) -{ - filter_coeff_ = low_pass_filter_coeff; -} - -void LowPassFilter::reset(double data) -{ - previous_measurements_[0] = data; - previous_measurements_[1] = data; - previous_measurements_[2] = data; - - previous_filtered_measurements_[0] = data; - previous_filtered_measurements_[1] = data; -} - -double LowPassFilter::filter(double new_measurement) -{ - // Push in the new measurement - previous_measurements_[2] = previous_measurements_[1]; - previous_measurements_[1] = previous_measurements_[0]; - previous_measurements_[0] = new_measurement; - - double new_filtered_msrmt = - (1. / (1. + filter_coeff_ * filter_coeff_ + 1.414 * filter_coeff_)) * - (previous_measurements_[2] + 2. * previous_measurements_[1] + previous_measurements_[0] - - (filter_coeff_ * filter_coeff_ - 1.414 * filter_coeff_ + 1.) * previous_filtered_measurements_[1] - - (-2. * filter_coeff_ * filter_coeff_ + 2.) * previous_filtered_measurements_[0]); - - // Store the new filtered measurement - previous_filtered_measurements_[1] = previous_filtered_measurements_[0]; - previous_filtered_measurements_[0] = new_filtered_msrmt; - - return new_filtered_msrmt; -} -} // namespace jog_arm diff --git a/moveit_experimental/jog_arm/src/jog_arm/teleop_examples/spacenav_to_twist.cpp b/moveit_experimental/jog_arm/src/jog_arm/teleop_examples/spacenav_to_twist.cpp deleted file mode 100644 index 69f4bf0cf3..0000000000 --- a/moveit_experimental/jog_arm/src/jog_arm/teleop_examples/spacenav_to_twist.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#include "geometry_msgs/TwistStamped.h" -#include "control_msgs/JointJog.h" -#include "ros/ros.h" -#include "sensor_msgs/Joy.h" - -namespace jog_arm -{ -static const int NUM_SPINNERS = 1; -static const int QUEUE_LENGTH = 1; - -class SpaceNavToTwist -{ -public: - SpaceNavToTwist() : spinner_(NUM_SPINNERS) - { - joy_sub_ = n_.subscribe("spacenav/joy", QUEUE_LENGTH, &SpaceNavToTwist::joyCallback, this); - twist_pub_ = n_.advertise("jog_arm_server/delta_jog_cmds", QUEUE_LENGTH); - joint_delta_pub_ = n_.advertise("jog_arm_server/joint_delta_jog_cmds", QUEUE_LENGTH); - - spinner_.start(); - ros::waitForShutdown(); - }; - -private: - // Convert incoming joy commands to TwistStamped commands for jogging. - // The TwistStamped component goes to jogging, while buttons 0 & 1 control - // joints directly. - void joyCallback(const sensor_msgs::Joy::ConstPtr& msg) - { - // Cartesian jogging with the axes - geometry_msgs::TwistStamped twist; - twist.header.stamp = ros::Time::now(); - twist.twist.linear.x = msg->axes[0]; - twist.twist.linear.y = msg->axes[1]; - twist.twist.linear.z = msg->axes[2]; - - twist.twist.angular.x = msg->axes[3]; - twist.twist.angular.y = msg->axes[4]; - twist.twist.angular.z = msg->axes[5]; - - // Joint jogging with the buttons - control_msgs::JointJog joint_deltas; - // This example is for a Motoman SIA5. joint_s is the base joint. - joint_deltas.joint_names.push_back("joint_s"); - - // Button 0: positive on the wrist joint - // Button 1: negative on the wrist joint - joint_deltas.velocities.push_back(msg->buttons[0] - msg->buttons[1]); - joint_deltas.header.stamp = ros::Time::now(); - - twist_pub_.publish(twist); - joint_delta_pub_.publish(joint_deltas); - } - - ros::NodeHandle n_; - ros::Subscriber joy_sub_; - ros::Publisher twist_pub_, joint_delta_pub_; - ros::AsyncSpinner spinner_; -}; -} // namespace jog_arm - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "spacenav_to_twist"); - - jog_arm::SpaceNavToTwist to_twist; - - return 0; -} diff --git a/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml b/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml deleted file mode 100644 index 15668db878..0000000000 --- a/moveit_experimental/jog_arm/test/arm_setup/config/jog_settings.yaml +++ /dev/null @@ -1,34 +0,0 @@ -use_gazebo: false # Whether the robot is started in a Gazebo simulation environment -check_collisions: true # Check collisions? -command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s -scale: # Only used if command_in_type=="unitless" - linear: 0.003 # Max linear velocity. Meters per publish_period. - rotational: 0.006 # Max angular velocity. Rads per publish_period. - joint: 0.01 # Max joint angular/linear velocity. Rads or Meters per publish period. -cartesian_command_in_topic: jog_arm_server/delta_jog_cmds # Topic for xyz commands -joint_command_in_topic: jog_arm_server/joint_delta_jog_cmds # Topic for angle commands -command_frame: base_link # TF frame that incoming cmds are given in -incoming_command_timeout: 1 # Stop jogging if X seconds elapsed without a new cmd -joint_topic: joint_states -move_group_name: panda_arm # Often 'manipulator' or 'arm' -lower_singularity_threshold: 30 # Start decelerating when the condition number hits this (close to singularity). Larger --> closer to singularity -hard_stop_singularity_threshold: 45 # Stop when the condition number hits this. Larger --> closer to singularity -collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] -planning_frame: base_link # The MoveIt! planning frame. Often 'base_link' -low_pass_filter_coeff: 2. # Larger-> more smoothing to jog commands, but more lag. -publish_period: 0.01 # 1/Nominal publish rate [seconds] -publish_delay: 0.005 # delay between calculation and execution start of command -collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. -num_halt_msgs_to_publish: 1 # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish -# Publish boolean warnings to this topic -warning_topic: jog_arm_server/warning -joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. -command_out_topic: jog_arm_server/command # Find this topic by 'rostopic list' or 'rostopic list | grep command' or 'rqt_graph' -# What type of topic does your robot driver expect? -# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController) -# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) -command_out_type: trajectory_msgs/JointTrajectory -# Can save some bandwidth as most robots only require positions or velocities -publish_joint_positions: true -publish_joint_velocities: true -publish_joint_accelerations: false diff --git a/moveit_experimental/jog_arm/test/arm_setup/launch/panda_move_group.launch b/moveit_experimental/jog_arm/test/arm_setup/launch/panda_move_group.launch deleted file mode 100644 index d7bc6b9055..0000000000 --- a/moveit_experimental/jog_arm/test/arm_setup/launch/panda_move_group.launch +++ /dev/null @@ -1,74 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_experimental/jog_arm/test/integration/test_jog_arm_integration.py b/moveit_experimental/jog_arm/test/integration/test_jog_arm_integration.py deleted file mode 100755 index 6689c0224a..0000000000 --- a/moveit_experimental/jog_arm/test/integration/test_jog_arm_integration.py +++ /dev/null @@ -1,79 +0,0 @@ -#!/usr/bin/env python -import time - -import pytest -import rospy -from geometry_msgs.msg import TwistStamped - -from control_msgs.msg import JointJog -from trajectory_msgs.msg import JointTrajectory - -JOG_ARM_SETTLE_TIME_S = 10 -ROS_SETTLE_TIME_S = 10 - -JOINT_JOG_COMMAND_TOPIC = 'jog_arm_server/joint_delta_jog_cmds' -CARTESIAN_JOG_COMMAND_TOPIC = 'jog_arm_server/delta_jog_cmds' - -COMMAND_OUT_TOPIC = 'jog_arm_server/command' - - -@pytest.fixture -def node(): - return rospy.init_node('pytest', anonymous=True) - - -class JointJogCmd(object): - def __init__(self): - self._pub = rospy.Publisher(JOINT_JOG_COMMAND_TOPIC, JointJog, queue_size=1) - - def send_cmd(self, joint_pos): - jj = JointJog() - jj.header.stamp = rospy.Time.now() - jj.joint_names = ['joint_{}'.format(i) for i in range(len(joint_pos))] - jj.deltas = list(map(float, joint_pos)) - self._pub.publish(jj) - - -class CartesianJogCmd(object): - def __init__(self): - self._pub = rospy.Publisher( - CARTESIAN_JOG_COMMAND_TOPIC, TwistStamped, queue_size=1 - ) - - def send_cmd(self, linear, angular): - ts = TwistStamped() - ts.header.stamp = rospy.Time.now() - ts.twist.linear.x, ts.twist.linear.y, ts.twist.linear.z = linear - ts.twist.angular.x, ts.twist.angular.y, ts.twist.angular.z = angular - self._pub.publish(ts) - -def test_jog_arm_generates_joint_trajectory_when_joint_jog_command_is_received(node): - sub = rospy.Subscriber( - COMMAND_OUT_TOPIC, JointTrajectory, lambda x: received.append(x) - ) - joint_cmd = JointJogCmd() - cartesian_cmd = CartesianJogCmd() - time.sleep(ROS_SETTLE_TIME_S) # wait for pub/subs to settle - time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for jog_arm server to init - - # Repeated zero-commands should produce no output, other than a few halt messages - # A subscriber in a different thread fills 'received' - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) - received = [] - rospy.sleep(1) - assert len(received) <= 2 # 2 is 'num_halt_msgs_to_publish' in the config file - - # This nonzero command should produce jogging output - # A subscriber in a different thread fills `received` - test_duration = 1 - publish_period = 0.01 # 'publish_period' from config file - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) - received = [] - rospy.sleep(test_duration) - # test_duration/publish_period is the expected number of messages in this duration. - # Allow a small +/- window due to rounding/timing errors - assert len(received) >= test_duration/publish_period - 5 - assert len(received) <= test_duration/publish_period + 5 diff --git a/moveit_experimental/jog_arm/test/launch/jog_arm_integration_test.test b/moveit_experimental/jog_arm/test/launch/jog_arm_integration_test.test deleted file mode 100644 index f9c48abc8c..0000000000 --- a/moveit_experimental/jog_arm/test/launch/jog_arm_integration_test.test +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_experimental/kinematics_cache/CATKIN_IGNORE b/moveit_experimental/kinematics_cache/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache/CMakeLists.txt b/moveit_experimental/kinematics_cache/v1/kinematics_cache/CMakeLists.txt index a8109a42b3..6b319e9beb 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache/CMakeLists.txt +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.4.6) +cmake_minimum_required(VERSION 3.1.3) include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) # Set the build type. Options are: diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache/include/kinematics_cache/kinematics_cache.h b/moveit_experimental/kinematics_cache/v1/kinematics_cache/include/kinematics_cache/kinematics_cache.h index 1d9f7fd217..763b738ab3 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache/include/kinematics_cache/kinematics_cache.h +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache/include/kinematics_cache/kinematics_cache.h @@ -1,42 +1,41 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Sachin Chitta -*********************************************************************/ - -#ifndef KINEMATICS_CACHE_H_ -#define KINEMATICS_CACHE_H_ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Sachin Chitta + *********************************************************************/ + +#pragma once #include #include @@ -183,6 +182,4 @@ class KinematicsCache }; typedef std::shared_ptr KinematicsCachePtr; -} - -#endif +} // namespace kinematics_cache diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache/src/kinematics_cache.cpp b/moveit_experimental/kinematics_cache/v1/kinematics_cache/src/kinematics_cache.cpp index 68c443c62a..f6950a1f4e 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache/src/kinematics_cache.cpp +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache/src/kinematics_cache.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Sachin Chitta + *********************************************************************/ #include #include @@ -372,4 +372,4 @@ void KinematicsCache::updateDistances(const geometry_msgs::Pose& pose) min_squared_distance_ = std::min(distance_squared, min_squared_distance_); max_squared_distance_ = std::max(distance_squared, max_squared_distance_); } -} +} // namespace kinematics_cache diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/CMakeLists.txt b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/CMakeLists.txt index b8b6ee1e04..81aa86cb8c 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/CMakeLists.txt +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.4.6) +cmake_minimum_required(VERSION 3.1.3) include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) # Set the build type. Options are: diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/include/kinematics_cache_ros/kinematics_cache_ros.h b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/include/kinematics_cache_ros/kinematics_cache_ros.h index ceaa9f86e6..bf8e35b420 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/include/kinematics_cache_ros/kinematics_cache_ros.h +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/include/kinematics_cache_ros/kinematics_cache_ros.h @@ -1,42 +1,41 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Sachin Chitta + *********************************************************************/ -#ifndef KINEMATICS_CACHE_ROS_H_ -#define KINEMATICS_CACHE_ROS_H_ +#pragma once #include #include @@ -69,6 +68,4 @@ class KinematicsCacheROS : public kinematics_cache::KinematicsCache planning_models::RobotModelPtr kinematic_model_; /** A kinematics model */ }; -} - -#endif +} // namespace kinematics_cache_ros diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/example.cpp b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/example.cpp index 98d4573c27..089ad3fd0f 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/example.cpp +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/example.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Sachin Chitta + *********************************************************************/ #include #include diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/kinematics_cache_ros.cpp b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/kinematics_cache_ros.cpp index 40342b1d31..7179b6c4cf 100644 --- a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/kinematics_cache_ros.cpp +++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/src/kinematics_cache_ros.cpp @@ -1,46 +1,46 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Sachin Chitta + *********************************************************************/ #include // ROS #include -// MoveIt! +// MoveIt #include #include #include @@ -85,4 +85,4 @@ bool KinematicsCacheROS::init(const kinematics_cache::KinematicsCache::Options& return true; } -} +} // namespace kinematics_cache_ros diff --git a/moveit_experimental/kinematics_cache/v2/kinematics_cache/CMakeLists.txt b/moveit_experimental/kinematics_cache/v2/kinematics_cache/CMakeLists.txt index d22d017782..6dcd3680b0 100644 --- a/moveit_experimental/kinematics_cache/v2/kinematics_cache/CMakeLists.txt +++ b/moveit_experimental/kinematics_cache/v2/kinematics_cache/CMakeLists.txt @@ -9,6 +9,9 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h b/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h index 0a7c793614..c6b4b6116e 100644 --- a/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h +++ b/moveit_experimental/kinematics_cache/v2/kinematics_cache/include/moveit/kinematics_cache/kinematics_cache.h @@ -1,42 +1,41 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Sachin Chitta -*********************************************************************/ - -#ifndef KINEMATICS_CACHE_H_ -#define KINEMATICS_CACHE_H_ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Sachin Chitta + *********************************************************************/ + +#pragma once #include #include @@ -103,7 +102,7 @@ class KinematicsCache * @param opt Parameters needed for defining the cache workspace * @return False if any error occured during initialization */ - bool initialize(kinematics::KinematicsBaseConstPtr& solver, const robot_model::RobotModelConstPtr& kinematic_model, + bool initialize(kinematics::KinematicsBaseConstPtr& solver, const moveit::core::RobotModelConstPtr& kinematic_model, const KinematicsCache::Options& opt); /** @brief Return the instance of the kinematics solver */ @@ -113,7 +112,7 @@ class KinematicsCache } /** @brief Return the instance of the kinematics model */ - const robot_model::RobotModelConstPtr& getModelInstance() const + const moveit::core::RobotModelConstPtr& getModelInstance() const { return kinematic_model_; } @@ -169,11 +168,11 @@ class KinematicsCache kinematics::KinematicsBaseConstPtr kinematics_solver_; /** An instance of the kinematics solver */ - robot_model::RobotModelConstPtr kinematic_model_; /** An instance of the kinematic model */ - robot_state::RobotStatePtr kinematic_state_; /** An instance of the kinematic state */ + moveit::core::RobotModelConstPtr kinematic_model_; /** An instance of the kinematic model */ + moveit::core::RobotStatePtr kinematic_state_; /** An instance of the kinematic state */ - const robot_model::JointModelGroup* joint_model_group_; /** Joint model group associated with this cache */ - std::shared_ptr joint_state_group_; /** Joint state corresponding to cache */ + const moveit::core::JointModelGroup* joint_model_group_; /** Joint model group associated with this cache */ + std::shared_ptr joint_state_group_; /** Joint state corresponding to cache */ // mutable std::vector solution_local_; /** Local pre-allocated storage */ @@ -181,6 +180,4 @@ class KinematicsCache }; typedef std::shared_ptr KinematicsCachePtr; -} - -#endif +} // namespace kinematics_cache diff --git a/moveit_experimental/kinematics_cache/v2/kinematics_cache/src/kinematics_cache.cpp b/moveit_experimental/kinematics_cache/v2/kinematics_cache/src/kinematics_cache.cpp index 127160ce31..7f30439919 100644 --- a/moveit_experimental/kinematics_cache/v2/kinematics_cache/src/kinematics_cache.cpp +++ b/moveit_experimental/kinematics_cache/v2/kinematics_cache/src/kinematics_cache.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Sachin Chitta + *********************************************************************/ #include #include @@ -46,15 +46,15 @@ KinematicsCache::KinematicsCache() : min_squared_distance_(1e6), max_squared_dis } bool KinematicsCache::initialize(kinematics::KinematicsBaseConstPtr& kinematics_solver, - const robot_model::RobotModelConstPtr& kinematic_model, + const moveit::core::RobotModelConstPtr& kinematic_model, const KinematicsCache::Options& opt) { options_ = opt; kinematics_solver_ = kinematics_solver; kinematic_model_ = kinematic_model; joint_model_group_ = kinematic_model_->getJointModelGroup(kinematics_solver_->getGroupName()); - kinematic_state_.reset(new robot_state::RobotState(kinematic_model)); - joint_state_group_.reset(new robot_state::JointStateGroup(kinematic_state_.get(), joint_model_group_)); + kinematic_state_.reset(new moveit::core::RobotState(kinematic_model)); + joint_state_group_.reset(new moveit::core::JointStateGroup(kinematic_state_.get(), joint_model_group_)); setup(opt); return true; @@ -380,4 +380,4 @@ void KinematicsCache::updateDistances(const geometry_msgs::Pose& pose) min_squared_distance_ = std::min(distance_squared, min_squared_distance_); max_squared_distance_ = std::max(distance_squared, max_squared_distance_); } -} +} // namespace kinematics_cache diff --git a/moveit_experimental/kinematics_constraint_aware/CATKIN_IGNORE b/moveit_experimental/kinematics_constraint_aware/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_experimental/kinematics_constraint_aware/CMakeLists.txt b/moveit_experimental/kinematics_constraint_aware/CMakeLists.txt index 8aaa5b86dc..12d18cecef 100644 --- a/moveit_experimental/kinematics_constraint_aware/CMakeLists.txt +++ b/moveit_experimental/kinematics_constraint_aware/CMakeLists.txt @@ -7,5 +7,8 @@ add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h index 4e8ab4348d..d64788df59 100644 --- a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h +++ b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_constraint_aware.h @@ -1,42 +1,41 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Sachin Chitta -*********************************************************************/ - -#ifndef MOVEIT_KINEMATICS_CONSTRAINT_AWARE_ -#define MOVEIT_KINEMATICS_CONSTRAINT_AWARE_ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Sachin Chitta + *********************************************************************/ + +#pragma once // System #include @@ -51,7 +50,7 @@ // Plugin #include -// MoveIt! +// MoveIt #include #include #include @@ -75,7 +74,7 @@ class KinematicsConstraintAware * @param group_name The name of the group to configure this solver for * @return False if any error occurs */ - KinematicsConstraintAware(const robot_model::RobotModelConstPtr& kinematic_model, const std::string& group_name); + KinematicsConstraintAware(const moveit::core::RobotModelConstPtr& kinematic_model, const std::string& group_name); /** @brief Solve the planning problem * @param planning_scene A const reference to the planning scene @@ -102,14 +101,14 @@ class KinematicsConstraintAware return group_name_; } - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return kinematic_model_; } private: EigenSTL::vector_Isometry3d transformPoses(const planning_scene::PlanningSceneConstPtr& planning_scene, - const robot_state::RobotState& kinematic_state, + const moveit::core::RobotState& kinematic_state, const std::vector& poses, const std::string& target_frame) const; @@ -119,20 +118,20 @@ class KinematicsConstraintAware kinematics_constraint_aware::KinematicsResponse& kinematics_response) const; geometry_msgs::Pose getTipFramePose(const planning_scene::PlanningSceneConstPtr& planning_scene, - const robot_state::RobotState& kinematic_state, const geometry_msgs::Pose& pose, + const moveit::core::RobotState& kinematic_state, const geometry_msgs::Pose& pose, const std::string& link_name, unsigned int sub_group_index) const; bool validityCallbackFn(const planning_scene::PlanningSceneConstPtr& planning_scene, const kinematics_constraint_aware::KinematicsRequest& request, kinematics_constraint_aware::KinematicsResponse& response, - robot_state::JointStateGroup* joint_state_group, + moveit::core::JointStateGroup* joint_state_group, const std::vector& joint_group_variable_values) const; std::vector sub_groups_names_; - robot_model::RobotModelConstPtr kinematic_model_; + moveit::core::RobotModelConstPtr kinematic_model_; - const robot_model::JointModelGroup* joint_model_group_; + const moveit::core::JointModelGroup* joint_model_group_; std::string group_name_; @@ -140,6 +139,4 @@ class KinematicsConstraintAware unsigned int ik_attempts_; }; -} - -#endif +} // namespace kinematics_constraint_aware diff --git a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h index b2f62e462b..3ca0c75c7e 100644 --- a/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h +++ b/moveit_experimental/kinematics_constraint_aware/include/moveit/kinematics_constraint_aware/kinematics_request_response.h @@ -1,42 +1,41 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Sachin Chitta -*********************************************************************/ - -#ifndef MOVEIT_KINEMATICS_CONSTRAINT_AWARE_KINEMATICS_REQUEST_RESPONSE_ -#define MOVEIT_KINEMATICS_CONSTRAINT_AWARE_KINEMATICS_REQUEST_RESPONSE_ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Sachin Chitta + *********************************************************************/ + +#pragma once // System #include @@ -44,7 +43,7 @@ // ROS msgs #include -// MoveIt! +// MoveIt #include #include #include @@ -69,7 +68,7 @@ class KinematicsRequest std::vector ik_link_names_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotStatePtr robot_state_; kinematic_constraints::KinematicConstraintSetPtr constraints_; @@ -79,7 +78,7 @@ class KinematicsRequest bool check_for_collisions_; - robot_state::StateValidityCallbackFn constraint_callback_; + moveit::core::StateValidityCallbackFn constraint_callback_; }; /** @@ -94,7 +93,7 @@ class KinematicsResponse virtual ~KinematicsResponse(){}; - robot_state::RobotStatePtr solution_; + moveit::core::RobotStatePtr solution_; std::vector constraint_eval_results_; @@ -104,6 +103,4 @@ class KinematicsResponse bool result_; }; -} - -#endif +} // namespace kinematics_constraint_aware diff --git a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp index a1e45998b4..141df0748a 100644 --- a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp +++ b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Sachin Chitta -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Sachin Chitta + *********************************************************************/ // // ROS msgs #include @@ -45,7 +45,7 @@ namespace kinematics_constraint_aware { -KinematicsConstraintAware::KinematicsConstraintAware(const robot_model::RobotModelConstPtr& kinematic_model, +KinematicsConstraintAware::KinematicsConstraintAware(const moveit::core::RobotModelConstPtr& kinematic_model, const std::string& group_name) { if (!kinematic_model->hasJointModelGroup(group_name)) @@ -118,7 +118,7 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt if (!response.solution_) { - response.solution_.reset(new robot_state::RobotState(planning_scene->getCurrentState())); + response.solution_.reset(new moveit::core::RobotState(planning_scene->getCurrentState())); } ros::WallTime start_time = ros::WallTime::now(); @@ -129,7 +129,7 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt } // Setup the seed and the values for all other joints in the robot - robot_state::RobotState kinematic_state = *request.robot_state_; + moveit::core::RobotState kinematic_state = *request.robot_state_; std::vector ik_link_names = request.ik_link_names_; // Transform request to tip frame if necessary @@ -147,8 +147,7 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt ik_link_names[i] = kinematic_model_->getJointModelGroup(sub_groups_names_[i])->getSolverInstance()->getTipFrame(); } - else if (!kinematic_model_->getJointModelGroup(sub_groups_names_[i]) - ->canSetStateFromIK(request.ik_link_names_[i])) + else if (!kinematic_model_->getJointModelGroup(sub_groups_names_[i])->canSetStateFromIK(request.ik_link_names_[i])) { ROS_ERROR_NAMED("kinematics_constraint_aware", "Could not find IK solver for link %s for group %s", request.ik_link_names_[i].c_str(), sub_groups_names_[i].c_str()); @@ -161,7 +160,7 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt EigenSTL::vector_Isometry3d goals = transformPoses(planning_scene, kinematic_state, request.pose_stamped_vector_, kinematic_model_->getModelFrame()); - robot_state::StateValidityCallbackFn constraint_callback_fn = + moveit::core::StateValidityCallbackFn constraint_callback_fn = boost::bind(&KinematicsConstraintAware::validityCallbackFn, this, planning_scene, request, response, _1, _2); bool result = false; @@ -198,7 +197,7 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt bool KinematicsConstraintAware::validityCallbackFn(const planning_scene::PlanningSceneConstPtr& planning_scene, const kinematics_constraint_aware::KinematicsRequest& request, kinematics_constraint_aware::KinematicsResponse& response, - robot_state::JointStateGroup* joint_state_group, + moveit::core::JointStateGroup* joint_state_group, const std::vector& joint_group_variable_values) const { joint_state_group->setVariableValues(joint_group_variable_values); @@ -321,7 +320,7 @@ bool KinematicsConstraintAware::convertServiceRequest( else kinematics_request.pose_stamped_vector_ = request.ik_request.pose_stamped_vector; - kinematics_request.robot_state_.reset(new robot_state::RobotState(planning_scene->getCurrentState())); + kinematics_request.robot_state_.reset(new moveit::core::RobotState(planning_scene->getCurrentState())); kinematics_request.robot_state_->setStateValues(request.ik_request.robot_state.joint_state); kinematics_request.constraints_.reset( new kinematic_constraints::KinematicConstraintSet(kinematic_model_, planning_scene->getTransforms())); @@ -330,13 +329,13 @@ bool KinematicsConstraintAware::convertServiceRequest( kinematics_request.group_name_ = request.ik_request.group_name; kinematics_request.check_for_collisions_ = true; - kinematics_response.solution_.reset(new robot_state::RobotState(planning_scene->getCurrentState())); + kinematics_response.solution_.reset(new moveit::core::RobotState(planning_scene->getCurrentState())); return true; } EigenSTL::vector_Isometry3d KinematicsConstraintAware::transformPoses( - const planning_scene::PlanningSceneConstPtr& planning_scene, const robot_state::RobotState& kinematic_state, + const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit::core::RobotState& kinematic_state, const std::vector& poses, const std::string& target_frame) const { Eigen::Isometry3d eigen_pose, eigen_pose_2; @@ -358,7 +357,7 @@ EigenSTL::vector_Isometry3d KinematicsConstraintAware::transformPoses( } geometry_msgs::Pose KinematicsConstraintAware::getTipFramePose( - const planning_scene::PlanningSceneConstPtr& planning_scene, const robot_state::RobotState& kinematic_state, + const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit::core::RobotState& kinematic_state, const geometry_msgs::Pose& pose, const std::string& link_name, unsigned int sub_group_index) const { geometry_msgs::Pose result; @@ -372,4 +371,4 @@ geometry_msgs::Pose KinematicsConstraintAware::getTipFramePose( result = tf2::toMsg(eigen_pose_in, result); return result; } -} +} // namespace kinematics_constraint_aware diff --git a/moveit_experimental/package.xml b/moveit_experimental/package.xml deleted file mode 100644 index e34d9ee6dc..0000000000 --- a/moveit_experimental/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - moveit_experimental - 1.0.1 - Experimental packages for MoveIt! - MoveIt! Release Team - - BSD - http://moveit.ros.org - - catkin - - control_msgs - geometry_msgs - joy_teleop - moveit_ros_planning_interface - rosparam_shortcuts - spacenav_node - tf2_ros - - ros_pytest - rostest - - - - - diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst index b827103069..bb6a34e373 100644 --- a/moveit_kinematics/CHANGELOG.rst +++ b/moveit_kinematics/CHANGELOG.rst @@ -2,6 +2,65 @@ Changelog for package moveit_kinematics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [fix] various issues with Noetic build (`#2327 `_) +* [fix] python3 issues (`#2323 `_) +* Contributors: G.A. vd. Hoorn, Michael GΓΆrner, Robert Haschke + +1.1.0 (2020-09-04) +------------------ +* [feature] Implementation of parameter TranslationXY2D IKFast (`#1949 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Delete IKCache copy constructor (`#1750 `_) +* [maint] Move NOLINT instructions to intended positions (`#2058 `_) +* [maint] clang-tidy fixes (`#2050 `_) (`#2004 `_, `#1419 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Fix various build issues on Windows (`#1880 `_) +* [maint] Fix compiler warnings (`#1773 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* [maint] Relax dependencies of moveit_kinematics (`#1529 `_) +* Contributors: Ayush Garg, Christian Henkel, Dave Coleman, Henning Kayser, Immanuel Martini, Jonathan Binney, Markus Vieth, Martin GΓΌnther, Michael Ferguson, Michael GΓΆrner, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan, edetleon, jschleicher, v4hn + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke, Michael GΓΆrner + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ +* Fix broken IKFast generator (`#2116 `_) +* Contributors: Robert Haschke + +1.0.3 (2020-04-26) +------------------ +* [feature] KDL IK: constrain wiggled joints to limits (`#1953 `_) +* [feature] IKFast: optional prefix for link names (`#1599 `_) + If you pass a `link_prefix` parameter in your `kinematics.yaml`, this string is prepended to the base and tip links. + It allows multi-robot setups (e.g. dual-arm) and still instantiate the same solver for both manipulators. +* [feature] IKFast: increase verbosity of generated script (`#1434 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [feature] IKFast: implement `Translation*AxisAngle4D` IK type (`#1823 `_) +* [fix] Fix possible division-by-zero (`#1809 `_) +* Contributors: Christian Henkel, Martin GΓΌnther, Max Krichenbauer, Michael GΓΆrner, Robert Haschke, Sean Yen, Yu, Yan, jschleicher + +1.0.2 (2019-06-28) +------------------ +* [fix] KDL IK solver: fix handling of mimic joints (`#1490 `_) +* [fix] Fix ROS apt-key in OpenRAVE docker image (`#1503 `_) +* [fix] Fix ikfast plugin-generator script (`#1492 `_, `#1449 `_) +* Contributors: Immanuel Martini, Michael GΓΆrner, Robert Haschke + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -97,7 +156,7 @@ Changelog for package moveit_kinematics 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * Contributors: Dave Coleman 0.9.4 (2017-02-06) diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index 2f66a856fb..dfe9e7342d 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/moveit_kinematics/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_kinematics) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -30,17 +34,19 @@ catkin_package( INCLUDE_DIRS ${THIS_PACKAGE_INCLUDE_DIRS} CATKIN_DEPENDS - pluginlib moveit_core + pluginlib + roscpp DEPENDS EIGEN3 ) -include_directories(${THIS_PACKAGE_INCLUDE_DIRS} +include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) +include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) + ) add_subdirectory(kdl_kinematics_plugin) add_subdirectory(lma_kinematics_plugin) diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt index 05005fa6a5..b295d3eb50 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt @@ -9,7 +9,10 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${Boost_FILESYSTEM_LIBRARY} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) if(trac_ik_kinematics_plugin_FOUND) include_directories(${trac_ik_kinematics_plugin_INCLUDE_DIRS}) @@ -27,7 +30,10 @@ if(trac_ik_kinematics_plugin_FOUND) target_link_libraries(${MOVEIT_LIB_NAME} ${trac_ik_kinematics_plugin_LIBRARIES}) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES COMPILE_DEFINITIONS "CACHED_IK_KINEMATICS_TRAC_IK") endif() -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) # This is just for testing purposes; the arms from Universal Robots have # analytic solvers, so caching just adds extra overhead. @@ -42,7 +48,10 @@ if(ur_kinematics_FOUND) moveit_cached_ik_kinematics_base ${ur${UR}_pluginlib} ${catkin_LIBRARIES}) - install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) endforeach() endif() diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/README.md b/moveit_kinematics/cached_ik_kinematics_plugin/README.md index 571d53ab2c..77aa5697cd 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/README.md +++ b/moveit_kinematics/cached_ik_kinematics_plugin/README.md @@ -46,7 +46,7 @@ This illustrates two points: (1) if the IK solver is slow, significant improveme Below is a complete list of all arguments: -- `robot`: the name of the corresponding MoveIt! config package +- `robot`: the name of the corresponding MoveIt config package - `group`: the joint group to measure (by default performance is reported for all joint groups) - `tip`: the name of the end effector (by default the default end effectors are used) - `num`: the number of IK calls per joint group diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.h index 045b2190a0..82a88c20d9 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.h @@ -239,4 +239,4 @@ bool CachedMultiTipIKKinematicsPlugin::searchPositionIK( CachedIKKinematicsPlugin::cache_.updateCache(nearest, poses, solution); return solution_found; } -} +} // namespace cached_ik_kinematics_plugin diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h index 649d89f7df..5f09634dcd 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h @@ -34,8 +34,7 @@ /* Author: Mark Moll */ -#ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_PLUGIN_ -#define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_PLUGIN_ +#pragma once #include #include @@ -91,7 +90,7 @@ class IKCache IKCache(); ~IKCache(); - IKCache(const IKCache&) = default; + IKCache(const IKCache&) = delete; /** get the entry from the IK cache that best matches a given pose */ const IKEntry& getBestApproximateIKSolution(const Pose& pose) const; @@ -99,7 +98,7 @@ class IKCache const IKEntry& getBestApproximateIKSolution(const std::vector& poses) const; /** initialize cache, read from disk if found */ void initializeCache(const std::string& robot_id, const std::string& group_name, const std::string& cache_name, - const unsigned int num_joints, Options opts = Options()); + const unsigned int num_joints, const Options& opts = Options()); /** insert (pose,config) as an entry if it's different enough from the most similar cache entry @@ -178,30 +177,30 @@ class IKCacheMap : public std::unordered_map }; // Helper class to enable/disable initialize() methods with new/old API -// hasRobotModelAPI::value provides a true/false constexpr depending on KinematicsPlugin offers the new API +// HasRobotModelApi::value provides a true/false constexpr depending on KinematicsPlugin offers the new Api // This uses SFINAE magic: https://jguegant.github.io/blogs/tech/sfinae-introduction.html template -struct hasRobotModelAPI : std::false_type +struct HasRobotModelApi : std::false_type { }; template -struct hasRobotModelAPI().initialize( +struct HasRobotModelApi().initialize( std::declval(), std::string(), std::string(), std::vector(), 0.0))> : std::true_type { }; template -struct hasRobotDescAPI : std::false_type +struct HasRobotDescApi : std::false_type { }; template -struct hasRobotDescAPI().KinematicsPlugin::initialize( std::string(), std::string(), std::string(), std::vector(), 0.0))> - : std::true_type + : std::true_type { }; @@ -266,7 +265,7 @@ class CachedIKKinematicsPlugin : public KinematicsPlugin availability of API in wrapped KinematicsPlugin class. However, as templates and virtual functions cannot be combined, we need helpers initializeImpl(). */ template - typename std::enable_if::value, bool>::type + typename std::enable_if::value, bool>::type initializeImpl(const moveit::core::RobotModel& robot_model, const std::string& group_name, const std::string& base_frame, const std::vector& tip_frames, double search_discretization) @@ -285,15 +284,15 @@ class CachedIKKinematicsPlugin : public KinematicsPlugin } template - typename std::enable_if::value, bool>::type - initializeImpl(const moveit::core::RobotModel&, const std::string&, const std::string&, - const std::vector&, double) + typename std::enable_if::value, bool>::type + initializeImpl(const moveit::core::RobotModel& /*unused*/, const std::string& /*unused*/, + const std::string& /*unused*/, const std::vector& /*unused*/, double /*unused*/) { return false; // API not supported } template - typename std::enable_if::value, bool>::type + typename std::enable_if::value, bool>::type initializeImpl(const std::string& robot_description, const std::string& group_name, const std::string& base_frame, const std::string& tip_frame, double search_discretization) { @@ -305,8 +304,9 @@ class CachedIKKinematicsPlugin : public KinematicsPlugin } template - typename std::enable_if::value, bool>::type - initializeImpl(const std::string&, const std::string&, const std::string&, const std::string&, double) + typename std::enable_if::value, bool>::type + initializeImpl(const std::string& /*unused*/, const std::string& /*unused*/, const std::string& /*unused*/, + const std::string& /*unused*/, double /*unused*/) { return false; // API not supported } @@ -345,7 +345,6 @@ class CachedMultiTipIKKinematicsPlugin : public CachedIKKinematicsPlugin -#ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_GREEDY_K_CENTERS_ -#define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_GREEDY_K_CENTERS_ +#pragma once #include #include @@ -85,7 +84,7 @@ class GreedyKCenters { // array containing the minimum distance between each data point // and the centers computed so far - std::vector minDist(data.size(), std::numeric_limits::infinity()); + std::vector min_dist(data.size(), std::numeric_limits::infinity()); centers.clear(); centers.reserve(k); @@ -97,20 +96,20 @@ class GreedyKCenters { unsigned ind = 0; const _T& center = data[centers[i - 1]]; - double maxDist = -std::numeric_limits::infinity(); + double max_dist = -std::numeric_limits::infinity(); for (unsigned j = 0; j < data.size(); ++j) { - if ((dists(j, i - 1) = distFun_(data[j], center)) < minDist[j]) - minDist[j] = dists(j, i - 1); + if ((dists(j, i - 1) = distFun_(data[j], center)) < min_dist[j]) + min_dist[j] = dists(j, i - 1); // the j-th center is the one furthest away from center 0,..,j-1 - if (minDist[j] > maxDist) + if (min_dist[j] > max_dist) { ind = j; - maxDist = minDist[j]; + max_dist = min_dist[j]; } } // no more centers available - if (maxDist < std::numeric_limits::epsilon()) + if (max_dist < std::numeric_limits::epsilon()) break; centers.push_back(ind); } @@ -128,6 +127,4 @@ class GreedyKCenters /** Random number generator used to select first center */ std::mt19937 generator_{ std::random_device{}() }; }; -} - -#endif +} // namespace cached_ik_kinematics_plugin diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h index dc026872fe..0a30b7b0df 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h @@ -36,8 +36,7 @@ // This file is a slightly modified version of -#ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_NEAREST_NEIGHBORS_ -#define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_NEAREST_NEIGHBORS_ +#pragma once #include #include @@ -115,6 +114,4 @@ class NearestNeighbors /** \brief The used distance function */ DistanceFunction distFun_; }; -} - -#endif +} // namespace cached_ik_kinematics_plugin diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h index 6d3b72f55e..927554efad 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h @@ -36,8 +36,7 @@ // This file is a slightly modified version of -#ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_NEAREST_NEIGHBORS_GNAT_ -#define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_NEAREST_NEIGHBORS_GNAT_ +#pragma once #include "NearestNeighbors.h" #include "GreedyKCenters.h" @@ -187,17 +186,17 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> { if (size_ == 0u) return false; - NearQueue nbhQueue; + NearQueue nbh_queue; // find data in tree - bool isPivot = nearestKInternal(data, 1, nbhQueue); - const _T* d = nbhQueue.top().first; + bool is_pivot = nearestKInternal(data, 1, nbh_queue); + const _T* d = nbh_queue.top().first; if (*d != data) return false; removed_.insert(d); size_--; // if we removed a pivot or if the capacity of removed elements // has been reached, we rebuild the entire GNAT - if (isPivot || removed_.size() >= removedCacheSize_) + if (is_pivot || removed_.size() >= removedCacheSize_) rebuildDataStructure(); return true; } @@ -206,10 +205,10 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> { if (size_) { - NearQueue nbhQueue; - nearestKInternal(data, 1, nbhQueue); - if (!nbhQueue.empty()) - return *nbhQueue.top().first; + NearQueue nbh_queue; + nearestKInternal(data, 1, nbh_queue); + if (!nbh_queue.empty()) + return *nbh_queue.top().first; } throw moveit::Exception("No elements found in nearest neighbors data structure"); } @@ -222,9 +221,9 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> return; if (size_) { - NearQueue nbhQueue; - nearestKInternal(data, k, nbhQueue); - postprocessNearest(nbhQueue, nbh); + NearQueue nbh_queue; + nearestKInternal(data, k, nbh_queue); + postprocessNearest(nbh_queue, nbh); } } @@ -234,9 +233,9 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> nbh.clear(); if (size_) { - NearQueue nbhQueue; - nearestRInternal(data, radius, nbhQueue); - postprocessNearest(nbhQueue, nbh); + NearQueue nbh_queue; + nearestRInternal(data, radius, nbh_queue); + postprocessNearest(nbh_queue, nbh); } } @@ -320,42 +319,42 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> // special case). bool nearestKInternal(const _T& data, std::size_t k, NearQueue& nbhQueue) const { - bool isPivot; + bool is_pivot; double dist; - NodeDist nodeDist; - NodeQueue nodeQueue; + NodeDist node_dist; + NodeQueue node_queue; dist = NearestNeighbors<_T>::distFun_(data, tree_->pivot_); - isPivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data, dist); - tree_->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot); - while (!nodeQueue.empty()) + is_pivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data, dist); + tree_->nearestK(*this, data, k, nbhQueue, node_queue, is_pivot); + while (!node_queue.empty()) { dist = nbhQueue.top().second; // note the difference with nearestRInternal - nodeDist = nodeQueue.top(); - nodeQueue.pop(); - if (nbhQueue.size() == k && - (nodeDist.second > nodeDist.first->maxRadius_ + dist || nodeDist.second < nodeDist.first->minRadius_ - dist)) + node_dist = node_queue.top(); + node_queue.pop(); + if (nbhQueue.size() == k && (node_dist.second > node_dist.first->maxRadius_ + dist || + node_dist.second < node_dist.first->minRadius_ - dist)) continue; - nodeDist.first->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot); + node_dist.first->nearestK(*this, data, k, nbhQueue, node_queue, is_pivot); } - return isPivot; + return is_pivot; } // \brief Return in nbhQueue the elements that are within distance radius of data. void nearestRInternal(const _T& data, double radius, NearQueue& nbhQueue) const { double dist = radius; // note the difference with nearestKInternal - NodeQueue nodeQueue; - NodeDist nodeDist; + NodeQueue node_queue; + NodeDist node_dist; tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_, NearestNeighbors<_T>::distFun_(data, tree_->pivot_)); - tree_->nearestR(*this, data, radius, nbhQueue, nodeQueue); - while (!nodeQueue.empty()) + tree_->nearestR(*this, data, radius, nbhQueue, node_queue); + while (!node_queue.empty()) { - nodeDist = nodeQueue.top(); - nodeQueue.pop(); - if (nodeDist.second > nodeDist.first->maxRadius_ + dist || nodeDist.second < nodeDist.first->minRadius_ - dist) + node_dist = node_queue.top(); + node_queue.pop(); + if (node_dist.second > node_dist.first->maxRadius_ + dist || node_dist.second < node_dist.first->minRadius_ - dist) continue; - nodeDist.first->nearestR(*this, data, radius, nbhQueue, nodeQueue); + node_dist.first->nearestR(*this, data, radius, nbhQueue, node_queue); } } // \brief Convert the internal data structure used for storing neighbors @@ -444,19 +443,19 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> else { std::vector dist(children_.size()); - double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_); - int minInd = 0; + double min_dist = dist[0] = gnat.distFun_(data, children_[0]->pivot_); + int min_ind = 0; for (unsigned int i = 1; i < children_.size(); ++i) - if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist) + if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < min_dist) { - minDist = dist[i]; - minInd = i; + min_dist = dist[i]; + min_ind = i; } for (unsigned int i = 0; i < children_.size(); ++i) - children_[i]->updateRange(minInd, dist[i]); - children_[minInd]->updateRadius(minDist); - children_[minInd]->add(gnat, data); + children_[i]->updateRange(min_ind, dist[i]); + children_[min_ind]->updateRadius(min_dist); + children_[min_ind]->add(gnat, data); } } // Return true iff the node needs to be split into child nodes. @@ -548,7 +547,7 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> { double dist; Node* child; - std::vector distToPivot(children_.size()); + std::vector dist_to_pivot(children_.size()); std::vector permutation(children_.size()); for (unsigned int i = 0; i < permutation.size(); ++i) permutation[i] = i; @@ -558,16 +557,16 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> if (permutation[i] >= 0) { child = children_[permutation[i]]; - distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_); - if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]])) + dist_to_pivot[permutation[i]] = gnat.distFun_(data, child->pivot_); + if (insertNeighborK(nbh, k, child->pivot_, data, dist_to_pivot[permutation[i]])) isPivot = true; if (nbh.size() == k) { dist = nbh.top().second; // note difference with nearestR for (unsigned int j = 0; j < children_.size(); ++j) if (permutation[j] >= 0 && i != j && - (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] || - distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]])) + (dist_to_pivot[permutation[i]] - dist > child->maxRange_[permutation[j]] || + dist_to_pivot[permutation[i]] + dist < child->minRange_[permutation[j]])) permutation[j] = -1; } } @@ -577,9 +576,9 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> if (permutation[i] >= 0) { child = children_[permutation[i]]; - if (nbh.size() < k || (distToPivot[permutation[i]] - dist <= child->maxRadius_ && - distToPivot[permutation[i]] + dist >= child->minRadius_)) - nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]])); + if (nbh.size() < k || (dist_to_pivot[permutation[i]] - dist <= child->maxRadius_ && + dist_to_pivot[permutation[i]] + dist >= child->minRadius_)) + nodeQueue.push(std::make_pair(child, dist_to_pivot[permutation[i]])); } } } @@ -602,7 +601,7 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> if (!children_.empty()) { Node* child; - std::vector distToPivot(children_.size()); + std::vector dist_to_pivot(children_.size()); std::vector permutation(children_.size()); for (unsigned int i = 0; i < permutation.size(); ++i) permutation[i] = i; @@ -612,11 +611,12 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> if (permutation[i] >= 0) { child = children_[permutation[i]]; - distToPivot[i] = gnat.distFun_(data, child->pivot_); - insertNeighborR(nbh, r, child->pivot_, distToPivot[i]); + dist_to_pivot[i] = gnat.distFun_(data, child->pivot_); + insertNeighborR(nbh, r, child->pivot_, dist_to_pivot[i]); for (unsigned int j = 0; j < children_.size(); ++j) - if (permutation[j] >= 0 && i != j && (distToPivot[i] - dist > child->maxRange_[permutation[j]] || - distToPivot[i] + dist < child->minRange_[permutation[j]])) + if (permutation[j] >= 0 && i != j && + (dist_to_pivot[i] - dist > child->maxRange_[permutation[j]] || + dist_to_pivot[i] + dist < child->minRange_[permutation[j]])) permutation[j] = -1; } @@ -624,8 +624,8 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> if (permutation[i] >= 0) { child = children_[permutation[i]]; - if (distToPivot[i] - dist <= child->maxRadius_ && distToPivot[i] + dist >= child->minRadius_) - nodeQueue.push(std::make_pair(child, distToPivot[i])); + if (dist_to_pivot[i] - dist <= child->maxRadius_ && dist_to_pivot[i] + dist >= child->minRadius_) + nodeQueue.push(std::make_pair(child, dist_to_pivot[i])); } } } @@ -719,6 +719,4 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> // \brief Cache of removed elements. std::unordered_set removed_; }; -} - -#endif +} // namespace cached_ik_kinematics_plugin diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp b/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp index 1f75dc5fa3..d565018280 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp +++ b/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp @@ -42,18 +42,16 @@ #include // register CachedIKKinematicsPlugin as a KinematicsBase implementation -PLUGINLIB_EXPORT_CLASS( - cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, - kinematics::KinematicsBase); +PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, + kinematics::KinematicsBase); // register CachedIKKinematicsPlugin as a KinematicsBase implementation // PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, // kinematics::KinematicsBase); // register CachedIKKinematicsPlugin as a KinematicsBase implementation -PLUGINLIB_EXPORT_CLASS( - cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, - kinematics::KinematicsBase); +PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, + kinematics::KinematicsBase); #ifdef CACHED_IK_KINEMATICS_TRAC_IK #include diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp b/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp index 41c994bbf0..28240a060f 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp +++ b/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp @@ -35,8 +35,6 @@ /* Author: Mark Moll */ #include -#include -#include #include #include @@ -61,7 +59,7 @@ IKCache::~IKCache() } void IKCache::initializeCache(const std::string& robot_id, const std::string& group_name, const std::string& cache_name, - const unsigned int num_joints, Options opts) + const unsigned int num_joints, const Options& opts) { // read ROS parameters max_cache_size_ = opts.max_cache_size; diff --git a/moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt index 49c1a345ec..1d5f8399cd 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt @@ -7,6 +7,12 @@ set(MOVEIT_LIB_NAME moveit_ikfast_kinematics_plugin) install( PROGRAMS scripts/auto_create_ikfast_moveit_plugin.sh + DESTINATION + ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +catkin_install_python( + PROGRAMS scripts/create_ikfast_moveit_plugin.py scripts/round_collada_numbers.py DESTINATION diff --git a/moveit_kinematics/ikfast_kinematics_plugin/README.md b/moveit_kinematics/ikfast_kinematics_plugin/README.md index 2f90c02007..c29d897011 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/README.md +++ b/moveit_kinematics/ikfast_kinematics_plugin/README.md @@ -1,9 +1,9 @@ -MoveIt! IKFast +MoveIt IKFast ========== * Authors: Dave Coleman, CU Boulder; Jeremy Zoss, SwRI; David Butterworth, KAIST, Mathias LΓΌdtke, Fraunhofer IPA * Date: 5/24/2013 -Generates a IKFast kinematics plugin for MoveIt! using [OpenRave](http://openrave.org/) generated cpp files. +Generates a IKFast kinematics plugin for MoveIt using [OpenRave](http://openrave.org/) generated cpp files. Tested on ROS Hydro and greater with Catkin using OpenRave 0.8 with a 6dof and 7dof robot arm manipulator. Does not work with greater than 7dof. diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh b/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh index 8d506f1091..a04e2077c7 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh @@ -8,6 +8,8 @@ # TODO: Would be nice to integrate this functionality directly into create_ikfast_moveit_plugin.py +set -e # fail on error + function print_help { echo "$(basename $0) [--help|-h] [--quiet|-q] [--keep] [--name|-n] [--pkg|-p] [--iktype|-t] " echo " input .urdf, .dae, or .cpp input file" diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py index ab5abaaead..bfa5785947 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py @@ -1,7 +1,7 @@ #! /usr/bin/env python from __future__ import print_function ''' -IKFast Plugin Generator for MoveIt! +IKFast Plugin Generator for MoveIt Creates a kinematics plugin using the output of IKFast from OpenRAVE. This plugin and the move_group node can be used as a general @@ -61,7 +61,7 @@ class InvalidROSPkgException(Exception): pass def get_pkg_dir(pkg_name): - raise InvalidROSPkgException + raise InvalidROSPkgException('Failed to locate ROS package {}'.format(pkg_name)) # Package containing this file plugin_gen_pkg = 'moveit_kinematics' @@ -71,13 +71,13 @@ def get_pkg_dir(pkg_name): def create_parser(): parser = argparse.ArgumentParser( - description="Generate an IKFast MoveIt! kinematic plugin") + description="Generate an IKFast MoveIt kinematic plugin") parser.add_argument("robot_name", help="The name of your robot") parser.add_argument("planning_group_name", help="The name of the planning group for which your IKFast solution was generated") parser.add_argument("ikfast_plugin_pkg", - help="The name of the MoveIt! IKFast Kinematics Plugin to be created/updated") + help="The name of the MoveIt IKFast Kinematics Plugin to be created/updated") parser.add_argument("base_link_name", help="The name of the base link that was used when generating your IKFast solution") parser.add_argument("eef_link_name", @@ -182,8 +182,7 @@ def create_ikfast_package(args): user_name = getuser() root.append(xmlElement("maintainer", email="%s@todo.todo" % user_name, text=user_name)) root.append(xmlElement("buildtool_depend", text="catkin")) - with open(pkg_xml_path, 'w') as f: - etree.ElementTree(root).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8") + etree.ElementTree(root).write(pkg_xml_path, xml_declaration=True, pretty_print=True, encoding="UTF-8") print("Created package.xml at: '%s'" % pkg_xml_path) @@ -247,8 +246,7 @@ def update_ikfast_package(args): # Write plugin definition to file plugin_file_name = ik_library_name + "_description.xml" plugin_file_path = args.ikfast_plugin_pkg_path + "/" + plugin_file_name - with open(plugin_file_path, 'w') as f: - etree.ElementTree(plugin_def).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8") + etree.ElementTree(plugin_def).write(plugin_file_path, xml_declaration=True, pretty_print=True, encoding="UTF-8") print("Created plugin definition at '%s'" % plugin_file_path) # Create CMakeLists file @@ -286,8 +284,7 @@ def update_ikfast_package(args): # Always write the package xml file, even if there are no changes, to ensure # proper encodings are used in the future (UTF-8) - with open(package_file_name, "w") as f: - etree.ElementTree(package_xml).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8") + etree.ElementTree(package_xml).write(package_file_name, xml_declaration=True, pretty_print=True, encoding="UTF-8") print("Wrote package.xml at '%s'" % package_file_name) # Create a script for easily updating the plugin in the future in case the plugin needs to be updated @@ -392,7 +389,7 @@ def main(): try: update_moveit_package(args) except Exception as e: - print("Failed to update MoveIt package:\n" + e.message) + print("Failed to update MoveIt package:\n" + str(e)) if __name__ == '__main__': diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py index 2e587ae49a..d299c51e5c 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py @@ -39,6 +39,8 @@ # Author: Dave Coleman # Desc: Rounds all the numbers to places +from __future__ import print_function + from lxml import etree import shlex import sys @@ -69,12 +71,12 @@ def doRound(values,decimal_places): decimal_places = int(sys.argv[3]) assert( len(sys.argv) < 5 ) # invalid num-arguments except: - print '\nUsage: round_collada_numbers.py ' - print 'Rounds all the numbers to places\n' + print('\nUsage: round_collada_numbers.py ') + print('Rounds all the numbers to places\n') sys.exit(-1) - print '\nCollada Number Rounder' - print 'Rounding numbers to', decimal_places, ' decimal places\n' + print('\nCollada Number Rounder') + print('Rounding numbers to', decimal_places, 'decimal places\n') # Read string from file f = open(input_file,'r') diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt index 5706256fb2..9f874d8071 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(_PACKAGE_NAME_) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) find_package(catkin REQUIRED COMPONENTS moveit_core @@ -13,7 +17,8 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(LAPACK REQUIRED) -include_directories(${catkin_INCLUDE_DIRS} include) +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) catkin_package() @@ -21,11 +26,13 @@ set(IKFAST_LIBRARY_NAME _LIBRARY_NAME_) add_library(${IKFAST_LIBRARY_NAME} src/_ROBOT_NAME___GROUP_NAME__ikfast_moveit_plugin.cpp) target_link_libraries(${IKFAST_LIBRARY_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${LAPACK_LIBRARIES}) # suppress warnings about unused variables in OpenRave's solver code -target_compile_options(${IKFAST_LIBRARY_NAME} PRIVATE -Wno-unused-variable) +target_compile_options(${IKFAST_LIBRARY_NAME} PRIVATE -Wno-unused-variable -Wno-unused-parameter) install(TARGETS ${IKFAST_LIBRARY_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install( FILES diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h index e672c974af..0728329205 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast.h @@ -304,7 +304,7 @@ class IkSolutionList : public IkSolutionListBase protected: std::list > _listsolutions; }; -} +} // namespace ikfast #endif // OPENRAVE_IKFAST_HEADER diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index c76082e880..5b7370c832 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -34,11 +34,11 @@ *********************************************************************/ /* - * IKFast Kinematics Solver Plugin for MoveIt! wrapping an ikfast solver from OpenRAVE. + * IKFast Kinematics Solver Plugin for MoveIt wrapping an ikfast solver from OpenRAVE. * * AUTO-GENERATED by create_ikfast_moveit_plugin.py in moveit_kinematics package. * - * This file, including the ikfast cpp from OpenRAVE below, forms a MoveIt! kinematics plugin. + * This file, including the ikfast cpp from OpenRAVE below, forms a MoveIt kinematics plugin. */ #include @@ -166,6 +166,9 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase const std::string IKFAST_TIP_FRAME_ = "_EEF_LINK_"; const std::string IKFAST_BASE_FRAME_ = "_BASE_LINK_"; + // prefix added to tip- and baseframe to allow different namespaces or multi-robot setups + std::string link_prefix_; + // The transform tip and base bool are set to true if this solver is used with a kinematic // chain that extends beyond the ikfast tip and base frame. The solution will be valid so // long as there are no active, passive, or mimic joints between either the ikfast_tip_frame @@ -212,10 +215,10 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase */ // Returns the IK solution that is within joint limits closest to ik_seed_state - bool getPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, std::vector& solution, - moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + bool + getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; /** * @brief Given a desired pose of the end-effector, compute the set joint angles solutions that are able to reach it. @@ -351,22 +354,22 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase bool getCount(int& count, const int& max_count, const int& min_count) const; /** - * @brief samples the designated redundant joint using the chosen discretization method - * @param method An enumeration flag indicating the discretization method to be used - * @param sampled_joint_vals Sampled joint values for the redundant joint - * @return True if sampling succeeded. - */ + * @brief samples the designated redundant joint using the chosen discretization method + * @param method An enumeration flag indicating the discretization method to be used + * @param sampled_joint_vals Sampled joint values for the redundant joint + * @return True if sampling succeeded. + */ bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector& sampled_joint_vals) const; /// Validate that we can compute a fixed transform between from and to links. bool computeRelativeTransform(const std::string& from, const std::string& to, Eigen::Isometry3d& transform, bool& differs_from_identity); /** - * @brief Transforms the input pose to the correct frame for the solver. This assumes that the group includes the - * entire solver chain and that any joints outside of the solver chain within the group are are fixed. - * @param ik_pose The pose to be transformed which should be in the correct frame for the group. - * @param ik_pose_chain The ik_pose to be populated with the apropriate pose for the solver - */ + * @brief Transforms the input pose to the correct frame for the solver. This assumes that the group includes the + * entire solver chain and that any joints outside of the solver chain within the group are are fixed. + * @param ik_pose The pose to be transformed which should be in the correct frame for the group. + * @param ik_pose_chain The ik_pose to be populated with the apropriate pose for the solver + */ void transformToChainFrame(const geometry_msgs::Pose& ik_pose, KDL::Frame& ik_pose_chain) const; }; // end class @@ -377,17 +380,14 @@ bool IKFastKinematicsPlugin::computeRelativeTransform(const std::string& from, c robot_state.reset(new RobotState(robot_model_)); robot_state->setToDefaultValues(); - auto* from_link = robot_state->getLinkModel(from); - auto* to_link = robot_state->getLinkModel(to); - if (!from_link) - ROS_ERROR_STREAM_NAMED(name_, "Could not find frame " << from); - if (!to_link) - ROS_ERROR_STREAM_NAMED(name_, "Could not find frame " << to); + bool has_link; // to suppress ROS_ERRORs for non-existent frames + auto* from_link = robot_model_->getLinkModel(from, &has_link); + auto* to_link = robot_model_->getLinkModel(to, &has_link); if (!from_link || !to_link) return false; - if (robot_state->getRobotModel()->getRigidlyConnectedParentLinkModel(from_link) != - robot_state->getRobotModel()->getRigidlyConnectedParentLinkModel(to_link)) + if (robot_model_->getRigidlyConnectedParentLinkModel(from_link) != + robot_model_->getRigidlyConnectedParentLinkModel(to_link)) { ROS_ERROR_STREAM_NAMED(name_, "Link frames " << from << " and " << to << " are not rigidly connected."); return false; @@ -409,19 +409,39 @@ bool IKFastKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo } storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); + if (!lookupParam("link_prefix", link_prefix_, std::string(""))) + { + ROS_INFO_NAMED(name_, "Using empty link_prefix."); + } + else + { + ROS_INFO_STREAM_NAMED(name_, "Using link_prefix: '" << link_prefix_ << "'"); + } + // verbose error output. subsequent checks in computeRelativeTransform return false then + if (!robot_model.hasLinkModel(tip_frames_[0])) + ROS_ERROR_STREAM_NAMED(name_, "tip frame '" << tip_frames_[0] << "' does not exist."); + if (!robot_model.hasLinkModel(base_frame_)) + ROS_ERROR_STREAM_NAMED(name_, "base_frame '" << base_frame_ << "' does not exist."); + + if (!robot_model.hasLinkModel(link_prefix_ + IKFAST_TIP_FRAME_)) + ROS_ERROR_STREAM_NAMED(name_, "prefixed tip frame '" << link_prefix_ + IKFAST_TIP_FRAME_ + << "' does not exist. " + "Please check your link_prefix parameter."); + if (!robot_model.hasLinkModel(link_prefix_ + IKFAST_BASE_FRAME_)) + ROS_ERROR_STREAM_NAMED(name_, "prefixed base frame '" << link_prefix_ + IKFAST_BASE_FRAME_ + << "' does not exist. " + "Please check your link_prefix parameter."); // This IKFast solution was generated with IKFAST_TIP_FRAME_ and IKFAST_BASE_FRAME_. // It is often the case that fixed joints are added to these links to model things like // a robot mounted on a table or a robot with an end effector attached to the last link. // To support these use cases, we store the transform from the IKFAST_BASE_FRAME_ to the // base_frame_ and IKFAST_TIP_FRAME_ the tip_frame_ and transform to the input pose accordingly - if (!computeRelativeTransform(tip_frames_[0], IKFAST_TIP_FRAME_, group_tip_to_chain_tip_, tip_transform_required_) || - !computeRelativeTransform(IKFAST_BASE_FRAME_, base_frame_, chain_base_to_group_base_, base_transform_required_)) + if (!computeRelativeTransform(tip_frames_[0], link_prefix_ + IKFAST_TIP_FRAME_, group_tip_to_chain_tip_, + tip_transform_required_) || + !computeRelativeTransform(link_prefix_ + IKFAST_BASE_FRAME_, base_frame_, chain_base_to_group_base_, + base_transform_required_)) { - if (!computeRelativeTransform(tip_frames_[0], IKFAST_TIP_FRAME_, group_tip_to_chain_tip_, tip_transform_required_)) - ROS_ERROR_NAMED(name_, "Failed to compute transform between IKFAST_TIP_FRAME_ and tip_frames_[0]"); - if (!computeRelativeTransform(IKFAST_BASE_FRAME_, base_frame_, chain_base_to_group_base_, base_transform_required_)) - ROS_ERROR_NAMED(name_, "Failed to compute transform between base_frame_ and IKFAST_BASE_FRAME_"); return false; } @@ -576,28 +596,26 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector 0 ? &vfree[0] : nullptr, solutions); return solutions.GetNumSolutions(); - case IKP_TranslationXAxisAngle4D: - case IKP_TranslationYAxisAngle4D: - case IKP_TranslationZAxisAngle4D: - // For *TranslationXAxisAngle4D*, *TranslationYAxisAngle4D*, *TranslationZAxisAngle4D* - end effector origin - // reaches desired 3D translation, manipulator direction makes a specific angle with x/y/z-axis (defined in the - // manipulator base link’s coordinate system) - ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet."); - return 0; - case IKP_TranslationLocalGlobal6D: // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end // effector coordinate system. ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet."); return 0; + case IKP_TranslationXY2D: + ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + case IKP_Rotation3D: case IKP_Lookat3D: - case IKP_TranslationXY2D: case IKP_TranslationXYOrientation3D: ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet."); return 0; + case IKP_TranslationXAxisAngle4D: + // For *TranslationXAxisAngle4D*, *TranslationYAxisAngle4D*, *TranslationZAxisAngle4D* - end effector origin + // reaches desired 3D translation, manipulator direction makes a specific angle with x/y/z-axis (defined in the + // manipulator base link's coordinate system) case IKP_TranslationXAxisAngleZNorm4D: double roll, pitch, yaw; // For **TranslationXAxisAngleZNorm4D** - end effector origin reaches desired 3D translation, manipulator @@ -607,6 +625,7 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector 0 ? &vfree[0] : nullptr, solutions); return solutions.GetNumSolutions(); + case IKP_TranslationYAxisAngle4D: case IKP_TranslationYAxisAngleXNorm4D: // For **TranslationYAxisAngleXNorm4D** - end effector origin reaches desired 3D translation, manipulator // direction needs to be orthogonal to x axis and be rotated at a certain angle starting from the y axis (defined @@ -615,6 +634,7 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector 0 ? &vfree[0] : nullptr, solutions); return solutions.GetNumSolutions(); + case IKP_TranslationZAxisAngle4D: case IKP_TranslationZAxisAngleYNorm4D: // For **TranslationZAxisAngleYNorm4D** - end effector origin reaches desired 3D translation, manipulator // direction needs to be orthogonal to y axis and be rotated at a certain angle starting from the z axis (defined @@ -905,8 +925,8 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose if (ik_seed_state.size() != num_joints_) { - ROS_ERROR_STREAM_NAMED(name_, "Seed state must have size " << num_joints_ << " instead of size " - << ik_seed_state.size()); + ROS_ERROR_STREAM_NAMED(name_, + "Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } @@ -940,7 +960,7 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose if (!consistency_limits.empty()) { - // MoveIt! replaced consistency_limit (scalar) w/ consistency_limits (vector) + // MoveIt replaced consistency_limit (scalar) w/ consistency_limits (vector) // Assume [0]th free_params element for now. Probably wrong. double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]); double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]); @@ -1375,7 +1395,7 @@ void IKFastKinematicsPlugin::transformToChainFrame(const geometry_msgs::Pose& ik } } -} // end namespace +} // namespace _NAMESPACE_ // register IKFastKinematicsPlugin as a KinematicsBase implementation #include diff --git a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt index 9388ae6f26..cd3ffc0891 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt @@ -7,5 +7,8 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp index 9cff711601..ce8ad72bb4 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp @@ -23,8 +23,7 @@ // linear relationship to that of another joint. // Copyright (C) 2013 Sachin Chitta, Willow Garage -#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_Mimic_HPP -#define KDL_CHAIN_IKSOLVERVEL_PINV_Mimic_HPP +#pragma once #include #include @@ -84,11 +83,12 @@ class ChainIkSolverVelMimicSVD : public ChainIkSolverVel * * where W_q and W_x are joint- and Cartesian weights respectively. * A smaller joint weight (< 1.0) will reduce the contribution of this joint to the solution. */ + // NOLINTNEXTLINE(readability-identifier-naming) int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out, const Eigen::VectorXd& joint_weights, const Eigen::Matrix& cartesian_weights); /// not implemented. - int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out) override + int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/) override { return -1; } @@ -115,5 +115,4 @@ class ChainIkSolverVelMimicSVD : public ChainIkSolverVel Jacobian jac_; // full Jacobian Jacobian jac_reduced_; // reduced Jacobian with contributions of mimic joints mapped onto active DoFs }; -} -#endif +} // namespace KDL diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp index 6814441850..6d64b63edc 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp @@ -34,8 +34,7 @@ /* Author: Sachin Chitta */ -#ifndef MOVEIT_KDL_KINEMATICS_PLUGIN_JOINT_MIMIC -#define MOVEIT_KDL_KINEMATICS_PLUGIN_JOINT_MIMIC +#pragma once namespace kdl_kinematics_plugin { @@ -70,6 +69,4 @@ class JointMimic active = false; } }; -} - -#endif +} // namespace kdl_kinematics_plugin diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h index 6f094a50e0..14d1f4b6fb 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h @@ -34,8 +34,7 @@ /* Author: Sachin Chitta, David Lu!!, Ugo Cupcic */ -#ifndef MOVEIT_ROS_PLANNING_KDL_KINEMATICS_PLUGIN_ -#define MOVEIT_ROS_PLANNING_KDL_KINEMATICS_PLUGIN_ +#pragma once // ROS #include @@ -53,7 +52,7 @@ #include #include -// MoveIt! +// MoveIt #include #include #include @@ -78,10 +77,10 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase */ KDLKinematicsPlugin(); - bool getPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, std::vector& solution, - moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + bool + getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; bool searchPositionIK( const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, @@ -123,26 +122,10 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase const std::vector& getLinkNames() const override; protected: - /** - * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solutions by randomly re-seeding on failure. - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param timeout The amount of time (in seconds) available to the solver - * @param solution the solution vector - * @param solution_callback A callback to validate an IK solution - * @param error_code an error code that encodes the reason for failure or success - * @param consistency_limits The returned solutuion will not deviate more than these from the seed - * @return True if a valid solution was found, false otherwise - */ - bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::MoveItErrorCodes& error_code, const std::vector& consistency_limits, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; - typedef Eigen::Matrix Twist; /// Solve position IK given initial joint values + // NOLINTNEXTLINE(readability-identifier-naming) int CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out, const unsigned int max_iter, const Eigen::VectorXd& joint_weights, const Twist& cartesian_weights) const; @@ -178,8 +161,8 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase unsigned int dimension_; ///< Dimension of the group moveit_msgs::KinematicSolverInfo solver_info_; ///< Stores information for the inverse kinematics solver - const robot_model::JointModelGroup* joint_model_group_; - robot_state::RobotStatePtr state_; + const moveit::core::JointModelGroup* joint_model_group_; + moveit::core::RobotStatePtr state_; KDL::Chain kdl_chain_; std::unique_ptr fk_solver_; std::vector mimic_joints_; @@ -195,6 +178,4 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase * = 0.0: perform position-only IK */ double orientation_vs_position_weight_; }; -} - -#endif +} // namespace kdl_kinematics_plugin diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp index 9162b505a6..3c3a1d234a 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp @@ -24,7 +24,6 @@ // Copyright (C) 2013 Sachin Chitta, Willow Garage #include -#include namespace { diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp index ace343ae39..e35e535fb3 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp @@ -128,10 +128,10 @@ void KDLKinematicsPlugin::getJointWeights() else joint_weights_[it - active_names.begin()] = weights[i]; } - ROS_INFO_STREAM_NAMED("kdl", "Joint weights for group '" - << getGroupName() << "': \n" - << Eigen::Map(joint_weights_.data(), joint_weights_.size()) - .transpose()); + ROS_INFO_STREAM_NAMED( + "kdl", "Joint weights for group '" + << getGroupName() << "': \n" + << Eigen::Map(joint_weights_.data(), joint_weights_.size()).transpose()); } bool KDLKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name, @@ -214,7 +214,7 @@ bool KDLKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model unsigned int joint_counter = 0; for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i) { - const robot_model::JointModel* jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName()); + const moveit::core::JointModel* jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName()); // first check whether it belongs to the set of active joints in the group if (jm->getMimic() == nullptr && jm->getVariableCount() > 0) @@ -244,7 +244,7 @@ bool KDLKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model { if (!mimic_joint.active) { - const robot_model::JointModel* joint_model = + const moveit::core::JointModel* joint_model = joint_model_group_->getJointModel(mimic_joint.joint_name)->getMimic(); for (JointMimic& mimic_joint_recal : mimic_joints_) { @@ -257,7 +257,7 @@ bool KDLKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model } // Setup the joint state groups that we need - state_.reset(new robot_state::RobotState(robot_model_)); + state_.reset(new moveit::core::RobotState(robot_model_)); fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); @@ -278,7 +278,7 @@ bool KDLKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, cons std::vector consistency_limits; // limit search to a single attempt by setting a timeout of zero - return searchPositionIK(ik_pose, ik_seed_state, 0.0, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, 0.0, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -289,7 +289,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -298,7 +298,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -309,7 +309,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c const kinematics::KinematicsQueryOptions& options) const { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options); } @@ -318,17 +318,6 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const -{ - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, - options); -} - -bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, - double timeout, std::vector& solution, - const IKCallbackFn& solution_callback, - moveit_msgs::MoveItErrorCodes& error_code, - const std::vector& consistency_limits, - const kinematics::KinematicsQueryOptions& options) const { ros::WallTime start_time = ros::WallTime::now(); if (!initialized_) @@ -340,8 +329,8 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c if (ik_seed_state.size() != dimension_) { - ROS_ERROR_STREAM_NAMED("kdl", "Seed state must have size " << dimension_ << " instead of size " - << ik_seed_state.size()); + ROS_ERROR_STREAM_NAMED("kdl", + "Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } @@ -439,8 +428,8 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, con KDL::Frame f; KDL::Twist delta_twist; KDL::JntArray delta_q(q_out.rows()), q_backup(q_out.rows()); - Eigen::ArrayXd extra_joint_weights; - extra_joint_weights.setOnes(joint_weights.rows()); + Eigen::ArrayXd extra_joint_weights(joint_weights.rows()); + extra_joint_weights.setOnes(); q_out = q_init; ROS_DEBUG_STREAM_NAMED("kdl", "Input: " << q_init); @@ -489,12 +478,14 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, con delta_q_norm); if (delta_q_norm < epsilon_) // stuck in singularity { - if (step_size < 0.005) // cannot reach target + if (step_size < epsilon_) // cannot reach target break; // wiggle joints last_delta_twist_norm = DBL_MAX; delta_q.data.setRandom(); delta_q.data *= std::min(0.1, delta_twist_norm); + clipToJointLimits(q_out, delta_q, extra_joint_weights); + extra_joint_weights.setOnes(); } KDL::Add(q_out, delta_q, q_out); diff --git a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt index 0397388312..1dadddcd56 100644 --- a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt @@ -5,5 +5,8 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h index 996080593a..84787e93be 100644 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h +++ b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h @@ -34,15 +34,14 @@ /* Author: Francisco Suarez-Ruiz */ -#ifndef MOVEIT_ROS_PLANNING_LMA_KINEMATICS_PLUGIN_H -#define MOVEIT_ROS_PLANNING_LMA_KINEMATICS_PLUGIN_H +#pragma once // ROS #include #include // ROS msgs -#include +#include #include #include #include @@ -53,7 +52,7 @@ #include #include -// MoveIt! +// MoveIt #include #include #include @@ -72,10 +71,10 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase */ LMAKinematicsPlugin(); - bool getPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, std::vector& solution, - moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + bool + getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; bool searchPositionIK( const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, @@ -116,24 +115,6 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase */ const std::vector& getLinkNames() const override; -protected: - /** - * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solutions by randomly re-seeding on failure. - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param timeout The amount of time (in seconds) available to the solver - * @param solution the solution vector - * @param solution_callback A callback to validate an IK solution - * @param error_code an error code that encodes the reason for failure or success - * @param consistency_limits The returned solutuion will not deviate more than these from the seed - * @return True if a valid solution was found, false otherwise - */ - bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::MoveItErrorCodes& error_code, const std::vector& consistency_limits, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; - private: bool timedOut(const ros::WallTime& start_time, double duration) const; @@ -165,11 +146,11 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase unsigned int dimension_; ///< Dimension of the group moveit_msgs::KinematicSolverInfo solver_info_; ///< Stores information for the inverse kinematics solver - const robot_model::JointModelGroup* joint_model_group_; - robot_state::RobotStatePtr state_; + const moveit::core::JointModelGroup* joint_model_group_; + moveit::core::RobotStatePtr state_; KDL::Chain kdl_chain_; std::unique_ptr fk_solver_; - std::vector joints_; + std::vector joints_; std::vector joint_names_; int max_solver_iterations_; @@ -181,6 +162,4 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase * = 0.0: perform position-only IK */ double orientation_vs_position_weight_; }; -} - -#endif +} // namespace lma_kinematics_plugin diff --git a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp index 2eda07ee0b..a55eb7dd36 100644 --- a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp +++ b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp @@ -110,7 +110,7 @@ bool LMAKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model return false; } - for (const robot_model::JointModel* jm : joint_model_group_->getJointModels()) + for (const moveit::core::JointModel* jm : joint_model_group_->getJointModels()) { if (jm->getType() == moveit::core::JointModel::REVOLUTE || jm->getType() == moveit::core::JointModel::PRISMATIC) { @@ -133,7 +133,7 @@ bool LMAKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model ROS_INFO_NAMED("lma", "Using position only ik"); // Setup the joint state groups that we need - state_.reset(new robot_state::RobotState(robot_model_)); + state_.reset(new moveit::core::RobotState(robot_model_)); fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); @@ -154,7 +154,7 @@ bool LMAKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, cons std::vector consistency_limits; // limit search to a single attempt by setting a timeout of zero - return searchPositionIK(ik_pose, ik_seed_state, 0.0, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, 0.0, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -165,7 +165,7 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -174,7 +174,7 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -185,17 +185,7 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c const kinematics::KinematicsQueryOptions& options) const { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, - options); -} - -bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, - double timeout, const std::vector& consistency_limits, - std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options) const -{ - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options); } @@ -216,10 +206,9 @@ bool LMAKinematicsPlugin::obeysLimits(const Eigen::VectorXd& values) const } bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, - double timeout, std::vector& solution, - const IKCallbackFn& solution_callback, + double timeout, const std::vector& consistency_limits, + std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, - const std::vector& consistency_limits, const kinematics::KinematicsQueryOptions& options) const { ros::WallTime start_time = ros::WallTime::now(); @@ -232,8 +221,8 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c if (ik_seed_state.size() != dimension_) { - ROS_ERROR_STREAM_NAMED("lma", "Seed state must have size " << dimension_ << " instead of size " - << ik_seed_state.size()); + ROS_ERROR_STREAM_NAMED("lma", + "Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 649b3ef22a..7a5861e0dd 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -1,8 +1,8 @@ - + moveit_kinematics - 1.0.1 - Package for all inverse kinematics solvers in MoveIt! + 1.1.1 + Package for all inverse kinematics solvers in MoveIt Dave Coleman Ioan Sucan @@ -11,7 +11,7 @@ Dave Coleman G.A. vd. Hoorn Jorge Nicho - MoveIt! Release Team + MoveIt Release Team BSD @@ -27,14 +27,22 @@ eigen tf2 tf2_kdl - orocos_kdl + orocos_kdl + liborocos-kdl-dev - python-lxml - liburdfdom-tools + + liburdfdom-tools + python-lxml + python3-lxml + python-yaml + python3-yaml rostest moveit_ros_planning - moveit_resources + moveit_resources_fanuc_description + moveit_resources_fanuc_moveit_config + moveit_resources_panda_description + moveit_resources_panda_moveit_config xmlrpcpp diff --git a/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt index b7829f2412..2cf14dd877 100644 --- a/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt @@ -5,5 +5,8 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h index b19d1e880a..4c1005c42b 100644 --- a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h +++ b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h @@ -33,14 +33,13 @@ *********************************************************************/ /* Author: Dave Coleman, Masaki Murooka - Desc: Connects MoveIt! to any inverse kinematics solver via a ROS service call + Desc: Connects MoveIt to any inverse kinematics solver via a ROS service call Supports planning groups with multiple tip frames \todo: better support for mimic joints \todo: better support for redundant joints */ -#ifndef MOVEIT_ROS_PLANNING_SRV_KINEMATICS_PLUGIN_ -#define MOVEIT_ROS_PLANNING_SRV_KINEMATICS_PLUGIN_ +#pragma once // ROS #include @@ -49,13 +48,11 @@ #include // ROS msgs -#include -#include -#include +#include #include #include -// MoveIt! +// MoveIt #include #include @@ -73,10 +70,10 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase */ SrvKinematicsPlugin(); - bool getPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, std::vector& solution, - moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + bool + getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; bool searchPositionIK( const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, @@ -100,6 +97,12 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + bool searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, + double timeout, const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const moveit::core::RobotState* context_state = nullptr) const override; + bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, std::vector& poses) const override; @@ -123,18 +126,6 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase const std::vector& getVariableNames() const; protected: - virtual bool - searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::MoveItErrorCodes& error_code, const std::vector& consistency_limits, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; - - virtual bool - searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, - double timeout, const std::vector& consistency_limits, std::vector& solution, - const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; - bool setRedundantJoints(const std::vector& redundant_joint_indices) override; private: @@ -150,14 +141,12 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase unsigned int dimension_; /** Dimension of the group */ - const robot_model::JointModelGroup* joint_model_group_; + const moveit::core::JointModelGroup* joint_model_group_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotStatePtr robot_state_; int num_possible_redundant_joints_; std::shared_ptr ik_service_client_; }; -} - -#endif +} // namespace srv_kinematics_plugin diff --git a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp index 0f33d7af73..a563048a51 100644 --- a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp +++ b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp @@ -34,6 +34,7 @@ /* Author: Dave Coleman, Masaki Murooka */ +#include #include #include #include @@ -109,7 +110,7 @@ bool SrvKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model lookupParam("kinematics_solver_service_name", ik_service_name, std::string("solve_ik")); // Setup the joint state groups that we need - robot_state_.reset(new robot_state::RobotState(robot_model_)); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); robot_state_->setToDefaultValues(); // Create the ROS service client @@ -172,8 +173,8 @@ bool SrvKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, cons { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, solution, IKCallbackFn(), error_code, - consistency_limits, options); + return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, consistency_limits, solution, IKCallbackFn(), + error_code, options); } bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, @@ -183,7 +184,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -192,7 +193,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, IKCallbackFn(), error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, options); } @@ -203,7 +204,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c const kinematics::KinematicsQueryOptions& options) const { std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options); } @@ -212,17 +213,6 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const -{ - return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits, - options); -} - -bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, - double timeout, std::vector& solution, - const IKCallbackFn& solution_callback, - moveit_msgs::MoveItErrorCodes& error_code, - const std::vector& consistency_limits, - const kinematics::KinematicsQueryOptions& options) const { // Convert single pose into a vector of one pose std::vector ik_poses; @@ -233,11 +223,12 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c } bool SrvKinematicsPlugin::searchPositionIK(const std::vector& ik_poses, - const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, - const IKCallbackFn& solution_callback, + const std::vector& ik_seed_state, double /*timeout*/, + const std::vector& /*consistency_limits*/, + std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options) const + const kinematics::KinematicsQueryOptions& /*options*/, + const moveit::core::RobotState* /*context_state*/) const { // Check if active if (!active_) @@ -250,8 +241,8 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vector #include #include #include @@ -77,14 +76,14 @@ int main(int argc, char* argv[]) spinner.start(); robot_model_loader::RobotModelLoader robot_model_loader; - const robot_model::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); + const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); planning_scene::PlanningScene planning_scene(kinematic_model); - robot_state::RobotState& kinematic_state = planning_scene.getCurrentStateNonConst(); + moveit::core::RobotState& kinematic_state = planning_scene.getCurrentStateNonConst(); collision_detection::CollisionRequest collision_request; collision_detection::CollisionResult collision_result; std::chrono::duration ik_time(0); std::chrono::time_point start; - std::vector groups; + std::vector groups; std::vector end_effectors; if (group == "all") diff --git a/moveit_kinematics/test/fanuc-ikfast.test b/moveit_kinematics/test/fanuc-ikfast.test index a5d09f891c..9de03483d5 100644 --- a/moveit_kinematics/test/fanuc-ikfast.test +++ b/moveit_kinematics/test/fanuc-ikfast.test @@ -1,7 +1,7 @@ - + diff --git a/moveit_kinematics/test/fanuc-kdl.test b/moveit_kinematics/test/fanuc-kdl.test index 6b44ced4d1..9b4fccb953 100644 --- a/moveit_kinematics/test/fanuc-kdl.test +++ b/moveit_kinematics/test/fanuc-kdl.test @@ -6,7 +6,7 @@ - + diff --git a/moveit_kinematics/test/panda-ikfast.test b/moveit_kinematics/test/panda-ikfast.test index 2094d88ddc..0359d5d096 100644 --- a/moveit_kinematics/test/panda-ikfast.test +++ b/moveit_kinematics/test/panda-ikfast.test @@ -1,7 +1,7 @@ - + diff --git a/moveit_kinematics/test/panda-kdl.test b/moveit_kinematics/test/panda-kdl.test index b7b52de74c..95a331cff8 100644 --- a/moveit_kinematics/test/panda-kdl.test +++ b/moveit_kinematics/test/panda-kdl.test @@ -6,7 +6,7 @@ - + diff --git a/moveit_kinematics/test/test_ikfast_plugins.sh b/moveit_kinematics/test/test_ikfast_plugins.sh index c023161a16..d96c9e19f8 100755 --- a/moveit_kinematics/test/test_ikfast_plugins.sh +++ b/moveit_kinematics/test/test_ikfast_plugins.sh @@ -7,6 +7,17 @@ # We will create ikfast plugins for fanuc and panda from moveit_resources # using the script auto_create_ikfast_moveit_plugin.sh +sudo update-alternatives --install /usr/bin/python python /usr/bin/python2 1 +sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 2 + +if [ "$ROS_DISTRO" == "noetic" ]; then + sudo update-alternatives --set python /usr/bin/python3 + travis_run sudo apt-get -qq install python3-lxml python3-yaml +else + sudo update-alternatives --set python /usr/bin/python2 + travis_run sudo apt-get -qq install python-lxml python-yaml +fi + # Clone moveit_resources for URDFs. They are not available before running docker. travis_run git clone -q --depth=1 https://github.com/ros-planning/moveit_resources /tmp/resources fanuc=/tmp/resources/fanuc_description/urdf/fanuc.urdf diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index 682a519909..7ca176cd0b 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -43,7 +43,7 @@ #include #include -// MoveIt! +// MoveIt #include #include #include @@ -77,7 +77,7 @@ class SharedData friend class KinematicsTest; typedef pluginlib::ClassLoader KinematicsLoader; - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; std::unique_ptr kinematics_loader_; std::string root_link_; std::string tip_link_; @@ -104,7 +104,7 @@ class SharedData ROS_INFO_STREAM("Loading robot model from " << ros::this_node::getNamespace() << "/" << ROBOT_DESCRIPTION_PARAM); // load robot model rdf_loader::RDFLoader rdf_loader(ROBOT_DESCRIPTION_PARAM); - robot_model_ = std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF()); + robot_model_ = std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF()); ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; // init ClassLoader @@ -203,7 +203,7 @@ class KinematicsTest : public ::testing::Test } public: - testing::AssertionResult isNear(const char* expr1, const char* expr2, const char* abs_error_expr, + testing::AssertionResult isNear(const char* expr1, const char* expr2, const char* /*abs_error_expr*/, const geometry_msgs::Point& val1, const geometry_msgs::Point& val2, double abs_error) { // clang-format off @@ -218,7 +218,7 @@ class KinematicsTest : public ::testing::Test << "Actual: " << expr2 << " [" << val2.x << ", " << val2.y << ", " << val2.z << "]"; // clang-format on } - testing::AssertionResult isNear(const char* expr1, const char* expr2, const char* abs_error_expr, + testing::AssertionResult isNear(const char* expr1, const char* expr2, const char* /*abs_error_expr*/, const geometry_msgs::Quaternion& val1, const geometry_msgs::Quaternion& val2, double abs_error) { @@ -261,7 +261,7 @@ class KinematicsTest : public ::testing::Test return testing::AssertionSuccess(); } - void searchIKCallback(const geometry_msgs::Pose& ik_pose, const std::vector& joint_state, + void searchIKCallback(const geometry_msgs::Pose& /*ik_pose*/, const std::vector& joint_state, moveit_msgs::MoveItErrorCodes& error_code) { std::vector link_names = { tip_link_ }; @@ -280,8 +280,8 @@ class KinematicsTest : public ::testing::Test } public: - robot_model::RobotModelPtr robot_model_; - robot_model::JointModelGroup* jmg_; + moveit::core::RobotModelPtr robot_model_; + moveit::core::JointModelGroup* jmg_; kinematics::KinematicsBasePtr kinematics_solver_; random_numbers::RandomNumberGenerator rng_{ 42 }; std::string root_link_; @@ -307,7 +307,7 @@ TEST_F(KinematicsTest, getFK) { std::vector joints(kinematics_solver_->getJointNames().size(), 0.0); const std::vector& tip_frames = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); for (unsigned int i = 0; i < num_fk_tests_; ++i) @@ -331,7 +331,7 @@ TEST_F(KinematicsTest, randomWalkIK) { std::vector seed, goal, solution; const std::vector& tip_frames = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); if (!seed_.empty()) @@ -466,7 +466,7 @@ TEST_F(KinematicsTest, unitIK) std::vector seed, sol; const std::vector& tip_frames = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); // initial joint pose from seed_ or defaults @@ -539,7 +539,7 @@ TEST_F(KinematicsTest, searchIK) moveit_msgs::MoveItErrorCodes error_code; solution.resize(kinematics_solver_->getJointNames().size(), 0.0); const std::vector& fk_names = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); unsigned int success = 0; @@ -573,7 +573,7 @@ TEST_F(KinematicsTest, searchIKWithCallback) moveit_msgs::MoveItErrorCodes error_code; solution.resize(kinematics_solver_->getJointNames().size(), 0.0); const std::vector& fk_names = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); unsigned int success = 0; @@ -613,7 +613,7 @@ TEST_F(KinematicsTest, getIK) moveit_msgs::MoveItErrorCodes error_code; solution.resize(kinematics_solver_->getJointNames().size(), 0.0); const std::vector& fk_names = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); for (unsigned int i = 0; i < num_ik_tests_; ++i) @@ -642,7 +642,7 @@ TEST_F(KinematicsTest, getIKMultipleSolutions) kinematics::KinematicsResult result; const std::vector& fk_names = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); unsigned int success = 0; @@ -685,7 +685,7 @@ TEST_F(KinematicsTest, getNearestIKSolution) std::vector seed, fk_values, solution; moveit_msgs::MoveItErrorCodes error_code; const std::vector& fk_names = kinematics_solver_->getTipFrames(); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); for (unsigned int i = 0; i < num_nearest_ik_tests_; ++i) diff --git a/moveit_planners/README.md b/moveit_planners/README.md index 0f0f685d07..5d35579eac 100644 --- a/moveit_planners/README.md +++ b/moveit_planners/README.md @@ -1,3 +1,3 @@ -# MoveIt! Planners +# MoveIt Planners Interfaces for the motion planning libraries used in MoveIt diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst index 3b2e6e9912..50f520c3ae 100644 --- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst @@ -2,6 +2,52 @@ Changelog for package chomp_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + +1.1.0 (2020-09-04) +------------------ +* [feature] Replace $(find xacro)/xacro -> xacro (`#2282 `_) +* [feature] Start new joint_state_publisher_gui on param use_gui (`#2257 `_) +* [feature] Optional cpp version setting (`#2166 `_) +* [feature] Change API of ChompPlanner::solve() to not use message +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] clang-tidy fixes (`#2050 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [fix] Small fixes to chomp planner (`#1407 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Add Missing License (`#1779 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Chittaranjan Srinivas Swaminathan, Dave Coleman, Jonathan Binney, Markus Vieth, Robert Haschke, Sean Yen, Tyler Weaver, Yoan Mollard + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Robert Haschke, Sean Yen + +1.0.2 (2019-06-28) +------------------ +* [fix] Fix chomp planner (`#1512 `_) + * Fix start-state handling + * remove time parameterization from planning code +* Contributors: Robert Haschke + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -48,7 +94,7 @@ Changelog for package chomp_interface 0.10.1 (2018-05-25) ------------------- * [fix] dependencies for chomp interface test (`#778 `_) -* [maintenance] MoveIt! tf2 migration (`#830 `_) +* [maintenance] MoveIt tf2 migration (`#830 `_) * Contributors: Bence Magyar, Dave Coleman, Ian McMahon, Mikael Arguedas, Robert Haschke, Stephan, Will Baker 0.9.11 (2017-12-25) diff --git a/moveit_planners/chomp/chomp_interface/CMakeLists.txt b/moveit_planners/chomp/chomp_interface/CMakeLists.txt index 2cddd07a5b..bacee69953 100644 --- a/moveit_planners/chomp/chomp_interface/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_interface/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_planners_chomp) -add_definitions(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) # find catkin in isolation so that CATKIN_ENABLE_TESTING is defined find_package(catkin REQUIRED) @@ -28,8 +32,8 @@ catkin_package( CATKIN_DEPENDS roscpp moveit_core pluginlib ) -include_directories( - include +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} @@ -53,7 +57,7 @@ install(DIRECTORY include/chomp_interface/ install(TARGETS ${PROJECT_NAME} chomp_planner_plugin ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) if(CATKIN_ENABLE_TESTING) diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h index 1272a130cc..11778c1951 100644 --- a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h @@ -34,8 +34,7 @@ /* Author: E. Gil Jones */ -#ifndef CHOMP_INTERFACE_CHOMP_INTERFACE_H -#define CHOMP_INTERFACE_CHOMP_INTERFACE_H +#pragma once #include #include @@ -43,7 +42,7 @@ namespace chomp_interface { -MOVEIT_CLASS_FORWARD(CHOMPInterface); +MOVEIT_CLASS_FORWARD(CHOMPInterface); // Defines CHOMPInterfacePtr, ConstPtr, WeakPtr... etc class CHOMPInterface : public chomp::ChompPlanner { @@ -63,6 +62,4 @@ class CHOMPInterface : public chomp::ChompPlanner chomp::ChompParameters params_; }; -} - -#endif +} // namespace chomp_interface diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h index 69169f8a66..31bdc0100f 100644 --- a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h @@ -34,15 +34,14 @@ /* Author: Chittaranjan Srinivas Swaminathan */ -#ifndef CHOMP_INTERFACE_CHOMP_PLANNING_CONTEXT_H -#define CHOMP_INTERFACE_CHOMP_PLANNING_CONTEXT_H +#pragma once #include #include namespace chomp_interface { -MOVEIT_CLASS_FORWARD(CHOMPPlanningContext); +MOVEIT_CLASS_FORWARD(CHOMPPlanningContext); // Defines CHOMPPlanningContextPtr, ConstPtr, WeakPtr... etc class CHOMPPlanningContext : public planning_interface::PlanningContext { @@ -53,7 +52,7 @@ class CHOMPPlanningContext : public planning_interface::PlanningContext void clear() override; bool terminate() override; - CHOMPPlanningContext(const std::string& name, const std::string& group, const robot_model::RobotModelConstPtr& model); + CHOMPPlanningContext(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model); ~CHOMPPlanningContext() override = default; @@ -65,5 +64,3 @@ class CHOMPPlanningContext : public planning_interface::PlanningContext }; } /* namespace chomp_interface */ - -#endif /* CHOMP_PLANNING_CONTEXT_H_ */ diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml index e7f232010e..0960164a84 100644 --- a/moveit_planners/chomp/chomp_interface/package.xml +++ b/moveit_planners/chomp/chomp_interface/package.xml @@ -1,13 +1,13 @@ moveit_planners_chomp - The interface for using CHOMP within MoveIt! + The interface for using CHOMP within MoveIt - 1.0.1 + 1.1.1 Gil Jones Chittaranjan Srinivas Swaminathan Dave Coleman - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp index 248435dee9..eec0b815d2 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp @@ -1,9 +1,38 @@ -/* - * chomp_planning_context.cpp +/********************************************************************* + * Software License Agreement (BSD License) * - * Created on: 27-Jul-2016 - * Author: ace - */ + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Chittaranjan Srinivas Swaminathan */ #include #include @@ -12,7 +41,7 @@ namespace chomp_interface { CHOMPPlanningContext::CHOMPPlanningContext(const std::string& name, const std::string& group, - const robot_model::RobotModelConstPtr& model) + const moveit::core::RobotModelConstPtr& model) : planning_interface::PlanningContext(name, group), robot_model_(model) { chomp_interface_ = CHOMPInterfacePtr(new CHOMPInterface()); diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp index 92697969b6..0a0a96bc9b 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp @@ -36,7 +36,6 @@ #include #include #include -#include #include #include @@ -50,7 +49,7 @@ class CHOMPPlannerManager : public planning_interface::PlannerManager { } - bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns) override + bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& /*ns*/) override { for (const std::string& group : model->getJointModelGroupNames()) { @@ -92,7 +91,7 @@ class CHOMPPlannerManager : public planning_interface::PlannerManager return context; } - bool canServiceRequest(const planning_interface::MotionPlanRequest& req) const override + bool canServiceRequest(const planning_interface::MotionPlanRequest& /*req*/) const override { // TODO: this is a dummy implementation // capabilities.dummy = false; diff --git a/moveit_planners/chomp/chomp_interface/test/rrbot_chomp_planning_pipeline.launch.xml b/moveit_planners/chomp/chomp_interface/test/rrbot_chomp_planning_pipeline.launch.xml index b3146c917b..4b3a1f6f54 100644 --- a/moveit_planners/chomp/chomp_interface/test/rrbot_chomp_planning_pipeline.launch.xml +++ b/moveit_planners/chomp/chomp_interface/test/rrbot_chomp_planning_pipeline.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch index 2bdfe3c0b5..ab54cf53fc 100644 --- a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch +++ b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch @@ -1,12 +1,12 @@ - + - + @@ -17,8 +17,10 @@ - - + + [move_group/fake_controller_joint_states] + + [move_group/fake_controller_joint_states] diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst index 0dc05a2151..d66596da94 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst @@ -2,6 +2,53 @@ Changelog for package chomp_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + +1.1.0 (2020-09-04) +------------------ +* [feature] Optional cpp version setting (`#2166 `_) +* [feature] Unified Collision Environment Integration (`#1584 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [fix] Fix calculation of potential (`#1651 `_) +* [fix] Fix Chomp planning adapter (`#1525 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#1419 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Bence Magyar, Dave Coleman, Jens P, Jonathan Binney, Markus Vieth, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Windows build fixes + * Fix header inclusion and other MSVC build errors (`#1636 `_) + * Remove GCC extensions (`#1583 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] Fix possible division-by-zero (`#1809 `_) +* Contributors: Max Krichenbauer, Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ +* [fix] Fix chomp planner (`#1512 `_) + * Fix start-state handling + * remove time parameterization from planning code +* Contributors: Robert Haschke + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -50,7 +97,7 @@ Changelog for package chomp_motion_planner 0.10.1 (2018-05-25) ------------------- * [fix] for chomp fixed base joint bug (`#870 `_) -* [maintenance] MoveIt! tf2 migration (`#830 `_) +* [maintenance] MoveIt tf2 migration (`#830 `_) * [maintenance] switch to ROS_LOGGER from CONSOLE_BRIDGE (`#874 `_) * Contributors: Bence Magyar, Dave Coleman, Ian McMahon, Mike Lautman, Xiaojian Ma diff --git a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt index 2ec61daeb2..ed698c3336 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(chomp_motion_planner) -add_definitions(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) find_package(catkin REQUIRED COMPONENTS roscpp @@ -13,7 +17,8 @@ catkin_package( LIBRARIES ${PROJECT_NAME} ) -include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) add_library(${PROJECT_NAME} src/chomp_cost.cpp @@ -33,5 +38,5 @@ install(DIRECTORY include/${PROJECT_NAME}/ install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h index bda7cbd3f9..7e76fbe1c6 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#ifndef CHOMP_COST_H_ -#define CHOMP_COST_H_ +#pragma once #include #include @@ -54,13 +53,13 @@ class ChompCost virtual ~ChompCost(); template - void getDerivative(Eigen::MatrixXd::ColXpr joint_trajectory, Eigen::MatrixBase& derivative) const; + void getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory, Eigen::MatrixBase& derivative) const; const Eigen::MatrixXd& getQuadraticCostInverse() const; const Eigen::MatrixXd& getQuadraticCost() const; - double getCost(Eigen::MatrixXd::ColXpr joint_trajectory) const; + double getCost(const Eigen::MatrixXd::ColXpr& joint_trajectory) const; double getMaxQuadCostInvValue() const; @@ -76,7 +75,8 @@ class ChompCost }; template -void ChompCost::getDerivative(Eigen::MatrixXd::ColXpr joint_trajectory, Eigen::MatrixBase& derivative) const +void ChompCost::getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory, + Eigen::MatrixBase& derivative) const { derivative = (quad_cost_full_ * (2.0 * joint_trajectory)); } @@ -91,11 +91,9 @@ inline const Eigen::MatrixXd& ChompCost::getQuadraticCost() const return quad_cost_; } -inline double ChompCost::getCost(Eigen::MatrixXd::ColXpr joint_trajectory) const +inline double ChompCost::getCost(const Eigen::MatrixXd::ColXpr& joint_trajectory) const { return joint_trajectory.dot(quad_cost_full_ * joint_trajectory); } } // namespace chomp - -#endif /* CHOMP_COST_H_ */ diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h index fba516334b..ea01bad078 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#ifndef CHOMP_OPTIMIZER_H_ -#define CHOMP_OPTIMIZER_H_ +#pragma once #include #include @@ -43,8 +42,7 @@ #include #include #include -#include -#include +#include #include #include @@ -86,25 +84,21 @@ class ChompOptimizer inline double getPotential(double field_distance, double radius, double clearence) { double d = field_distance - radius; - double potential = 0.0; - // three cases below: - if (d >= clearence) + if (d >= clearence) // everything is fine { - potential = 0.0; + return 0.0; } - else if (d >= 0.0) + else if (d >= 0.0) // transition phase, no collision yet { - double diff = (d - clearence); - double gradient_magnitude = diff * clearence; // (diff / clearance) - potential = 0.5 * gradient_magnitude * diff; + const double diff = (d - clearence); + const double gradient_magnitude = diff / clearence; + return 0.5 * gradient_magnitude * diff; // 0.5 * (d - clearance)^2 / clearance } - else // if d < 0.0 + else // d < 0.0: collision { - potential = -d + 0.5 * clearence; + return -d + 0.5 * clearence; // linearly increase, starting from 0.5 * clearance } - - return potential; } template void getJacobian(int trajectoryPoint, Eigen::Vector3d& collision_point_pos, std::string& jointName, @@ -136,8 +130,7 @@ class ChompOptimizer moveit::core::RobotState state_; moveit::core::RobotState start_state_; const moveit::core::JointModelGroup* joint_model_group_; - const collision_detection::CollisionWorldHybrid* hy_world_; - const collision_detection::CollisionRobotHybrid* hy_robot_; + const collision_detection::CollisionEnvHybrid* hy_env_; std::vector joint_costs_; collision_detection::GroupStateRepresentationPtr gsr_; @@ -223,6 +216,4 @@ class ChompOptimizer void computeJointProperties(int trajectoryPoint); bool isCurrentTrajectoryMeshToMeshCollisionFree() const; }; -} - -#endif /* CHOMP_OPTIMIZER_H_ */ +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h index f1446c9d8a..ab78f274ca 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#ifndef CHOMP_PARAMETERS_H_ -#define CHOMP_PARAMETERS_H_ +#pragma once #include @@ -98,5 +97,3 @@ class ChompParameters }; } // namespace chomp - -#endif /* CHOMP_PARAMETERS_H_ */ diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h index 30d77fa7a0..b47308edde 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h @@ -34,8 +34,7 @@ /* Author: E. Gil Jones */ -#ifndef _CHOMP_PLANNER_H_ -#define _CHOMP_PLANNER_H_ +#pragma once #include #include @@ -54,6 +53,4 @@ class ChompPlanner const planning_interface::MotionPlanRequest& req, const ChompParameters& params, planning_interface::MotionPlanDetailedResponse& res) const; }; -} - -#endif +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h index 359ce88b24..e197008a2c 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#ifndef CHOMP_TRAJECTORY_H_ -#define CHOMP_TRAJECTORY_H_ +#pragma once #include #include @@ -295,11 +294,11 @@ template void ChompTrajectory::getJointVelocities(size_t traj_point, Eigen::MatrixBase& velocities) { velocities.setZero(); - double invTime = 1.0 / discretization_; + double inv_time = 1.0 / discretization_; for (int k = -DIFF_RULE_LENGTH / 2; k <= DIFF_RULE_LENGTH / 2; k++) { - velocities += (invTime * DIFF_RULES[0][k + DIFF_RULE_LENGTH / 2]) * trajectory_.row(traj_point + k).transpose(); + velocities += (inv_time * DIFF_RULES[0][k + DIFF_RULE_LENGTH / 2]) * trajectory_.row(traj_point + k).transpose(); } } @@ -307,6 +306,4 @@ inline double ChompTrajectory::getDuration() const { return duration_; } -} - -#endif /* CHOMP_TRAJECTORY_H_ */ +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.h index 0b9a5214ae..b5fb5b7a66 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#ifndef CHOMP_UTILS_H_ -#define CHOMP_UTILS_H_ +#pragma once #include #include @@ -86,5 +85,3 @@ static inline double shortestAngularDistance(double start, double end) } } // namespace chomp - -#endif /* CHOMP_UTILS_H_ */ diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h index d906439cef..45b265474b 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h @@ -34,8 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#ifndef MULTIVARIATE_GAUSSIAN_H_ -#define MULTIVARIATE_GAUSSIAN_H_ +#pragma once #include #include @@ -92,6 +91,4 @@ void MultivariateGaussian::sample(Eigen::MatrixBase& output) output(i) = gaussian_(); output = mean_ + covariance_cholesky_ * output; } -} - -#endif /* MULTIVARIATE_GAUSSIAN_H_ */ +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml index 8abb126185..343c3641e1 100644 --- a/moveit_planners/chomp/chomp_motion_planner/package.xml +++ b/moveit_planners/chomp/chomp_motion_planner/package.xml @@ -2,12 +2,12 @@ chomp_motion_planner chomp_motion_planner - 1.0.1 + 1.1.1 Gil Jones Mrinal Kalakrishnan Chittaranjan Srinivas Swaminathan - MoveIt! Release Team + MoveIt Release Team BSD http://ros.org/wiki/chomp_motion_planner diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_cost.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_cost.cpp index 3543ba073a..9a8ef78bb3 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_cost.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_cost.cpp @@ -43,8 +43,8 @@ using namespace std; namespace chomp { -ChompCost::ChompCost(const ChompTrajectory& trajectory, int joint_number, const std::vector& derivative_costs, - double ridge_factor) +ChompCost::ChompCost(const ChompTrajectory& trajectory, int /* joint_number */, + const std::vector& derivative_costs, double ridge_factor) { int num_vars_all = trajectory.getNumPoints(); int num_vars_free = num_vars_all - 2 * (DIFF_RULE_LENGTH - 1); diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index 6158d6f3e5..bd8b655928 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -43,12 +43,15 @@ #include #include #include +#include namespace chomp { double getRandomDouble() { - return ((double)random() / (double)RAND_MAX); + std::default_random_engine seed; + std::uniform_real_distribution<> uniform(0.0, 1.0); + return uniform(seed); } ChompOptimizer::ChompOptimizer(ChompTrajectory* trajectory, const planning_scene::PlanningSceneConstPtr& planning_scene, @@ -75,21 +78,14 @@ ChompOptimizer::ChompOptimizer(ChompTrajectory* trajectory, const planning_scene ROS_INFO_STREAM("Active collision detector is: " + planning_scene->getActiveCollisionDetectorName()); - hy_world_ = dynamic_cast( - planning_scene->getCollisionWorld(planning_scene->getActiveCollisionDetectorName()).get()); - if (!hy_world_) + hy_env_ = dynamic_cast( + planning_scene->getCollisionEnv(planning_scene->getActiveCollisionDetectorName()).get()); + if (!hy_env_) { ROS_WARN_STREAM("Could not initialize hybrid collision world from planning scene"); return; } - hy_robot_ = dynamic_cast( - planning_scene->getCollisionRobot(planning_scene->getActiveCollisionDetectorName()).get()); - if (!hy_robot_) - { - ROS_WARN_STREAM("Could not initialize hybrid collision robot from planning scene"); - return; - } initialize(); } @@ -107,8 +103,7 @@ void ChompOptimizer::initialize() collision_detection::CollisionResult res; req.group_name = planning_group_; ros::WallTime wt = ros::WallTime::now(); - hy_world_->getCollisionGradients(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), state_, - &planning_scene_->getAllowedCollisionMatrix(), gsr_); + hy_env_->getCollisionGradients(req, res, state_, &planning_scene_->getAllowedCollisionMatrix(), gsr_); ROS_INFO_STREAM("First coll check took " << (ros::WallTime::now() - wt)); num_collision_points_ = 0; for (const collision_detection::GradientInfo& gradient : gsr_->gradients_) @@ -615,7 +610,7 @@ void ChompOptimizer::calculateCollisionIncrements() // This is faster and guaranteed to converge, but it may take more iterations in the worst case. if (parameters_->use_stochastic_descent_) { - start_point = (int)(((double)random() / (double)RAND_MAX) * (free_vars_end_ - free_vars_start_) + free_vars_start_); + start_point = (int)(getRandomDouble() * (free_vars_end_ - free_vars_start_) + free_vars_start_); if (start_point < free_vars_start_) start_point = free_vars_start_; if (start_point > free_vars_end_) @@ -937,8 +932,7 @@ void ChompOptimizer::performForwardKinematics() setRobotStateFromPoint(group_trajectory_, i); ros::WallTime grad = ros::WallTime::now(); - hy_world_->getCollisionGradients(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), state_, nullptr, - gsr_); + hy_env_->getCollisionGradients(req, res, state_, nullptr, gsr_); total_dur += (ros::WallTime::now() - grad); computeJointProperties(i); state_is_in_collision_[i] = false; diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp index 1a46e85fb3..80986bf63c 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp @@ -56,8 +56,8 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s } // get the specified start state - robot_state::RobotState start_state = planning_scene->getCurrentState(); - robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); + moveit::core::RobotState start_state = planning_scene->getCurrentState(); + moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); if (!start_state.satisfiesBounds()) { @@ -85,7 +85,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s } const size_t goal_index = trajectory.getNumPoints() - 1; - robot_state::RobotState goal_state(start_state); + moveit::core::RobotState goal_state(start_state); for (const moveit_msgs::JointConstraint& joint_constraint : req.goal_constraints[0].joint_constraints) goal_state.setVariablePosition(joint_constraint.joint_name, joint_constraint.position); if (!goal_state.satisfiesBounds()) @@ -179,16 +179,16 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s return false; } - ROS_DEBUG_NAMED("chomp_planner", "Optimization took %f sec to create", - (ros::WallTime::now() - create_time).toSec()); + ROS_DEBUG_NAMED("chomp_planner", "Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec()); bool optimization_result = optimizer->optimize(); // replan with updated parameters if no solution is found if (params_nonconst.enable_failure_recovery_) { - ROS_INFO_NAMED("chomp_planner", "Planned with Chomp Parameters (learning_rate, ridge_factor, " - "planning_time_limit, max_iterations), attempt: # %d ", + ROS_INFO_NAMED("chomp_planner", + "Planned with Chomp Parameters (learning_rate, ridge_factor, " + "planning_time_limit, max_iterations), attempt: # %d ", (replan_count + 1)); ROS_INFO_NAMED("chomp_planner", "Learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ", params_nonconst.learning_rate_, params_nonconst.ridge_factor_, @@ -223,9 +223,9 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s for (size_t i = 0; i < trajectory.getNumPoints(); i++) { const Eigen::MatrixXd::RowXpr source = trajectory.getTrajectoryPoint(i); - auto state = std::make_shared(start_state); + auto state = std::make_shared(start_state); size_t joint_index = 0; - for (const robot_state::JointModel* jm : result->getGroup()->getActiveJointModels()) + for (const moveit::core::JointModel* jm : result->getGroup()->getActiveJointModels()) { assert(jm->getVariableCount() == 1); state->setVariablePosition(jm->getFirstVariableIndex(), source[joint_index++]); @@ -254,7 +254,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s // check that final state is within goal tolerances kinematic_constraints::JointConstraint jc(planning_scene->getRobotModel()); - const robot_state::RobotState& last_state = result->getLastWayPoint(); + const moveit::core::RobotState& last_state = result->getLastWayPoint(); for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints[0].joint_constraints) { if (!jc.configure(constraint) || !jc.decide(last_state).satisfied) diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp index 8580682a20..af2c9c9c04 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp @@ -36,7 +36,6 @@ #include #include -#include namespace chomp { @@ -60,8 +59,7 @@ ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_m init(); } -ChompTrajectory::ChompTrajectory(const ChompTrajectory& source_traj, const std::string& group_name, - int diff_rule_length) +ChompTrajectory::ChompTrajectory(const ChompTrajectory& source_traj, const std::string& group_name, int diff_rule_length) : planning_group_name_(group_name), discretization_(source_traj.discretization_) { num_joints_ = source_traj.getNumJoints(); @@ -155,7 +153,7 @@ void ChompTrajectory::fillInMinJerk() // calculate the spline coefficients for each joint: // (these are for the special case of zero start and end vel and acc) - double coeff[num_joints_][6]; + std::vector coeff(num_joints_); for (size_t i = 0; i < num_joints_; i++) { double x0 = (*this)(start_index, i); @@ -197,8 +195,8 @@ bool ChompTrajectory::fillInFromTrajectory(const robot_trajectory::RobotTrajecto const size_t max_output_index = getNumPoints() - 1; const size_t max_input_index = trajectory.getWayPointCount() - 1; - const robot_model::JointModelGroup* group = trajectory.getGroup(); - robot_state::RobotState interpolated(trajectory.getRobotModel()); + const moveit::core::JointModelGroup* group = trajectory.getGroup(); + moveit::core::RobotState interpolated(trajectory.getRobotModel()); for (size_t i = 0; i <= max_output_index; i++) { double fraction = static_cast(i * max_input_index) / max_output_index; @@ -211,14 +209,14 @@ bool ChompTrajectory::fillInFromTrajectory(const robot_trajectory::RobotTrajecto return true; } -void ChompTrajectory::assignCHOMPTrajectoryPointFromRobotState(const robot_state::RobotState& source, +void ChompTrajectory::assignCHOMPTrajectoryPointFromRobotState(const moveit::core::RobotState& source, size_t chomp_trajectory_point_index, - const robot_model::JointModelGroup* group) + const moveit::core::JointModelGroup* group) { Eigen::MatrixXd::RowXpr target = getTrajectoryPoint(chomp_trajectory_point_index); assert(group->getActiveJointModels().size() == static_cast(target.cols())); size_t joint_index = 0; - for (const robot_state::JointModel* jm : group->getActiveJointModels()) + for (const moveit::core::JointModel* jm : group->getActiveJointModels()) { assert(jm->getVariableCount() == 1); target[joint_index++] = source.getVariablePosition(jm->getFirstVariableIndex()); diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst index 5ac9064e2d..413398e078 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst @@ -2,6 +2,42 @@ Changelog for package moveit_chomp_optimizer_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + +1.1.0 (2020-09-04) +------------------ +* [feature] Optional cpp version setting (`#2166 `_) +* [feature] Allow ROS namespaces for planning request adapters (`#1530 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [fix] Fix Chomp planning adapter (`#1525 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Henning Kayser, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* Contributors: Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ +* [fix] Chomp planning adapter: Fix spurious error message Fix "Found empty JointState message" +* Contributors: Robert Haschke + 1.0.1 (2019-03-08) ------------------ * [fix] segfault in chomp adapter (`#1377 `_) diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt index f5a743686c..d17f10a72a 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_chomp_optimizer_adapter) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) find_package(catkin REQUIRED COMPONENTS moveit_core @@ -11,7 +15,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package() -include_directories(${catkin_INCLUDE_DIRS}) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) add_library(${PROJECT_NAME} src/chomp_optimizer_adapter.cpp) set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -23,5 +27,5 @@ install(FILES chomp_optimizer_adapter_plugin_description.xml install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml index f5a6d016a7..1920747a59 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml +++ b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml @@ -3,10 +3,10 @@ moveit_chomp_optimizer_adapter MoveIt planning request adapter utilizing chomp for solution optimization - 1.0.1 + 1.1.1 Raghavender Sahdev Raghavender Sahdev - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp index ec025a3114..5a9a9d5405 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp +++ b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp @@ -111,8 +111,7 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter if (!nh.getParam("smoothness_cost_jerk", params_.smoothness_cost_jerk_)) { params_.smoothness_cost_jerk_ = 0.0; - ROS_INFO_STREAM( - "Param smoothness_cost_jerk_ was not set. Using default value: " << params_.smoothness_cost_jerk_); + ROS_INFO_STREAM("Param smoothness_cost_jerk_ was not set. Using default value: " << params_.smoothness_cost_jerk_); } if (!nh.getParam("ridge_factor", params_.ridge_factor_)) { @@ -166,7 +165,7 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { // following call to planner() calls the OMPL planner and stores the trajectory inside the MotionPlanResponse res // variable which is then used by CHOMP for optimization of the computed trajectory diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst index 68168623d7..051d06bfc2 100644 --- a/moveit_planners/moveit_planners/CHANGELOG.rst +++ b/moveit_planners/moveit_planners/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package moveit_planners ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + +1.1.0 (2020-09-04) +------------------ + +1.0.6 (2020-08-19) +------------------ + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ diff --git a/moveit_planners/moveit_planners/CMakeLists.txt b/moveit_planners/moveit_planners/CMakeLists.txt index e6e2b34540..753bb7b4d9 100644 --- a/moveit_planners/moveit_planners/CMakeLists.txt +++ b/moveit_planners/moveit_planners/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_planners) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index 77d65918bf..b01309b3f2 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -1,12 +1,12 @@ moveit_planners - 1.0.1 + 1.1.1 Metapacakge that installs all available planners for MoveIt Ioan Sucan Sachin Chitta Dave Coleman - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst index 06a4259f1c..5bf99c74e5 100644 --- a/moveit_planners/ompl/CHANGELOG.rst +++ b/moveit_planners/ompl/CHANGELOG.rst @@ -2,6 +2,43 @@ Changelog for package moveit_planners_ompl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + +1.1.0 (2020-09-04) +------------------ + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* [fix] Fix memcpy bug in copyJointToOMPLState in ompl interface (`#2239 `_) +* Contributors: Jeroen, Markus Vieth, Robert Haschke, Michael GΓΆrner + +1.0.5 (2020-07-08) +------------------ +* [feature] Added support for hybridize/interpolate flags in ModelBasedPlanningContext via ompl_planning.yaml (`#2171 `_, `#2172 `_) +* Contributors: Constantinos, Mark Moll + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Cleanup OMPL dynamic reconfigure config (`#1649 `_) + * Reduce minimum number of waypoints in solution to 2 +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Michael GΓΆrner, Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -79,7 +116,7 @@ Changelog for package moveit_planners_ompl 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * Contributors: Dave Coleman 0.9.4 (2017-02-06) @@ -117,7 +154,7 @@ Changelog for package moveit_planners_ompl * renamed newGoal to new_goal for keeping with formatting * setting GroupStateValidityCallbackFn member for constraint_sampler member and implementing callbacks for state validity checking * added functions to check validit of state, and also to act as callback for constraint sampler -* Added copy function from MoveIt! robot_state joint values to ompl state +* Added copy function from MoveIt robot_state joint values to ompl state * fix for demo constraints database linking error * Namespaced less useful debug output to allow to be easily silenced using ros console * Contributors: Dave Coleman, Dave Hershberger, Sachin Chitta, arjungm diff --git a/moveit_planners/ompl/CMakeLists.txt b/moveit_planners/ompl/CMakeLists.txt index e68728812f..16c3c8a544 100644 --- a/moveit_planners/ompl/CMakeLists.txt +++ b/moveit_planners/ompl/CMakeLists.txt @@ -1,8 +1,12 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_planners_ompl) # At least C++11 required for OMPL -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -29,14 +33,17 @@ catkin_package( INCLUDE_DIRS ompl_interface/include CATKIN_DEPENDS + dynamic_reconfigure moveit_core + roscpp DEPENDS OMPL ) -include_directories(ompl_interface/include - ${Boost_INCLUDE_DIRS} +include_directories(ompl_interface/include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${OMPL_INCLUDE_DIRS}) add_subdirectory(ompl_interface) diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index cc9181efee..b8092d65b6 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -3,7 +3,6 @@ set(MOVEIT_LIB_NAME moveit_ompl_interface) add_library(${MOVEIT_LIB_NAME} src/ompl_interface.cpp src/planning_context_manager.cpp - src/constraints_library.cpp src/model_based_planning_context.cpp src/parameterization/model_based_state_space.cpp src/parameterization/model_based_state_space_factory.cpp @@ -15,6 +14,7 @@ add_library(${MOVEIT_LIB_NAME} src/detail/state_validity_checker.cpp src/detail/projection_evaluators.cpp src/detail/goal_union.cpp + src/detail/constraints_library.cpp src/detail/constrained_sampler.cpp src/detail/constrained_valid_state_sampler.cpp src/detail/constrained_goal_sampler.cpp @@ -27,11 +27,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES} $ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") -add_executable(moveit_ompl_planner src/ompl_planner.cpp) -target_link_libraries(moveit_ompl_planner ${MOVEIT_LIB_NAME}) -set_target_properties(moveit_ompl_planner PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") - -add_executable(moveit_generate_state_database src/generate_state_database.cpp) +add_executable(moveit_generate_state_database scripts/generate_state_database.cpp) target_link_libraries(moveit_generate_state_database ${MOVEIT_LIB_NAME}) set_target_properties(moveit_generate_state_database PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") set_target_properties(moveit_generate_state_database PROPERTIES OUTPUT_NAME "generate_state_database") @@ -40,16 +36,28 @@ add_library(moveit_ompl_planner_plugin src/ompl_planner_manager.cpp) set_target_properties(moveit_ompl_planner_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(moveit_ompl_planner_plugin ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} moveit_ompl_planner moveit_ompl_planner_plugin moveit_generate_state_database +install(TARGETS ${MOVEIT_LIB_NAME} moveit_ompl_planner_plugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) +install(TARGETS moveit_generate_state_database RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_state_space test/test_state_space.cpp) - target_link_libraries(test_state_space ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + target_link_libraries(test_state_space ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES}) set_target_properties(test_state_space PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + + find_package(rostest REQUIRED) + find_package(eigen_conversions REQUIRED) + + add_rostest_gtest(test_planning_context_manager + test/test_planning_context_manager.test + test/test_planning_context_manager.cpp) + target_link_libraries(test_planning_context_manager ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) + + catkin_add_gtest(test_state_validity_checker test/test_state_validity_checker.cpp) + target_link_libraries(test_state_validity_checker ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES}) + set_target_properties(test_state_validity_checker PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") endif() diff --git a/moveit_planners/ompl/ompl_interface/cfg/OMPLDynamicReconfigure.cfg b/moveit_planners/ompl/ompl_interface/cfg/OMPLDynamicReconfigure.cfg index 50757b73df..e9db604e9d 100755 --- a/moveit_planners/ompl/ompl_interface/cfg/OMPLDynamicReconfigure.cfg +++ b/moveit_planners/ompl/ompl_interface/cfg/OMPLDynamicReconfigure.cfg @@ -4,10 +4,10 @@ PACKAGE = "moveit_planners_ompl" from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("simplify_solutions", bool_t, 1, "Flag indicating whether computed motion plans are also simplified", True) -gen.add("minimum_waypoint_count", int_t, 2, "Set the minimum number of waypoints to include in a motion plan", 10, 2, 10000) -gen.add("maximum_waypoint_distance", double_t, 3, "The maximum distance between consecutive waypoints along the solution path (0.0 means 'ignore')", 0.0, 0.0, 50.0) -gen.add("link_for_exploration_tree", str_t, 4, "Show the exploration tree for a particular link", "") -gen.add("display_random_valid_states", bool_t, 5, "Flag indicating whether random valid states are to be published", False) +gen.add("simplify_solutions", bool_t, 0, "Flag indicating whether computed motion plans are also simplified", True) +gen.add("minimum_waypoint_count", int_t, 0, "Set the minimum number of waypoints to include in a motion plan", 2, 2, 10000) +gen.add("maximum_waypoint_distance", double_t, 0, "The maximum distance between consecutive waypoints along the solution path (0.0 means 'ignore')", 0.0, 0.0, 50.0) +gen.add("link_for_exploration_tree", str_t, 0, "Show the exploration tree for a particular link", "") +gen.add("display_random_valid_states", bool_t, 0, "Flag indicating whether random valid states are to be published", False) exit(gen.generate(PACKAGE, PACKAGE, "OMPLDynamicReconfigure")) diff --git a/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h b/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h index fba9e700d3..ce7a56b432 100644 --- a/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h +++ b/moveit_planners/ompl/ompl_interface/cfg/cpp/ompl_interface_ros/OMPLDynamicReconfigureConfig.h @@ -43,8 +43,7 @@ // Author: Blaise Gassend -#ifndef __ompl_interface_ros__OMPLDYNAMICRECONFIGURECONFIG_H__ -#define __ompl_interface_ros__OMPLDYNAMICRECONFIGURECONFIG_H__ +#pragma once #include #include @@ -63,7 +62,7 @@ class OMPLDynamicReconfigureConfigStatics; class OMPLDynamicReconfigureConfig { public: - MOVEIT_CLASS_FORWARD(AbstractParamDescription); + MOVEIT_CLASS_FORWARD(AbstractParamDescription); // Defines AbstractParamDescriptionPtr, ConstPtr, WeakPtr... etc class AbstractParamDescription : public dynamic_reconfigure::ParamDescription { public: @@ -142,7 +141,7 @@ class OMPLDynamicReconfigureConfig } }; - MOVEIT_CLASS_FORWARD(AbstractGroupDescription); + MOVEIT_CLASS_FORWARD(AbstractGroupDescription); // Defines AbstractGroupDescriptionPtr, ConstPtr, WeakPtr... etc class AbstractGroupDescription : public dynamic_reconfigure::Group { public: @@ -531,6 +530,4 @@ inline const OMPLDynamicReconfigureConfigStatics* OMPLDynamicReconfigureConfig:: return statics; } -} - -#endif // __OMPLDYNAMICRECONFIGURERECONFIGURATOR_H__ +} // namespace ompl_interface_ros diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h index 4eb0b16ec2..8dea8329f3 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINED_GOAL_SAMPLER_ -#define MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINED_GOAL_SAMPLER_ +#pragma once #include #include @@ -58,20 +57,19 @@ class ConstrainedGoalSampler : public ompl::base::GoalLazySamples private: bool sampleUsingConstraintSampler(const ompl::base::GoalLazySamples* gls, ompl::base::State* new_goal); - bool stateValidityCallback(ompl::base::State* new_goal, robot_state::RobotState const* state, - const robot_model::JointModelGroup*, const double*, bool verbose = false) const; - bool checkStateValidity(ompl::base::State* new_goal, const robot_state::RobotState& state, + bool stateValidityCallback(ompl::base::State* new_goal, moveit::core::RobotState const* state, + const moveit::core::JointModelGroup* /*jmg*/, const double* /*jpos*/, + bool verbose = false) const; + bool checkStateValidity(ompl::base::State* new_goal, const moveit::core::RobotState& state, bool verbose = false) const; const ModelBasedPlanningContext* planning_context_; kinematic_constraints::KinematicConstraintSetPtr kinematic_constraint_set_; constraint_samplers::ConstraintSamplerPtr constraint_sampler_; ompl::base::StateSamplerPtr default_sampler_; - robot_state::RobotState work_state_; + moveit::core::RobotState work_state_; unsigned int invalid_sampled_constraints_; bool warned_invalid_samples_; unsigned int verbose_display_; }; -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h index a0554bfbf7..d0f1d4951d 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINED_SAMPLER_ -#define MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINED_SAMPLER_ +#pragma once #include #include @@ -72,11 +71,9 @@ class ConstrainedSampler : public ompl::base::StateSampler const ModelBasedPlanningContext* planning_context_; ompl::base::StateSamplerPtr default_; constraint_samplers::ConstraintSamplerPtr constraint_sampler_; - robot_state::RobotState work_state_; + moveit::core::RobotState work_state_; unsigned int constrained_success_; unsigned int constrained_failure_; double inv_dim_; }; -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h index 7a504f7fbe..8dbcd93a5e 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_valid_state_sampler.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINED_VALID_STATE_SAMPLER_ -#define MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINED_VALID_STATE_SAMPLER_ +#pragma once #include #include @@ -46,7 +45,7 @@ namespace ompl_interface { class ModelBasedPlanningContext; -MOVEIT_CLASS_FORWARD(ValidStateSampler); +MOVEIT_CLASS_FORWARD(ValidStateSampler); // Defines ValidStateSamplerPtr, ConstPtr, WeakPtr... etc /** @class ValidConstrainedSampler * This class defines a sampler that tries to find a valid sample that satisfies the specified constraints */ @@ -65,10 +64,8 @@ class ValidConstrainedSampler : public ompl::base::ValidStateSampler kinematic_constraints::KinematicConstraintSetPtr kinematic_constraint_set_; constraint_samplers::ConstraintSamplerPtr constraint_sampler_; ompl::base::StateSamplerPtr default_sampler_; - robot_state::RobotState work_state_; + moveit::core::RobotState work_state_; double inv_dim_; ompl::RNG rng_; }; -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h index 5636257e46..132e45b778 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINT_APPROXIMATION_ -#define MOVEIT_OMPL_INTERFACE_DETAIL_CONSTRAINT_APPROXIMATION_ +#pragma once #include #include @@ -74,6 +73,4 @@ struct ConstraintApproximation }; MOVEIT_DECLARE_PTR(ConstraintApproximations, std::vector) -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/constraints_library.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h similarity index 63% rename from moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/constraints_library.h rename to moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h index 7615ad1561..2b4c8c759c 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/constraints_library.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h @@ -1,44 +1,43 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_CONSTRAINTS_LIBRARY_ -#define MOVEIT_OMPL_INTERFACE_CONSTRAINTS_LIBRARY_ +#pragma once #include -#include +#include #include #include #include @@ -156,12 +155,12 @@ struct ConstraintApproximationConstructionResults double sampling_success_rate; }; -MOVEIT_CLASS_FORWARD(ConstraintsLibrary); +MOVEIT_CLASS_FORWARD(ConstraintsLibrary); // Defines ConstraintsLibraryPtr, ConstPtr, WeakPtr... etc class ConstraintsLibrary { public: - ConstraintsLibrary(const PlanningContextManager& pcontext) : context_manager_(pcontext) + ConstraintsLibrary(ModelBasedPlanningContext* pcontext) : context_(pcontext) { } @@ -191,14 +190,13 @@ class ConstraintsLibrary const ConstraintApproximationPtr& getConstraintApproximation(const moveit_msgs::Constraints& msg) const; private: - ompl::base::StateStoragePtr constructConstraintApproximation( - const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling, - const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options, - ConstraintApproximationConstructionResults& result); + ompl::base::StateStoragePtr + constructConstraintApproximation(ModelBasedPlanningContext* pcontext, const moveit_msgs::Constraints& constr_sampling, + const moveit_msgs::Constraints& constr_hard, + const ConstraintApproximationConstructionOptions& options, + ConstraintApproximationConstructionResults& result); - const PlanningContextManager& context_manager_; + ModelBasedPlanningContext* context_; std::map constraint_approximations_; }; -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h index 00cf1d830f..62f1529c35 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -81,4 +81,4 @@ class GoalSampleableRegionMux : public ompl::base::GoalSampleableRegion std::vector goals_; mutable unsigned int gindex_; }; -} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h index 8fd837ad9d..aad7b06f76 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h @@ -1,46 +1,46 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DETAIL_PROJECTION_EVALUATORS_ -#define MOVEIT_OMPL_INTERFACE_DETAIL_PROJECTION_EVALUATORS_ +#pragma once #include #include #include +// TODO: remove when ROS Lunar and older are no longer supported #if OMPL_VERSION_VALUE >= 1004000 // Version greater than 1.4.0 typedef Eigen::Ref OMPLProjection; #else // All other versions @@ -64,7 +64,7 @@ class ProjectionEvaluatorLinkPose : public ompl::base::ProjectionEvaluator private: const ModelBasedPlanningContext* planning_context_; - const robot_model::LinkModel* link_; + const moveit::core::LinkModel* link_; TSStateStorage tss_; }; @@ -82,6 +82,4 @@ class ProjectionEvaluatorJointValue : public ompl::base::ProjectionEvaluator private: std::vector variables_; }; -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h index c485879007..54259e9d8e 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DETAIL_STATE_VALIDITY_CHECKER_ -#define MOVEIT_OMPL_INTERFACE_DETAIL_STATE_VALIDITY_CHECKER_ +#pragma once #include #include @@ -71,12 +70,6 @@ class StateValidityChecker : public ompl::base::StateValidityChecker void setVerbose(bool flag); protected: - bool isValidWithoutCache(const ompl::base::State* state, bool verbose) const; - bool isValidWithoutCache(const ompl::base::State* state, double& dist, bool verbose) const; - - bool isValidWithCache(const ompl::base::State* state, bool verbose) const; - bool isValidWithCache(const ompl::base::State* state, double& dist, bool verbose) const; - const ModelBasedPlanningContext* planning_context_; std::string group_name_; TSStateStorage tss_; @@ -88,6 +81,4 @@ class StateValidityChecker : public ompl::base::StateValidityChecker collision_detection::CollisionRequest collision_request_with_cost_; bool verbose_; }; -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h index a19b69bfda..ab6d38fc66 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_DEATIL_THREADSAFE_STATE_STORAGE_ -#define MOVEIT_OMPL_INTERFACE_DEATIL_THREADSAFE_STATE_STORAGE_ +#pragma once #include #include @@ -46,16 +45,15 @@ namespace ompl_interface class TSStateStorage { public: - TSStateStorage(const robot_model::RobotModelPtr& robot_model); - TSStateStorage(const robot_state::RobotState& start_state); + TSStateStorage(const moveit::core::RobotModelPtr& robot_model); + TSStateStorage(const moveit::core::RobotState& start_state); ~TSStateStorage(); - robot_state::RobotState* getStateStorage() const; + moveit::core::RobotState* getStateStorage() const; private: - robot_state::RobotState start_state_; - mutable std::map thread_states_; + moveit::core::RobotState start_state_; + mutable std::map thread_states_; mutable std::mutex lock_; }; -} -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index 90f5034502..288439e6e9 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_MODEL_BASED_PLANNING_CONTEXT_ -#define MOVEIT_OMPL_INTERFACE_MODEL_BASED_PLANNING_CONTEXT_ +#pragma once #include #include @@ -53,8 +52,8 @@ namespace ob = ompl::base; namespace og = ompl::geometric; namespace ot = ompl::tools; -MOVEIT_CLASS_FORWARD(ModelBasedPlanningContext); -MOVEIT_CLASS_FORWARD(ConstraintsLibrary); +MOVEIT_CLASS_FORWARD(ModelBasedPlanningContext); // Defines ModelBasedPlanningContextPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(ConstraintsLibrary); // Defines ConstraintsLibraryPtr, ConstPtr, WeakPtr... etc struct ModelBasedPlanningContextSpecification; typedef std::function config_; ConfiguredPlannerSelector planner_selector_; - ConstraintsLibraryConstPtr constraints_library_; constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; ModelBasedStateSpacePtr state_space_; - std::vector subspaces_; og::SimpleSetupPtr ompl_simple_setup_; // pass in the correct simple setup type }; @@ -104,17 +101,17 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext spec_.config_ = config; } - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return spec_.state_space_->getRobotModel(); } - const robot_model::JointModelGroup* getJointModelGroup() const + const moveit::core::JointModelGroup* getJointModelGroup() const { return spec_.state_space_->getJointModelGroup(); } - const robot_state::RobotState& getCompleteInitialRobotState() const + const moveit::core::RobotState& getCompleteInitialRobotState() const { return complete_initial_robot_state_; } @@ -236,25 +233,25 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext void setPlanningVolume(const moveit_msgs::WorkspaceParameters& wparams); - void setCompleteInitialState(const robot_state::RobotState& complete_initial_robot_state); + void setCompleteInitialState(const moveit::core::RobotState& complete_initial_robot_state); bool setGoalConstraints(const std::vector& goal_constraints, const moveit_msgs::Constraints& path_constraints, moveit_msgs::MoveItErrorCodes* error); bool setPathConstraints(const moveit_msgs::Constraints& path_constraints, moveit_msgs::MoveItErrorCodes* error); - void setConstraintsApproximations(const ConstraintsLibraryConstPtr& constraints_library) + void setConstraintsApproximations(const ConstraintsLibraryPtr& constraints_library) { - spec_.constraints_library_ = constraints_library; + constraints_library_ = constraints_library; } - bool useStateValidityCache() const + ConstraintsLibraryPtr getConstraintsLibraryNonConst() { - return use_state_validity_cache_; + return constraints_library_; } - void useStateValidityCache(bool flag) + const ConstraintsLibraryPtr& getConstraintsLibrary() const { - use_state_validity_cache_ = flag; + return constraints_library_; } bool simplifySolutions() const @@ -267,13 +264,23 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext simplify_solutions_ = flag; } + void setInterpolation(bool flag) + { + interpolate_ = flag; + } + + void setHybridize(bool flag) + { + hybridize_ = flag; + } + /* @brief Solve the planning problem. Return true if the problem is solved @param timeout The time to spend on solving @param count The number of runs to combine the paths of, in an attempt to generate better quality paths */ bool solve(double timeout, unsigned int count); - /* @brief Benchmark the planning problem. Return true on succesful saving of benchmark results + /* @brief Benchmark the planning problem. Return true on successful saving of benchmark results @param timeout The time to spend on solving @param count The number of runs to average in the computation of the benchmark @param filename The name of the file to which the benchmark results are to be saved (automatic names can be @@ -305,7 +312,22 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext void convertPath(const og::PathGeometric& pg, robot_trajectory::RobotTrajectory& traj) const; - virtual void configure(); + /** @brief Look up param server 'constraint_approximations' and use its value as the path to load constraint + * approximations to */ + bool loadConstraintApproximations(const ros::NodeHandle& nh); + + /** @brief Look up param server 'constraint_approximations' and use its value as the path to save constraint + * approximations to */ + bool saveConstraintApproximations(const ros::NodeHandle& nh); + + /** \brief Configure ompl_simple_setup_ and optionally the constraints_library_. + * + * ompl_simple_setup_ gets a start state, state sampler, and state validity checker. + * + * \param nh ROS node handle used to load the constraint approximations. + * \param use_constraints_approximations Set to true if we want to load the constraint approximation. + * */ + virtual void configure(const ros::NodeHandle& nh, bool use_constraints_approximations); protected: void preSolve(); @@ -319,12 +341,40 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext virtual void useConfig(); virtual ob::GoalPtr constructGoal(); + /* @brief Construct a planner termination condition, by default a simple time limit + @param timeout The maximum time (in seconds) that can be used for planning + @param start The point in time from which planning is considered to have started + + An additional planner termination condition can be specified per planner + configuration in ompl_planning.yaml via the `termination_condition` parameter. + Possible values are: + + * `Iteration[num]`: Terminate after `num` iterations. Here, `num` should be replaced + with a positive integer. + * `CostConvergence[solutions_window,epsilon]`: Terminate after the cost (as specified + by an optimization objective) has converged. The parameter `solutions_window` + specifies the minimum number of solutions to use in deciding whether a planner has + converged. The parameter `epsilon` is the threshold to consider for convergence. + This should be a positive number close to 0. If the cumulative moving average does + not change by a relative fraction of epsilon after a new better solution is found, + convergence has been reached. + * `ExactSolution`: Terminate as soon as an exact solution is found or a timeout + occurs. This modifies the behavior of anytime/optimizing planners to terminate + upon discovering the first feasible solution. + + In all cases, the planner will terminate when either the user-specified termination + condition is satisfied or the time limit specified by `timeout` has been reached, + whichever occurs first. + */ + virtual ob::PlannerTerminationCondition constructPlannerTerminationCondition(double timeout, + const ompl::time::point& start); + void registerTerminationCondition(const ob::PlannerTerminationCondition& ptc); void unregisterTerminationCondition(); ModelBasedPlanningContextSpecification spec_; - robot_state::RobotState complete_initial_robot_state_; + moveit::core::RobotState complete_initial_robot_state_; /// the OMPL planning context; this contains the problem definition and the planner used og::SimpleSetupPtr ompl_simple_setup_; @@ -372,10 +422,17 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext /// needed) unsigned int minimum_waypoint_count_; - bool use_state_validity_cache_; + /// when false, clears planners before running solve() + bool multi_query_planning_enabled_; + + ConstraintsLibraryPtr constraints_library_; bool simplify_solutions_; + + // if false the final solution is not interpolated + bool interpolate_; + + // if false parallel plan returns the first solution found + bool hybridize_; }; } // namespace ompl_interface - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h index ef25ce8452..0bbfb4a562 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h @@ -1,44 +1,42 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_OMPL_INTERFACE_ -#define MOVEIT_OMPL_INTERFACE_OMPL_INTERFACE_ +#pragma once #include -#include #include #include #include @@ -48,7 +46,7 @@ #include #include -/** \brief The MoveIt! interface to OMPL */ +/** \brief The MoveIt interface to OMPL */ namespace ompl_interface { /** @class OMPLInterface @@ -58,13 +56,13 @@ class OMPLInterface public: /** \brief Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the specified * NodeHandle */ - OMPLInterface(const robot_model::RobotModelConstPtr& robot_model, const ros::NodeHandle& nh = ros::NodeHandle("~")); + OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const ros::NodeHandle& nh = ros::NodeHandle("~")); /** \brief Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the specified NodeHandle. However, planner configurations are used as specified in \e pconfig instead of reading them from the ROS parameter server */ - OMPLInterface(const robot_model::RobotModelConstPtr& robot_model, + OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const planning_interface::PlannerConfigurationMap& pconfig, const ros::NodeHandle& nh = ros::NodeHandle("~")); @@ -87,9 +85,6 @@ class OMPLInterface const planning_interface::MotionPlanRequest& req, moveit_msgs::MoveItErrorCodes& error_code) const; - ModelBasedPlanningContextPtr getPlanningContext(const std::string& config, - const std::string& factory_type = "") const; - const PlanningContextManager& getPlanningContextManager() const { return context_manager_; @@ -100,16 +95,6 @@ class OMPLInterface return context_manager_; } - ConstraintsLibrary& getConstraintsLibrary() - { - return *constraints_library_; - } - - const ConstraintsLibrary& getConstraintsLibrary() const - { - return *constraints_library_; - } - constraint_samplers::ConstraintSamplerManager& getConstraintSamplerManager() { return *constraint_sampler_manager_; @@ -129,11 +114,6 @@ class OMPLInterface { return use_constraints_approximations_; } - - void loadConstraintApproximations(const std::string& path); - - void saveConstraintApproximations(const std::string& path); - bool simplifySolutions() const { return simplify_solutions_; @@ -144,14 +124,6 @@ class OMPLInterface simplify_solutions_ = flag; } - /** @brief Look up param server 'constraint_approximations' and use its value as the path to save constraint - * approximations to */ - bool saveConstraintApproximations(); - - /** @brief Look up param server 'constraint_approximations' and use its value as the path to load constraint - * approximations to */ - bool loadConstraintApproximations(); - /** @brief Print the status of this node*/ void printStatus(); @@ -178,13 +150,12 @@ class OMPLInterface ros::NodeHandle nh_; /// The ROS node handle /** \brief The kinematic model for which motion plans are computed */ - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; PlanningContextManager context_manager_; - ConstraintsLibraryPtr constraints_library_; bool use_constraints_approximations_; bool simplify_solutions_; @@ -192,6 +163,4 @@ class OMPLInterface private: constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_; }; -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h index 6b4dbb5229..91d8867a10 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_JOINT_SPACE_JOINT_MODEL_STATE_SPACE_ -#define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_JOINT_SPACE_JOINT_MODEL_STATE_SPACE_ +#pragma once #include @@ -47,7 +46,10 @@ class JointModelStateSpace : public ModelBasedStateSpace static const std::string PARAMETERIZATION_TYPE; JointModelStateSpace(const ModelBasedStateSpaceSpecification& spec); -}; -} -#endif + const std::string& getParameterizationType() const override + { + return PARAMETERIZATION_TYPE; + } +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h index 5ff8e4dab2..d0e3830294 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_JOINT_SPACE_JOINT_MODEL_STATE_SPACE_FACTORY_ -#define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_JOINT_SPACE_JOINT_MODEL_STATE_SPACE_FACTORY_ +#pragma once #include @@ -47,11 +46,9 @@ class JointModelStateSpaceFactory : public ModelBasedStateSpaceFactory JointModelStateSpaceFactory(); int canRepresentProblem(const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const robot_model::RobotModelConstPtr& robot_model) const override; + const moveit::core::RobotModelConstPtr& robot_model) const override; protected: ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; }; -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h index 3ef5246888..e334322a98 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_MODEL_BASED_STATE_SPACE_ -#define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_MODEL_BASED_STATE_SPACE_ +#pragma once #include #include @@ -52,22 +51,22 @@ typedef std::functiongetJointModelGroup(group_name)) { if (!joint_model_group_) throw std::runtime_error("Group '" + group_name + "' was not found"); } - robot_model::RobotModelConstPtr robot_model_; - const robot_model::JointModelGroup* joint_model_group_; - robot_model::JointBoundsVector joint_bounds_; + moveit::core::RobotModelConstPtr robot_model_; + const moveit::core::JointModelGroup* joint_model_group_; + moveit::core::JointBoundsVector joint_bounds_; }; OMPL_CLASS_FORWARD(ModelBasedStateSpace); @@ -87,7 +86,7 @@ class ModelBasedStateSpace : public ompl::base::StateSpace IS_GOAL_STATE = 16 }; - StateType() : ompl::base::State(), values(NULL), tag(-1), flags(0), distance(0.0) + StateType() : ompl::base::State(), values(nullptr), tag(-1), flags(0), distance(0.0) { } @@ -201,12 +200,14 @@ class ModelBasedStateSpace : public ompl::base::StateSpace ompl::base::StateSamplerPtr allocDefaultStateSampler() const override; - const robot_model::RobotModelConstPtr& getRobotModel() const + virtual const std::string& getParameterizationType() const = 0; + + const moveit::core::RobotModelConstPtr& getRobotModel() const { return spec_.robot_model_; } - const robot_model::JointModelGroup* getJointModelGroup() const + const moveit::core::JointModelGroup* getJointModelGroup() const { return spec_.joint_model_group_; } @@ -227,30 +228,30 @@ class ModelBasedStateSpace : public ompl::base::StateSpace /// Set the planning volume for the possible SE2 and/or SE3 components of the state space virtual void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ); - const robot_model::JointBoundsVector& getJointsBounds() const + const moveit::core::JointBoundsVector& getJointsBounds() const { return spec_.joint_bounds_; } /// Copy the data from an OMPL state to a set of joint states. // The joint states \b must be specified in the same order as the joint models in the constructor - virtual void copyToRobotState(robot_state::RobotState& rstate, const ompl::base::State* state) const; + virtual void copyToRobotState(moveit::core::RobotState& rstate, const ompl::base::State* state) const; /// Copy the data from a set of joint states to an OMPL state. // The joint states \b must be specified in the same order as the joint models in the constructor - virtual void copyToOMPLState(ompl::base::State* state, const robot_state::RobotState& rstate) const; + virtual void copyToOMPLState(ompl::base::State* state, const moveit::core::RobotState& rstate) const; /** - * \brief Copy a single joint's values (which might have multiple variables) from a MoveIt! robot_state to an OMPL + * \brief Copy a single joint's values (which might have multiple variables) from a MoveIt robot_state to an OMPL * state. * \param state - output OMPL state with single joint modified - * \param robot_state - input MoveIt! state to get the joint value from + * \param robot_state - input MoveIt state to get the joint value from * \param joint_model - the joint to copy values of * \param ompl_state_joint_index - the index of the joint in the ompl state (passed in for efficiency, you should * cache this index) * e.g. ompl_state_joint_index = joint_model_group_->getVariableGroupIndex("virtual_joint"); */ - virtual void copyJointToOMPLState(ompl::base::State* state, const robot_state::RobotState& robot_state, + virtual void copyJointToOMPLState(ompl::base::State* state, const moveit::core::RobotState& robot_state, const moveit::core::JointModel* joint_model, int ompl_state_joint_index) const; double getTagSnapToSegment() const; @@ -258,8 +259,8 @@ class ModelBasedStateSpace : public ompl::base::StateSpace protected: ModelBasedStateSpaceSpecification spec_; - std::vector joint_bounds_storage_; - std::vector joint_model_vector_; + std::vector joint_bounds_storage_; + std::vector joint_model_vector_; unsigned int variable_count_; size_t state_values_size_; @@ -270,5 +271,3 @@ class ModelBasedStateSpace : public ompl::base::StateSpace double tag_snap_to_segment_complement_; }; } // namespace ompl_interface - -#endif diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h index f369b52588..e511f7590c 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_MODEL_BASED_STATE_SPACE_FACTORY_ -#define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_MODEL_BASED_STATE_SPACE_FACTORY_ +#pragma once #include #include @@ -43,7 +42,7 @@ namespace ompl_interface { -MOVEIT_CLASS_FORWARD(ModelBasedStateSpaceFactory); +MOVEIT_CLASS_FORWARD(ModelBasedStateSpaceFactory); // Defines ModelBasedStateSpaceFactoryPtr, ConstPtr, WeakPtr... etc class ModelBasedStateSpaceFactory { @@ -68,12 +67,10 @@ class ModelBasedStateSpaceFactory request \e req for group \e group. The group \e group must always be specified and takes precedence over \e req.group_name, which may be different */ virtual int canRepresentProblem(const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const robot_model::RobotModelConstPtr& robot_model) const = 0; + const moveit::core::RobotModelConstPtr& robot_model) const = 0; protected: virtual ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const = 0; std::string type_; }; -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h index 9f6fbaf9ee..5adb82bb44 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_WORK_SPACE_POSE_MODEL_STATE_SPACE_ -#define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_WORK_SPACE_POSE_MODEL_STATE_SPACE_ +#pragma once #include #include @@ -56,7 +55,7 @@ class PoseModelStateSpace : public ModelBasedStateSpace POSE_COMPUTED = 512 }; - StateType() : ModelBasedStateSpace::StateType(), poses(NULL) + StateType() : ModelBasedStateSpace::StateType(), poses(nullptr) { flags |= JOINTS_COMPUTED; } @@ -108,14 +107,19 @@ class PoseModelStateSpace : public ModelBasedStateSpace bool computeStateK(ompl::base::State* state) const; void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ) override; - void copyToOMPLState(ompl::base::State* state, const robot_state::RobotState& rstate) const override; + void copyToOMPLState(ompl::base::State* state, const moveit::core::RobotState& rstate) const override; void sanityChecks() const override; + const std::string& getParameterizationType() const override + { + return PARAMETERIZATION_TYPE; + } + private: struct PoseComponent { - PoseComponent(const robot_model::JointModelGroup* subgroup, - const robot_model::JointModelGroup::KinematicsSolver& k); + PoseComponent(const moveit::core::JointModelGroup* subgroup, + const moveit::core::JointModelGroup::KinematicsSolver& k); bool computeStateFK(StateType* full_state, unsigned int idx) const; bool computeStateIK(StateType* full_state, unsigned int idx) const; @@ -125,7 +129,7 @@ class PoseModelStateSpace : public ModelBasedStateSpace return subgroup_->getName() < o.subgroup_->getName(); } - const robot_model::JointModelGroup* subgroup_; + const moveit::core::JointModelGroup* subgroup_; kinematics::KinematicsBasePtr kinematics_solver_; std::vector bijection_; ompl::base::StateSpacePtr state_space_; @@ -135,6 +139,4 @@ class PoseModelStateSpace : public ModelBasedStateSpace std::vector poses_; double jump_factor_; }; -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h index fc6d616d11..82b5f48ee6 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_WORK_SPACE_POSE_MODEL_STATE_SPACE_FACTORY_ -#define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_WORK_SPACE_POSE_MODEL_STATE_SPACE_FACTORY_ +#pragma once #include @@ -47,11 +46,9 @@ class PoseModelStateSpaceFactory : public ModelBasedStateSpaceFactory PoseModelStateSpaceFactory(); int canRepresentProblem(const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const robot_model::RobotModelConstPtr& robot_model) const override; + const moveit::core::RobotModelConstPtr& robot_model) const override; protected: ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; }; -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index 4d08768b4b..3912b8a3a3 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -1,57 +1,85 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OMPL_INTERFACE_PLANNING_CONTEXT_MANAGER_ -#define MOVEIT_OMPL_INTERFACE_PLANNING_CONTEXT_MANAGER_ +#pragma once #include #include #include #include -#include +#include + #include #include namespace ompl_interface { +class MultiQueryPlannerAllocator +{ +public: + MultiQueryPlannerAllocator() = default; + ~MultiQueryPlannerAllocator(); + + template + ob::PlannerPtr allocatePlanner(const ob::SpaceInformationPtr& si, const std::string& new_name, + const ModelBasedPlanningContextSpecification& spec); + +private: + template + ob::PlannerPtr allocatePlannerImpl(const ob::SpaceInformationPtr& si, const std::string& new_name, + const ModelBasedPlanningContextSpecification& spec, bool load_planner_data = false, + bool store_planner_data = false, const std::string& file_path = ""); + + template + inline ob::Planner* allocatePersistentPlanner(const ob::PlannerData& data); + + // Storing multi-query planners + std::map planners_; + + std::map planner_data_storage_paths_; + + // Store and load planner data + ob::PlannerDataStorage storage_; +}; + class PlanningContextManager { public: - PlanningContextManager(robot_model::RobotModelConstPtr robot_model, + PlanningContextManager(moveit::core::RobotModelConstPtr robot_model, constraint_samplers::ConstraintSamplerManagerPtr csm); ~PlanningContextManager(); @@ -136,17 +164,24 @@ class PlanningContextManager minimum_waypoint_count_ = mwc; } - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } - ModelBasedPlanningContextPtr getPlanningContext(const std::string& config, - const std::string& factory_type = "") const; - + /** \brief Returns a planning context to OMPLInterface, which in turn passes it to OMPLPlannerManager. + * + * This function checks the input and reads planner specific configurations. + * Then it creates the planning context with PlanningContextManager::createPlanningContext. + * Finally, it puts the context into a state appropriate for planning. + * This last step involves setting the start, goal, and state validity checker using the method + * ModelBasedPlanningContext::configure. + * + * */ ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, - moveit_msgs::MoveItErrorCodes& error_code) const; + moveit_msgs::MoveItErrorCodes& error_code, const ros::NodeHandle& nh, + bool use_constraints_approximations) const; void registerPlannerAllocator(const std::string& planner_id, const ConfiguredPlannerAllocator& pa) { @@ -178,6 +213,9 @@ class PlanningContextManager void registerDefaultPlanners(); void registerDefaultStateSpaces(); + template + void registerPlannerAllocatorHelper(const std::string& planner_id); + /** \brief This is the function that constructs new planning contexts if no previous ones exist that are suitable */ ModelBasedPlanningContextPtr getPlanningContext(const planning_interface::PlannerConfigurationSettings& config, const StateSpaceFactoryTypeSelector& factory_selector, @@ -189,7 +227,7 @@ class PlanningContextManager const moveit_msgs::MotionPlanRequest& req) const; /** \brief The kinematic model for which motion plans are computed */ - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; @@ -224,10 +262,11 @@ class PlanningContextManager /// needed) unsigned int minimum_waypoint_count_; + /// Multi-query planner allocator + MultiQueryPlannerAllocator planner_allocator_; + private: MOVEIT_STRUCT_FORWARD(CachedContexts); CachedContextsPtr cached_contexts_; }; -} - -#endif +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/launch/generate_state_database.launch b/moveit_planners/ompl/ompl_interface/launch/generate_state_database.launch new file mode 100644 index 0000000000..7aa2479d8a --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/launch/generate_state_database.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/moveit_planners/ompl/ompl_interface/src/generate_state_database.cpp b/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp similarity index 82% rename from moveit_planners/ompl/ompl_interface/src/generate_state_database.cpp rename to moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp index f1cd7d56c6..55548966ca 100644 --- a/moveit_planners/ompl/ompl_interface/src/generate_state_database.cpp +++ b/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp @@ -36,17 +36,20 @@ /* Authors: Ioan Sucan, Michael Goerner */ #include +#include #include #include #include +#include #include +#include #include #include -static const std::string LOGNAME = "generate_state_database"; +constexpr char LOGNAME[] = "generate_state_database"; static const std::string ROBOT_DESCRIPTION = "robot_description"; @@ -118,26 +121,47 @@ void computeDB(const planning_scene::PlanningScenePtr& scene, struct GenerateSta scene->getCurrentStateNonConst().update(); ompl_interface::OMPLInterface ompl_interface(scene->getRobotModel()); + planning_interface::MotionPlanRequest req; + req.group_name = params.planning_group; + req.path_constraints = params.constraints; + moveit::core::robotStateToRobotStateMsg(scene->getCurrentState(), req.start_state); + req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + scene->getCurrentState(), scene->getRobotModel()->getJointModelGroup(params.planning_group))); + + ompl_interface::ModelBasedPlanningContextPtr context = ompl_interface.getPlanningContext(scene, req); ROS_INFO_STREAM_NAMED(LOGNAME, "Generating Joint Space Constraint Approximation Database for constraint:\n" << params.constraints); ompl_interface::ConstraintApproximationConstructionResults result = - ompl_interface.getConstraintsLibrary().addConstraintApproximation(params.constraints, params.planning_group, - scene, params.construction_opts); + context->getConstraintsLibraryNonConst()->addConstraintApproximation(params.constraints, params.planning_group, + scene, params.construction_opts); if (!result.approx) { ROS_FATAL_NAMED(LOGNAME, "Failed to generate approximation."); return; } - ompl_interface.getConstraintsLibrary().saveConstraintApproximations(params.output_folder); + context->getConstraintsLibraryNonConst()->saveConstraintApproximations(params.output_folder); ROS_INFO_STREAM_NAMED(LOGNAME, "Successfully generated Joint Space Constraint Approximation Database for constraint:\n" << params.constraints); ROS_INFO_STREAM_NAMED(LOGNAME, "The database has been saved in your local folder '" << params.output_folder << "'"); } +/** + * Generates a database of states that follow the given constraints. + * An example of the constraint yaml that should be loaded to rosparam: + * "name: tool0_upright + * constraints: + * - type: orientation + * frame_id: world + * link_name: tool0 + * orientation: [0, 0, 0] + * tolerances: [0.01, 0.01, 3.15] + * weight: 1.0 + * " + */ int main(int argc, char** argv) { ros::init(argc, argv, "construct_tool_constraint_database", ros::init_options::AnonymousName); @@ -165,7 +189,7 @@ int main(int argc, char** argv) } } - if (kinematic_constraints::isEmpty(params.constraints)) + if (moveit::core::isEmpty(params.constraints)) { ROS_FATAL_NAMED(LOGNAME, "Abort. Constraint description is an empty set of constraints."); return 1; diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp index 04a7fc83e3..24a81ebe9f 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp @@ -41,6 +41,11 @@ #include +namespace ompl_interface +{ +constexpr char LOGNAME[] = "constrained_goal_sampler"; +} // namespace ompl_interface + ompl_interface::ConstrainedGoalSampler::ConstrainedGoalSampler(const ModelBasedPlanningContext* pc, kinematic_constraints::KinematicConstraintSetPtr ks, constraint_samplers::ConstraintSamplerPtr cs) @@ -58,12 +63,12 @@ ompl_interface::ConstrainedGoalSampler::ConstrainedGoalSampler(const ModelBasedP { if (!constraint_sampler_) default_sampler_ = si_->allocStateSampler(); - ROS_DEBUG_NAMED("constrained_goal_sampler", "Constructed a ConstrainedGoalSampler instance at address %p", this); + ROS_DEBUG_NAMED(LOGNAME, "Constructed a ConstrainedGoalSampler instance at address %p", this); startSampling(); } bool ompl_interface::ConstrainedGoalSampler::checkStateValidity(ob::State* new_goal, - const robot_state::RobotState& state, + const moveit::core::RobotState& state, bool verbose) const { planning_context_->getOMPLStateSpace()->copyToOMPLState(new_goal, state); @@ -71,12 +76,12 @@ bool ompl_interface::ConstrainedGoalSampler::checkStateValidity(ob::State* new_g } bool ompl_interface::ConstrainedGoalSampler::stateValidityCallback(ob::State* new_goal, - robot_state::RobotState const* state, - const robot_model::JointModelGroup* jmg, + moveit::core::RobotState const* state, + const moveit::core::JointModelGroup* jmg, const double* jpos, bool verbose) const { // we copy the state to not change the seed state - robot_state::RobotState solution_state(*state); + moveit::core::RobotState solution_state(*state); solution_state.setJointGroupPositions(jmg, jpos); solution_state.update(); return checkStateValidity(new_goal, solution_state, verbose); @@ -116,7 +121,7 @@ bool ompl_interface::ConstrainedGoalSampler::sampleUsingConstraintSampler(const if (constraint_sampler_) { // makes the constraint sampler also perform a validity callback - robot_state::GroupStateValidityCallbackFn gsvcf = + moveit::core::GroupStateValidityCallbackFn gsvcf = std::bind(&ompl_interface::ConstrainedGoalSampler::stateValidityCallback, this, new_goal, std::placeholders::_1, // pointer to state std::placeholders::_2, // const* joint model group @@ -138,9 +143,9 @@ bool ompl_interface::ConstrainedGoalSampler::sampleUsingConstraintSampler(const if (!warned_invalid_samples_ && invalid_sampled_constraints_ >= (attempts_so_far * 8) / 10) { warned_invalid_samples_ = true; - ROS_WARN_NAMED("constrained_goal_sampler", "More than 80%% of the sampled goal states " - "fail to satisfy the constraints imposed on the goal sampler. " - "Is the constrained sampler working correctly?"); + ROS_WARN_NAMED(LOGNAME, "More than 80%% of the sampled goal states " + "fail to satisfy the constraints imposed on the goal sampler. " + "Is the constrained sampler working correctly?"); } } } diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_valid_state_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_valid_state_sampler.cpp index 3ce0351928..db39b8ef33 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constrained_valid_state_sampler.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constrained_valid_state_sampler.cpp @@ -40,6 +40,11 @@ #include +namespace ompl_interface +{ +constexpr char LOGNAME[] = "constrained_valid_state_sampler"; +} // namespace ompl_interface + ompl_interface::ValidConstrainedSampler::ValidConstrainedSampler(const ModelBasedPlanningContext* pc, kinematic_constraints::KinematicConstraintSetPtr ks, constraint_samplers::ConstraintSamplerPtr cs) @@ -52,8 +57,7 @@ ompl_interface::ValidConstrainedSampler::ValidConstrainedSampler(const ModelBase if (!constraint_sampler_) default_sampler_ = si_->allocStateSampler(); inv_dim_ = si_->getStateSpace()->getDimension() > 0 ? 1.0 / (double)si_->getStateSpace()->getDimension() : 1.0; - ROS_DEBUG_NAMED("constrained_valid_state_sampler", "Constructed a ValidConstrainedSampler instance at address %p", - this); + ROS_DEBUG_NAMED(LOGNAME, "Constructed a ValidConstrainedSampler instance at address %p", this); } bool ompl_interface::ValidConstrainedSampler::project(ompl::base::State* state) diff --git a/moveit_planners/ompl/ompl_interface/src/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp similarity index 68% rename from moveit_planners/ompl/ompl_interface/src/constraints_library.cpp rename to moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index 9c65e53513..475f34ca14 100644 --- a/moveit_planners/ompl/ompl_interface/src/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -34,17 +34,19 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include #include #include #include +#include +#include +#include +#include #include namespace ompl_interface { +constexpr char LOGNAME[] = "constraints_library"; + namespace { template @@ -219,7 +221,7 @@ ompl_interface::ConstraintApproximation::ConstraintApproximation( } ompl::base::StateSamplerAllocator -ompl_interface::ConstraintApproximation::getStateSamplerAllocator(const moveit_msgs::Constraints& msg) const +ompl_interface::ConstraintApproximation::getStateSamplerAllocator(const moveit_msgs::Constraints& /*unused*/) const { if (state_storage_->size() == 0) return ompl::base::StateSamplerAllocator(); @@ -227,10 +229,11 @@ ompl_interface::ConstraintApproximation::getStateSamplerAllocator(const moveit_m milestones_); } /* -void ompl_interface::ConstraintApproximation::visualizeDistribution(const std::string &link_name, unsigned int count, +void ompl_interface::ConstraintApproximation::visualizeDistribution(const +std::string &link_name, unsigned int count, visualization_msgs::MarkerArray &arr) const { - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToDefaultValues(); ompl::RNG rng; @@ -246,7 +249,8 @@ visualization_msgs::MarkerArray &arr) const { state_storage_->getStateSpace()->as()->copyToRobotState(robot_state, state_storage_->getState(rng.uniformInt(0, state_storage_->size() - 1))); - const Eigen::Vector3d &pos = robot_state.getLinkState(link_name)->getGlobalLinkTransform().translation(); + const Eigen::Vector3d &pos = +robot_state.getLinkState(link_name)->getGlobalLinkTransform().translation(); visualization_msgs::Marker mk; mk.header.stamp = ros::Time::now(); @@ -273,12 +277,14 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: std::ifstream fin((path + "/manifest").c_str()); if (!fin.good()) { - ROS_WARN_NAMED("constraints_library", "Manifest not found in folder '%s'. Not loading constraint approximations.", + ROS_WARN_NAMED(LOGNAME, + "Manifest not found in folder '%s'. Not loading " + "constraint approximations.", path.c_str()); return; } - ROS_INFO_NAMED("constraints_library", "Loading constrained space approximations from '%s'...", path.c_str()); + ROS_INFO_NAMED(LOGNAME, "Loading constrained space approximations from '%s'...", path.c_str()); while (fin.good() && !fin.eof()) { @@ -301,37 +307,47 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: if (fin.eof()) break; fin >> filename; - ROS_INFO_NAMED("constraints_library", "Loading constraint approximation of type '%s' for group '%s' from '%s'...", - state_space_parameterization.c_str(), group.c_str(), filename.c_str()); - const ModelBasedPlanningContextPtr& pc = context_manager_.getPlanningContext(group, state_space_parameterization); - if (pc) + + if (context_->getGroupName() != group && + context_->getOMPLStateSpace()->getParameterizationType() != state_space_parameterization) { - moveit_msgs::Constraints msg; - hexToMsg(serialization, msg); - auto* cass = new ConstraintApproximationStateStorage(pc->getOMPLSimpleSetup()->getStateSpace()); - cass->load((path + "/" + filename).c_str()); - ConstraintApproximationPtr cap(new ConstraintApproximation(group, state_space_parameterization, explicit_motions, - msg, filename, ompl::base::StateStoragePtr(cass), - milestones)); - if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end()) - ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", - cap->getName().c_str()); - constraint_approximations_[cap->getName()] = cap; - std::size_t sum = 0; - for (std::size_t i = 0; i < cass->size(); ++i) - sum += cass->getMetadata(i).first.size(); - ROS_INFO_NAMED("constraints_library", "Loaded %lu states (%lu milestones) and %lu connections (%0.1lf per state) " - "for constraint named '%s'%s", - cass->size(), cap->getMilestoneCount(), sum, (double)sum / (double)cap->getMilestoneCount(), - msg.name.c_str(), explicit_motions ? ". Explicit motions included." : ""); + ROS_INFO_NAMED(LOGNAME, + "Ignoring constraint approximation of type '%s' " + "for group '%s' from '%s'...", + state_space_parameterization.c_str(), group.c_str(), filename.c_str()); + continue; } + + ROS_INFO_NAMED(LOGNAME, + "Loading constraint approximation of type '%s' for " + "group '%s' from '%s'...", + state_space_parameterization.c_str(), group.c_str(), filename.c_str()); + moveit_msgs::Constraints msg; + hexToMsg(serialization, msg); + auto* cass = new ConstraintApproximationStateStorage(context_->getOMPLSimpleSetup()->getStateSpace()); + cass->load((std::string{ path }.append("/").append(filename)).c_str()); + ConstraintApproximationPtr cap(new ConstraintApproximation(group, state_space_parameterization, explicit_motions, + msg, filename, ompl::base::StateStoragePtr(cass), + milestones)); + if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end()) + ROS_WARN_NAMED(LOGNAME, "Overwriting constraint approximation named '%s'", cap->getName().c_str()); + constraint_approximations_[cap->getName()] = cap; + std::size_t sum = 0; + for (std::size_t i = 0; i < cass->size(); ++i) + sum += cass->getMetadata(i).first.size(); + ROS_INFO_NAMED(LOGNAME, + "Loaded %lu states (%lu milestones) and %lu " + "connections (%0.1lf per state) " + "for constraint named '%s'%s", + cass->size(), cap->getMilestoneCount(), sum, (double)sum / (double)cap->getMilestoneCount(), + msg.name.c_str(), explicit_motions ? ". Explicit motions included." : ""); } - ROS_INFO_NAMED("constraints_library", "Done loading constrained space approximations."); + ROS_INFO_NAMED(LOGNAME, "Done loading constrained space approximations."); } void ompl_interface::ConstraintsLibrary::saveConstraintApproximations(const std::string& path) { - ROS_INFO_NAMED("constraints_library", "Saving %u constrained space approximations to '%s'", + ROS_INFO_NAMED(LOGNAME, "Saving %u constrained space approximations to '%s'", (unsigned int)constraint_approximations_.size(), path.c_str()); try { @@ -358,7 +374,7 @@ void ompl_interface::ConstraintsLibrary::saveConstraintApproximations(const std: it->second->getStateStorage()->store((path + "/" + it->second->getFilename()).c_str()); } else - ROS_ERROR_NAMED("constraints_library", "Unable to save constraint approximation to '%s'", path.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Unable to save constraint approximation to '%s'", path.c_str()); fout.close(); } @@ -393,65 +409,70 @@ ompl_interface::ConstraintsLibrary::getConstraintApproximation(const moveit_msgs } ompl_interface::ConstraintApproximationConstructionResults -ompl_interface::ConstraintsLibrary::addConstraintApproximation( - const moveit_msgs::Constraints& constr, const std::string& group, - const planning_scene::PlanningSceneConstPtr& scene, const ConstraintApproximationConstructionOptions& options) +ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::Constraints& constr, + const std::string& group, + const planning_scene::PlanningSceneConstPtr& scene, + const ConstraintApproximationConstructionOptions& options) { return addConstraintApproximation(constr, constr, group, scene, options); } ompl_interface::ConstraintApproximationConstructionResults -ompl_interface::ConstraintsLibrary::addConstraintApproximation( - const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard, - const std::string& group, const planning_scene::PlanningSceneConstPtr& scene, - const ConstraintApproximationConstructionOptions& options) +ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::Constraints& constr_sampling, + const moveit_msgs::Constraints& constr_hard, + const std::string& group, + const planning_scene::PlanningSceneConstPtr& scene, + const ConstraintApproximationConstructionOptions& options) { ConstraintApproximationConstructionResults res; - ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, options.state_space_parameterization); - if (pc) + if (context_->getGroupName() != group && + context_->getOMPLStateSpace()->getParameterizationType() != options.state_space_parameterization) { - pc->clear(); - pc->setPlanningScene(scene); - pc->setCompleteInitialState(scene->getCurrentState()); - - ros::WallTime start = ros::WallTime::now(); - ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, options, res); - ROS_INFO_NAMED("constraints_library", "Spent %lf seconds constructing the database", - (ros::WallTime::now() - start).toSec()); - if (ss) - { - ConstraintApproximationPtr ca(new ConstraintApproximation( - group, options.state_space_parameterization, options.explicit_motions, constr_hard, - group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + - ".ompldb", - ss, res.milestones)); - if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end()) - ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", ca->getName().c_str()); - constraint_approximations_[ca->getName()] = ca; - res.approx = ca; - } - else - ROS_ERROR_NAMED("constraints_library", "Unable to construct constraint approximation for group '%s'", - group.c_str()); + ROS_INFO_NAMED(LOGNAME, "Ignoring constraint approximation of type '%s' for group '%s'...", + options.state_space_parameterization.c_str(), group.c_str()); + return res; } + + context_->clear(); + context_->setPlanningScene(scene); + context_->setCompleteInitialState(scene->getCurrentState()); + + ros::WallTime start = ros::WallTime::now(); + ompl::base::StateStoragePtr state_storage = + constructConstraintApproximation(context_, constr_sampling, constr_hard, options, res); + ROS_INFO_NAMED(LOGNAME, "Spent %lf seconds constructing the database", (ros::WallTime::now() - start).toSec()); + if (state_storage) + { + ConstraintApproximationPtr constraint_approx(new ConstraintApproximation( + group, options.state_space_parameterization, options.explicit_motions, constr_hard, + group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + + ".ompldb", + state_storage, res.milestones)); + if (constraint_approximations_.find(constraint_approx->getName()) != constraint_approximations_.end()) + ROS_WARN_NAMED(LOGNAME, "Overwriting constraint approximation named '%s'", constraint_approx->getName().c_str()); + constraint_approximations_[constraint_approx->getName()] = constraint_approx; + res.approx = constraint_approx; + } + else + ROS_ERROR_NAMED(LOGNAME, "Unable to construct constraint approximation for group '%s'", group.c_str()); return res; } ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstraintApproximation( - const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling, + ModelBasedPlanningContext* pcontext, const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options, ConstraintApproximationConstructionResults& result) { // state storage structure ConstraintApproximationStateStorage* cass = new ConstraintApproximationStateStorage(pcontext->getOMPLStateSpace()); - ob::StateStoragePtr sstor(cass); + ob::StateStoragePtr state_storage(cass); // construct a sampler for the sampling constraints kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel()); - robot_state::Transforms no_transforms(pcontext->getRobotModel()->getModelFrame()); + moveit::core::Transforms no_transforms(pcontext->getRobotModel()->getModelFrame()); kset.add(constr_hard, no_transforms); - const robot_state::RobotState& default_state = pcontext->getCompleteInitialRobotState(); + const moveit::core::RobotState& default_state = pcontext->getCompleteInitialRobotState(); unsigned int attempts = 0; @@ -462,43 +483,44 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra // construct the constrained states - robot_state::RobotState robot_state(default_state); + moveit::core::RobotState robot_state(default_state); const constraint_samplers::ConstraintSamplerManagerPtr& csmng = pcontext->getConstraintSamplerManager(); - ConstrainedSampler* csmp = nullptr; + ConstrainedSampler* constrained_sampler = nullptr; if (csmng) { - constraint_samplers::ConstraintSamplerPtr cs = + constraint_samplers::ConstraintSamplerPtr constraint_sampler = csmng->selectSampler(pcontext->getPlanningScene(), pcontext->getJointModelGroup()->getName(), constr_sampling); - if (cs) - csmp = new ConstrainedSampler(pcontext.get(), cs); + if (constraint_sampler) + constrained_sampler = new ConstrainedSampler(pcontext, constraint_sampler); } - ob::StateSamplerPtr ss(csmp ? ob::StateSamplerPtr(csmp) : pcontext->getOMPLStateSpace()->allocDefaultStateSampler()); + ob::StateSamplerPtr ss(constrained_sampler ? ob::StateSamplerPtr(constrained_sampler) : + pcontext->getOMPLStateSpace()->allocDefaultStateSampler()); ompl::base::ScopedState<> temp(pcontext->getOMPLStateSpace()); int done = -1; bool slow_warn = false; ompl::time::point start = ompl::time::now(); - while (sstor->size() < options.samples) + while (state_storage->size() < options.samples) { ++attempts; - int done_now = 100 * sstor->size() / options.samples; + int done_now = 100 * state_storage->size() / options.samples; if (done != done_now) { done = done_now; - ROS_INFO_NAMED("constraints_library", "%d%% complete (kept %0.1lf%% sampled states)", done, - 100.0 * (double)sstor->size() / (double)attempts); + ROS_INFO_NAMED(LOGNAME, "%d%% complete (kept %0.1lf%% sampled states)", done, + 100.0 * (double)state_storage->size() / (double)attempts); } - if (!slow_warn && attempts > 10 && attempts > sstor->size() * 100) + if (!slow_warn && attempts > 10 && attempts > state_storage->size() * 100) { slow_warn = true; - ROS_WARN_NAMED("constraints_library", "Computation of valid state database is very slow..."); + ROS_WARN_NAMED(LOGNAME, "Computation of valid state database is very slow..."); } - if (attempts > options.samples && sstor->size() == 0) + if (attempts > options.samples && state_storage->size() == 0) { - ROS_ERROR_NAMED("constraints_library", "Unable to generate any samples"); + ROS_ERROR_NAMED(LOGNAME, "Unable to generate any samples"); break; } @@ -506,32 +528,31 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra pcontext->getOMPLStateSpace()->copyToRobotState(robot_state, temp.get()); if (kset.decide(robot_state).satisfied) { - if (sstor->size() < options.samples) + if (state_storage->size() < options.samples) { - temp->as()->tag = sstor->size(); - sstor->addState(temp.get()); + temp->as()->tag = state_storage->size(); + state_storage->addState(temp.get()); } } } result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start); - ROS_INFO_NAMED("constraints_library", "Generated %u states in %lf seconds", (unsigned int)sstor->size(), + ROS_INFO_NAMED(LOGNAME, "Generated %u states in %lf seconds", (unsigned int)state_storage->size(), result.state_sampling_time); - if (csmp) + if (constrained_sampler) { - result.sampling_success_rate = csmp->getConstrainedSamplingRate(); - ROS_INFO_NAMED("constraints_library", "Constrained sampling rate: %lf", result.sampling_success_rate); + result.sampling_success_rate = constrained_sampler->getConstrainedSamplingRate(); + ROS_INFO_NAMED(LOGNAME, "Constrained sampling rate: %lf", result.sampling_success_rate); } - result.milestones = sstor->size(); + result.milestones = state_storage->size(); if (options.edges_per_sample > 0) { - ROS_INFO_NAMED("constraints_library", "Computing graph connections (max %u edges per sample) ...", - options.edges_per_sample); + ROS_INFO_NAMED(LOGNAME, "Computing graph connections (max %u edges per sample) ...", options.edges_per_sample); - // construct connexions + // construct connections const ob::StateSpacePtr& space = pcontext->getOMPLSimpleSetup()->getStateSpace(); - unsigned int milestones = sstor->size(); + unsigned int milestones = state_storage->size(); std::vector int_states(options.max_explicit_points, nullptr); pcontext->getOMPLSimpleSetup()->getSpaceInformation()->allocStates(int_states); @@ -545,25 +566,25 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra if (done != done_now) { done = done_now; - ROS_INFO_NAMED("constraints_library", "%d%% complete", done); + ROS_INFO_NAMED(LOGNAME, "%d%% complete", done); } if (cass->getMetadata(j).first.size() >= options.edges_per_sample) continue; - const ob::State* sj = sstor->getState(j); + const ob::State* sj = state_storage->getState(j); for (std::size_t i = j + 1; i < milestones; ++i) { if (cass->getMetadata(i).first.size() >= options.edges_per_sample) continue; - double d = space->distance(sstor->getState(i), sj); + double d = space->distance(state_storage->getState(i), sj); if (d >= options.max_edge_length) continue; unsigned int isteps = std::min(options.max_explicit_points, d / options.explicit_points_resolution); double step = 1.0 / (double)isteps; bool ok = true; - space->interpolate(sstor->getState(i), sj, step, int_states[0]); + space->interpolate(state_storage->getState(i), sj, step, int_states[0]); for (unsigned int k = 1; k < isteps; ++k) { double this_step = step / (1.0 - (k - 1) * step); @@ -583,13 +604,13 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra if (options.explicit_motions) { - cass->getMetadata(i).second[j].first = sstor->size(); + cass->getMetadata(i).second[j].first = state_storage->size(); for (unsigned int k = 0; k < isteps; ++k) { int_states[k]->as()->tag = -1; - sstor->addState(int_states[k]); + state_storage->addState(int_states[k]); } - cass->getMetadata(i).second[j].second = sstor->size(); + cass->getMetadata(i).second[j].second = state_storage->size(); cass->getMetadata(j).second[i] = cass->getMetadata(i).second[j]; } @@ -601,15 +622,16 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra } result.state_connection_time = ompl::time::seconds(ompl::time::now() - start); - ROS_INFO_NAMED("constraints_library", "Computed possible connexions in %lf seconds. Added %d connexions", + ROS_INFO_NAMED(LOGNAME, "Computed possible connections in %lf seconds. Added %d connections", result.state_connection_time, good); pcontext->getOMPLSimpleSetup()->getSpaceInformation()->freeStates(int_states); - return sstor; + return state_storage; } - // TODO(davetcoleman): this function did not originally return a value, causing compiler warnings in ROS Melodic + // TODO(davetcoleman): this function did not originally return a value, + // causing compiler warnings in ROS Melodic // Update with more intelligent logic as needed - ROS_ERROR_NAMED("constraints_library", "No StateStoragePtr found - implement better solution here."); - return sstor; + ROS_ERROR_NAMED(LOGNAME, "No StateStoragePtr found - implement better solution here."); + return state_storage; } diff --git a/moveit_planners/ompl/ompl_interface/src/detail/projection_evaluators.cpp b/moveit_planners/ompl/ompl_interface/src/detail/projection_evaluators.cpp index 53fd8a6498..cb938df902 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/projection_evaluators.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/projection_evaluators.cpp @@ -65,7 +65,7 @@ void ompl_interface::ProjectionEvaluatorLinkPose::defaultCellSizes() void ompl_interface::ProjectionEvaluatorLinkPose::project(const ompl::base::State* state, OMPLProjection projection) const { - robot_state::RobotState* s = tss_.getStateStorage(); + moveit::core::RobotState* s = tss_.getStateStorage(); planning_context_->getOMPLStateSpace()->copyToRobotState(*s, state); const Eigen::Vector3d& o = s->getGlobalLinkTransform(link_).translation(); diff --git a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp index 5d47880ea6..e2b43f2d74 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -39,6 +39,11 @@ #include #include +namespace ompl_interface +{ +constexpr char LOGNAME[] = "state_validity_checker"; +} // namespace ompl_interface + ompl_interface::StateValidityChecker::StateValidityChecker(const ModelBasedPlanningContext* pc) : ompl::base::StateValidityChecker(pc->getOMPLSimpleSetup()->getSpaceInformation()) , planning_context_(pc) @@ -70,129 +75,19 @@ void ompl_interface::StateValidityChecker::setVerbose(bool flag) bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, bool verbose) const { - // moveit::Profiler::ScopedBlock sblock("isValid"); - return planning_context_->useStateValidityCache() ? isValidWithCache(state, verbose) : - isValidWithoutCache(state, verbose); -} - -bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, double& dist, bool verbose) const -{ - // moveit::Profiler::ScopedBlock sblock("isValid"); - return planning_context_->useStateValidityCache() ? isValidWithCache(state, dist, verbose) : - isValidWithoutCache(state, dist, verbose); -} - -double ompl_interface::StateValidityChecker::cost(const ompl::base::State* state) const -{ - double cost = 0.0; - - robot_state::RobotState* robot_state = tss_.getStateStorage(); - planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); - - // Calculates cost from a summation of distance to obstacles times the size of the obstacle - collision_detection::CollisionResult res; - planning_context_->getPlanningScene()->checkCollision(collision_request_with_cost_, res, *robot_state); - - for (const collision_detection::CostSource& cost_source : res.cost_sources) - cost += cost_source.cost * cost_source.getVolume(); - - return cost; -} - -double ompl_interface::StateValidityChecker::clearance(const ompl::base::State* state) const -{ - robot_state::RobotState* robot_state = tss_.getStateStorage(); - planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); - - collision_detection::CollisionResult res; - planning_context_->getPlanningScene()->checkCollision(collision_request_with_distance_, res, *robot_state); - return res.collision ? 0.0 : (res.distance < 0.0 ? std::numeric_limits::infinity() : res.distance); -} - -bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base::State* state, bool verbose) const -{ - // check bounds - if (!si_->satisfiesBounds(state)) - { - if (verbose) - ROS_INFO_NAMED("state_validity_checker", "State outside bounds"); - return false; - } - - // convert ompl state to MoveIt! robot state - robot_state::RobotState* robot_state = tss_.getStateStorage(); - planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); - - // check path constraints - const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints(); - if (kset && !kset->decide(*robot_state, verbose).satisfied) - return false; - - // check feasibility - if (!planning_context_->getPlanningScene()->isStateFeasible(*robot_state, verbose)) - return false; - - // check collision avoidance - collision_detection::CollisionResult res; - planning_context_->getPlanningScene()->checkCollision( - verbose ? collision_request_simple_verbose_ : collision_request_simple_, res, *robot_state); - return !res.collision; -} - -bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base::State* state, double& dist, - bool verbose) const -{ - if (!si_->satisfiesBounds(state)) - { - if (verbose) - ROS_INFO_NAMED("state_validity_checker", "State outside bounds"); - return false; - } - - robot_state::RobotState* robot_state = tss_.getStateStorage(); - planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); - - // check path constraints - const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints(); - if (kset) - { - kinematic_constraints::ConstraintEvaluationResult cer = kset->decide(*robot_state, verbose); - if (!cer.satisfied) - { - dist = cer.distance; - return false; - } - } - - // check feasibility - if (!planning_context_->getPlanningScene()->isStateFeasible(*robot_state, verbose)) - { - dist = 0.0; - return false; - } - - // check collision avoidance - collision_detection::CollisionResult res; - planning_context_->getPlanningScene()->checkCollision( - verbose ? collision_request_with_distance_verbose_ : collision_request_with_distance_, res, *robot_state); - dist = res.distance; - return !res.collision; -} - -bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::State* state, bool verbose) const -{ + // Use cached validity if it is available if (state->as()->isValidityKnown()) return state->as()->isMarkedValid(); if (!si_->satisfiesBounds(state)) { if (verbose) - ROS_INFO_NAMED("state_validity_checker", "State outside bounds"); + ROS_INFO_NAMED(LOGNAME, "State outside bounds"); const_cast(state)->as()->markInvalid(); return false; } - robot_state::RobotState* robot_state = tss_.getStateStorage(); + moveit::core::RobotState* robot_state = tss_.getStateStorage(); planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); // check path constraints @@ -225,9 +120,9 @@ bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::St return !res.collision; } -bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::State* state, double& dist, - bool verbose) const +bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, double& dist, bool verbose) const { + // Use cached validity and distance if they are available if (state->as()->isValidityKnown() && state->as()->isGoalDistanceKnown()) { @@ -238,12 +133,12 @@ bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::St if (!si_->satisfiesBounds(state)) { if (verbose) - ROS_INFO_NAMED("state_validity_checker", "State outside bounds"); + ROS_INFO_NAMED(LOGNAME, "State outside bounds"); const_cast(state)->as()->markInvalid(0.0); return false; } - robot_state::RobotState* robot_state = tss_.getStateStorage(); + moveit::core::RobotState* robot_state = tss_.getStateStorage(); planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); // check path constraints @@ -273,3 +168,30 @@ bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::St dist = res.distance; return !res.collision; } + +double ompl_interface::StateValidityChecker::cost(const ompl::base::State* state) const +{ + double cost = 0.0; + + moveit::core::RobotState* robot_state = tss_.getStateStorage(); + planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); + + // Calculates cost from a summation of distance to obstacles times the size of the obstacle + collision_detection::CollisionResult res; + planning_context_->getPlanningScene()->checkCollision(collision_request_with_cost_, res, *robot_state); + + for (const collision_detection::CostSource& cost_source : res.cost_sources) + cost += cost_source.cost * cost_source.getVolume(); + + return cost; +} + +double ompl_interface::StateValidityChecker::clearance(const ompl::base::State* state) const +{ + moveit::core::RobotState* robot_state = tss_.getStateStorage(); + planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); + + collision_detection::CollisionResult res; + planning_context_->getPlanningScene()->checkCollision(collision_request_with_distance_, res, *robot_state); + return res.collision ? 0.0 : (res.distance < 0.0 ? std::numeric_limits::infinity() : res.distance); +} \ No newline at end of file diff --git a/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp b/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp index d829852774..e0cfa8245e 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp @@ -36,13 +36,13 @@ #include -ompl_interface::TSStateStorage::TSStateStorage(const robot_model::RobotModelPtr& robot_model) +ompl_interface::TSStateStorage::TSStateStorage(const moveit::core::RobotModelPtr& robot_model) : start_state_(robot_model) { start_state_.setToDefaultValues(); } -ompl_interface::TSStateStorage::TSStateStorage(const robot_state::RobotState& start_state) : start_state_(start_state) +ompl_interface::TSStateStorage::TSStateStorage(const moveit::core::RobotState& start_state) : start_state_(start_state) { } @@ -52,15 +52,15 @@ ompl_interface::TSStateStorage::~TSStateStorage() delete thread_state.second; } -robot_state::RobotState* ompl_interface::TSStateStorage::getStateStorage() const +moveit::core::RobotState* ompl_interface::TSStateStorage::getStateStorage() const { - robot_state::RobotState* st = nullptr; + moveit::core::RobotState* st = nullptr; std::unique_lock slock(lock_); /// \todo use Thread Local Storage? - std::map::const_iterator it = + std::map::const_iterator it = thread_states_.find(std::this_thread::get_id()); if (it == thread_states_.end()) { - st = new robot_state::RobotState(start_state_); + st = new moveit::core::RobotState(start_state_); thread_states_[std::this_thread::get_id()] = st; } else diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 16aeb154e3..1a37096d9f 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -35,6 +35,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include @@ -42,22 +43,39 @@ #include #include #include -#include +#include + #include #include #include +#include #include #include #include #include #include +// TODO: remove when ROS Melodic and older are no longer supported +#if OMPL_VERSION_VALUE < 1005000 +#include +#else +// IterationTerminationCondition was moved to a separate file and +// CostConvergenceTerminationCondition was added in OMPL 1.5.0. +#include +#include +#endif #include "ompl/base/objectives/PathLengthOptimizationObjective.h" #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h" #include "ompl/base/objectives/MinimaxObjective.h" #include "ompl/base/objectives/StateCostIntegralObjective.h" #include "ompl/base/objectives/MaximizeMinClearanceObjective.h" +#include + +namespace ompl_interface +{ +constexpr char LOGNAME[] = "model_based_planning_context"; +} // namespace ompl_interface ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std::string& name, const ModelBasedPlanningContextSpecification& spec) @@ -76,25 +94,60 @@ ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std:: , max_planning_threads_(0) , max_solution_segment_length_(0.0) , minimum_waypoint_count_(0) - , use_state_validity_cache_(true) + , multi_query_planning_enabled_(false) // maintain "old" behavior by default , simplify_solutions_(true) + , interpolate_(true) + , hybridize_(true) { complete_initial_robot_state_.update(); + + constraints_library_ = std::make_shared(this); +} + +void ompl_interface::ModelBasedPlanningContext::configure(const ros::NodeHandle& nh, bool use_constraints_approximations) +{ + loadConstraintApproximations(nh); + if (!use_constraints_approximations) + { + setConstraintsApproximations(ConstraintsLibraryPtr()); + } + complete_initial_robot_state_.update(); ompl_simple_setup_->getStateSpace()->computeSignature(space_signature_); ompl_simple_setup_->getStateSpace()->setStateSamplerAllocator( std::bind(&ModelBasedPlanningContext::allocPathConstrainedSampler, this, std::placeholders::_1)); + + // convert the input state to the corresponding OMPL state + ompl::base::ScopedState<> ompl_start_state(spec_.state_space_); + spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState()); + ompl_simple_setup_->setStartState(ompl_start_state); + ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(new StateValidityChecker(this))); + + if (path_constraints_ && constraints_library_) + { + const ConstraintApproximationPtr& constraint_approx = + constraints_library_->getConstraintApproximation(path_constraints_msg_); + if (constraint_approx) + { + getOMPLStateSpace()->setInterpolationFunction(constraint_approx->getInterpolationFunction()); + ROS_INFO_NAMED(LOGNAME, "Using precomputed interpolation states"); + } + } + + useConfig(); + if (ompl_simple_setup_->getGoal()) + ompl_simple_setup_->setup(); } void ompl_interface::ModelBasedPlanningContext::setProjectionEvaluator(const std::string& peval) { if (!spec_.state_space_) { - ROS_ERROR_NAMED("model_based_planning_context", "No state space is configured yet"); + ROS_ERROR_NAMED(LOGNAME, "No state space is configured yet"); return; } - ob::ProjectionEvaluatorPtr pe = getProjectionEvaluator(peval); - if (pe) - spec_.state_space_->registerDefaultProjection(pe); + ob::ProjectionEvaluatorPtr projection_eval = getProjectionEvaluator(peval); + if (projection_eval) + spec_.state_space_->registerDefaultProjection(projection_eval); } ompl::base::ProjectionEvaluatorPtr @@ -106,7 +159,7 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str if (getRobotModel()->hasLinkModel(link_name)) return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorLinkPose(this, link_name)); else - ROS_ERROR_NAMED("model_based_planning_context", + ROS_ERROR_NAMED(LOGNAME, "Attempted to set projection evaluator with respect to position of link '%s', " "but that link is not known to the kinematic model.", link_name.c_str()); @@ -119,114 +172,85 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str std::stringstream ss(joints); while (ss.good() && !ss.eof()) { - std::string v; - ss >> v >> std::ws; - if (getJointModelGroup()->hasJointModel(v)) + std::string joint; + ss >> joint >> std::ws; + if (getJointModelGroup()->hasJointModel(joint)) { - unsigned int vc = getJointModelGroup()->getJointModel(v)->getVariableCount(); - if (vc > 0) + unsigned int variable_count = getJointModelGroup()->getJointModel(joint)->getVariableCount(); + if (variable_count > 0) { - int idx = getJointModelGroup()->getVariableGroupIndex(v); - for (unsigned int q = 0; q < vc; ++q) + int idx = getJointModelGroup()->getVariableGroupIndex(joint); + for (unsigned int q = 0; q < variable_count; ++q) j.push_back(idx + q); } else - ROS_WARN_NAMED("model_based_planning_context", "%s: Ignoring joint '%s' in projection since it has 0 DOF", - name_.c_str(), v.c_str()); + ROS_WARN_NAMED(LOGNAME, "%s: Ignoring joint '%s' in projection since it has 0 DOF", name_.c_str(), + joint.c_str()); } else - ROS_ERROR_NAMED("model_based_planning_context", + ROS_ERROR_NAMED(LOGNAME, "%s: Attempted to set projection evaluator with respect to value of joint " "'%s', but that joint is not known to the group '%s'.", - name_.c_str(), v.c_str(), getGroupName().c_str()); + name_.c_str(), joint.c_str(), getGroupName().c_str()); } if (j.empty()) - ROS_ERROR_NAMED("model_based_planning_context", "%s: No valid joints specified for joint projection", - name_.c_str()); + ROS_ERROR_NAMED(LOGNAME, "%s: No valid joints specified for joint projection", name_.c_str()); else return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorJointValue(this, j)); } else - ROS_ERROR_NAMED("model_based_planning_context", - "Unable to allocate projection evaluator based on description: '%s'", peval.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Unable to allocate projection evaluator based on description: '%s'", peval.c_str()); return ob::ProjectionEvaluatorPtr(); } ompl::base::StateSamplerPtr -ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const ompl::base::StateSpace* ss) const +ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const ompl::base::StateSpace* state_space) const { - if (spec_.state_space_.get() != ss) + if (spec_.state_space_.get() != state_space) { - ROS_ERROR_NAMED("model_based_planning_context", - "%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str()); + ROS_ERROR_NAMED(LOGNAME, "%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str()); return ompl::base::StateSamplerPtr(); } - ROS_DEBUG_NAMED("model_based_planning_context", - "%s: Allocating a new state sampler (attempts to use path constraints)", name_.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "%s: Allocating a new state sampler (attempts to use path constraints)", name_.c_str()); if (path_constraints_) { - if (spec_.constraints_library_) + if (constraints_library_) { - const ConstraintApproximationPtr& ca = - spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_); - if (ca) + const ConstraintApproximationPtr& constraint_approx = + constraints_library_->getConstraintApproximation(path_constraints_msg_); + if (constraint_approx) { - ompl::base::StateSamplerAllocator c_ssa = ca->getStateSamplerAllocator(path_constraints_msg_); - if (c_ssa) + ompl::base::StateSamplerAllocator state_sampler_allocator = + constraint_approx->getStateSamplerAllocator(path_constraints_msg_); + if (state_sampler_allocator) { - ompl::base::StateSamplerPtr res = c_ssa(ss); - if (res) + ompl::base::StateSamplerPtr state_sampler = state_sampler_allocator(state_space); + if (state_sampler) { - ROS_INFO_NAMED("model_based_planning_context", + ROS_INFO_NAMED(LOGNAME, "%s: Using precomputed state sampler (approximated constraint space) for constraint '%s'", name_.c_str(), path_constraints_msg_.name.c_str()); - return res; + return state_sampler; } } } } - constraint_samplers::ConstraintSamplerPtr cs; + constraint_samplers::ConstraintSamplerPtr constraint_sampler; if (spec_.constraint_sampler_manager_) - cs = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(), - path_constraints_->getAllConstraints()); + constraint_sampler = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(), + path_constraints_->getAllConstraints()); - if (cs) + if (constraint_sampler) { - ROS_INFO_NAMED("model_based_planning_context", "%s: Allocating specialized state sampler for state space", - name_.c_str()); - return ob::StateSamplerPtr(new ConstrainedSampler(this, cs)); + ROS_INFO_NAMED(LOGNAME, "%s: Allocating specialized state sampler for state space", name_.c_str()); + return ob::StateSamplerPtr(new ConstrainedSampler(this, constraint_sampler)); } } - ROS_DEBUG_NAMED("model_based_planning_context", "%s: Allocating default state sampler for state space", - name_.c_str()); - return ss->allocDefaultStateSampler(); -} - -void ompl_interface::ModelBasedPlanningContext::configure() -{ - // convert the input state to the corresponding OMPL state - ompl::base::ScopedState<> ompl_start_state(spec_.state_space_); - spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState()); - ompl_simple_setup_->setStartState(ompl_start_state); - ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(new StateValidityChecker(this))); - - if (path_constraints_ && spec_.constraints_library_) - { - const ConstraintApproximationPtr& ca = - spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_); - if (ca) - { - getOMPLStateSpace()->setInterpolationFunction(ca->getInterpolationFunction()); - ROS_INFO_NAMED("model_based_planning_context", "Using precomputed interpolation states"); - } - } - - useConfig(); - if (ompl_simple_setup_->getGoal()) - ompl_simple_setup_->setup(); + ROS_DEBUG_NAMED(LOGNAME, "%s: Allocating default state sampler for state space", name_.c_str()); + return state_space->allocDefaultStateSampler(); } void ompl_interface::ModelBasedPlanningContext::useConfig() @@ -275,60 +299,75 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() std::string optimizer; ompl::base::OptimizationObjectivePtr objective; it = cfg.find("optimization_objective"); - if (it == cfg.end()) - { - optimizer = "PathLengthOptimizationObjective"; - ROS_DEBUG_NAMED("model_based_planning_context", "No optimization objective specified, defaulting to %s", - optimizer.c_str()); - } - else + if (it != cfg.end()) { optimizer = it->second; cfg.erase(it); - } - if (optimizer == "PathLengthOptimizationObjective") - { - objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); - } - else if (optimizer == "MinimaxObjective") - { - objective.reset(new ompl::base::MinimaxObjective(ompl_simple_setup_->getSpaceInformation())); - } - else if (optimizer == "StateCostIntegralObjective") - { - objective.reset(new ompl::base::StateCostIntegralObjective(ompl_simple_setup_->getSpaceInformation())); - } - else if (optimizer == "MechanicalWorkOptimizationObjective") - { - objective.reset(new ompl::base::MechanicalWorkOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); + if (optimizer == "PathLengthOptimizationObjective") + { + objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); + } + else if (optimizer == "MinimaxObjective") + { + objective.reset(new ompl::base::MinimaxObjective(ompl_simple_setup_->getSpaceInformation())); + } + else if (optimizer == "StateCostIntegralObjective") + { + objective.reset(new ompl::base::StateCostIntegralObjective(ompl_simple_setup_->getSpaceInformation())); + } + else if (optimizer == "MechanicalWorkOptimizationObjective") + { + objective.reset(new ompl::base::MechanicalWorkOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); + } + else if (optimizer == "MaximizeMinClearanceObjective") + { + objective.reset(new ompl::base::MaximizeMinClearanceObjective(ompl_simple_setup_->getSpaceInformation())); + } + else + { + objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); + } + + ompl_simple_setup_->setOptimizationObjective(objective); } - else if (optimizer == "MaximizeMinClearanceObjective") + + // Don't clear planner data if multi-query planning is enabled + it = cfg.find("multi_query_planning_enabled"); + if (it != cfg.end()) + multi_query_planning_enabled_ = boost::lexical_cast(it->second); + + // check whether the path returned by the planner should be interpolated + it = cfg.find("interpolate"); + if (it != cfg.end()) { - objective.reset(new ompl::base::MaximizeMinClearanceObjective(ompl_simple_setup_->getSpaceInformation())); + interpolate_ = boost::lexical_cast(it->second); + cfg.erase(it); } - else + + // check whether solution paths from parallel planning should be hybridized + it = cfg.find("hybridize"); + if (it != cfg.end()) { - objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation())); + hybridize_ = boost::lexical_cast(it->second); + cfg.erase(it); } - ompl_simple_setup_->setOptimizationObjective(objective); - // remove the 'type' parameter; the rest are parameters for the planner itself it = cfg.find("type"); if (it == cfg.end()) { if (name_ != getGroupName()) - ROS_WARN_NAMED("model_based_planning_context", "%s: Attribute 'type' not specified in planner configuration", - name_.c_str()); + ROS_WARN_NAMED(LOGNAME, "%s: Attribute 'type' not specified in planner configuration", name_.c_str()); } else { std::string type = it->second; cfg.erase(it); - ompl_simple_setup_->setPlannerAllocator(std::bind(spec_.planner_selector_(type), std::placeholders::_1, - name_ != getGroupName() ? name_ : "", std::cref(spec_))); - ROS_INFO_NAMED("model_based_planning_context", + const std::string planner_name = getGroupName() + "/" + name_; + ompl_simple_setup_->setPlannerAllocator( + std::bind(spec_.planner_selector_(type), std::placeholders::_1, planner_name, std::cref(spec_))); + ROS_INFO_NAMED(LOGNAME, "Planner configuration '%s' will use planner '%s'. " "Additional configuration parameters will be set when the planner is constructed.", name_.c_str(), type.c_str()); @@ -346,9 +385,9 @@ void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_m if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 && wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 && wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0) - ROS_WARN_NAMED("model_based_planning_context", "It looks like the planning volume was not specified."); + ROS_WARN_NAMED(LOGNAME, "It looks like the planning volume was not specified."); - ROS_DEBUG_NAMED("model_based_planning_context", + ROS_DEBUG_NAMED(LOGNAME, "%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = " "[%f, %f], z = [%f, %f]", name_.c_str(), wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y, wparams.max_corner.y, @@ -395,7 +434,7 @@ void ompl_interface::ModelBasedPlanningContext::interpolateSolution() void ompl_interface::ModelBasedPlanningContext::convertPath(const ompl::geometric::PathGeometric& pg, robot_trajectory::RobotTrajectory& traj) const { - robot_state::RobotState ks = complete_initial_robot_state_; + moveit::core::RobotState ks = complete_initial_robot_state_; for (std::size_t i = 0; i < pg.getStateCount(); ++i) { spec_.state_space_->copyToRobotState(ks, pg.getState(i)); @@ -424,27 +463,87 @@ ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal() std::vector goals; for (kinematic_constraints::KinematicConstraintSetPtr& goal_constraint : goal_constraints_) { - constraint_samplers::ConstraintSamplerPtr cs; + constraint_samplers::ConstraintSamplerPtr constraint_sampler; if (spec_.constraint_sampler_manager_) - cs = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(), - goal_constraint->getAllConstraints()); - if (cs) + constraint_sampler = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(), + goal_constraint->getAllConstraints()); + if (constraint_sampler) { - ob::GoalPtr g = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraint, cs)); - goals.push_back(g); + ob::GoalPtr goal = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraint, constraint_sampler)); + goals.push_back(goal); } } if (!goals.empty()) return goals.size() == 1 ? goals[0] : ompl::base::GoalPtr(new GoalSampleableRegionMux(goals)); else - ROS_ERROR_NAMED("model_based_planning_context", "Unable to construct goal representation"); + ROS_ERROR_NAMED(LOGNAME, "Unable to construct goal representation"); return ob::GoalPtr(); } +ompl::base::PlannerTerminationCondition +ompl_interface::ModelBasedPlanningContext::constructPlannerTerminationCondition(double timeout, + const ompl::time::point& start) +{ + auto it = spec_.config_.find("termination_condition"); + if (it == spec_.config_.end()) + return ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)); + std::string termination_string = it->second; + std::vector termination_and_params; + boost::split(termination_and_params, termination_string, boost::is_any_of("[ ,]")); + + if (termination_and_params.empty()) + ROS_ERROR_NAMED(LOGNAME, "Termination condition not specified"); + // Terminate if a maximum number of iterations is exceeded or a timeout occurs. + // The semantics of "iterations" are planner-specific, but typically it corresponds to the number of times + // an attempt was made to grow a roadmap/tree. + else if (termination_and_params[0] == "Iteration") + { + if (termination_and_params.size() > 1) + return ob::plannerOrTerminationCondition( + ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)), + ob::IterationTerminationCondition(std::stoul(termination_and_params[1]))); + else + ROS_ERROR_NAMED(LOGNAME, "Missing argument to Iteration termination condition"); + } +// TODO: remove when ROS Melodic and older are no longer supported +#if OMPL_VERSION_VALUE >= 1005000 + // Terminate if the cost has converged or a timeout occurs. + // Only useful for anytime/optimizing planners. + else if (termination_and_params[0] == "CostConvergence") + { + std::size_t solutions_window = 10u; + double epsilon = 0.1; + if (termination_and_params.size() > 1) + { + solutions_window = std::stoul(termination_and_params[1]); + if (termination_and_params.size() > 2) + epsilon = moveit::core::toDouble(termination_and_params[2]); + } + return ob::plannerOrTerminationCondition( + ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)), + ob::CostConvergenceTerminationCondition(ompl_simple_setup_->getProblemDefinition(), solutions_window, epsilon)); + } +#endif + // Terminate as soon as an exact solution is found or a timeout occurs. + // This modifies the behavior of anytime/optimizing planners to terminate upon discovering + // the first feasible solution. + else if (termination_and_params[0] == "ExactSolution") + { + return ob::plannerOrTerminationCondition( + ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)), + ob::exactSolnPlannerTerminationCondition(ompl_simple_setup_->getProblemDefinition())); + } + else + ROS_ERROR_NAMED(LOGNAME, "Unknown planner termination condition"); + + // return a planner termination condition to suppress compiler warning + return ob::plannerAlwaysTerminatingCondition(); +} + void ompl_interface::ModelBasedPlanningContext::setCompleteInitialState( - const robot_state::RobotState& complete_initial_robot_state) + const moveit::core::RobotState& complete_initial_robot_state) { complete_initial_robot_state_ = complete_initial_robot_state; complete_initial_robot_state_.update(); @@ -452,7 +551,21 @@ void ompl_interface::ModelBasedPlanningContext::setCompleteInitialState( void ompl_interface::ModelBasedPlanningContext::clear() { - ompl_simple_setup_->clear(); + if (!multi_query_planning_enabled_) + ompl_simple_setup_->clear(); +// TODO: remove when ROS Melodic and older are no longer supported +#if OMPL_VERSION_VALUE >= 1005000 + else + { + // For LazyPRM and LazyPRMstar we assume that the environment *could* have changed + // This means that we need to reset the validity flags for every node and edge in + // the roadmap. For PRM and PRMstar we assume that the environment is static. If + // this is not the case, then multi-query planning should not be enabled. + auto planner = dynamic_cast(ompl_simple_setup_->getPlanner().get()); + if (planner != nullptr) + planner->clearValidity(); + } +#endif ompl_simple_setup_->clearStartStates(); ompl_simple_setup_->setGoal(ob::GoalPtr()); ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr()); @@ -462,7 +575,7 @@ void ompl_interface::ModelBasedPlanningContext::clear() } bool ompl_interface::ModelBasedPlanningContext::setPathConstraints(const moveit_msgs::Constraints& path_constraints, - moveit_msgs::MoveItErrorCodes* error) + moveit_msgs::MoveItErrorCodes* /*error*/) { // ******************* set the path constraints to use path_constraints_.reset(new kinematic_constraints::KinematicConstraintSet(getRobotModel())); @@ -490,8 +603,7 @@ bool ompl_interface::ModelBasedPlanningContext::setGoalConstraints( if (goal_constraints_.empty()) { - ROS_WARN_NAMED("model_based_planning_context", "%s: No goal constraints specified. There is no problem to solve.", - name_.c_str()); + ROS_WARN_NAMED(LOGNAME, "%s: No goal constraints specified. There is no problem to solve.", name_.c_str()); if (error) error->val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; return false; @@ -545,7 +657,7 @@ void ompl_interface::ModelBasedPlanningContext::preSolve() // clear previously computed solutions ompl_simple_setup_->getProblemDefinition()->clearSolutionPaths(); const ob::PlannerPtr planner = ompl_simple_setup_->getPlanner(); - if (planner) + if (planner && !multi_query_planning_enabled_) planner->clear(); startSampling(); ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter(); @@ -556,10 +668,10 @@ void ompl_interface::ModelBasedPlanningContext::postSolve() stopSampling(); int v = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getValidMotionCount(); int iv = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getInvalidMotionCount(); - ROS_DEBUG_NAMED("model_based_planning_context", "There were %d valid motions and %d invalid motions.", v, iv); + ROS_DEBUG_NAMED(LOGNAME, "There were %d valid motions and %d invalid motions.", v, iv); if (ompl_simple_setup_->getProblemDefinition()->hasApproximateSolution()) - ROS_WARN_NAMED("model_based_planning_context", "Computed solution is approximate"); + ROS_WARN_NAMED(LOGNAME, "Computed solution is approximate"); } bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res) @@ -572,11 +684,13 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion simplifySolution(request_.allowed_planning_time - ptime); ptime += getLastSimplifyTime(); } - interpolateSolution(); + + if (interpolate_) + interpolateSolution(); // fill the response - ROS_DEBUG_NAMED("model_based_planning_context", "%s: Returning successful solution with %lu states", - getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount()); + ROS_DEBUG_NAMED(LOGNAME, "%s: Returning successful solution with %lu states", getName().c_str(), + getOMPLSimpleSetup()->getSolutionPath().getStateCount()); res.trajectory_.reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName())); getSolutionPath(*res.trajectory_); @@ -585,7 +699,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion } else { - ROS_INFO_NAMED("model_based_planning_context", "Unable to solve the planning problem"); + ROS_INFO_NAMED(LOGNAME, "Unable to solve the planning problem"); res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; return false; } @@ -616,22 +730,25 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion getSolutionPath(*res.trajectory_.back()); } - ompl::time::point start_interpolate = ompl::time::now(); - interpolateSolution(); - res.processing_time_.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate)); - res.description_.emplace_back("interpolate"); - res.trajectory_.resize(res.trajectory_.size() + 1); - res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName())); - getSolutionPath(*res.trajectory_.back()); + if (interpolate_) + { + ompl::time::point start_interpolate = ompl::time::now(); + interpolateSolution(); + res.processing_time_.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate)); + res.description_.emplace_back("interpolate"); + res.trajectory_.resize(res.trajectory_.size() + 1); + res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName())); + getSolutionPath(*res.trajectory_.back()); + } // fill the response - ROS_DEBUG_NAMED("model_based_planning_context", "%s: Returning successful solution with %lu states", - getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount()); + ROS_DEBUG_NAMED(LOGNAME, "%s: Returning successful solution with %lu states", getName().c_str(), + getOMPLSimpleSetup()->getSolutionPath().getStateCount()); return true; } else { - ROS_INFO_NAMED("model_based_planning_context", "Unable to solve the planning problem"); + ROS_INFO_NAMED(LOGNAME, "Unable to solve the planning problem"); res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; return false; } @@ -644,11 +761,10 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i preSolve(); bool result = false; - if (count <= 1) + if (count <= 1 || multi_query_planning_enabled_) // multi-query planners should always run in single instances { - ROS_DEBUG_NAMED("model_based_planning_context", "%s: Solving the planning problem once...", name_.c_str()); - ob::PlannerTerminationCondition ptc = - ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)); + ROS_DEBUG_NAMED(LOGNAME, "%s: Solving the planning problem once...", name_.c_str()); + ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); registerTerminationCondition(ptc); result = ompl_simple_setup_->solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION; last_plan_time_ = ompl_simple_setup_->getLastPlanComputationTime(); @@ -656,8 +772,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i } else { - ROS_DEBUG_NAMED("model_based_planning_context", "%s: Solving the planning problem %u times...", name_.c_str(), - count); + ROS_DEBUG_NAMED(LOGNAME, "%s: Solving the planning problem %u times...", name_.c_str(), count); ompl_parallel_plan_.clearHybridizationPaths(); if (count <= max_planning_threads_) { @@ -669,17 +784,15 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i for (unsigned int i = 0; i < count; ++i) ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal())); - ob::PlannerTerminationCondition ptc = - ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)); + ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); registerTerminationCondition(ptc); - result = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION; + result = ompl_parallel_plan_.solve(ptc, 1, count, hybridize_) == ompl::base::PlannerStatus::EXACT_SOLUTION; last_plan_time_ = ompl::time::seconds(ompl::time::now() - start); unregisterTerminationCondition(); } else { - ob::PlannerTerminationCondition ptc = - ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)); + ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); registerTerminationCondition(ptc); int n = count / max_planning_threads_; result = true; @@ -692,7 +805,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i else for (unsigned int i = 0; i < max_planning_threads_; ++i) ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal())); - bool r = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION; + bool r = ompl_parallel_plan_.solve(ptc, 1, count, hybridize_) == ompl::base::PlannerStatus::EXACT_SOLUTION; result = result && r; } n = count % max_planning_threads_; @@ -705,7 +818,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i else for (int i = 0; i < n; ++i) ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal())); - bool r = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION; + bool r = ompl_parallel_plan_.solve(ptc, 1, count, hybridize_) == ompl::base::PlannerStatus::EXACT_SOLUTION; result = result && r; } last_plan_time_ = ompl::time::seconds(ompl::time::now() - start); @@ -737,3 +850,29 @@ bool ompl_interface::ModelBasedPlanningContext::terminate() ptc_->terminate(); return true; } + +bool ompl_interface::ModelBasedPlanningContext::saveConstraintApproximations(const ros::NodeHandle& nh) +{ + std::string constraint_path; + if (nh.getParam("constraint_approximations_path", constraint_path)) + { + constraints_library_->saveConstraintApproximations(constraint_path); + return true; + } + ROS_WARN_NAMED(LOGNAME, "ROS param 'constraint_approximations' not found. Unable to save constraint approximations"); + return false; +} + +bool ompl_interface::ModelBasedPlanningContext::loadConstraintApproximations(const ros::NodeHandle& nh) +{ + std::string constraint_path; + if (nh.getParam("constraint_approximations_path", constraint_path)) + { + constraints_library_->loadConstraintApproximations(constraint_path); + std::stringstream ss; + constraints_library_->printConstraintApproximations(ss); + ROS_INFO_STREAM(ss.str()); + return true; + } + return false; +} diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index d5ee41b812..97d239af3f 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -42,36 +42,37 @@ #include #include -ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& robot_model, +namespace ompl_interface +{ +constexpr char LOGNAME[] = "ompl_interface"; +} // namespace ompl_interface + +ompl_interface::OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const ros::NodeHandle& nh) : nh_(nh) , robot_model_(robot_model) , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager()) , context_manager_(robot_model, constraint_sampler_manager_) - , constraints_library_(new ConstraintsLibrary(context_manager_)) , use_constraints_approximations_(true) , simplify_solutions_(true) { - ROS_INFO("Initializing OMPL interface using ROS parameters"); + ROS_DEBUG_NAMED(LOGNAME, "Initializing OMPL interface using ROS parameters"); loadPlannerConfigurations(); - loadConstraintApproximations(); loadConstraintSamplers(); } -ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& robot_model, +ompl_interface::OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const planning_interface::PlannerConfigurationMap& pconfig, const ros::NodeHandle& nh) : nh_(nh) , robot_model_(robot_model) , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager()) , context_manager_(robot_model, constraint_sampler_manager_) - , constraints_library_(new ConstraintsLibrary(context_manager_)) , use_constraints_approximations_(true) , simplify_solutions_(true) { - ROS_INFO("Initializing OMPL interface using specified configuration"); + ROS_DEBUG_NAMED(LOGNAME, "Initializing OMPL interface using specified configuration"); setPlannerConfigurations(pconfig); - loadConstraintApproximations(); loadConstraintSamplers(); } @@ -82,7 +83,7 @@ void ompl_interface::OMPLInterface::setPlannerConfigurations(const planning_inte planning_interface::PlannerConfigurationMap pconfig2 = pconfig; // construct default configurations for planning groups that don't have configs already passed in - for (const robot_model::JointModelGroup* group : robot_model_->getJointModelGroups()) + for (const moveit::core::JointModelGroup* group : robot_model_->getJointModelGroups()) { if (pconfig.find(group->getName()) == pconfig.end()) { @@ -95,8 +96,9 @@ void ompl_interface::OMPLInterface::setPlannerConfigurations(const planning_inte context_manager_.setPlannerConfigurations(pconfig2); } -ompl_interface::ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::getPlanningContext( - const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req) const +ompl_interface::ModelBasedPlanningContextPtr +ompl_interface::OMPLInterface::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req) const { moveit_msgs::MoveItErrorCodes dummy; return getPlanningContext(planning_scene, req, dummy); @@ -107,16 +109,8 @@ ompl_interface::OMPLInterface::getPlanningContext(const planning_scene::Planning const planning_interface::MotionPlanRequest& req, moveit_msgs::MoveItErrorCodes& error_code) const { - ModelBasedPlanningContextPtr ctx = context_manager_.getPlanningContext(planning_scene, req, error_code); - if (ctx) - configureContext(ctx); - return ctx; -} - -ompl_interface::ModelBasedPlanningContextPtr -ompl_interface::OMPLInterface::getPlanningContext(const std::string& config, const std::string& factory_type) const -{ - ModelBasedPlanningContextPtr ctx = context_manager_.getPlanningContext(config, factory_type); + ModelBasedPlanningContextPtr ctx = + context_manager_.getPlanningContext(planning_scene, req, error_code, nh_, use_constraints_approximations_); if (ctx) configureContext(ctx); return ctx; @@ -124,49 +118,9 @@ ompl_interface::OMPLInterface::getPlanningContext(const std::string& config, con void ompl_interface::OMPLInterface::configureContext(const ModelBasedPlanningContextPtr& context) const { - if (use_constraints_approximations_) - context->setConstraintsApproximations(constraints_library_); - else - context->setConstraintsApproximations(ConstraintsLibraryPtr()); context->simplifySolutions(simplify_solutions_); } -void ompl_interface::OMPLInterface::loadConstraintApproximations(const std::string& path) -{ - constraints_library_->loadConstraintApproximations(path); - std::stringstream ss; - constraints_library_->printConstraintApproximations(ss); - ROS_INFO_STREAM(ss.str()); -} - -void ompl_interface::OMPLInterface::saveConstraintApproximations(const std::string& path) -{ - constraints_library_->saveConstraintApproximations(path); -} - -bool ompl_interface::OMPLInterface::saveConstraintApproximations() -{ - std::string cpath; - if (nh_.getParam("constraint_approximations_path", cpath)) - { - saveConstraintApproximations(cpath); - return true; - } - ROS_WARN("ROS param 'constraint_approximations' not found. Unable to save constraint approximations"); - return false; -} - -bool ompl_interface::OMPLInterface::loadConstraintApproximations() -{ - std::string cpath; - if (nh_.getParam("constraint_approximations_path", cpath)) - { - loadConstraintApproximations(cpath); - return true; - } - return false; -} - void ompl_interface::OMPLInterface::loadConstraintSamplers() { constraint_sampler_manager_loader_.reset( @@ -181,14 +135,14 @@ bool ompl_interface::OMPLInterface::loadPlannerConfiguration( XmlRpc::XmlRpcValue xml_config; if (!nh_.getParam("planner_configs/" + planner_id, xml_config)) { - ROS_ERROR("Could not find the planner configuration '%s' on the param server", planner_id.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Could not find the planner configuration '%s' on the param server", planner_id.c_str()); return false; } if (xml_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("A planning configuration should be of type XmlRpc Struct type (for configuration '%s')", - planner_id.c_str()); + ROS_ERROR_NAMED(LOGNAME, "A planning configuration should be of type XmlRpc Struct type (for configuration '%s')", + planner_id.c_str()); return false; } @@ -230,10 +184,14 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() std::map specific_group_params; for (const std::string& k : KNOWN_GROUP_PARAMS) { - if (nh_.hasParam(group_name + "/" + k)) + std::string param_name{ group_name }; + param_name += "/"; + param_name += k; + + if (nh_.hasParam(param_name)) { std::string value; - if (nh_.getParam(group_name + "/" + k, value)) + if (nh_.getParam(param_name, value)) { if (!value.empty()) specific_group_params[k] = value; @@ -241,7 +199,7 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() } double value_d; - if (nh_.getParam(group_name + "/" + k, value_d)) + if (nh_.getParam(param_name, value_d)) { // convert to string using no locale specific_group_params[k] = moveit::core::toString(value_d); @@ -249,14 +207,14 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() } int value_i; - if (nh_.getParam(group_name + "/" + k, value_i)) + if (nh_.getParam(param_name, value_i)) { specific_group_params[k] = std::to_string(value_i); continue; } bool value_b; - if (nh_.getParam(group_name + "/" + k, value_b)) + if (nh_.getParam(param_name, value_b)) { specific_group_params[k] = std::to_string(value_b); continue; @@ -289,9 +247,10 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() { if (config_names.getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_ERROR("The planner_configs argument of a group configuration " - "should be an array of strings (for group '%s')", - group_name.c_str()); + ROS_ERROR_NAMED(LOGNAME, + "The planner_configs argument of a group configuration " + "should be an array of strings (for group '%s')", + group_name.c_str()); continue; } @@ -299,7 +258,8 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() { if (config_names[j].getType() != XmlRpc::XmlRpcValue::TypeString) { - ROS_ERROR("Planner configuration names must be of type string (for group '%s')", group_name.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Planner configuration names must be of type string (for group '%s')", + group_name.c_str()); continue; } @@ -314,10 +274,10 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() for (const std::pair& config : pconfig) { - ROS_DEBUG_STREAM_NAMED("parameters", "Parameters for configuration '" << config.first << "'"); + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Parameters for configuration '" << config.first << "'"); for (const std::pair& parameters : config.second.config) - ROS_DEBUG_STREAM_NAMED("parameters", " - " << parameters.first << " = " << parameters.second); + ROS_DEBUG_STREAM_NAMED(LOGNAME, " - " << parameters.first << " = " << parameters.second); } setPlannerConfigurations(pconfig); @@ -325,5 +285,5 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() void ompl_interface::OMPLInterface::printStatus() { - ROS_INFO("OMPL ROS interface is running."); + ROS_INFO_NAMED(LOGNAME, "OMPL ROS interface is running."); } diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner.cpp deleted file mode 100644 index 4f4390f496..0000000000 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include -#include -#include -#include -#include -#include -#include - -static const std::string PLANNER_NODE_NAME = "ompl_planning"; // name of node -static const std::string PLANNER_SERVICE_NAME = - "plan_kinematic_path"; // name of the advertised service (within the ~ namespace) -static const std::string ROBOT_DESCRIPTION = - "robot_description"; // name of the robot description (a param name, so it can be changed externally) - -class OMPLPlannerService -{ -public: - OMPLPlannerService(planning_scene_monitor::PlanningSceneMonitor& psm, bool debug = false) - : nh_("~"), psm_(psm), ompl_interface_(psm.getPlanningScene()->getRobotModel()), debug_(debug) - { - plan_service_ = nh_.advertiseService(PLANNER_SERVICE_NAME, &OMPLPlannerService::computePlan, this); - if (debug_) - { - pub_plan_ = nh_.advertise("display_motion_plan", 100); - pub_request_ = nh_.advertise("motion_plan_request", 100); - } - } - - bool computePlan(moveit_msgs::GetMotionPlan::Request& req, moveit_msgs::GetMotionPlan::Response& res) - { - ROS_INFO("Received new planning request..."); - if (debug_) - pub_request_.publish(req.motion_plan_request); - planning_interface::MotionPlanResponse response; - - ompl_interface::ModelBasedPlanningContextPtr context = - ompl_interface_.getPlanningContext(psm_.getPlanningScene(), req.motion_plan_request); - if (!context) - { - ROS_ERROR_STREAM_NAMED("computePlan", "No planning context found"); - return false; - } - context->clear(); - - bool result = context->solve(response); - - if (debug_) - { - if (result) - displaySolution(res.motion_plan_response); - std::stringstream ss; - ROS_INFO("%s", ss.str().c_str()); - } - return result; - } - - void displaySolution(const moveit_msgs::MotionPlanResponse& mplan_res) - { - moveit_msgs::DisplayTrajectory d; - d.model_id = psm_.getPlanningScene()->getRobotModel()->getName(); - d.trajectory_start = mplan_res.trajectory_start; - d.trajectory.resize(1, mplan_res.trajectory); - pub_plan_.publish(d); - } - - void status() - { - ompl_interface_.printStatus(); - ROS_INFO("Responding to planning and bechmark requests"); - if (debug_) - ROS_INFO("Publishing debug information"); - } - -private: - ros::NodeHandle nh_; - planning_scene_monitor::PlanningSceneMonitor& psm_; - ompl_interface::OMPLInterface ompl_interface_; - ros::ServiceServer plan_service_; - ros::ServiceServer display_states_service_; - ros::Publisher pub_plan_; - ros::Publisher pub_request_; - bool debug_; -}; - -int main(int argc, char** argv) -{ - ros::init(argc, argv, PLANNER_NODE_NAME); - - bool debug = false; - for (int i = 1; i < argc; ++i) - if (strncmp(argv[i], "--debug", 7) == 0) - debug = true; - - ros::AsyncSpinner spinner(1); - spinner.start(); - ros::NodeHandle nh; - - std::shared_ptr tf_buffer = std::make_shared(); - std::shared_ptr tf_listener = - std::make_shared(*tf_buffer, nh); - planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION, tf_buffer); - if (psm.getPlanningScene()) - { - psm.startWorldGeometryMonitor(); - psm.startSceneMonitor(); - psm.startStateMonitor(); - - OMPLPlannerService pservice(psm, debug); - pservice.status(); - ros::waitForShutdown(); - } - else - ROS_ERROR("Planning scene not configured"); - - return 0; -} diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index e111784818..4e213d775f 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -56,6 +56,8 @@ namespace ompl_interface { using namespace moveit_planners_ompl; +constexpr char LOGNAME[] = "ompl_planner_manager"; + #define OMPL_ROS_LOG(ros_log_level) \ { \ ROSCONSOLE_DEFINE_LOCATION(true, ros_log_level, ROSCONSOLE_NAME_PREFIX ".ompl"); \ @@ -102,7 +104,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager ompl::msg::useOutputHandler(output_handler_.get()); } - bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns) override + bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) override { if (!ns.empty()) nh_ = ros::NodeHandle(ns); @@ -177,7 +179,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager ROS_INFO_STREAM("Displaying states for context " << pc->getName()); const og::SimpleSetup &ss = pc->getOMPLSimpleSetup(); ob::ValidStateSamplerPtr vss = ss.getSpaceInformation()->allocValidStateSampler(); - robot_state::RobotState robot_state = pc->getPlanningScene()->getCurrentState(); + moveit::core::RobotState robot_state = pc->getPlanningScene()->getCurrentState(); ob::ScopedState<> rstate1(ss.getStateSpace()); ob::ScopedState<> rstate2(ss.getStateSpace()); ros::WallDuration wait(2); @@ -191,7 +193,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager pc->getOMPLStateSpace()->copyToRobotState(robot_state, rstate1.get()); robot_state.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms(); moveit_msgs::DisplayRobotState state_msg; - robot_state::robotStateToRobotStateMsg(robot_state, state_msg.state); + moveit::core::robotStateToRobotStateMsg(robot_state, state_msg.state); pub_valid_states_.publish(state_msg); n = (n + 1) % 2; if (n == 0) @@ -208,7 +210,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager msg.model_id = pc->getRobotModel()->getName(); msg.trajectory.resize(1); traj.getRobotTrajectoryMsg(msg.trajectory[0]); - robot_state::robotStateToRobotStateMsg(traj.getFirstWayPoint(), msg.trajectory_start); + moveit::core::robotStateToRobotStateMsg(traj.getFirstWayPoint(), msg.trajectory_start); pub_valid_traj_.publish(msg); } rstate2 = rstate1; @@ -224,7 +226,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager { ompl::base::PlannerData pd(pc->getOMPLSimpleSetup()->getSpaceInformation()); pc->getOMPLSimpleSetup()->getPlannerData(pd); - robot_state::RobotState robot_state = planning_scene->getCurrentState(); + moveit::core::RobotState robot_state = planning_scene->getCurrentState(); visualization_msgs::MarkerArray arr; std_msgs::ColorRGBA color; color.r = 1.0f; @@ -259,19 +261,19 @@ class OMPLPlannerManager : public planning_interface::PlannerManager } */ - void dynamicReconfigureCallback(OMPLDynamicReconfigureConfig& config, uint32_t level) + void dynamicReconfigureCallback(OMPLDynamicReconfigureConfig& config, uint32_t /*level*/) { if (config.link_for_exploration_tree.empty() && !planner_data_link_name_.empty()) { pub_markers_.shutdown(); planner_data_link_name_.clear(); - ROS_INFO("Not displaying OMPL exploration data structures."); + ROS_INFO_NAMED(LOGNAME, "Not displaying OMPL exploration data structures."); } else if (!config.link_for_exploration_tree.empty() && planner_data_link_name_.empty()) { pub_markers_ = nh_.advertise("ompl_planner_data_marker_array", 5); planner_data_link_name_ = config.link_for_exploration_tree; - ROS_INFO("Displaying OMPL exploration data structures for %s", planner_data_link_name_.c_str()); + ROS_INFO_NAMED(LOGNAME, "Displaying OMPL exploration data structures for %s", planner_data_link_name_.c_str()); } ompl_interface_->simplifySolutions(config.simplify_solutions); diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space.cpp index 476a90684d..8182da68f5 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp index cbaa560146..a351fcdf65 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -43,8 +43,8 @@ ompl_interface::JointModelStateSpaceFactory::JointModelStateSpaceFactory() : Mod } int ompl_interface::JointModelStateSpaceFactory::canRepresentProblem( - const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const robot_model::RobotModelConstPtr& robot_model) const + const std::string& /*group*/, const moveit_msgs::MotionPlanRequest& /*req*/, + const moveit::core::RobotModelConstPtr& /*robot_model*/) const { return 100; } @@ -52,5 +52,5 @@ int ompl_interface::JointModelStateSpaceFactory::canRepresentProblem( ompl_interface::ModelBasedStateSpacePtr ompl_interface::JointModelStateSpaceFactory::allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const { - return ModelBasedStateSpacePtr(new JointModelStateSpace(space_spec)); + return std::make_shared(space_spec); } diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp index 847ffb9f12..7c19e05029 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp @@ -1,42 +1,47 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ #include #include +namespace ompl_interface +{ +constexpr char LOGNAME[] = "model_based_state_space"; +} // namespace ompl_interface + ompl_interface::ModelBasedStateSpace::ModelBasedStateSpace(ModelBasedStateSpaceSpecification spec) : ompl::base::StateSpace(), spec_(std::move(spec)) { @@ -49,8 +54,7 @@ ompl_interface::ModelBasedStateSpace::ModelBasedStateSpace(ModelBasedStateSpaceS // make sure we have bounds for every joint stored within the spec (use default bounds if not specified) if (!spec_.joint_bounds_.empty() && spec_.joint_bounds_.size() != joint_model_vector_.size()) { - ROS_ERROR_NAMED("model_based_state_space", - "Joint group '%s' has incorrect bounds specified. Using the default bounds instead.", + ROS_ERROR_NAMED(LOGNAME, "Joint group '%s' has incorrect bounds specified. Using the default bounds instead.", spec_.joint_model_group_->getName().c_str()); spec_.joint_bounds_.clear(); } @@ -86,7 +90,7 @@ double ompl_interface::ModelBasedStateSpace::getTagSnapToSegment() const void ompl_interface::ModelBasedStateSpace::setTagSnapToSegment(double snap) { if (snap < 0.0 || snap > 1.0) - ROS_WARN_NAMED("model_based_state_space", + ROS_WARN_NAMED(LOGNAME, "Snap to segment for tags is a ratio. It's value must be between 0.0 and 1.0. " "Value remains as previously set (%lf)", tag_snap_to_segment_); @@ -133,14 +137,13 @@ void ompl_interface::ModelBasedStateSpace::serialize(void* serialization, const void ompl_interface::ModelBasedStateSpace::deserialize(ompl::base::State* state, const void* serialization) const { state->as()->tag = *reinterpret_cast(serialization); - memcpy(state->as()->values, reinterpret_cast(serialization) + sizeof(int), - state_values_size_); + memcpy(state->as()->values, reinterpret_cast(serialization) + sizeof(int), state_values_size_); } unsigned int ompl_interface::ModelBasedStateSpace::getDimension() const { unsigned int d = 0; - for (const robot_model::JointModel* i : joint_model_vector_) + for (const moveit::core::JointModel* i : joint_model_vector_) d += i->getStateSpaceDimension(); return d; } @@ -153,7 +156,7 @@ double ompl_interface::ModelBasedStateSpace::getMaximumExtent() const double ompl_interface::ModelBasedStateSpace::getMeasure() const { double m = 1.0; - for (const robot_model::JointModel::Bounds* bounds : spec_.joint_bounds_) + for (const moveit::core::JointModel::Bounds* bounds : spec_.joint_bounds_) { for (const moveit::core::VariableBounds& bound : *bounds) { @@ -227,14 +230,14 @@ void ompl_interface::ModelBasedStateSpace::setPlanningVolume(double minX, double double minZ, double maxZ) { for (std::size_t i = 0; i < joint_model_vector_.size(); ++i) - if (joint_model_vector_[i]->getType() == robot_model::JointModel::PLANAR) + if (joint_model_vector_[i]->getType() == moveit::core::JointModel::PLANAR) { joint_bounds_storage_[i][0].min_position_ = minX; joint_bounds_storage_[i][0].max_position_ = maxX; joint_bounds_storage_[i][1].min_position_ = minY; joint_bounds_storage_[i][1].max_position_ = maxY; } - else if (joint_model_vector_[i]->getType() == robot_model::JointModel::FLOATING) + else if (joint_model_vector_[i]->getType() == moveit::core::JointModel::FLOATING) { joint_bounds_storage_[i][0].min_position_ = minX; joint_bounds_storage_[i][0].max_position_ = maxX; @@ -250,8 +253,8 @@ ompl::base::StateSamplerPtr ompl_interface::ModelBasedStateSpace::allocDefaultSt class DefaultStateSampler : public ompl::base::StateSampler { public: - DefaultStateSampler(const ompl::base::StateSpace* space, const robot_model::JointModelGroup* group, - const robot_model::JointBoundsVector* joint_bounds) + DefaultStateSampler(const ompl::base::StateSpace* space, const moveit::core::JointModelGroup* group, + const moveit::core::JointBoundsVector* joint_bounds) : ompl::base::StateSampler(space), joint_model_group_(group), joint_bounds_(joint_bounds) { } @@ -276,8 +279,8 @@ ompl::base::StateSamplerPtr ompl_interface::ModelBasedStateSpace::allocDefaultSt protected: random_numbers::RandomNumberGenerator moveit_rng_; - const robot_model::JointModelGroup* joint_model_group_; - const robot_model::JointBoundsVector* joint_bounds_; + const moveit::core::JointModelGroup* joint_model_group_; + const moveit::core::JointBoundsVector* joint_bounds_; }; return ompl::base::StateSamplerPtr(static_cast( @@ -291,7 +294,7 @@ void ompl_interface::ModelBasedStateSpace::printSettings(std::ostream& out) cons void ompl_interface::ModelBasedStateSpace::printState(const ompl::base::State* state, std::ostream& out) const { - for (const robot_model::JointModel* j : joint_model_vector_) + for (const moveit::core::JointModel* j : joint_model_vector_) { out << j->getName() << " = "; const int idx = spec_.joint_model_group_->getVariableGroupIndex(j->getName()); @@ -315,7 +318,7 @@ void ompl_interface::ModelBasedStateSpace::printState(const ompl::base::State* s out << "Tag: " << state->as()->tag << std::endl; } -void ompl_interface::ModelBasedStateSpace::copyToRobotState(robot_state::RobotState& rstate, +void ompl_interface::ModelBasedStateSpace::copyToRobotState(moveit::core::RobotState& rstate, const ompl::base::State* state) const { rstate.setJointGroupPositions(spec_.joint_model_group_, state->as()->values); @@ -323,7 +326,7 @@ void ompl_interface::ModelBasedStateSpace::copyToRobotState(robot_state::RobotSt } void ompl_interface::ModelBasedStateSpace::copyToOMPLState(ompl::base::State* state, - const robot_state::RobotState& rstate) const + const moveit::core::RobotState& rstate) const { rstate.copyJointGroupPositions(spec_.joint_model_group_, state->as()->values); // clear any cached info (such as validity known or not) @@ -331,13 +334,13 @@ void ompl_interface::ModelBasedStateSpace::copyToOMPLState(ompl::base::State* st } void ompl_interface::ModelBasedStateSpace::copyJointToOMPLState(ompl::base::State* state, - const robot_state::RobotState& robot_state, + const moveit::core::RobotState& robot_state, const moveit::core::JointModel* joint_model, int ompl_state_joint_index) const { // Copy one joint (multiple variables possibly) memcpy(getValueAddressAtIndex(state, ompl_state_joint_index), - robot_state.getVariablePositions() + joint_model->getFirstVariableIndex() * sizeof(double), + robot_state.getVariablePositions() + joint_model->getFirstVariableIndex(), joint_model->getVariableCount() * sizeof(double)); // clear any cached info (such as validity known or not) diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp index de23960b62..1a3158654b 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index 7433f35ec0..771dd5973c 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -40,6 +40,11 @@ #include +namespace ompl_interface +{ +constexpr char LOGNAME[] = "pose_model_state_space"; +} // namespace ompl_interface + const std::string ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE = "PoseModel"; ompl_interface::PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec) @@ -51,13 +56,13 @@ ompl_interface::PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSp poses_.emplace_back(spec.joint_model_group_, spec.joint_model_group_->getGroupKinematics().first); else if (!spec.joint_model_group_->getGroupKinematics().second.empty()) { - const robot_model::JointModelGroup::KinematicsSolverMap& m = spec.joint_model_group_->getGroupKinematics().second; + const moveit::core::JointModelGroup::KinematicsSolverMap& m = spec.joint_model_group_->getGroupKinematics().second; for (const auto& it : m) poses_.emplace_back(it.first, it.second); } if (poses_.empty()) - ROS_ERROR_NAMED("pose_model_state_space", "No kinematics solvers specified. Unable to construct a " - "PoseModelStateSpace"); + ROS_ERROR_NAMED(LOGNAME, "No kinematics solvers specified. Unable to construct a " + "PoseModelStateSpace"); else std::sort(poses_.begin(), poses_.end()); setName(getName() + "_" + PARAMETERIZATION_TYPE); @@ -177,7 +182,7 @@ void ompl_interface::PoseModelStateSpace::setPlanningVolume(double minX, double } ompl_interface::PoseModelStateSpace::PoseComponent::PoseComponent( - const robot_model::JointModelGroup* subgroup, const robot_model::JointModelGroup::KinematicsSolver& k) + const moveit::core::JointModelGroup* subgroup, const moveit::core::JointModelGroup::KinematicsSolver& k) : subgroup_(subgroup), kinematics_solver_(k.allocator_(subgroup)), bijection_(k.bijection_) { state_space_.reset(new ompl::base::SE3StateSpace()); @@ -338,7 +343,7 @@ ompl::base::StateSamplerPtr ompl_interface::PoseModelStateSpace::allocDefaultSta } void ompl_interface::PoseModelStateSpace::copyToOMPLState(ompl::base::State* state, - const robot_state::RobotState& rstate) const + const moveit::core::RobotState& rstate) const { ModelBasedStateSpace::copyToOMPLState(state, rstate); state->as()->setJointsComputed(true); diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp index 80dd48b35d..5d912db8f2 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp @@ -44,13 +44,13 @@ ompl_interface::PoseModelStateSpaceFactory::PoseModelStateSpaceFactory() : Model int ompl_interface::PoseModelStateSpaceFactory::canRepresentProblem( const std::string& group, const moveit_msgs::MotionPlanRequest& req, - const robot_model::RobotModelConstPtr& robot_model) const + const moveit::core::RobotModelConstPtr& robot_model) const { - const robot_model::JointModelGroup* jmg = robot_model->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); if (jmg) { - const std::pair& - slv = jmg->getGroupKinematics(); + const std::pair& slv = jmg->getGroupKinematics(); bool ik = false; // check that we have a direct means to compute IK if (slv.first) @@ -86,5 +86,5 @@ int ompl_interface::PoseModelStateSpaceFactory::canRepresentProblem( ompl_interface::ModelBasedStateSpacePtr ompl_interface::PoseModelStateSpaceFactory::allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const { - return ModelBasedStateSpacePtr(new PoseModelStateSpace(space_spec)); + return std::make_shared(space_spec); } diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 88a0666871..75f870a493 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -37,10 +37,9 @@ #include #include #include -#include -#include #include +#include #include #include #include @@ -76,6 +75,8 @@ using namespace std::placeholders; namespace ompl_interface { +constexpr char LOGNAME[] = "planning_context_manager"; + struct PlanningContextManager::CachedContexts { std::map, std::vector > contexts_; @@ -84,7 +85,145 @@ struct PlanningContextManager::CachedContexts } // namespace ompl_interface -ompl_interface::PlanningContextManager::PlanningContextManager(robot_model::RobotModelConstPtr robot_model, +ompl_interface::MultiQueryPlannerAllocator::~MultiQueryPlannerAllocator() +{ + // Store all planner data + for (const auto& entry : planner_data_storage_paths_) + { + ROS_INFO("Storing planner data"); + ob::PlannerData data(planners_[entry.first]->getSpaceInformation()); + planners_[entry.first]->getPlannerData(data); + storage_.store(data, entry.second.c_str()); + } +} + +template +ompl::base::PlannerPtr ompl_interface::MultiQueryPlannerAllocator::allocatePlanner( + const ob::SpaceInformationPtr& si, const std::string& new_name, const ModelBasedPlanningContextSpecification& spec) +{ + // Store planner instance if multi-query planning is enabled + auto cfg = spec.config_; + auto it = cfg.find("multi_query_planning_enabled"); + bool multi_query_planning_enabled = false; + if (it != cfg.end()) + { + multi_query_planning_enabled = boost::lexical_cast(it->second); + cfg.erase(it); + } + if (multi_query_planning_enabled) + { + // If we already have an instance, use that one + auto planner_map_it = planners_.find(new_name); + if (planner_map_it != planners_.end()) + return planner_map_it->second; + + // Certain multi-query planners allow loading and storing the generated planner data. This feature can be + // selectively enabled for loading and storing using the bool parameters 'load_planner_data' and + // 'store_planner_data'. The storage file path is set using the parameter 'planner_data_path'. + // File read and write access are handled by the PlannerDataStorage class. If the file path is invalid + // an error message is printed and the planner is constructed/destructed with default values. + it = cfg.find("load_planner_data"); + bool load_planner_data = false; + if (it != cfg.end()) + { + load_planner_data = boost::lexical_cast(it->second); + cfg.erase(it); + } + it = cfg.find("store_planner_data"); + bool store_planner_data = false; + if (it != cfg.end()) + { + store_planner_data = boost::lexical_cast(it->second); + cfg.erase(it); + } + it = cfg.find("planner_data_path"); + std::string planner_data_path; + if (it != cfg.end()) + { + planner_data_path = it->second; + cfg.erase(it); + } + // Store planner instance for multi-query use + planners_[new_name] = + allocatePlannerImpl(si, new_name, spec, load_planner_data, store_planner_data, planner_data_path); + return planners_[new_name]; + } + else + { + // Return single-shot planner instance + return allocatePlannerImpl(si, new_name, spec); + } +} + +template +ompl::base::PlannerPtr ompl_interface::MultiQueryPlannerAllocator::allocatePlannerImpl( + const ob::SpaceInformationPtr& si, const std::string& new_name, const ModelBasedPlanningContextSpecification& spec, + bool load_planner_data, bool store_planner_data, const std::string& file_path) +{ + ob::PlannerPtr planner; + // Try to initialize planner with loaded planner data + if (load_planner_data) + { + ROS_INFO("Loading planner data"); + ob::PlannerData data(si); + storage_.load(file_path.c_str(), data); + planner.reset(allocatePersistentPlanner(data)); + if (!planner) + ROS_ERROR_NAMED(LOGNAME, + "Creating a '%s' planner from persistent data is not supported. Going to create a new instance.", + new_name.c_str()); + } + if (!planner) + planner.reset(new T(si)); + if (!new_name.empty()) + planner->setName(new_name); + planner->params().setParams(spec.config_, true); + // Remember which planner instances to store when the destructor is called + if (store_planner_data) + planner_data_storage_paths_[new_name] = file_path; + return planner; +} + +// default implementation +template +inline ompl::base::Planner* +ompl_interface::MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& /*data*/) +{ + return nullptr; +}; +// TODO: remove when ROS Melodic and older are no longer supported +// namespace is scoped instead of global because of GCC bug 56480 +#if OMPL_VERSION_VALUE >= 1005000 +namespace ompl_interface +{ +template <> +inline ompl::base::Planner* +MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) +{ + return new og::PRM(data); +}; +template <> +inline ompl::base::Planner* +MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) +{ + return new og::PRMstar(data); +}; +template <> +inline ompl::base::Planner* +MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) +{ + return new og::LazyPRM(data); +}; +template <> +inline ompl::base::Planner* +MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) +{ + return new og::LazyPRMstar(data); +}; +} // namespace ompl_interface +#endif + +ompl_interface::PlanningContextManager::PlanningContextManager(moveit::core::RobotModelConstPtr robot_model, constraint_samplers::ConstraintSamplerManagerPtr csm) : robot_model_(std::move(robot_model)) , constraint_sampler_manager_(std::move(csm)) @@ -102,23 +241,6 @@ ompl_interface::PlanningContextManager::PlanningContextManager(robot_model::Robo ompl_interface::PlanningContextManager::~PlanningContextManager() = default; -namespace -{ -using namespace ompl_interface; - -template -static ompl::base::PlannerPtr allocatePlanner(const ob::SpaceInformationPtr& si, const std::string& new_name, - const ModelBasedPlanningContextSpecification& spec) -{ - ompl::base::PlannerPtr planner(new T(si)); - if (!new_name.empty()) - planner->setName(new_name); - planner->params().setParams(spec.config_, true); - planner->setup(); - return planner; -} -} // namespace - ompl_interface::ConfiguredPlannerAllocator ompl_interface::PlanningContextManager::plannerSelector(const std::string& planner) const { @@ -127,86 +249,47 @@ ompl_interface::PlanningContextManager::plannerSelector(const std::string& plann return it->second; else { - ROS_ERROR_NAMED("planning_context_manager", "Unknown planner: '%s'", planner.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Unknown planner: '%s'", planner.c_str()); return ConfiguredPlannerAllocator(); } } +template +void ompl_interface::PlanningContextManager::registerPlannerAllocatorHelper(const std::string& planner_id) +{ + registerPlannerAllocator(planner_id, [&](const ob::SpaceInformationPtr& si, const std::string& new_name, + const ModelBasedPlanningContextSpecification& spec) { + return planner_allocator_.allocatePlanner(si, new_name, spec); + }); +} + void ompl_interface::PlanningContextManager::registerDefaultPlanners() { - registerPlannerAllocator( // - "geometric::RRT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::RRTConnect", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::LazyRRT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::TRRT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::EST", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::SBL", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::KPIECE", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::BKPIECE", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::LBKPIECE", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::RRTstar", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::PRM", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::PRMstar", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::FMT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::BFMT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::PDST", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::STRIDE", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::BiTRRT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::LBTRRT", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::BiEST", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::ProjEST", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::LazyPRM", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::LazyPRMstar", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::SPARS", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - registerPlannerAllocator( // - "geometric::SPARStwo", // - std::bind(&allocatePlanner, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + registerPlannerAllocatorHelper("geometric::AnytimePathShortening"); + registerPlannerAllocatorHelper("geometric::BFMT"); + registerPlannerAllocatorHelper("geometric::BiEST"); + registerPlannerAllocatorHelper("geometric::BiTRRT"); + registerPlannerAllocatorHelper("geometric::BKPIECE"); + registerPlannerAllocatorHelper("geometric::EST"); + registerPlannerAllocatorHelper("geometric::FMT"); + registerPlannerAllocatorHelper("geometric::KPIECE"); + registerPlannerAllocatorHelper("geometric::LazyPRM"); + registerPlannerAllocatorHelper("geometric::LazyPRMstar"); + registerPlannerAllocatorHelper("geometric::LazyRRT"); + registerPlannerAllocatorHelper("geometric::LBKPIECE"); + registerPlannerAllocatorHelper("geometric::LBTRRT"); + registerPlannerAllocatorHelper("geometric::PDST"); + registerPlannerAllocatorHelper("geometric::PRM"); + registerPlannerAllocatorHelper("geometric::PRMstar"); + registerPlannerAllocatorHelper("geometric::ProjEST"); + registerPlannerAllocatorHelper("geometric::RRT"); + registerPlannerAllocatorHelper("geometric::RRTConnect"); + registerPlannerAllocatorHelper("geometric::RRTstar"); + registerPlannerAllocatorHelper("geometric::SBL"); + registerPlannerAllocatorHelper("geometric::SPARS"); + registerPlannerAllocatorHelper("geometric::SPARStwo"); + registerPlannerAllocatorHelper("geometric::STRIDE"); + registerPlannerAllocatorHelper("geometric::TRRT"); } void ompl_interface::PlanningContextManager::registerDefaultStateSpaces() @@ -226,28 +309,9 @@ void ompl_interface::PlanningContextManager::setPlannerConfigurations( planner_configs_ = pconfig; } -ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext( - const std::string& config, const std::string& factory_type) const -{ - auto pc = planner_configs_.find(config); - - if (pc != planner_configs_.end()) - { - moveit_msgs::MotionPlanRequest req; // dummy request with default values - return getPlanningContext( - pc->second, - std::bind(&PlanningContextManager::getStateSpaceFactory1, this, std::placeholders::_1, factory_type), req); - } - else - { - ROS_ERROR_NAMED("planning_context_manager", "Planning configuration '%s' was not found", config.c_str()); - return ModelBasedPlanningContextPtr(); - } -} - ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext( const planning_interface::PlannerConfigurationSettings& config, - const StateSpaceFactoryTypeSelector& factory_selector, const moveit_msgs::MotionPlanRequest& req) const + const StateSpaceFactoryTypeSelector& factory_selector, const moveit_msgs::MotionPlanRequest& /*req*/) const { const ompl_interface::ModelBasedStateSpaceFactoryPtr& factory = factory_selector(config.group); @@ -262,7 +326,7 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana for (const ModelBasedPlanningContextPtr& cached_context : cached_contexts->second) if (cached_context.unique()) { - ROS_DEBUG_NAMED("planning_context_manager", "Reusing cached planning context"); + ROS_DEBUG_NAMED(LOGNAME, "Reusing cached planning context"); context = cached_context; break; } @@ -282,28 +346,8 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana // Choose the correct simple setup type to load context_spec.ompl_simple_setup_.reset(new ompl::geometric::SimpleSetup(context_spec.state_space_)); - bool state_validity_cache = true; - if (config.config.find("subspaces") != config.config.end()) - { - context_spec.config_.erase("subspaces"); - // if the planner operates at subspace level the cache may be unsafe - state_validity_cache = false; - boost::char_separator sep(" "); - boost::tokenizer > tok(config.config.at("subspaces"), sep); - for (boost::tokenizer >::iterator beg = tok.begin(); beg != tok.end(); ++beg) - { - const ompl_interface::ModelBasedStateSpaceFactoryPtr& sub_fact = factory_selector(*beg); - if (sub_fact) - { - ModelBasedStateSpaceSpecification sub_space_spec(robot_model_, *beg); - context_spec.subspaces_.push_back(sub_fact->getNewStateSpace(sub_space_spec)); - } - } - } - - ROS_DEBUG_NAMED("planning_context_manager", "Creating new planning context"); + ROS_DEBUG_NAMED(LOGNAME, "Creating new planning context"); context.reset(new ModelBasedPlanningContext(config.name, context_spec)); - context->useStateValidityCache(state_validity_cache); { std::unique_lock slock(cached_contexts_->lock_); cached_contexts_->contexts_[std::make_pair(config.name, factory->getType())].push_back(context); @@ -323,59 +367,59 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana return context; } -const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningContextManager::getStateSpaceFactory1( - const std::string& /* dummy */, const std::string& factory_type) const +const ompl_interface::ModelBasedStateSpaceFactoryPtr& +ompl_interface::PlanningContextManager::getStateSpaceFactory1(const std::string& /* dummy */, + const std::string& factory_type) const { auto f = factory_type.empty() ? state_space_factories_.begin() : state_space_factories_.find(factory_type); if (f != state_space_factories_.end()) return f->second; else { - ROS_ERROR_NAMED("planning_context_manager", "Factory of type '%s' was not found", factory_type.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Factory of type '%s' was not found", factory_type.c_str()); static const ModelBasedStateSpaceFactoryPtr EMPTY; return EMPTY; } } -const ompl_interface::ModelBasedStateSpaceFactoryPtr& ompl_interface::PlanningContextManager::getStateSpaceFactory2( - const std::string& group, const moveit_msgs::MotionPlanRequest& req) const +const ompl_interface::ModelBasedStateSpaceFactoryPtr& +ompl_interface::PlanningContextManager::getStateSpaceFactory2(const std::string& group, + const moveit_msgs::MotionPlanRequest& req) const { // find the problem representation to use auto best = state_space_factories_.end(); - int prev_priority = -1; + int prev_priority = 0; for (auto it = state_space_factories_.begin(); it != state_space_factories_.end(); ++it) { int priority = it->second->canRepresentProblem(group, req, robot_model_); - if (priority > 0) - if (best == state_space_factories_.end() || priority > prev_priority) - { - best = it; - prev_priority = priority; - } + if (priority > prev_priority) + { + best = it; + prev_priority = priority; + } } if (best == state_space_factories_.end()) { - ROS_ERROR_NAMED("planning_context_manager", "There are no known state spaces that can represent the given planning " - "problem"); + ROS_ERROR_NAMED(LOGNAME, "There are no known state spaces that can represent the given planning " + "problem"); static const ModelBasedStateSpaceFactoryPtr EMPTY; return EMPTY; } else { - ROS_DEBUG_NAMED("planning_context_manager", "Using '%s' parameterization for solving problem", best->first.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "Using '%s' parameterization for solving problem", best->first.c_str()); return best->second; } } -ompl_interface::ModelBasedPlanningContextPtr -ompl_interface::PlanningContextManager::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::MotionPlanRequest& req, - moveit_msgs::MoveItErrorCodes& error_code) const +ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext( + const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::MotionPlanRequest& req, + moveit_msgs::MoveItErrorCodes& error_code, const ros::NodeHandle& nh, bool use_constraints_approximation) const { if (req.group_name.empty()) { - ROS_ERROR_NAMED("planning_context_manager", "No group specified to plan for"); + ROS_ERROR_NAMED(LOGNAME, "No group specified to plan for"); error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return ModelBasedPlanningContextPtr(); } @@ -384,7 +428,7 @@ ompl_interface::PlanningContextManager::getPlanningContext(const planning_scene: if (!planning_scene) { - ROS_ERROR_NAMED("planning_context_manager", "No planning scene supplied as input"); + ROS_ERROR_NAMED(LOGNAME, "No planning scene supplied as input"); return ModelBasedPlanningContextPtr(); } @@ -396,7 +440,7 @@ ompl_interface::PlanningContextManager::getPlanningContext(const planning_scene: req.group_name + "[" + req.planner_id + "]" : req.planner_id); if (pc == planner_configs_.end()) - ROS_WARN_NAMED("planning_context_manager", + ROS_WARN_NAMED(LOGNAME, "Cannot find planning configuration for group '%s' using planner '%s'. Will use defaults instead.", req.group_name.c_str(), req.planner_id.c_str()); } @@ -406,8 +450,7 @@ ompl_interface::PlanningContextManager::getPlanningContext(const planning_scene: pc = planner_configs_.find(req.group_name); if (pc == planner_configs_.end()) { - ROS_ERROR_NAMED("planning_context_manager", "Cannot find planning configuration for group '%s'", - req.group_name.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Cannot find planning configuration for group '%s'", req.group_name.c_str()); return ModelBasedPlanningContextPtr(); } } @@ -434,7 +477,7 @@ ompl_interface::PlanningContextManager::getPlanningContext(const planning_scene: { context->clear(); - robot_state::RobotStatePtr start_state = planning_scene->getCurrentStateUpdated(req.start_state); + moveit::core::RobotStatePtr start_state = planning_scene->getCurrentStateUpdated(req.start_state); // Setup the context context->setPlanningScene(planning_scene); @@ -450,13 +493,13 @@ ompl_interface::PlanningContextManager::getPlanningContext(const planning_scene: try { - context->configure(); - ROS_DEBUG_NAMED("planning_context_manager", "%s: New planning context is set.", context->getName().c_str()); + context->configure(nh, use_constraints_approximation); + ROS_DEBUG_NAMED(LOGNAME, "%s: New planning context is set.", context->getName().c_str()); error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; } catch (ompl::Exception& ex) { - ROS_ERROR_NAMED("planning_context_manager", "OMPL encountered an error: %s", ex.what()); + ROS_ERROR_NAMED(LOGNAME, "OMPL encountered an error: %s", ex.what()); context.reset(); } } diff --git a/moveit_planners/ompl/ompl_interface/test/load_test_robot.h b/moveit_planners/ompl/ompl_interface/test/load_test_robot.h new file mode 100644 index 0000000000..94c606a14a --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/load_test_robot.h @@ -0,0 +1,130 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of KU Leuven nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ + +#pragma once + +#include +#include +#include + +namespace ompl_interface_testing +{ +/** \brief Robot independent test class setup + * + * The desired robot tests can be impelmented in a derived class in a generic way. + * + * It is implemented this way to avoid the ros specific test framework + * outside moveit_ros. + * + * (This is an (uglier) alternative to using the rostest framework + * and reading the robot settings from the parameter server. + * Then we have several rostest launch files that load the parameters + * for a specific robot and run the same compiled tests for all robots.) + * + * based on + * https://stackoverflow.com/questions/38207346/specify-constructor-arguments-for-a-google-test-fixture/38218657 + * (answer by PiotrNycz) + * + * --- example.cpp --- + * + * #include + * #include "load_test_robot.h" + * + * class GenericTests : public ompl_interface_testing::LoadTestRobot, public testing::Test + * { + * public: + * GenericTests(const std::string& robot_name, const std::string& group_name) + * : LoadTestRobot(robot_name, group_name) { } + * void SetUp() override { } + * void TearDown() override { } + * + * void someTest() + * { + * // use robot_model_, robot_state_, .. here + * EXPECT_TRUE(true); + * } + * }; + * + * // now you can run `someTest()` on different robots: + * + * class PandaTest : public GenericTests + * { + * protected: + * PandaTest() : GenericTests("panda", "panda_arm") { } + * }; + * + * TEST_F(PandaTest, someTest) + * { + * someTest(); + * } + * + * --- end example.cpp --- + * + * + * */ +class LoadTestRobot +{ +protected: + LoadTestRobot(const std::string& robot_name, const std::string& group_name) + : group_name_(group_name), robot_name_(robot_name) + { + // load robot + robot_model_ = moveit::core::loadTestingRobotModel(robot_name_); + robot_state_ = std::make_shared(robot_model_); + robot_state_->setToDefaultValues(); + joint_model_group_ = robot_state_->getJointModelGroup(group_name_); + + // extract useful parameters for tests + num_dofs_ = joint_model_group_->getVariableCount(); + ee_link_name_ = joint_model_group_->getLinkModelNames().back(); + base_link_name_ = robot_model_->getRootLinkName(); + + ROS_INFO_STREAM("Created test robot named: " << robot_name_ << " for planning group " << group_name_); + } + +protected: + const std::string group_name_; + const std::string robot_name_; + + moveit::core::RobotModelPtr robot_model_; + robot_state::RobotStatePtr robot_state_; + const robot_state::JointModelGroup* joint_model_group_; + + std::size_t num_dofs_; + std::string base_link_name_; + std::string ee_link_name_; +}; +} // namespace ompl_interface_testing diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp new file mode 100644 index 0000000000..9bdd7c303d --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp @@ -0,0 +1,358 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of KU Leuven nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ + +/** Test the creation of a ModelBasedPlanningContext through the PlanningContextManager. + * + * The tests use a default robot state as start state, and then moves the last joint 0.1 radians/meters to create a + * joint goal. This creates an extremely simple planning problem to test general mechanics of the interface. + * + * In the test with path constraints, we create an orientation constraint around the start orientation of the + * end-effector, with a tolerance on the rotation larger than the change in the last joint of the goal state. + * This makes sure the path constraints are easy to satisfy. + * + * TODO(jeroendm) I also tried something similar with position constraints, but get a segmentation fault + * that occurs in the 'geometric_shapes' package, in the method 'useDimensions' in 'bodies.h'. + * git permalink: + * https://github.com/ros-planning/geometric_shapes/blob/df0478870b8592ce789ee1919f3124058c4327d7/include/geometric_shapes/bodies.h#L196 + * + **/ + +#include "load_test_robot.h" + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include + +/** \brief Generic implementation of the tests that can be executed on different robots. **/ +class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public testing::Test +{ +public: + TestPlanningContext(const std::string& robot_name, const std::string& group_name) + : LoadTestRobot(robot_name, group_name) + { + } + + // /*************************************************************************** + // * START Test implementations + // * ************************************************************************/ + + void testSimpleRequest(const std::vector& start, const std::vector& goal) + { + // create all the test specific input necessary to make the getPlanningContext call possible + planning_interface::PlannerConfigurationSettings pconfig_settings; + pconfig_settings.group = group_name_; + pconfig_settings.name = group_name_; + pconfig_settings.config = { { "enforce_joint_model_state_space", "0" } }; + + planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } }; + moveit_msgs::MoveItErrorCodes error_code; + planning_interface::MotionPlanRequest request = createRequest(start, goal); + + // setup the planning context manager + ompl_interface::PlanningContextManager pcm(robot_model_, constraint_sampler_manager_); + pcm.setPlannerConfigurations(pconfig_map); + + // see if it returns the expected planning context + auto pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_handle_, false); + + // the planning context should have a simple setup created + EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); + + // the OMPL state space in the planning context should be of type JointModelStateSpace + EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); + + // there did not magically appear path constraints in the planning context + EXPECT_TRUE(pc->getPathConstraints()->empty()); + + // solve the planning problem + planning_interface::MotionPlanDetailedResponse res; + ASSERT_TRUE(pc->solve(res)); + } + + void testPathConstraints(const std::vector& start, const std::vector& goal) + { + // create all the test specific input necessary to make the getPlanningContext call possible + planning_interface::PlannerConfigurationSettings pconfig_settings; + pconfig_settings.group = group_name_; + pconfig_settings.name = group_name_; + pconfig_settings.config = { { "enforce_joint_model_state_space", "0" } }; + + planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } }; + moveit_msgs::MoveItErrorCodes error_code; + planning_interface::MotionPlanRequest request = createRequest(start, goal); + + // give it some more time, as rejection sampling of the path constraints occasionally takes some time + request.allowed_planning_time = 10.0; + + // create path constraints around start state, to make sure they are satisfied + robot_state_->setJointGroupPositions(joint_model_group_, start); + Eigen::Isometry3d ee_pose = robot_state_->getGlobalLinkTransform(ee_link_name_); + geometry_msgs::Quaternion ee_orientation; + tf::quaternionEigenToMsg(Eigen::Quaterniond(ee_pose.rotation()), ee_orientation); + + // setup the planning context manager + ompl_interface::PlanningContextManager pcm(robot_model_, constraint_sampler_manager_); + pcm.setPlannerConfigurations(pconfig_map); + + // ORIENTATION CONSTRAINTS + // *********************** + request.path_constraints.orientation_constraints.push_back(createOrientationConstraint(ee_orientation)); + + // See if the planning context manager returns the expected planning context + auto pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_handle_, false); + + EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); + + // As the joint_model_group_ has no IK solver initialized, we expect a joint model state space + EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); + + planning_interface::MotionPlanDetailedResponse response; + ASSERT_TRUE(pc->solve(response)); + + // Are the path constraints created in the planning context? + auto path_constraints = pc->getPathConstraints(); + EXPECT_FALSE(path_constraints->empty()); + EXPECT_EQ(path_constraints->getOrientationConstraints().size(), 1u); + EXPECT_TRUE(path_constraints->getPositionConstraints().empty()); + EXPECT_TRUE(path_constraints->getJointConstraints().empty()); + EXPECT_TRUE(path_constraints->getVisibilityConstraints().empty()); + + // Check if all the states in the solution satisfy the path constraints. + // A detailed response returns 3 solutions: the ompl solution, the simplified solution and the interpolated + // solution. We test all of them here. + for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response.trajectory_) + { + for (std::size_t pt_index = { 0 }; pt_index < trajectory->getWayPointCount(); ++pt_index) + { + EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied); + } + } + + // POSITION CONSTRAINTS + // *********************** + request.path_constraints.orientation_constraints.clear(); + request.path_constraints.position_constraints.push_back(createPositionConstraint( + { ee_pose.translation().x(), ee_pose.translation().y(), ee_pose.translation().z() }, { 0.1, 0.1, 0.1 })); + + // See if the planning context manager returns the expected planning context + pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_handle_, false); + + EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); + + // As the joint_model_group_ has no IK solver initialized, we expect a joint model state space + EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); + + // Create a new response, because the solve method does not clear the given respone + planning_interface::MotionPlanDetailedResponse response2; + ASSERT_TRUE(pc->solve(response2)); + + // Are the path constraints created in the planning context? + path_constraints = pc->getPathConstraints(); + EXPECT_FALSE(path_constraints->empty()); + EXPECT_EQ(path_constraints->getPositionConstraints().size(), 1u); + EXPECT_TRUE(path_constraints->getOrientationConstraints().empty()); + + // Check if all the states in the solution satisfy the path constraints. + // A detailed response returns 3 solutions: the ompl solution, the simplified solution and the interpolated + // solution. We test all of them here. + for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response2.trajectory_) + { + for (std::size_t pt_index = { 0 }; pt_index < trajectory->getWayPointCount(); ++pt_index) + { + EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied); + } + } + } + + // /*************************************************************************** + // * END Test implementation + // * ************************************************************************/ + +protected: + void SetUp() override + { + // create all the fixed input necessary for all planning context managers + constraint_sampler_manager_ = std::make_shared(); + planning_scene_ = std::make_shared(robot_model_); + } + + void TearDown() override + { + } + + /** Create a planning request to plan from a given start state to a joint space goal. **/ + planning_interface::MotionPlanRequest createRequest(const std::vector& start, + const std::vector& goal) const + { + planning_interface::MotionPlanRequest request; + request.group_name = group_name_; + request.allowed_planning_time = 5.0; + + // fill out start state in request + robot_state::RobotState start_state(robot_model_); + start_state.setToDefaultValues(); + start_state.setJointGroupPositions(joint_model_group_, start); + moveit::core::robotStateToRobotStateMsg(start_state, request.start_state); + + // fill out goal state in request + robot_state::RobotState goal_state(robot_model_); + goal_state.setToDefaultValues(); + goal_state.setJointGroupPositions(joint_model_group_, goal); + moveit_msgs::Constraints joint_goal = + kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_, 0.001); + request.goal_constraints.push_back(joint_goal); + + return request; + } + + /** \brief Helper function to create a position constraint. **/ + moveit_msgs::PositionConstraint createPositionConstraint(std::array position, + std::array dimensions) + { + shape_msgs::SolidPrimitive box; + box.type = shape_msgs::SolidPrimitive::BOX; + box.dimensions.resize(3); + box.dimensions[shape_msgs::SolidPrimitive::BOX_X] = dimensions[0]; + box.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = dimensions[1]; + box.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = dimensions[2]; + + geometry_msgs::Pose box_pose; + box_pose.position.x = position[0]; + box_pose.position.y = position[1]; + box_pose.position.z = position[2]; + box_pose.orientation.w = 1.0; + + moveit_msgs::PositionConstraint pc; + pc.header.frame_id = base_link_name_; + pc.link_name = ee_link_name_; + pc.constraint_region.primitives.push_back(box); + pc.constraint_region.primitive_poses.push_back(box_pose); + + return pc; + } + + /** \brief Helper function to create a orientation constraint. **/ + moveit_msgs::OrientationConstraint createOrientationConstraint(const geometry_msgs::Quaternion& nominal_orientation) + { + moveit_msgs::OrientationConstraint oc; + oc.header.frame_id = base_link_name_; + oc.link_name = ee_link_name_; + oc.orientation = nominal_orientation; + oc.absolute_x_axis_tolerance = 0.3; + oc.absolute_y_axis_tolerance = 0.3; + oc.absolute_z_axis_tolerance = 0.3; + + return oc; + } + + ompl_interface::ModelBasedStateSpacePtr state_space_; + ompl_interface::ModelBasedPlanningContextSpecification planning_context_spec_; + ompl_interface::ModelBasedPlanningContextPtr planning_context_; + planning_scene::PlanningScenePtr planning_scene_; + + constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; + + /** Ideally we add an IK plugin to the joint_model_group_ to test the PoseModel state space, using the pluginlib to + * load the default KDL plugin? **/ + // std::shared_ptr ik_plugin_; + + /** we need a node handle to call getPlanningRequest, but it is never used, as we disable the + * 'use_constraints_approximation' option. **/ + ros::NodeHandle node_handle_; +}; + +/*************************************************************************** + * Run all tests on the Panda robot + * ************************************************************************/ +class PandaTestPlanningContext : public TestPlanningContext +{ +protected: + PandaTestPlanningContext() : TestPlanningContext("panda", "panda_arm") + { + } +}; + +TEST_F(PandaTestPlanningContext, testSimpleRequest) +{ + // use the panda "ready" state from the srdf config as start state + // we know this state should be within limits and self-collision free + testSimpleRequest({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }, { 0, -0.785, 0, -2.356, 0, 1.571, 0.685 }); +} + +TEST_F(PandaTestPlanningContext, testPathConstraints) +{ + testPathConstraints({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }, { 0, -0.785, 0, -2.356, 0, 1.571, 0.685 }); +} + +/*************************************************************************** + * Run all tests on the Fanuc robot + * ************************************************************************/ +class FanucTestPlanningContext : public TestPlanningContext +{ +protected: + FanucTestPlanningContext() : TestPlanningContext("fanuc", "manipulator") + { + } +}; + +TEST_F(FanucTestPlanningContext, testSimpleRequest) +{ + testSimpleRequest({ 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0.1 }); +} + +TEST_F(FanucTestPlanningContext, testPathConstraints) +{ + testPathConstraints({ 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0.1 }); +} + +/*************************************************************************** + * MAIN + * ************************************************************************/ +int main(int argc, char** argv) +{ + ros::init(argc, argv, "planning_context_manager_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test new file mode 100644 index 0000000000..64a3e6c087 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp index cb477140eb..879c7330d3 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp @@ -34,6 +34,8 @@ /* Author: Ioan Sucan */ +#include + #include #include @@ -41,34 +43,20 @@ #include #include +#include #include #include #include #include +constexpr double EPSILON = std::numeric_limits::epsilon(); + class LoadPlanningModelsPr2 : public testing::Test { protected: void SetUp() override { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); - - srdf_model_.reset(new srdf::Model()); - std::string xml_string; - std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - urdf_model_ = urdf::parseURDF(xml_string); - } - srdf_model_->initFile(*urdf_model_, (res_path / "pr2_description/srdf/robot.xml").string()); - robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_model_)); + robot_model_ = moveit::core::loadTestingRobotModel("pr2"); }; void TearDown() override @@ -76,11 +64,7 @@ class LoadPlanningModelsPr2 : public testing::Test } protected: - robot_model::RobotModelPtr robot_model_; - urdf::ModelInterfaceSharedPtr urdf_model_; - srdf::ModelSharedPtr srdf_model_; - bool urdf_ok_; - bool srdf_ok_; + moveit::core::RobotModelPtr robot_model_; }; TEST_F(LoadPlanningModelsPr2, StateSpace) @@ -107,19 +91,19 @@ TEST_F(LoadPlanningModelsPr2, StateSpace) TEST_F(LoadPlanningModelsPr2, StateSpaces) { ompl_interface::ModelBasedStateSpaceSpecification spec1(robot_model_, "right_arm"); - ompl_interface::ModelBasedStateSpace ss1(spec1); + ompl_interface::JointModelStateSpace ss1(spec1); ss1.setup(); ompl_interface::ModelBasedStateSpaceSpecification spec2(robot_model_, "left_arm"); - ompl_interface::ModelBasedStateSpace ss2(spec2); + ompl_interface::JointModelStateSpace ss2(spec2); ss2.setup(); ompl_interface::ModelBasedStateSpaceSpecification spec3(robot_model_, "whole_body"); - ompl_interface::ModelBasedStateSpace ss3(spec3); + ompl_interface::JointModelStateSpace ss3(spec3); ss3.setup(); ompl_interface::ModelBasedStateSpaceSpecification spec4(robot_model_, "arms"); - ompl_interface::ModelBasedStateSpace ss4(spec4); + ompl_interface::JointModelStateSpace ss4(spec4); ss4.setup(); std::ofstream fout("ompl_interface_test_state_space_diagram2.dot"); @@ -129,15 +113,15 @@ TEST_F(LoadPlanningModelsPr2, StateSpaces) TEST_F(LoadPlanningModelsPr2, StateSpaceCopy) { ompl_interface::ModelBasedStateSpaceSpecification spec(robot_model_, "right_arm"); - ompl_interface::JointModelStateSpace ss(spec); - ss.setPlanningVolume(-1, 1, -1, 1, -1, 1); - ss.setup(); + ompl_interface::JointModelStateSpace joint_model_state_space(spec); + joint_model_state_space.setPlanningVolume(-1, 1, -1, 1, -1, 1); + joint_model_state_space.setup(); std::ofstream fout("ompl_interface_test_state_space_diagram1.dot"); - ss.diagram(fout); + joint_model_state_space.diagram(fout); bool passed = false; try { - ss.sanityChecks(); + joint_model_state_space.sanityChecks(); passed = true; } catch (ompl::Exception& ex) @@ -146,28 +130,58 @@ TEST_F(LoadPlanningModelsPr2, StateSpaceCopy) } EXPECT_TRUE(passed); - robot_state::RobotState robot_state(robot_model_); + moveit::core::RobotState robot_state(robot_model_); robot_state.setToRandomPositions(); - EXPECT_TRUE(robot_state.distance(robot_state) < 1e-12); - ompl::base::State* state = ss.allocState(); + EXPECT_TRUE(robot_state.distance(robot_state) < EPSILON); + ompl::base::State* state = joint_model_state_space.allocState(); for (int i = 0; i < 10; ++i) { - robot_state::RobotState robot_state2(robot_state); - EXPECT_TRUE(robot_state.distance(robot_state2) < 1e-12); - ss.copyToOMPLState(state, robot_state); - robot_state.setToRandomPositions(robot_state.getRobotModel()->getJointModelGroup(ss.getJointModelGroupName())); + moveit::core::RobotState robot_state2(robot_state); + EXPECT_TRUE(robot_state.distance(robot_state2) < EPSILON); + joint_model_state_space.copyToOMPLState(state, robot_state); + robot_state.setToRandomPositions( + robot_state.getRobotModel()->getJointModelGroup(joint_model_state_space.getJointModelGroupName())); std::cout << (robot_state.getGlobalLinkTransform("r_wrist_roll_link").translation() - robot_state2.getGlobalLinkTransform("r_wrist_roll_link").translation()) << std::endl; - EXPECT_TRUE(robot_state.distance(robot_state2) > 1e-12); - ss.copyToRobotState(robot_state, state); + EXPECT_TRUE(robot_state.distance(robot_state2) > EPSILON); + joint_model_state_space.copyToRobotState(robot_state, state); std::cout << (robot_state.getGlobalLinkTransform("r_wrist_roll_link").translation() - robot_state2.getGlobalLinkTransform("r_wrist_roll_link").translation()) << std::endl; - EXPECT_TRUE(robot_state.distance(robot_state2) < 1e-12); + EXPECT_TRUE(robot_state.distance(robot_state2) < EPSILON); + } + + // repeat the above test with a different method to copy the state to OMPL + for (int i = 0; i < 10; ++i) + { + // create two equal states + moveit::core::RobotState robot_state2(robot_state); + EXPECT_LT(robot_state.distance(robot_state2), EPSILON); + + // copy the first state to OMPL as backup (this is where the 'different' method comes into play) + const moveit::core::JointModelGroup* joint_model_group = + robot_state.getRobotModel()->getJointModelGroup(joint_model_state_space.getJointModelGroupName()); + std::vector joint_model_names = joint_model_group->getActiveJointModelNames(); + for (std::size_t joint_index = 0; joint_index < joint_model_group->getVariableCount(); ++joint_index) + { + const moveit::core::JointModel* joint_model = joint_model_group->getJointModel(joint_model_names[joint_index]); + EXPECT_NE(joint_model, nullptr); + joint_model_state_space.copyJointToOMPLState(state, robot_state, joint_model, joint_index); + } + + // and change the joint values of the moveit state, so it is different that robot_state2 + robot_state.setToRandomPositions( + robot_state.getRobotModel()->getJointModelGroup(joint_model_state_space.getJointModelGroupName())); + EXPECT_GT(robot_state.distance(robot_state2), EPSILON); + + // copy the backup values in the OMPL state back to the first state, + // and check if it is still equal to the second + joint_model_state_space.copyToRobotState(robot_state, state); + EXPECT_LT(robot_state.distance(robot_state2), EPSILON); } - ss.freeState(state); + joint_model_state_space.freeState(state); } int main(int argc, char** argv) diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp new file mode 100644 index 0000000000..9b937e2a63 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp @@ -0,0 +1,342 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of KU Leuven nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ +/** + * This test checks the basics of a StateValidityChecker: + * - Can we create one? + * - States inside and outside joint limits. + * - States that are in self-collision. + * - Position constraints on the robot's end-effector link. + * + * It does not yet test: + * - Collision with objects in the environment. + * - Orientation constraints, visibility constraints, ... + * - A user-specified feasibility function in the planning scene. + * + * The test do show what is minimally required to create a working StateValidityChecker. + **/ + +#include "load_test_robot.h" + +#include +#include + +#include + +#include +#include +#include +#include + +#include + +/** \brief This flag sets the verbosity level for the state validity checker. **/ +constexpr bool VERBOSE{ false }; + +constexpr char LOGNAME[] = "test_state_validity_checker"; + +/** \brief Pretty print std:vectors **/ +std::ostream& operator<<(std::ostream& os, const std::vector& v) +{ + os << "( "; + for (auto value : v) + os << value << ", "; + os << " )"; + return os; +} + +/** \brief Generic implementation of the tests that can be executed on different robots. **/ +class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, public testing::Test +{ +public: + TestStateValidityChecker(const std::string& robot_name, const std::string& group_name) + : LoadTestRobot(robot_name, group_name) + { + } + + /*************************************************************************** + * START Test implementations + * ************************************************************************/ + + void testConstructor() + { + ompl::base::StateValidityCheckerPtr checker = + std::make_shared(planning_context_.get()); + } + + /** This test takes a state that is inside the joint limits and collision free as input. **/ + void testJointLimits(const std::vector& position_in_limits) + { + // create a validity checker for this test + auto checker = std::make_shared(planning_context_.get()); + checker->setVerbose(VERBOSE); + + robot_state_->setJointGroupPositions(joint_model_group_, position_in_limits); + + // use a scoped OMPL state so we don't have to call allocState and freeState + // (as recommended in the OMPL documantion) + ompl::base::ScopedState<> ompl_state(state_space_); + state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); + + ROS_DEBUG_STREAM_NAMED(LOGNAME, ompl_state.reals()); + + // assume the given position is not in self-collision + // and there are no collision objects or path constraints so this state should be valid + EXPECT_TRUE(checker->isValid(ompl_state.get())); + + // move first joint obviously outside any joint limits + ompl_state->as()->values[0] = std::numeric_limits::max(); + ompl_state->as()->clearKnownInformation(); + + ROS_DEBUG_STREAM_NAMED(LOGNAME, ompl_state.reals()); + + EXPECT_FALSE(checker->isValid(ompl_state.get())); + } + + /** This test takes a state that is known to be in self-collision and inside the joint limits as input. **/ + void testSelfCollision(const std::vector& position_in_self_collision) + { + // create a validity checker for this test + auto checker = std::make_shared(planning_context_.get()); + checker->setVerbose(VERBOSE); + + robot_state_->setJointGroupPositions(joint_model_group_, position_in_self_collision); + + // use a scoped OMPL state so we don't have to call allocState and freeState + // (as recommended in the OMPL documantion) + ompl::base::ScopedState<> ompl_state(state_space_); + state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); + + ROS_DEBUG_STREAM_NAMED(LOGNAME, ompl_state.reals()); + + // the given state is known to be in self-collision, we check it here + EXPECT_FALSE(checker->isValid(ompl_state.get())); + + // but it should respect the joint limits + EXPECT_TRUE(robot_state_->satisfiesBounds()); + } + + void testPathConstraints(const std::vector& position_in_joint_limits) + { + ASSERT_NE(planning_context_, nullptr) << "Initialize planning context before adding path constraints."; + + // set the robot to a known position that is withing the joint limits and collision free + robot_state_->setJointGroupPositions(joint_model_group_, position_in_joint_limits); + + // create position constraints around the given robot state + moveit_msgs::Constraints path_constraints; + Eigen::Isometry3d ee_pose = robot_state_->getGlobalLinkTransform(ee_link_name_); + path_constraints.name = "test_position_constraints"; + path_constraints.position_constraints.push_back(createPositionConstraint( + { ee_pose.translation().x(), ee_pose.translation().y(), ee_pose.translation().z() }, { 0.1, 0.1, 0.1 })); + + moveit_msgs::MoveItErrorCodes error_code_not_used; + ASSERT_TRUE(planning_context_->setPathConstraints(path_constraints, &error_code_not_used)); + + auto checker = std::make_shared(planning_context_.get()); + checker->setVerbose(VERBOSE); + + // use a scoped OMPL state so we don't have to call allocState and freeState + // (as recommended in the OMPL documantion) + ompl::base::ScopedState<> ompl_state(state_space_); + state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); + + ROS_DEBUG_STREAM_NAMED(LOGNAME, ompl_state.reals()); + + EXPECT_TRUE(checker->isValid(ompl_state.get())); + + // move the position constraints away from the curren end-effector position to make it fail + moveit_msgs::Constraints path_constraints_2(path_constraints); + path_constraints_2.position_constraints.at(0).constraint_region.primitive_poses.at(0).position.z += 0.2; + + ASSERT_TRUE(planning_context_->setPathConstraints(path_constraints_2, &error_code_not_used)); + + // clear the cached validity of the state before checking again, + // otherwise the path constraints will not be checked. + ompl_state->as()->clearKnownInformation(); + + EXPECT_FALSE(checker->isValid(ompl_state.get())); + } + + /*************************************************************************** + * END Test implementation + * ************************************************************************/ + +protected: + void SetUp() override + { + // setup all the input we need to create a StateValidityChecker + setupStateSpace(); + setupPlanningContext(); + }; + + void setupStateSpace() + { + ompl_interface::ModelBasedStateSpaceSpecification space_spec(robot_model_, group_name_); + state_space_ = std::make_shared(space_spec); + state_space_->computeLocations(); // this gets normally called in the state space factory + } + + void setupPlanningContext() + { + ASSERT_NE(state_space_, nullptr) << "Initialize state space before creating the planning context."; + + planning_context_spec_.state_space_ = state_space_; + planning_context_spec_.ompl_simple_setup_ = std::make_shared(state_space_); + planning_context_ = + std::make_shared(group_name_, planning_context_spec_); + + planning_scene_ = std::make_shared(robot_model_); + planning_context_->setPlanningScene(planning_scene_); + moveit::core::RobotState start_state(robot_model_); + start_state.setToDefaultValues(); + planning_context_->setCompleteInitialState(start_state); + } + + /** \brief Helper function to create a position constraint. **/ + moveit_msgs::PositionConstraint createPositionConstraint(std::array position, + std::array dimensions) + { + shape_msgs::SolidPrimitive box; + box.type = shape_msgs::SolidPrimitive::BOX; + box.dimensions.resize(3); + box.dimensions[shape_msgs::SolidPrimitive::BOX_X] = dimensions[0]; + box.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = dimensions[1]; + box.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = dimensions[2]; + + geometry_msgs::Pose box_pose; + box_pose.position.x = position[0]; + box_pose.position.y = position[1]; + box_pose.position.z = position[2]; + box_pose.orientation.w = 1.0; + + moveit_msgs::PositionConstraint position_constraint; + position_constraint.header.frame_id = base_link_name_; + position_constraint.link_name = ee_link_name_; + position_constraint.constraint_region.primitives.push_back(box); + position_constraint.constraint_region.primitive_poses.push_back(box_pose); + + // set the default weight to avoid warning in test output + position_constraint.weight = 1.0; + + return position_constraint; + } + + ompl_interface::ModelBasedStateSpacePtr state_space_; + ompl_interface::ModelBasedPlanningContextSpecification planning_context_spec_; + ompl_interface::ModelBasedPlanningContextPtr planning_context_; + planning_scene::PlanningScenePtr planning_scene_; +}; + +// /*************************************************************************** +// * Run all tests on the Panda robot +// * ************************************************************************/ +class PandaValidity : public TestStateValidityChecker +{ +protected: + PandaValidity() : TestStateValidityChecker("panda", "panda_arm") + { + } +}; + +TEST_F(PandaValidity, testConstructor) +{ + testConstructor(); +} + +TEST_F(PandaValidity, testJointLimits) +{ + // use the panda "ready" state from the srdf config + // we know this state should be within limits and self-collision free + testJointLimits({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }); +} + +TEST_F(PandaValidity, testSelfCollision) +{ + // the given state has self collision between "hand" and "panda_link2" + // (I just tried a couple of random states until I found one that collided.) + testSelfCollision({ 2.31827, -0.169668, 2.5225, -2.98568, -0.36355, 0.808339, 0.0843406 }); +} + +TEST_F(PandaValidity, testPathConstraints) +{ + // use the panda "ready" state from the srdf config + // we know this state should be within limits and self-collision free + testPathConstraints({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }); +} + +/*************************************************************************** + * Run all tests on the Fanuc robot + * ************************************************************************/ +class FanucTest : public TestStateValidityChecker +{ +protected: + FanucTest() : TestStateValidityChecker("fanuc", "manipulator") + { + } +}; + +TEST_F(FanucTest, createStateValidityChecker) +{ + testConstructor(); +} + +TEST_F(FanucTest, testJointLimits) +{ + // I assume the Fanucs's zero state is within limits and self-collision free + testJointLimits({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }); +} + +TEST_F(FanucTest, testSelfCollision) +{ + // the given state has self collision between "base_link" and "link_5" + // (I just tried a couple of random states until I found one that collided.) + testSelfCollision({ -2.95993, -0.682185, -2.43873, -0.939784, 3.0544, 0.882294 }); +} + +TEST_F(FanucTest, testPathConstraints) +{ + // I assume the Fanucs's zero state is within limits and self-collision free + testPathConstraints({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }); +} + +/*************************************************************************** + * MAIN + * ************************************************************************/ +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index 1f2f7e6fc0..379fdf337a 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -1,12 +1,12 @@ moveit_planners_ompl - 1.0.1 - MoveIt! interface to OMPL + 1.1.1 + MoveIt interface to OMPL Ioan Sucan Dave Coleman - MoveIt! Release Team + MoveIt Release Team BSD @@ -25,8 +25,12 @@ tf2_ros pluginlib - moveit_resources + moveit_resources_pr2_description + moveit_resources_fanuc_description + moveit_resources_panda_description rosunit + rostest + eigen_conversions diff --git a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst new file mode 100644 index 0000000000..b711926f83 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pilz_industrial_motion_planner +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* [feature] Add Pilz industrial motion planner (`#1893 `_) +* Contributors: Pilz GmbH and Co. KG, Christian Henkel, Immanuel Martini, Joachim Schleicher, rfeistenauer + diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt new file mode 100644 index 0000000000..5f9be95485 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -0,0 +1,520 @@ +cmake_minimum_required(VERSION 3.1.3) +project(pilz_industrial_motion_planner) + +## Add support for C++11, supported in ROS Kinetic and newer +add_definitions(-std=c++11) +add_definitions(-Wall) +add_definitions(-Wextra) +add_definitions(-Wno-unused-parameter) + +find_package(catkin REQUIRED COMPONENTS + eigen_conversions + joint_limits_interface + kdl_conversions + moveit_core + moveit_msgs + moveit_ros_move_group + moveit_ros_planning + moveit_ros_planning_interface + pluginlib + roscpp + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros +) + +find_package(orocos_kdl) +find_package(Boost REQUIRED COMPONENTS ) + +if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) + find_package(code_coverage REQUIRED) + APPEND_COVERAGE_COMPILER_FLAGS() +endif() + +################ +## Clang tidy ## +################ +if(CATKIN_ENABLE_CLANG_TIDY) + find_program( + CLANG_TIDY_EXE + NAMES "clang-tidy" + DOC "Path to clang-tidy executable" + ) + if(NOT CLANG_TIDY_EXE) + message(FATAL_ERROR "clang-tidy not found.") + else() + message(STATUS "clang-tidy found: ${CLANG_TIDY_EXE}") + set(CMAKE_CXX_CLANG_TIDY "${CLANG_TIDY_EXE}") + endif() +endif() + +################################### +## catkin specific configuration ## +################################### +catkin_package( + CATKIN_DEPENDS + moveit_msgs + roscpp + tf2_geometry_msgs +) + +########### +## Build ## +########### +include_directories( + include +) + +# To avoid Warnings from clang-tidy declare system +include_directories( + SYSTEM + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME}_lib + src/${PROJECT_NAME}.cpp + src/planning_context_loader.cpp + src/joint_limits_validator.cpp + src/joint_limits_aggregator.cpp + src/joint_limits_container.cpp + src/cartesian_limits_aggregator.cpp + src/cartesian_limit.cpp + src/limits_container.cpp + src/trajectory_functions.cpp + src/plan_components_builder.cpp +) + +target_link_libraries(${PROJECT_NAME}_lib + ${catkin_LIBRARIES} +) + +############# +## Plugins ## +############# +add_library(${PROJECT_NAME} + src/${PROJECT_NAME}.cpp + src/planning_context_loader.cpp + src/joint_limits_aggregator.cpp + src/joint_limits_container.cpp + src/limits_container.cpp + src/cartesian_limit.cpp + src/cartesian_limits_aggregator.cpp + ) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES}) + +add_library(planning_context_loader_ptp + src/planning_context_loader_ptp.cpp + src/planning_context_loader.cpp + src/trajectory_functions.cpp + src/trajectory_generator.cpp + src/trajectory_generator_ptp.cpp + src/velocity_profile_atrap.cpp + src/joint_limits_container.cpp + ) +target_link_libraries(planning_context_loader_ptp + ${catkin_LIBRARIES}) + +add_library(planning_context_loader_lin + src/planning_context_loader_lin.cpp + src/planning_context_loader.cpp + src/trajectory_functions.cpp + src/trajectory_generator.cpp + src/trajectory_generator_lin.cpp + src/velocity_profile_atrap.cpp + ) +target_link_libraries(planning_context_loader_lin + ${catkin_LIBRARIES}) + +add_library(planning_context_loader_circ + src/planning_context_loader_circ.cpp + src/planning_context_loader.cpp + src/trajectory_functions.cpp + src/trajectory_generator.cpp + src/trajectory_generator_circ.cpp + src/path_circle_generator.cpp + ) +target_link_libraries(planning_context_loader_circ + ${catkin_LIBRARIES}) + +add_library(command_list_manager + src/command_list_manager.cpp + src/plan_components_builder.cpp) +target_link_libraries(command_list_manager + ${catkin_LIBRARIES}) +add_dependencies(command_list_manager + ${catkin_EXPORTED_TARGETS}) + +add_library(sequence_capability + src/move_group_sequence_action.cpp + src/move_group_sequence_service.cpp + src/plan_components_builder.cpp + src/command_list_manager.cpp + src/trajectory_blender_transition_window.cpp + src/joint_limits_aggregator.cpp # do we need joint limits and cartesian_limit here? + src/joint_limits_container.cpp + src/limits_container.cpp + src/cartesian_limit.cpp + src/cartesian_limits_aggregator.cpp + ) +target_link_libraries(sequence_capability + ${catkin_LIBRARIES}) +add_dependencies(sequence_capability + ${catkin_EXPORTED_TARGETS}) + +############# +## Install ## +############# +install(FILES + plugins/sequence_capability_plugin_description.xml + plugins/${PROJECT_NAME}_plugin_description.xml + plugins/planning_context_plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/plugins + ) + +## Mark libraries for installation +install(TARGETS + ${PROJECT_NAME} + planning_context_loader_ptp + planning_context_loader_lin + planning_context_loader_circ + command_list_manager + sequence_capability + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +############# +## Testing ## +############# +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + find_package(${PROJECT_NAME}_testutils REQUIRED) + + include_directories( + ${${PROJECT_NAME}_testutils_INCLUDE_DIRS} + ) + + if(ENABLE_COVERAGE_TESTING) + include(CodeCoverage) + APPEND_COVERAGE_COMPILER_FLAGS() + endif() + + ######################### + ####Integration Tests#### + ######################### + set(${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES + sequence_capability + ${PROJECT_NAME} + planning_context_loader_ptp + planning_context_loader_lin + planning_context_loader_circ + ) + + # Planning Integration tests + add_rostest_gmock(integrationtest_sequence_action_preemption + test/integrationtest_sequence_action_preemption.test + test/integrationtest_sequence_action_preemption.cpp + ) + target_link_libraries(integrationtest_sequence_action_preemption + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + add_rostest_gmock(integrationtest_sequence_action + test/integrationtest_sequence_action.test + test/integrationtest_sequence_action.cpp + ) + target_link_libraries(integrationtest_sequence_action + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + add_rostest(test/integrationtest_sequence_action_capability_with_gripper.test + DEPENDENCIES integrationtest_sequence_action + ) + + add_rostest(test/integrationtest_sequence_action_capability_frankaemika_panda.test + DEPENDENCIES integrationtest_sequence_action + ) + + add_rostest_gtest(integrationtest_sequence_service_capability + test/integrationtest_sequence_service_capability.test + test/integrationtest_sequence_service_capability.cpp + ) + target_link_libraries(integrationtest_sequence_service_capability + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + add_rostest(test/integrationtest_sequence_service_capability_frankaemika_panda.test + DEPENDENCIES integrationtest_sequence_service_capability ) + + add_rostest(test/integrationtest_sequence_service_capability_with_gripper.test + DEPENDENCIES integrationtest_sequence_service_capability + ) + + # Planning Integration tests + add_rostest_gtest(integrationtest_command_planning + test/integrationtest_command_planning.test + test/integrationtest_command_planning.cpp + test/test_utils.cpp + ) + target_link_libraries(integrationtest_command_planning + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + add_rostest(test/integrationtest_command_planning_with_gripper.test + DEPENDENCIES integrationtest_command_planning + ) + + add_rostest(test/integrationtest_command_planning_frankaemika_panda.test + DEPENDENCIES integrationtest_command_planning ) + + # Blending Integration tests + add_rostest_gtest(integrationtest_command_list_manager + test/integrationtest_command_list_manager.test + test/integrationtest_command_list_manager.cpp + test/test_utils.cpp + ) + target_link_libraries(integrationtest_command_list_manager + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + add_rostest_gtest(integrationtest_get_solver_tip_frame + test/integrationtest_get_solver_tip_frame.test + test/integrationtest_get_solver_tip_frame.cpp + ) + target_link_libraries(integrationtest_get_solver_tip_frame + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + add_rostest_gtest(integrationtest_plan_components_builder + test/integrationtest_plan_components_builder.test + test/integrationtest_plan_components_builder.cpp + ) + target_link_libraries(integrationtest_plan_components_builder + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_INTEGRATIONTEST_LIBRARIES} + ) + + ################## + ####Unit Tests#### + ################## + add_library(${PROJECT_NAME}_testhelpers + test/test_utils.cpp + src/trajectory_generator.cpp + src/trajectory_generator_circ.cpp + src/trajectory_generator_lin.cpp + src/trajectory_generator_ptp.cpp + src/path_circle_generator.cpp + src/velocity_profile_atrap.cpp + ) + + target_link_libraries(${PROJECT_NAME}_testhelpers ${PROJECT_NAME}_lib) + + ## Add gtest based cpp test target and link libraries + catkin_add_gtest(unittest_${PROJECT_NAME}_direct + test/unittest_${PROJECT_NAME}_direct.cpp + src/planning_context_loader_ptp.cpp + ) + target_link_libraries(unittest_${PROJECT_NAME}_direct + ${catkin_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + ## Add gtest based cpp test target and link libraries + catkin_add_gtest(unittest_velocity_profile_atrap + test/unittest_velocity_profile_atrap.cpp + src/velocity_profile_atrap.cpp + ) + target_link_libraries(unittest_velocity_profile_atrap + ${catkin_LIBRARIES} + ) + + catkin_add_gtest(unittest_trajectory_generator + test/unittest_trajectory_generator.cpp + src/trajectory_generator.cpp + ) + target_link_libraries(unittest_trajectory_generator + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # Trajectory Generator Unit Test + add_rostest_gtest(unittest_trajectory_functions + test/unittest_trajectory_functions.test + test/unittest_trajectory_functions.cpp + ) + target_link_libraries(unittest_trajectory_functions + ${catkin_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # unittest for trajectory blender transition window + add_rostest_gtest(unittest_trajectory_blender_transition_window + test/unittest_trajectory_blender_transition_window.test + test/unittest_trajectory_blender_transition_window.cpp + src/trajectory_blender_transition_window.cpp + ) + target_link_libraries(unittest_trajectory_blender_transition_window + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # trajectory generator Unit Test + add_rostest_gtest(unittest_trajectory_generator_common + test/unittest_trajectory_generator_common.test + test/unittest_trajectory_generator_common.cpp + ) + target_link_libraries(unittest_trajectory_generator_common + ${catkin_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # trajectory generator circ Unit Test + add_rostest_gtest(unittest_trajectory_generator_circ + test/unittest_trajectory_generator_circ.test + test/unittest_trajectory_generator_circ.cpp + ) + target_link_libraries(unittest_trajectory_generator_circ + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # trajectory generator lin Unit Test + add_rostest_gtest(unittest_trajectory_generator_lin + test/unittest_trajectory_generator_lin.test + test/unittest_trajectory_generator_lin.cpp + ) + target_link_libraries(unittest_trajectory_generator_lin + ${catkin_LIBRARIES} + ${${PROJECT_NAME}_testutils_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # trajectory generator ptp Unit Test + add_rostest_gtest(unittest_trajectory_generator_ptp + test/unittest_trajectory_generator_ptp.test + test/unittest_trajectory_generator_ptp.cpp + ) + target_link_libraries(unittest_trajectory_generator_ptp + ${catkin_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # Command Planner Unit Test + add_rostest_gtest(unittest_${PROJECT_NAME} + test/unittest_${PROJECT_NAME}.test + test/unittest_${PROJECT_NAME}.cpp + ) + target_link_libraries(unittest_${PROJECT_NAME} + ${catkin_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # JointLimits Unit Test + add_rostest_gtest(unittest_joint_limit + test/unittest_joint_limit.test + test/unittest_joint_limit.cpp + ) + target_link_libraries(unittest_joint_limit + ${catkin_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # JointLimitsAggregator Unit Test + add_rostest_gtest(unittest_joint_limits_aggregator + test/unittest_joint_limits_aggregator.test + test/unittest_joint_limits_aggregator.cpp + ) + target_link_libraries(unittest_joint_limits_aggregator + ${catkin_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # JointLimitsContainer Unit Test + catkin_add_gtest(unittest_joint_limits_container + test/unittest_joint_limits_container.cpp + ) + target_link_libraries(unittest_joint_limits_container + ${catkin_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # JointLimitsValidator Unit Test + catkin_add_gtest(unittest_joint_limits_validator + test/unittest_joint_limits_validator.cpp + ) + target_link_libraries(unittest_joint_limits_validator + ${catkin_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # Cartesian Limits Aggregator Unit Test + add_rostest_gtest(unittest_cartesian_limits_aggregator + test/unittest_cartesian_limits_aggregator.test + test/unittest_cartesian_limits_aggregator.cpp + ) + target_link_libraries(unittest_cartesian_limits_aggregator + ${catkin_LIBRARIES} + ${PROJECT_NAME}_lib + ) + + # PlanningContextLoaderPTP Unit Test + add_rostest_gtest(unittest_planning_context_loaders + test/unittest_planning_context_loaders.test + test/unittest_planning_context_loaders.cpp + ) + target_link_libraries(unittest_planning_context_loaders + ${catkin_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # PlanningContext Unit Test (Typed test) + add_rostest_gtest(unittest_planning_context + test/unittest_planning_context.test + test/unittest_planning_context.cpp + src/planning_context_loader_circ.cpp + src/planning_context_loader_lin.cpp + src/planning_context_loader_ptp.cpp + ) + target_link_libraries(unittest_planning_context + ${catkin_LIBRARIES} + ${PROJECT_NAME}_testhelpers + ) + + # GetTipFrames Unit Test + catkin_add_gmock(unittest_get_solver_tip_frame + test/unittest_get_solver_tip_frame.cpp + ) + target_link_libraries(unittest_get_solver_tip_frame + ${catkin_LIBRARIES} + ) + + # to run: catkin_make -DCMAKE_BUILD_TYPE=Debug -DENABLE_COVERAGE_TESTING=ON package_name_coverage + if(ENABLE_COVERAGE_TESTING) + set(COVERAGE_EXCLUDES "*/${PROJECT_NAME}/test*") + add_code_coverage( + NAME ${PROJECT_NAME}_coverage + # specifying dependencies in a reliable way is on open issue + # see https://github.com/mikeferguson/code_coverage/pull/14 + #DEPENDENCIES tests + ) + endif() +endif() diff --git a/moveit_planners/pilz_industrial_motion_planner/README.md b/moveit_planners/pilz_industrial_motion_planner/README.md new file mode 100644 index 0000000000..632224f5ec --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/README.md @@ -0,0 +1,4 @@ +Please consult tutorials and the official [documentation](https://moveit.ros.org/documentation/concepts/). + +For details about the blend algorithm please refer to +![doc/MotionBlendAlgorithmDescription.pdf](doc/MotionBlendAlgorithmDescription.pdf). diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/.gitignore b/moveit_planners/pilz_industrial_motion_planner/doc/.gitignore new file mode 100644 index 0000000000..2edfaf4621 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/.gitignore @@ -0,0 +1,12 @@ +*.aux +*.bbl +*.blg +*.dvi +*.log +*.ps +*.tps +*.fdb_latexmk +*.fls +*.out +*.synctex.gz +*.pdf \ No newline at end of file diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.pdf b/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.pdf new file mode 100644 index 0000000000..7a3f127e88 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.pdf differ diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.tex b/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.tex new file mode 100644 index 0000000000..a040465426 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.tex @@ -0,0 +1,114 @@ +\documentclass{article} +%-------------------------------------------------------- +\usepackage{hyperref} +\usepackage{caption} +\usepackage{subcaption} + +\usepackage{graphicx} + + +\usepackage[left=2.9cm, right=2.9cm, top=3cm, bottom=2cm]{geometry} + +\usepackage{fancyhdr} +\pagestyle{fancy} +\fancyhf{} +\rhead{31.01.2019} +\lhead{Pilz GmbH \& Co. KG} +\cfoot{\thepage} +\renewcommand{\headrulewidth}{0pt} +\renewcommand{\footrulewidth}{0pt} + +\begin{document} +\title{Motion Blending} +\author{Pilz GmbH \& Co. KG} +\date{31.01.2019} + +\maketitle + +\begin{abstract} +\noindent This article describes the techniques used to blend the position and orientation of two robot trajectories within the {\tt pilz\_industrial\_motion\_planner}\cite{pilztrajectorygeneration} ROS package. +\end{abstract} + + +\newpage + +\section{Position blending} + +\subsection{Introduction} + +Suppose we have two trajectories $x_1(t)$, $t\in[0,T_1]$ and $x_2(t)$, $t\in[0,T_2]$ and we want to make a transition from $x_1(t)$ to $x_2(t)$. The transition window technique assumes that the transition happens in a predefined time window. During this transition window, the resulted trajectory (blending trajectory) is given by: +\begin{equation} + x_b(t) = x_1(t) + \alpha(s(t))(x_2(t)-x_1(t)), t \in [t_0, t_0 + T] +\end{equation} +in which $x_b(t)$ is the transition trajectory. $t_0$ is the start time of transition window and $T$ represents the transition time. $\alpha(s)$ is the blend function and $s$ is normalized time parameter: +\begin{equation} +s = \frac{t - t_0}{T} +\label{eq:1} +\end{equation} +which changes from 0 to 1 during the transition window. Following polynomial is selected as $\alpha(s)$ so that the boundary conditions at the start and end point of the transition window are fulfilled \cite{lloyd1993trajectory}: +\begin{equation} +\alpha(s) = 6s^5 - 15s^4 + 10s^3. +\label{eq:2} +\end{equation} + +\subsection{Application for blending robot trajectory} +We want to move the robot from $p_1$ to $p_2$, then from $p_2$ to $p_3$. $p_2$ is a blending way-point which means that it does not need to be reached exactly. We want the robot moves alongside $p_2$ without stop. The whole process is described below with an one dimensional example. + +\begin{enumerate} + \item Generate motion trajectories $x_1(t)$ from $p_1$ to $p_2$ and $x_2(t)$ from $p_2$ to $p_3$. $x_1(t)$ and $x_2(t)$ both start and stop with zero velocity/acceleration. Both trajectories start with time zero. As a simple example we generated two one-dimensional linear trajectories in Figure.\ref{ori_traj}. For robot motion without blending, the two trajectories are executed one after the other, which means $x_2(t)$ needs to be timely shifted by the duration of $x_1(t)$. + \begin{figure}[ht]% + \includegraphics[width=0.9\columnwidth]{figure/original_trajectories.eps}% + \caption{One-dimensional linear trajectory}% + \label{ori_traj} + \end{figure} + + \item According to the blending radius $r$, the points $p_{b1}$ on $x_1(t)$ and $p_{b2}$ on $x_2(t)$ which intersects with the blending sphere are computed. We also compute the durations $d_1$ for moving from $p_{b1}$ to $p_2$ on $x_1$ and $d_2$ for moving from $p_2$ to $p_{b2}$ on $x_2$. The transition window should start earliest from the time of $p_{b1}$ on $x_1(t)$ is reached, and ends latest at the time of $p_{b2}$ on $x_2(t)$ is reached. In the example, we take $r=3$ and $p_{b1} = p_{b2} = 5$ (see Figure.\ref{ori_traj}). + + \item Timely shift the $x_2(t)$ and select the transition window time $T$ according to the above rules. In order to avoid stop on the blending trajectory, the time shift $T_s$ of $x_2(t)$ should be smaller than the duration of $x_1(t)$. We now have the second trajectory as $x_2(t-T_s)$ for blending. In the example the duration of $x_1(t)$ is $6s$. Figure.\ref{blend_case_1} shows a blending case that we shift the $x_2(t)$ with $6s$, which is almost the same as motion without blending. Figure.\ref{blend_case_2} shows a blending case that we shift the $x_2(t)$ with $3.5s$ and the blending starts at 3.5s, ends at 5.5s. Figure.\ref{blend_case_3} shows a blending case that we shift the $x_2(t)$ with $4.5s$ and the blending starts at 3.5s, ends at 6.5s. +\end{enumerate} + +In the actual implementation, we make the following choice given the durations $d_1$ and $d_2$ inside the blending sphere: +\begin{itemize} +\item If $d_1\leq d_2$ we shift exactly to the time when $p_{b1}$ is reached on $x_1$ (when the blending sphere is entered), +\item if $d_1>d_2$ we compute $T_s$ such that $T_s+d_2=T_1$. This choice minimizes increases in acceleration/deceleration on the resulting blend trajectory. +\end{itemize} + + +\section{Blending the orientation} +To blend the orientation, the method described in \cite{dantam2014orientation} is used. The equations (18)-(20) in \cite{dantam2014orientation} are used to calculate the orientation along the blend trajectory. In our application, due to the fact that the orientation change along the original (not blended) trajectories has smooth acceleration and deceleration phases, (18) and (19) from paper \cite{dantam2014orientation} do not need to be calculated.\newline +For the sake of clarity, it is important to note that our functions for $u_{ij}(t)$ and $u_{jk}(t)$ are different. However, we account for this difference by not explicitly calculating (18) and (19) and using the given samples of the original (not blended) trajectories instead. Furthermore, (\ref{eq:2}) is used for $u_{j}(t)$, in other words, $u_{j}(t) = \alpha(s(t))$. + + +\begin{figure}[ht] + \vspace*{-5mm} + \centering + \begin{subfigure}[ht]{0.8\textwidth}% + \centering + \includegraphics[width=0.8\columnwidth]{figure/blend_case_1.eps}% + \caption{Motion blend case 1: $T_s = 6s$, blending starts at 3.5s, ends at 8s. The resulted blending trajectory comes to a stop in the middle.}% + \label{blend_case_1}% + \end{subfigure} + + \begin{subfigure}[ht]{0.8\textwidth}% + \centering + \includegraphics[width=0.8\columnwidth]{figure/blend_case_2.eps}% + \caption{Motion blend case 2: $T_s = 3.5s$, blending starts at 3.5s, ends at 5.5s. The resulted blending trajectory smoothly transits from first trajectory to the second trajectory. The velocity profile has no jumps.}% + \label{blend_case_2}% + \end{subfigure} + + \begin{subfigure}[ht]{0.8\textwidth}% + \centering + \includegraphics[width=0.8\columnwidth]{figure/blend_case_3.eps}% + \caption{Motion blend case 3: $T_s = 4.5s$, blending starts at 3.5s, ends at 6.5s. The resulted blending trajectory smoothly transits from first trajectory to the second trajectory. The velocity profile has no jumps.}% + \label{blend_case_3}% + \end{subfigure} + +\end{figure} + + +\begin{thebibliography}{9} +\bibitem {lloyd1993trajectory}Lloyd, John and Hayward, Vincent \textit{Trajectory generation for sensor-driven and time-varying tasks}, The International journal of robotics research, 1993 +\bibitem {dantam2014orientation}Dantam, Neil and Stilman, Mike \textit{Spherical Parabolic Blends for Robot Workspace Trajectories}, International Conference on Intelligent Robots and Systems (IROS), 2014 +\bibitem{pilztrajectorygeneration}\url{https://github.com/ros-planning/moveit/tree/master/moveit_planners/pilz_industrial_motion_planner} +\end{thebibliography} +\end{document} diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/README.md b/moveit_planners/pilz_industrial_motion_planner/doc/README.md new file mode 100644 index 0000000000..65966e8415 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/README.md @@ -0,0 +1,22 @@ +# Trajectory generation + +## Overview sequence processing +The following diagram shows the steps to process and execute +a list of commands given as sequence. The diagram also shows the resulting data +structures of each processing step. +![OverviewProcessing](sequence_processing_overview.png) + +## Overview classes PlanningContext +The following diagram shows the different classes responsible for the loading +of the different planning contexts (e.g. Ptp, Lin, Circ) and the +relationship between them. +![DiagClassPlanningContext](diag_class_planning_context.png) + +## Relationship MoveGroupCapabilities and ComandListManager +The following diagram shows the relationship between the MoveGroupCapabilities +and the CommandListManager. +![DiagClassCapabilities](diag_class_capabilities.png) + +## Blending algorithm description +A description of the used blending algorithm can be found +[here](MotionBlendAlgorithmDescription.pdf). diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.png b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.png new file mode 100644 index 0000000000..eeb13715b7 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.uxf b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.uxf new file mode 100644 index 0000000000..6922b787e5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_capabilities.uxf @@ -0,0 +1,413 @@ + + // Uncomment the following line to change the fontsize and font: +fontsize=10 +// fontfamily=SansSerif //possible: SansSerif,Serif,Monospaced + + +////////////////////////////////////////////////////////////////////////////////////////////// +// Welcome to UMLet! +// +// Double-click on elements to add them to the diagram, or to copy them +// Edit elements by modifying the text in this panel +// Hold Ctrl to select multiple elements +// Use Ctrl+mouse to select via lasso +// +// Use +/- or Ctrl+mouse wheel to zoom +// Drag a whole relation at its central square icon +// +// Press Ctrl+C to copy the whole diagram to the system clipboard (then just paste it to, eg, Word) +// Edit the files in the "palettes" directory to create your own element palettes +// +// Select "Custom Elements > New..." to create new element types +////////////////////////////////////////////////////////////////////////////////////////////// + + +// This text will be stored with each diagram; use it for notes. + 7 + + UMLClass + + 819 + 308 + 147 + 56 + + MoveGroupCapability +-- + + + + + UMLClass + + 917 + 420 + 168 + 63 + + MoveGroupSequenceAction +-- + + + + + + UMLClass + + 742 + 420 + 140 + 63 + + MoveGroupSequenceService +-- + + + + + + Relation + + 833 + 357 + 21 + 77 + + lt=<<- + 10.0;10.0;10.0;90.0 + + + Relation + + 938 + 357 + 21 + 77 + + lt=<<- + 10.0;10.0;10.0;90.0 + + + UMLNote + + 980 + 308 + 98 + 21 + + Plugin interface + + + + Relation + + 959 + 308 + 35 + 21 + + lt=.. + 30.0;10.0;10.0;10.0 + + + UMLClass + + 763 + 532 + 168 + 63 + + CommandListManager +-- + + + + + + + UMLClass + + 1155 + 525 + 126 + 35 + + plan_execution::PlanExecution +-- ++planAndExecute() + + + + Relation + + 1204 + 469 + 21 + 70 + + lt=<<<<- + 10.0;10.0;10.0;80.0 + + + UMLClass + + 1155 + 427 + 112 + 49 + + MoveGroupContext +-- ++ plan_execution_ + + + + + Relation + + 1078 + 448 + 91 + 21 + + lt=<<<<- + 10.0;10.0;110.0;10.0 + + + UMLClass + + 749 + 644 + 182 + 63 + + TrajectoryBlenderTransitionWindow +-- + + + + + Relation + + 924 + 476 + 21 + 70 + + lt=<<<<- + 10.0;10.0;10.0;80.0 + + + Relation + + 784 + 476 + 21 + 70 + + lt=<<<<- + 10.0;10.0;10.0;80.0 + + + UMLState + + 329 + 553 + 203 + 70 + + /planning_interface::PlannerManager/ +CommandPlanner + + + + Relation + + 392 + 616 + 21 + 63 + + lt=<- + 10.0;70.0;10.0;10.0 + + + Relation + + 434 + 616 + 21 + 63 + + lt=<- + 10.0;10.0;10.0;70.0 + + + UMLState + + 168 + 665 + 126 + 49 + + Plugin +PTP TrajectoryGenerator + + + + + UMLState + + 546 + 665 + 133 + 49 + + Plugin +CIRC TrajectoryGenerator + + + + + UMLState + + 364 + 665 + 126 + 49 + + Plugin +LIN TrajectoryGenerator + + + + + Relation + + 210 + 574 + 133 + 105 + + lt=<- + 10.0;130.0;10.0;10.0;170.0;10.0 + + + Relation + + 224 + 588 + 119 + 91 + + lt=<- + 150.0;10.0;10.0;10.0;10.0;110.0 + + + Relation + + 525 + 588 + 126 + 91 + + lt=<- + 160.0;110.0;160.0;10.0;10.0;10.0 + + + UMLGeneric + + 161 + 539 + 532 + 182 + + MoveIt Planning Pipeline +halign=left +fg=blue + + + + Text + + 252 + 637 + 203 + 21 + + planning_interface::MotionPlanRequest + + + + Text + + 441 + 637 + 210 + 21 + + planning_pipeline::MotionPlanResponse + + + + Relation + + 525 + 574 + 140 + 105 + + lt=<- + 10.0;10.0;180.0;10.0;180.0;130.0 + + + Relation + + 686 + 546 + 91 + 28 + + lt=<- +<<uses>> + 10.0;20.0;110.0;20.0 + + + Relation + + 826 + 588 + 42 + 70 + + lt=<- +<<uses>> + 10.0;80.0;10.0;10.0 + + + UMLClass + + 147 + 287 + 959 + 448 + + Motion planning +layer=-1000 +halign=left + + + + UMLClass + + 1127 + 287 + 168 + 448 + + Motion execution +layer=-1000 +halign=left + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png new file mode 100644 index 0000000000..724cdf1911 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf new file mode 100644 index 0000000000..1a8a3bc7c5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf @@ -0,0 +1,906 @@ + + 7 + + UMLClass + + 287 + 273 + 567 + 133 + + PlanningContextLoader +-- +... +-- ++getAlgorithm(): std::string ++setModel(model: moveit::core::RobotModelConstPtr&): bool ++setLimits(joint_limits: pilz_industrial_motion_planner::JointLimitsContainer&): bool ++loadContext(planning_context: planning_interface::PlanningContextPtr&, name: std::string&, group: std::string&): bool +#loadContext<T>(planning_context: planning_interface::PlanningContextPtr&, name: std::string&, group: std::string&): bool +-- + + + + UMLGeneric + + 0 + 525 + 259 + 70 + + symbol=component +PlanningContextLoaderPTP +-- +-- ++loadContext(planning_context, name, group) +-- + + + + UMLGeneric + + 448 + 525 + 259 + 70 + + symbol=component +PlanningContextLoaderLIN +-- +-- ++loadContext(planning_context, name, group) +-- + + + + Relation + + 119 + 350 + 182 + 189 + + lt=<<- + 240.0;10.0;10.0;10.0;10.0;250.0 + + + Relation + + 567 + 399 + 21 + 140 + + lt=<<- + 10.0;10.0;10.0;180.0 + + + UMLGeneric + + 35 + 756 + 133 + 35 + + PlanningContextPTP +-- +-- + + + + UMLGeneric + + 511 + 749 + 126 + 35 + + PlanningContextLIN +-- +-- + + + + UMLClass + + 1260 + 616 + 119 + 126 + + template=T +PlanningContextBase +-- +-- + + + + Text + + 21 + 609 + 175 + 14 + + <<bind>><T->TrajectoryGeneratorPTP> + + + + Relation + + 133 + 644 + 1141 + 126 + + lt=<<- + 1610.0;10.0;10.0;10.0;10.0;160.0 + + + Relation + + 602 + 672 + 672 + 91 + + lt=<<- + 940.0;10.0;10.0;10.0;10.0;110.0 + + + Text + + 490 + 609 + 175 + 14 + + <<bind>><T->TrajectoryGeneratorLIN> + + + + UMLGeneric + + 511 + 1001 + 126 + 35 + + TrajectoryGeneratorLIN +-- +-- + + + + Relation + + 98 + 588 + 21 + 182 + + lt=<.. + 10.0;240.0;10.0;10.0 + + + UMLGeneric + + 35 + 1001 + 126 + 35 + + TrajectoryGeneratorPTP +-- +-- + + + + UMLGeneric + + 1176 + 861 + 308 + 98 + + TrajectoryGenerator +-- +-- +TrajectoryGenerator(RobotModelConstPtr, LimitsContainer) +generate(request, response, sampling_time) + + + + + Relation + + 567 + 588 + 21 + 175 + + lt=<.. + 10.0;230.0;10.0;10.0 + + + Relation + + 567 + 777 + 21 + 238 + + lt=<<<<- +m1=1 + 10.0;10.0;10.0;320.0 + + + Relation + + 98 + 784 + 21 + 231 + + lt=<<<<- +m1=1 + 10.0;10.0;10.0;310.0 + + + Relation + + 602 + 903 + 588 + 112 + + lt=<<- + 820.0;10.0;10.0;10.0;10.0;140.0 + + + Relation + + 126 + 875 + 1064 + 140 + + lt=<<- + 1500.0;10.0;10.0;10.0;10.0;180.0 + + + UMLNote + + 0 + 441 + 98 + 49 + + Plugin + + + + + Relation + + 42 + 483 + 21 + 56 + + lt=.. + 10.0;10.0;10.0;60.0 + + + UMLClass + + 1589 + 525 + 147 + 112 + + JointLimitsContainer +-- +-- +-- + + + + + UMLClass + + 1792 + 525 + 147 + 112 + + CartesianLimit +-- ++max_trans_vel_: double ++max_trans_acc_: double ++max_trans_dec_: double ++max_rot_vel_: double ++max_rot_acc_: double ++max_rot_dec_: double +-- +-- + + + + + UMLClass + + 1673 + 399 + 196 + 91 + + LimitsContainer +-- +-- ++hasCartesianLimit(): bool ++getCartesianLimit(): pilz_industrial_motion_planner::CartesianLimit ++hasJointLimits(): bool ++getJointLimits(): pilz_industrial_motion_planner::JointLimitsContainer +-- + + + + + Relation + + 1764 + 483 + 126 + 56 + + lt=<<<<- +m1=1 + 10.0;10.0;160.0;60.0 + + + Relation + + 1659 + 483 + 70 + 56 + + lt=<<<<- +m1=1 + 80.0;10.0;10.0;60.0 + + + UMLClass + + 168 + 1232 + 126 + 42 + + /KDL::VelocityProfile/ +-- +-- +-- + + + + + UMLClass + + 28 + 1302 + 147 + 63 + + VelocityProfileATrap +-- ++ SetProfile ++ SetProfileDuration ++ SetProfileAllDurations +-- +-- + + + + + + Relation + + 126 + 1246 + 56 + 70 + + lt=<<- + 60.0;10.0;10.0;10.0;10.0;80.0 + + + Relation + + 91 + 1029 + 21 + 287 + + lt=<<<<- +m1=n + 10.0;10.0;10.0;390.0 + + + UMLClass + + 525 + 1316 + 91 + 42 + + Package::KDL +Path_Line +-- +-- + + + + + UMLClass + + 833 + 1316 + 84 + 42 + + Package::KDL +Path_Circle +-- +-- + + + + + UMLClass + + 980 + 1316 + 105 + 42 + + Package::KDL +Path_Circle_Wrapper +-- +-- + + + + + UMLNote + + 1393 + 1176 + 98 + 49 + + Class to manage the Cartesian path +bg=white + + + + Relation + + 1330 + 1190 + 77 + 21 + + lt=.. + 10.0;10.0;90.0;10.0 + + + Relation + + 560 + 1029 + 21 + 301 + + lt=<<<<- + 10.0;10.0;10.0;410.0 + + + UMLGeneric + + 882 + 1001 + 168 + 35 + + TrajectoryGeneratorCIRC +-- +-- + + + + Relation + + 1022 + 1029 + 21 + 301 + + lt=<<<<- + 10.0;10.0;10.0;410.0 + + + Relation + + 581 + 1197 + 693 + 133 + + lt=<<- + 970.0;10.0;10.0;10.0;10.0;170.0 + + + Relation + + 889 + 1232 + 385 + 98 + + lt=<<- + 530.0;10.0;10.0;10.0;10.0;120.0 + + + Relation + + 1050 + 1267 + 224 + 63 + + lt=<<- + 300.0;10.0;10.0;10.0;10.0;70.0 + + + UMLGeneric + + 896 + 742 + 126 + 35 + + PlanningContextCIRC +-- +-- + + + + Relation + + 952 + 770 + 21 + 245 + + lt=<<<<- +m1=1 + 10.0;10.0;10.0;330.0 + + + Relation + + 987 + 700 + 287 + 56 + + lt=<<- + 390.0;10.0;10.0;10.0;10.0;60.0 + + + UMLGeneric + + 826 + 525 + 259 + 70 + + symbol=component +PlanningContextLoaderCIRC +-- +-- ++loadContext(planning_context, name, group) +-- + + + + Relation + + 952 + 588 + 21 + 168 + + lt=<.. + 10.0;220.0;10.0;10.0 + + + Text + + 875 + 609 + 175 + 14 + + <<bind>><T->TrajectoryGeneratorLIN> + + + + Relation + + 987 + 931 + 203 + 84 + + lt=<<- + 270.0;10.0;10.0;10.0;10.0;100.0 + + + Relation + + 847 + 350 + 119 + 189 + + lt=<<- + 10.0;10.0;150.0;10.0;150.0;250.0 + + + Relation + + 91 + 469 + 434 + 70 + + lt=.. + 10.0;10.0;600.0;10.0;600.0;80.0 + + + Relation + + 91 + 448 + 812 + 91 + + lt=.. + 10.0;10.0;1140.0;10.0;1140.0;110.0 + + + UMLClass + + 287 + 126 + 567 + 63 + + CommandPlanner +-- +-- ++getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, +const planning_interface::MotionPlanRequest& req, +moveit_msgs::MoveItErrorCodes& error_code):planning_interface::PlanningContextPtr +-- + + + + + Relation + + 567 + 182 + 21 + 105 + + lt=<<<<- +m1=n + 10.0;10.0;10.0;130.0 + + + UMLClass + + 476 + 0 + 203 + 49 + + PlanningInterface::PlannerManager +-- +-- + +-- + + + + Relation + + 567 + 42 + 21 + 98 + + lt=<<- + 10.0;10.0;10.0;120.0 + + + UMLClass + + 1211 + 518 + 203 + 49 + + PlanningInterface::PlanningContext +-- +-- + +-- + + + + Relation + + 1309 + 560 + 21 + 77 + + lt=<<- + 10.0;10.0;10.0;90.0 + + + Relation + + 910 + 1330 + 84 + 21 + + lt=<<<<- + 100.0;10.0;10.0;10.0 + + + UMLClass + + 308 + 1302 + 147 + 63 + + Package::KDL +VelocityProfile_Trap +-- ++ SetProfile ++ SetProfileDuration +-- +-- + + + + + + Relation + + 420 + 1008 + 476 + 308 + + lt=<<<<- + 660.0;10.0;410.0;10.0;410.0;140.0;10.0;140.0;10.0;420.0 + + + UMLClass + + 770 + 1141 + 126 + 42 + + Package::KDL +Trajectory_Segment +-- +-- + + + + + Relation + + 889 + 1029 + 63 + 147 + + lt=<<<<- + 70.0;10.0;70.0;190.0;10.0;190.0 + + + Relation + + 287 + 1246 + 63 + 70 + + lt=<<- + 10.0;10.0;70.0;10.0;70.0;80.0 + + + UMLClass + + 784 + 1064 + 98 + 35 + + /KDL::Trajectory/ +-- +-- +-- + + + + + Relation + + 826 + 1092 + 21 + 63 + + lt=<<- + 10.0;10.0;10.0;70.0 + + + UMLClass + + 1260 + 1183 + 77 + 112 + + /KDL::Path/ +-- +-- + + + + + Relation + + 588 + 1029 + 196 + 147 + + lt=<<<<- + 10.0;10.0;10.0;190.0;260.0;190.0 + + + Relation + + 357 + 1008 + 168 + 308 + + lt=<<<<- + 220.0;10.0;10.0;10.0;10.0;420.0 + + diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_1.eps b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_1.eps new file mode 100644 index 0000000000..91d0feecd9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_1.eps @@ -0,0 +1,1648 @@ +%!PS-Adobe-3.0 EPSF-3.0 +%%Creator: (MATLAB, The Mathworks, Inc. Version 9.2.0.556344 \(R2017a\). Operating System: Windows 7) +%%Title: I:/Entw-Technologien/Studenten/temp_kai/blend_case_1.eps +%%CreationDate: 2017-10-06T19:06:40 +%%Pages: (atend) +%%BoundingBox: 0 0 774 521 +%%LanguageLevel: 3 +%%EndComments +%%BeginProlog +%%BeginResource: procset (Apache XML Graphics Std ProcSet) 1.2 0 +%%Version: 1.2 0 +%%Copyright: (Copyright 2001-2003,2010 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/bd{bind def}bind def +/ld{load def}bd +/GR/grestore ld +/M/moveto ld +/LJ/setlinejoin ld +/C/curveto ld +/f/fill ld +/LW/setlinewidth ld +/GC/setgray ld +/t/show ld +/N/newpath ld +/CT/concat ld +/cp/closepath ld +/S/stroke ld +/L/lineto ld +/CC/setcmykcolor ld +/A/ashow ld +/GS/gsave ld +/RC/setrgbcolor ld +/RM/rmoveto ld +/ML/setmiterlimit ld +/re {4 2 roll M +1 index 0 rlineto +0 exch rlineto +neg 0 rlineto +cp } bd +/_ctm matrix def +/_tm matrix def +/BT { _ctm currentmatrix pop matrix _tm copy pop 0 0 moveto } bd +/ET { _ctm setmatrix } bd +/iTm { _ctm setmatrix _tm concat } bd +/Tm { _tm astore pop iTm 0 0 moveto } bd +/ux 0.0 def +/uy 0.0 def +/F { + /Tp exch def + /Tf exch def + Tf findfont Tp scalefont setfont + /cf Tf def /cs Tp def +} bd +/ULS {currentpoint /uy exch def /ux exch def} bd +/ULE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add moveto Tcx uy To add lineto + Tt setlinewidth stroke + grestore +} bd +/OLE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs add moveto Tcx uy To add cs add lineto + Tt setlinewidth stroke + grestore +} bd +/SOE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs 10 mul 26 idiv add moveto Tcx uy To add cs 10 mul 26 idiv add lineto + Tt setlinewidth stroke + grestore +} bd +/QT { +/Y22 exch store +/X22 exch store +/Y21 exch store +/X21 exch store +currentpoint +/Y21 load 2 mul add 3 div exch +/X21 load 2 mul add 3 div exch +/X21 load 2 mul /X22 load add 3 div +/Y21 load 2 mul /Y22 load add 3 div +/X22 load /Y22 load curveto +} bd +/SSPD { +dup length /d exch dict def +{ +/v exch def +/k exch def +currentpagedevice k known { +/cpdv currentpagedevice k get def +v cpdv ne { +/upd false def +/nullv v type /nulltype eq def +/nullcpdv cpdv type /nulltype eq def +nullv nullcpdv or +{ +/upd true def +} { +/sametype v type cpdv type eq def +sametype { +v type /arraytype eq { +/vlen v length def +/cpdvlen cpdv length def +vlen cpdvlen eq { +0 1 vlen 1 sub { +/i exch def +/obj v i get def +/cpdobj cpdv i get def +obj cpdobj ne { +/upd true def +exit +} if +} for +} { +/upd true def +} ifelse +} { +v type /dicttype eq { +v { +/dv exch def +/dk exch def +/cpddv cpdv dk get def +dv cpddv ne { +/upd true def +exit +} if +} forall +} { +/upd true def +} ifelse +} ifelse +} if +} ifelse +upd true eq { +d k v put +} if +} if +} if +} forall +d length 0 gt { +d setpagedevice +} if +} bd +/RE { % /NewFontName [NewEncodingArray] /FontName RE - + findfont dup length dict begin + { + 1 index /FID ne + {def} {pop pop} ifelse + } forall + /Encoding exch def + /FontName 1 index def + currentdict definefont pop + end +} bind def +%%EndResource +%%BeginResource: procset (Apache XML Graphics EPS ProcSet) 1.0 0 +%%Version: 1.0 0 +%%Copyright: (Copyright 2002-2003 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/BeginEPSF { %def +/b4_Inc_state save def % Save state for cleanup +/dict_count countdictstack def % Count objects on dict stack +/op_count count 1 sub def % Count objects on operand stack +userdict begin % Push userdict on dict stack +/showpage { } def % Redefine showpage, { } = null proc +0 setgray 0 setlinecap % Prepare graphics state +1 setlinewidth 0 setlinejoin +10 setmiterlimit [ ] 0 setdash newpath +/languagelevel where % If level not equal to 1 then +{pop languagelevel % set strokeadjust and +1 ne % overprint to their defaults. +{false setstrokeadjust false setoverprint +} if +} if +} bd +/EndEPSF { %def +count op_count sub {pop} repeat % Clean up stacks +countdictstack dict_count sub {end} repeat +b4_Inc_state restore +} bd +%%EndResource +%%EndProlog +%%Page: 1 1 +%%PageBoundingBox: 0 0 774 521 +%%BeginPageSetup +[1 0 0 -1 0 521] CT +%%EndPageSetup +GS +[0.6 0 0 0.60069 0 0.20001] CT +1 GC +N +0 0 1290 867 re +f +GR +GS +[0.6 0 0 0.60069 0 0.20001] CT +N +0 0 M +0 867 L +1290 867 L +1290 0 L +cp +clip +1 GC +N +0 0 1290 868 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1283 286 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +0.941 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.48 0 0 0.48055 0 33.71863] CT +[1 0 0 1 0 0] CT +N +0 -69.75 M +0 286.5 L +1603.75 286.5 L +1603.75 -69.75 L +cp +clip +GS +0 0 translate +1284 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Wd;*"]RJ?g=lsJ]:"^N!^JqJ&N0pjXffo+9<\GSZd=Asr^Jj\`gnWKAW"Fg]ukHtS*Q +MgT5QlW?uD#_u/G`2SPP'fF5e9\c\P^,kYJ5eE:[j5oCaDKi4Qp6Lp"]ggq8kK)a$%!"=BP>O^;fEK%$ +HC-?[8AqK`3#+G0X[LSK%YosU04)0km3'K:;6&/&t4,#1R$1CFk/S[,-u]:ccG7c<6. +f_iq/3#p2"I1ORSS-d\kFY#i`&=9MC/#J^](X"[jrsZD[e+% +*;BDD@H+B;lH3#/]*[?laMn&UV_+2\4K*0,hRdA26^PFn@,GOEL9rpd71kKfdhQ)";&Ams]#doj,oB)s+J-/ao= +7%B.%9Qc.r=rR,[_K&U\IB[]0-K8u4S;JZdpu@F-3-!s#Pq+e2gph\'CAI8]e#0tYo#l%1Irn$5DYI/` +l2:AKds_T3>?Udpfs?/jHM!-o4:Cr*OX"MbT"T71dWAj&OGHgI#Oq&EQ*kN(Mk`e_D3q43Ot/%5dse.# +M8KpFMd4ZGB&W(-YCH,b-iMqOp=X(iJ:6[YQYL-kKDc*B=pRZl(\.NhEOGHgI+kr??CHaaf +j#$YrE($4RikGHkmpE_PESS;d$PR0D:=lE`6.b!/VPtM##765H'bOLWWb9YL/X(+t:i,/KHZD(i:.5il +qWk?BIJ;QkRn/lKq"a*F+8;CpKo*-2&)U^>!hT'X^`s/B3#.RMKXCD[imlSJkKR!c(p(J&$]E6fLc?,D +UtQE/q=2egj`cK-I+Y82#D_;n>osu*_Ao@WJD7@+]Sd(RYeQ/mC)7Ub@,BM,#1Rj +\p3c)ace+0H0:ahcSoe!4Ep5Kj,ZERqtBDshes8Wc8oglp;bd"/ccKTPtgdp,#TC7V!=kJ[6N64 +b?[8ijD[MWMOJ,m-I+ZS=3\M9DKk047n8$LCu2>qO!Z.3<()U?8@9D7X,jing,QbG*0,hR;:Eb+`*&u_ +eNA@lgmpqOr,R@MLf4m7&jdMCEI-j2%m:l0kEj'uE@P^l=dV27Zu=/7i,K8o>22cd,702d'p0[rUk+NS +j.6Naj7HSs3$M*d9f!.jB:Wq4l-rr>*^91`f,I!\$&LVVOWm#cmIuI+[u'CP+o-VPWH'sB:.7l5$PU": +lZCX9T^1at7CWqOcsh_t/esMC\WBin=$)sJrBtJ^oR(U;,#1R$1CFl*;!W5,H_S"QHuKH.&e.\M.@LaG +qERdRO&*H5[%5)HE@P^l=dZ_0?`3dM(9gHI+`e_NGU*0,hR;:Eb+`3j]Xm^rZb +p=!1t!?[brL;Zb_&Lh<9Ut+#?:1oWEfZ*.n2A6hQE[kgm=dZ^BXj]H%oi`o^VMk7SrM4*@>^M5%JN3LfpLIVO<-p4+q`[q^gYGPHRWpmH&joS;KkbhN5n7 +jm)-rR((996a8Sk_pR=uQlRYkRTZ#fk18eB9>KSt"?\%On;1'U.HVbKRZ5c,h_K&=DUKM(`71cbBkCf5 +En>$m-/]A"hf]Od=b618jtFsH3rj+'pq +MF]I7+Ur=ZY.M4KWTL9+VF!^G1CFk?i,q*C=ho1OYK'#S0aE*c<()Uh%;G-@aPH8*\!saE3m#$8`*lP@ +e',\I9tF()/ch#]"L(P7$ci@@3#.Qbc9nU\&mb);"rjHrb]uVs$5GGC/54iJf'/Pp^uKl!m1Sn4Ekj%t +7AO%AONJO6l>#,oZ]3Eg$W.KF;c2#f_Sk!+e*S)?f3ACN/79N>^aDlGUfa^[b"t3Lj)jtCl(3m`P?Y,G +BSMD5Ael_2BUt0p&bI.9Y9,m5l$uP6'p2r;6Q3@PH;M**om*Bj/7M%Y;c2#f_SfI?[.[Y?$mc!.i?dkt)mS;NbI(1s1^h?\F7oC(u0lEjnUD;p(AS;Ki\ +Ud8Q9/oTsE:1qekTPU,>9h!""W+fjPC/jfnW.tr8?NTXiqh7ajc.L'hO<2.1SNV>N-",^Pn;1'U.>1Z1grAT6V[2,HVR$==O[2E11CLO7s!,=3>G1`^/Du;M +J?;aJ#`1MUO[;Tcl>N160aeNK0lqoFLc?,,bq-N3b.Cbu$`jSOA7M83]B=g.XA`"&M)Z646:guTD4Id) +dJ;@)ie>%[QAUJs;:Eb+$Y@rf4*ktsmPhEITBoaZU_"/Y.O'S.-^OrUd;@&+='p1gR%GF"N +WVQ2;gh`K]&TK:8RTXmdll:J0lokA$02LO2NRfO-KLD>f80Oe+Q*[#:nCB_\@Lb:kX,&_M29d[7?+k+8 +LYk7,iCP,?e.Cr1'LF!=7ALLCTdaV?h/9]6+tin=b%E^>V'=nue24N$+mXX?[iF*LHJhYM(@GQ-/Uu9/ +YYIb5,iVck$6^Fq?L>bsqe&-1gGl;Zi`+DaZZ)\^\6;$k@*B(`p8.VZkk6BRVu?X.%-s,l66]/nU_(sR +$uVRV]O!;i@Bc%o&TK:8RTVUI2>Q\5S>1KqoB=P=H^2H/-&%G:;\:I31gP_QVoRU7FS7XgX,&_M29d\" +@-'XZB%0McWf>J3PAe]/6YCZ([#0Z<;:Eb+qc)t;g$9tdqgI0q +s"hDA*I`cGVkak:E@N/dOCh%kjfW41h5kLakkb;8h'4ZCDB-&%G:;\:I3 +1lKNP=ZN%QR>>Klj0X]";c2!ZGo#i*d^crio9c.0:?peZNljUM98DR4=f8&,&ofBFKofnJF\gHU)lK$g +Q<%X.j0X]";c2$+IMT(-i7n9>n+`;pZJ/UVofq_G%_VDS5p5A +.QWrVNK"H4pu&3V^=r0?k?o(n:V6SOo9S@Be*2g:HLClQDcIfZb9'se:R(mihX!A^GM9,s;@&+='p1gR +Xg)s-oZ.CUe%/MP,2uRD!3a'+b44N4O)B=,R:gEp^0U?TZTeh!+5e`+Ym6o[^&Q$FnnWT)bbXrZH`LnR +F#EYDlil9pZ;*t7,iVck$6ch$K8n>;]D)kcj0X]";phDD?+X.WgT+DZhRe"#P>=A&F6:^"-B/BMqVgn\hc[ +526,)gMai4VVR]s]_e=THhVCNJ!J-\s(ccMricjYW7ZYB4Xf52n+KraUcgE5dq5P)J7 +S&dW:Koh*h&LiC_C?99>SFY4#'->H=Bb_5XU5=9pmC-Z8IH-;GqHmB4?S>c3rUnd_r?m*hGCR*/do9r0 +pn(p,p]goiZG"M0M_b!]L[#&DM#bJ2bIEoXNaLf;\hhL"_)1ASZUc9$Yc4E9B!M_De( +mQW_==Z-!;=.>U>SVR*1+'dZF&$+s$:.ZTSi`&;Q+jH"8NRJD60q6iUXWlb(1m&_9DNn;69Z2Qj4F-MY +$ST.HmQ[--/:?N0o!2&04+$[^j]Aq'cFDYUNf<6t$cC[B(s5c5\B:(a:6b<(%':c)^i` +l+0BA,]0&P&TK:ha9Nf70^0YC^]42G?Q62X9B6$oBHc]+'LF!=7ALLCI+p]U/H]ph;`>;D2bpE03#,/` +\l71LDn\7(Q,1(H%@R1X$T%[];c3mX3#.QB8;j2N`SGFf6sT`Y6#X]A,2uQYWH77J4nm`QrSJqc;[LIK +:.ZTSi`&;Q+YB"#T"M_K=hA["Fn1O@.O%VVPIbr)=rD6cHgS4fp[3J-n],:fS/E<+G;BU?B:Y2P8AqK` +'LGtk[2Z$Qn!UMh4NYTW'bR@<"1WC5b44N4O)DRKiPP!:[qi_2/52RV8dZ^RV]o"EBq^O?3'D3sfqiSq +]=*c[djcP:70g0(Z1643+4X0''sfT6egW&$7%B-*+X@7N:60\-arVM"5Wf +gDd%PS^."cNG`U'*dMB>'bR@\CI3'H9k`(Nam[WgZZ3SV<8qf)]4U([]/UQ`bq)PYF\2ua&Z6gN#`6&''ibq;^rGU^ALLlmELHog +785QG'5*l!6:f%oL_N0"DJV`P2GW]ZKca(.2GR1j/8CVVQoODr'LF!=7ALJ-EI1_c4d``k,.;J0a.fH! +Ko*-28KfA=<(%':&\$)DWen5Z^,89_NRJ9YZ)DAK@H+@e;@&+='p1gR`(a3.R>B&%gY$B&oVRVs$c]HM +BkCf5XA`"&M)Z646:f$8XJ2/Ce+]J,h!K"535%!cMje5n-I+XuKoh*h&LiB$>bn*V)lJH49Y>*"oE&t_ +C78:m39ueC66]/nU_"/YnVRN0:@EWinbn1*ZMF\m9iY!d,<=^FZR>>^MSl:Y< +9f!.jB:Y2P8AqK`'LEur^S5He/sC5PARaeX@o:_!_SfTTU_"/Y.O'S.Lhj#b1(g^mDK\)6'62@f80K8<)R$)_j7LdJ;RbCdZZ3S<*/g7t(+.$L(:.ZTSi`&;Q+VAqN'b+uLhQ<6&D4GJaa.fH!Ko*-28KfA=<(%':&N?8<[P&;TRYL9,`&:B"MdoW5 +beh;$Vkak:E@N/dOGKB87D3#GH$Qqo^,89_NRP#f=O]160QGYm.>1*ZMF\m9ib"mYe^`2FVc,elgGh@> +AtbspAel`=3[KKALf4m7KLFV^<)d-MY*k))]A61#Ve=HE<&Cf&_SfTTU_"/Y.O'S.Lmtf0`bh4eH8+lr +?R@kY>"/&'/52RV8dZ^RV]o"EM7AtUBP?Gun?[tPM:fi4feB`Ibmh!5ERlQfKLD>f80K8Uq>)/eciSS6iOAq@L'?U/iuV'5*l!6:f%oL_N0"Y/M-@ +M0O/cjMpH##89o1f +80K8$G4sG.[D\Z&R48k-^r:@2:383[KKALf4m7KLFUC=&c'Q1@s'j`+<93ZuJ-C^+5WUAms]# +;c3mX3#.QB8Am)H7D7E7dgmNobX!gI7FPf`-I+XuKoh*h&LiB$gd5!tEoZeP#8/;@2:38 +3[KKALf4m7KLFSs<]%=+HM=b&[sm!i4eW%\k`D3dM6frK+XH&H6jb(L\$-TI)lGVVS^HK%NRO(nfSU/u +=dV27PS?G.:.:ri&ofDTg(`B1mP,=.3*f0G9f!.jB:Y2P8AqK`'LF!=7JE']\'2MF-=RplCT47HAms]# +;c3mX3#.QB8AqK@LGBInDOdjN9K_]D3HJ0L1p\lUegW&$7%B-*+XH&H1tnmfdt8/X/e`6N`p!_-e24Kc +ERlQfKLD>f80K8<<)dMn`p)f80K8<<"q0,j7N:3DO$''!?Oq9 +U8F?4M6frK+XH&H6jb(LVkOm;2bjlJS%-r#SDT=T.VbDTQoODr'LF!=7ALJ-E@OT1h`*b]#S15gT#Zi> +.QW)M++Po1*ZMF\m9i`+F#1(c/mO.D*iB]HIW-?]:`oePCT%)50R_SfTTU_"/Y.O'S.Lf712b!]J=hRVK& +XeJBm^3ob+WDi]oI^:_JegW&$7%B-*+XH&HFEkfDS?AI"eEqG2g:;3,7?dSi:O[?PgpnO,prCF`SNhV< +=7R3S/52RV8dZ^RV]o"EM)Z4j[S\!A#B`(ue[B=bGp6.#n8P$e!L_N0":.8+4lZKhq0`uE22GUdX@q4QbYJ5P\ +h04Wl,$eg>kF[6Qg`Ku@-I+XuKoh*h&LiB$S;LoKR>B&%gZ`fp-K=-T]]6AV-;s5e^3o`ss!qYe5'H;F +b:gUmmQWE6'5*l!6:f%oL_N0":.6t;=1`HulPWCPmcH';S6"%5/mc28o&Ro:hKc$so?Ea`c^d/lpu@Fm +^3t@`pR?knZVF(8,iVck$6^"ZU7#"uS6fB&)P2Wf+&"d9)lH[mWA=0R\od2hZ")=>LnB.ml-DTo]]T+9 +_SfTTU_"/Y.O'S.Lf;][)qm87ipjru,+eMJBb_Z4qYL&D++F"02rEutSRup^fAZX/K`E@H+@e;@&+='p1gR`2T]kb"u?TEOsolXu3t6>").A$PR0DP!V^XWJD6T,#1Su +@*HXT_$:XW2S!g2785QG'5*l!6:f%oL_N0":.=baf>d%uAKkGuB&g7URV!<^c8m8*OGHgI.>1*ZMWW9< +NRK+(26_((785QG'5*l!6:f%oL_N0":.=cSfBY"A0:0e)bY4P;EEYe*1CFk/SWZ<1`2SP,66]/no;t^) +A`2rt3].RjI,hSkRV!<^c8m8*OGHgI.>1*ZMAAStMS%\^bsYh9mH-\n:91=-Ams]#;c3mX3#.QB8AqK@ +U"!nKIe[=1aYutqW+fjPiiH'V#`1MUO[;UWWFua!_S[sZ&bf4?329d[GS97F2+VCUr;@&+='eucu`SGFf7,714mG'\'U8F?4M6frK+XH&H6jb(LV]nU2 +h)J\'L2$=s`b?/Fbq)PYF\2ua&Z6gN#`1MU;.n+ml+mT<0/"u^XK5+mL!D]b0QE-HU_"/Y.O'S.Lf4o_ +;I(X+*e\`%SG:0f,McWEVb(D7-I1#1'LF!=7ALJ-E@PE!gCs,*ddCiEiFV$!g,i&N0P4bQ*:6<@,iVck +$6^"Z+stk+oub))l#^YeioT:`X[b72`%4TPUHL&Z7%B-*+XH&H`.=1f\'2KB'=!PG=j4b$3SKZ`8AqK` +'LF!=70Jm4q!?\?2&iI^5FC)7iW-;=EK9ag&/&t4,)),7;\=#GU2_-MLE9.%kq_d[R"^BfL`SSeOL8:.:ri&ofBFDXWJ*7(g+B=`^"@)N%fI*lBO&S>/4rKLD>f +80K8<<(,LE(%$.3fTV[l1(+cYBMSsY&Z6gN#`1MU&N=\u@kJE*4e?nDH.c#!0aeZXBMSsY&Z6gN#`1MU +&U3`%b##a`]X9hIf.9$@'_`C?^=-\&iZ.>W+XH&H6jb(LV]t,p'igTW]A53jnGS3Re*3qdl3E3k_SeIg +P/4rKLD>f80K8<<((!?'ifUYLPFg9:FMg`U"Cni +S>/4rKLD>f80K8<<(,N8SSeOL8:.:ri&ofBFD@.,/4rKLD>f80K8<<(,Lu/)'/_r89TSCMR]aH`7$X@H&9o8KfA=<(%':&Z6i4Atl$5L>no)1::mG2@C0W +'C1nV',#:7,)),7;\=#UC9oU6ZaEPB/Kk_(,#S]'3#*#aUD44H`I"'DMoB:: +ib!;'dPaE9:.:ri&ofBFDUp?'or@0h=#Zf/cVV837*j:dC_e@8f%2?&+XH&H6jb(LV]t,(<)KjN>@(Xm +F"!WfB/>s0M/1L"AX3XZ3#.QB8AqK`\2IAe4fZ$HmFKP!]:W4gV2N<)@H-"N:74C7,#1S&&/&t4,-KH2 +osP<8/DfET`J6VnRtZ]7of?Z-#`1MUO[;UWWJJ'pWMmC14EM,!4m(`k@2:2-DUp8c,p_JL6jb(LV]t-_ +Z_G,S=FC/#TeHif2kTE#6RO/=<(%':&Z6i4k.IED'X+WU/h`9WM[tMER(,.`V]6Hq&Z6gN#`1MU&[t"? +m@K4HKiseQ"^7f\8P$e!L_N0":.<)QBLUHVHmm4U`%C16-(C!P;\:I37%B-j;c2#f_SfTTU_"/Y.O'S. +Lf4n*.VbDTQoODr'LF!=7ALJ-E@N/-785QG'5*l!6:f%oL_N0":.<+#W+fjPiiH'V#`1MUO[;UWWJEMK +mHN(q5Q0lJYD0d$Z*Ei8#9W=3qJKnRiiH'V#`1MUO[;UWWJEMKe[otieu;TGY$\q-pr-<@H0+hkj%$RA +o?9#"Dr7_n'?n>3=f8&,&ofBFKoh*h&TK;#EV&>;1M>!t@$kXOI/3=XO$*!_;bmkI_G<;a`3[cRg]-ZqDnfSlU"AUiqkMBP`*'"6 +&/&t4,)),7;\="!)X6Rk^OGOSa+!Q:?^-$WS;"`Vp=o/m2r8eSs.*_9/mio&p".7_;.a7rXA`"&M)Z64 +6:f%oLc?-/<$pD4GOKEe&&j1-n9aW;=+:%l47X^nPi%d`Gjp&TSilhc$SRJ9'95hdg[2]^4.STcIbHH+Gp"Bl<(%':&Z6i4(:g/Ss*?pDZgb[V +D]%kA5I2PdOGHgI.>1*ZMMP3YeZ2aV0?`4_F(b""%aDj4I.2\MdMtI:PS?G.:.:ri&ofBF:.;X0h7>dX +@q0$f80K8<<(*76RV!<^c8m8*OGHgI.>1*ZMMP2.e',\I=f8&, +&ofBFKoh*h&TK:8RTZ#fDRV^pQbY]p&ofBFKoh*h&TK:8RTZ";'p1gR`2SP,66]/n`2T\Lre?8.&LiB$ +S;Ki\,iVckS;NctDjWL!O[;UWWJD6T,#1SFWMjP1L5Ah@.>1*ZMF\m9i`&=9MON[GQoGhC6:f%oL_N0" +:.<+#W+l6cM,P!8KLD>f80K8<<(*76Rc\OU3#.QB8AqK`'LF!=7Du1(C&WjD;\:I37%B-*+XH&H7%B.' +k&6VH'p1gR`2SP,66]/n`2T\Lre?8.&LiB$S;Ki\,iVckS;NctDjWL!O[;UWWJD6T,#1SFWMjP1L5Ah@ +.>1*ZMF\m9i`&=9MON[GQoGhC6:f%oL_N0":.<+#W+l6cM,P!8KLD>f80K8<<(*76Rc\OU3#.QB8AqK` +'LF!=7Du1(Bh&1(!+9D4HV+JBzzzzzzzzz!!!"tZa$e*_$;'0p!m-mINc)NqYGN;kd/T!S+[4u*o+JXq +bM_=(]g)Pmci9='g16q!mB7[ +ls?,1Z]Lmf*B(SPadNrY/i&9\oQ(mr[+gT]mB/(]h^ifUms>*s8;K;I +SmJrV+[/t;Z6R_IpX]%ieoJ$h=sN7F<$MR;tc&&fR9LS8pEJOVKn<:A;[?:H\O@Zi/dqrV#"-n +%@9<:%&hE1@+8la,V0ejcrpD?R_nDV4quS0u[3_Hi<%ZIjr:YKaS^@_Ap@PIe9E,kKimPqt]EI5PE`>n +%\o%'OMJi<";4U,d[O*\))G3]W^"NaI^OGdH/QjmlPWR^1hgSf/!oJ7_NW*#%? +ZPtbl`\&e`E,L?gqJ46\%hn/(LMR;s2r@$7]GV75QCNn?PP/8n(t`"rpY\-qq9<<;91<4V!d*Q>7Nj3q +q^_2.p?[r%,@upTJ(V_Bu0K@$2j_"P4B%fGma^FW[hi=CF*p'[RB?@M_3J,6'm\o[%p=dT;9pYC&:_<8]H5:ds@q;db>plGHbZ +EiI'GoT(05rsJmpj`4^J,IXmr(f?K4FR(u^\l9i9`%9+!bK6^HhZsjp,Fif`Alsks2J'GRoAaJ)`MZ!^ +VHQ?MOZeSnq@ldR+=p)m6:C1UQU_"^i08qJ,F:DaHV04$Zc<_qq^^565M7ZGj;9+hK\(?TE"iPj`'jE+ +8iq1UcEsjQ'J*TI.)F>qqL$-2XOR6Rg`"G::WB8qH;H"GFX@YY!;jC&#W0nqU(@QFQ^r?BtkEek3h';h +u2tes6IH!X88r$Qim-NDgu!Je:.21+81KIpYUIMBO;tMa%q3]e&NMVXIOmFYHQirH1Ut0o;>BV2@d'CA +ms\6AmqUm.B<8^g9d,\=kG0IQKhuWDnc&0cr^,NSOpNmo#ml"T +De+hds`IIe[*RcHZTs#%/]ePB:kH$Y$JXtpr18pHV?k%V#$e-cm?>Sro\e1ktcdiXd;HA$r&HO\N,2&Z +`mBr>j'D'Hg\H-qS.0*G7NR(f3^2a%,U"b-FL+`Ys.*4^\m1>TmV:B09%mSF?5$rT)/$$0AWUm8 +Q@uSq:2Ztec3eJ]6DF1Bk:jeMQ_n\ffd;6j^qE&.rre'Y'TU3CTkEpn(ta=5Q:H%R59K]'__1<(LNC_K +^X?XHZ/:Nh#Rfl`oi%9R,@i(2s!d$jd0=\?XLQjldjN\5@*-t^A6o\Btl9(k2O@_&C-1F/$PtpSJr./9k*mU)[e^7<&S>G]XMf2Fe>_^uo +]233hRn,Tg96>+\T?rZ$ST/3m+ATBIAHo8Va!oQ7F@!P+gbHrG3&AE,]_$:zzz!!'f$q>UJuAP96~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 33.71863] CT +[1 0 0 1 0 0] CT +N +-1284 -69.75 M +-1284 286.5 L +319.75 286.5 L +319.75 -69.75 L +cp +clip +GS +0 0 translate +320 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WIqY,?hoG8OWe;(N#3ajLkLEb@^8BI&2TF\6>2(lYTuV)L8867 +7]aPH1l$HiU^X+\[<\qWW^fLi +e(hKTa)4Ud6sX)hpF;h^<4"a7Y9!Qc6hHj(qF-/&2eQ.Jn&:2$cc#bRdnbJlop:*3h;-mj<2FflSNhW' +`f1pIZuA,'burTk]5#lr\fqRm%AuslZ7P_KqsT%-eQ9ZMldr=YfW_(1?.lf8p$226e%b\;Mm6B/-KmC$ +[F"BlWDekuI/NNT1M>"_NQU4O(]3?aDe_;kZ>*CG-1B"'KaS\3L1aZtY@9]=PTGNGm^g +4d2d7oU@i]n@,g59pmcun])`L?c3[tnC+9CXfW90leoV?2d!BC5QCX[htYp5CW"]rp@a7`IX';:8bMcL +ldi1rT4"6PGAbNc-#,sE2E!H73H?D#^1,'=GM[VC0`2kc%?#PJd\TUOqWb-V?0MaK_bQ^]Z$#B`qsM'l +pGOldl1DXLcfitfJ(8%Pn(t/SoS^6=%Go[5)^joPn](l,p3ZT(@=@2)^]2##Dqh]pQa^eadb3:k%^"p< +R<.u^:#0shp?`_RAj#hCF#rU;]rP5`k2`E\].?rB/5Q=uuqtrBaiHj.X=gRa=opYC7 +Vk<7[,1SIbs,8f6hR[j10H;kH,-D^XeZ!H,*JY+FHL8,l1__DcF`dTYFcCf1@uVOZn`%OOd/)hFPIdi? +_d,Vs6Q/3'r>(9NT;0$H(TrP8>q5k'5p'MXDD`j%<#R,JN+nmd:qb7cTbbR +50VTFW$-E7VG4'M4uekWDu.?DYa1T@kk;r177m-0K]3HD;b6Z3fR*VGaa6@rj6)5YBse,oC&CGkRHe<@ +>X!Z:]T/U3:S+YCkp0?.bQ@qLL%>(7?[1S^9_=6i:n-o<7s%/$/Wfb;`T"jpeSP.!NP)8\#L@_cRPejf +jn5+WE;HD<],L->EG5gIfs<';V8'cPUTMh(Fns7L1E%e(Dau=kVP^4WH2XTnS:9_SY/^@(Pq,r$!?*JD +L5&[__M&A?c5YV3eiC$rRhBT)C'(^W;L[Vn&)^?Jc'MIZYe6T6!K:a:S=4N.-m4qTToV,")!l1\sOpkVm/H +M[\!OcKH&^U>MKg0/>r13NN"*X(M?5XnJ_mVtB@e^>o^MDpGAsh^Y!Xp?'Vh/M/R%LT"k1RUob"YsZ8d +X+c!MqY[V!=0J2ZIJV?d7u)hRqYL&S/mVXf?Z0*CYsZ8dX+c!Mo_bsTCt?\>F6Cj4Q5_c#B]XI*g,2@G +BL0Bn/$ZpkCBE1G5(Dq7^d(C[8,19ei:cQm@8>#:I_rXihjftai70]9>ntR*S$7,*?7g5oaeF42DpE*k +hl<)/[P7W"U"9X2h<"T[o1$JTX+c#'B)Ynl\^RHP?at&`(r=69qTW0"%:3cp5Q;W$jn6,M^H&@kI`o\-Z'h9BSC&uF7)(aW?Y"tAF%3pc&H1aMIZYfMbo-I( +6hHj(qF-/&2eVc@%+0"=kSG,2hji(I1.8l;X879DAomA)lK[d(c1^B+"pWAC3AWWSR-eA'BMR:*d;R=1 +^>G1.>c1;cQMIG7NupTh9mZBkal9oFe'(k24t!Jofuln#3MYJJT.ZsIb;>KR2JkKoi.,CX`cRBf#lWnE +I^=I61$[ANI]HB:#![P4FDHR[IF/M46B'As-M\kj:\$'>W-)[.oHM"/=l6#jg2a``Q6]oY:[D1'=05[BEMm?LGdF\@*SgBdc?XE]B`B3k'=:/b-aN0Q$Qp^O61 +m>"?#/3.n?f\s`'m-O*&kg74H61cOU.g9jTR:u>IO;r?S` +927:DB2PI>7uc_uQBi:<YkB`kWL'EnX9f;4QF:hqb*()B5-\sP`Qk&DDj6,Lt*gWFk]eLEpK4gg) +2XIA>^LW6)Gf_C-&iBJ\>Zer\/92A?ENcN$B[:,ZiidOg,28#<@I-SYVO#jO_ +@`@t6WEGcFlgh>mbG*+$MpZIgliZ$&VnFg681BOXNJ[0aIEsR*gi'.U_M&BF?/]Vi*r=ECVY+*!BT^Th +_\CR.W(loRZE!H8#]6f:bOktR?N7Mo,V@@1M_=$-kMu-4Oe\T'fh%bFO+Io?ZqsCkLB-28*gZ!\"agb*- +[F%n7!MePBeO%#n__X[FE3Zm]GJSk71M=uAT5T\'ru>X-FnOP#F4!3K&bWfELb]4(%!`c[:I%ke''-ofs524 +UcY$6RP\pI`&$AR?!TQpjC[9a_NSTKp;XA&oKc0i8TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]u +dB=A_KcU,$>+BVCMW7k$BHLTk/0jC1e6GKb*W8G1RW9)L+:QO2_Q]6H&QV"q(4[KD, +s*hAa(7FoVKo*?CH$,P1DiFm8N+]5c\G$0paBoB=$s^UL4B\ZZF8oK*/$ +(t7O!@LgBlmf#6fhMrQNX&A1TA,0-8r/'%o8><'OFC!0PotBuZeuoT8TB2EQr9ENj"Q@g=i?8'Vn8W\4 +\^ot)*k";VBDIkT=4D.a:XB%*Z?+[a_fS/EYFapKqru3MolY]N>^cRsJ,V6tJ%dJK\sHdsj)8PHH$;5? ++$NM^KN$^F=`AGE_VJRFE@;OW)#pU8qU/JpX5MZWqZzzz!,C?`$c)b2s56`'e]qsSt1p8Q:Q?[T+h^K7UFeHTBmFt +NZ%,Un)fb:gWcdaAb6mHa)-5C`YkhgFIhRT8SGhga8O4E05//en&\Z^$k`HKhQ@hYQ;s:ZrT>)/1bPR& +d9YSXmKq=2jgSWJt."rXfk=gYUfdbVRT#\oip[O^b-"kffGN]))C"heP_c^3o`T:HbNVl+t2%f9NI5`V +of*_[g29lSAF5*/V@",Hn98j@dPD*??0h^AG(Y`E''B_7S'^.mENLqWXp8HgeXeQ$uSj`*RQ`pi0Ti.9 +I:cc?sbm>4u1@lBKrl4*Kt8ie]$XSN?GoYd`M=qiOAid=k5[0);Xt`jY#IGf ++%YW``fSY?SK=jE+pKH)+!t79?WsdFDaINl<(rJ,XihUPGgo3[`WGV@L]ZYjJqDqBpI[;4B#H0(;[M\b-]H.-aZWtPBJY2iobSY +rHd.@bl>?bX1^\(>Eg!8N4#TqTYr042JCh0VQjzE;%&'Z?jq~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 0.07987] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 356.5 L +1603.75 356.5 L +1603.75 0.25 L +cp +clip +GS +0 0 translate +1284 70 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 70 0 0] + /Height 70 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W4c^QD)?sS.r.6biY#7Yb,R!3ErW*]^ncLFrO:XRQT^dLs6DTIe7#D[-"^<0*82ONp6j8:6_s%`d +;]p`+/X)UH0^GpMpN^-#@pbQDTr`6mGP6L1hOY6b>lap*zzzzzzzzzz!!%lSo#i]$s8MoNNNZ[%]"#7P +]H$tP!;c,mhgTqPrV+)*p=o"p?[r%I]mKMA<^-BDUW^j(I6B^n!!#RUf3a"j4$+r#Dmp+HT:_IZ]Qiu+ +p$:3os86q`LX/0]Cl_@)!!!"^q"sNp^\mZGqtKPDHLtmR$\.s1LS&tb8c.U.hgAAZ@5aUWHDnFU!!$sl +eo]k8rqPN+%mG7/j@T0&]:CSPf3a#m?Tru=]\Amu>Fn!)[\aFN:qg?Y.3]'a,#4"d6BI"e,3#D3WNtL^ +q<+AO<@R?Zi?*ODU&=Jnn%S,Eq8%[g?[VC_We[m6M3B-H&1$82La3+[3%6%HMUn9__V]kCIJII-LqED, +n%JHZs8;IgO0J$.lLB,s(p5BAZ1"U+0b>ViUkg9!M)Zq.mZ^YWZS&o4IgFVe,*EK_ihVAU"Go)e,#4$H +S*Rp-(Di*oB(r*W,Qrr`.3]'a,#4#OPG7a4\o<(oXHtSI[WeZgl!@)"N\*qp'Tn%k&LU!m+\9OC&TL^U +hHUjL4ad%odn`2F^YIm+Q*NS@:4X+AKHLjS73j$I`(:P\;RG:i(Ht&J05l.4o]b\nYCHE5VmE_9DDba2 +$POmV,U#^s8J+N"&ogf;S.r*7fS)t*IJ).(HhZgK5(EM#a-W-ZCEC;--;KFh6BI"e,*EK_ihVAi.Cu'O +Y?q2&j,`M1E6T.F:5VS/OGK)EOq8nuUkguZb0:\K:5VS/OGK)EOq8nuUkguZb0:\K:5VS/OGK)EOq8nu +UkguZb0:\K:5VS/OGK)EOq8nuUkguZb0:\K:5VS/OGK)EOq8nuUkguZb0:\K:5VS/OGK)EOq8nuUkguZ +b0:\KDEK!/q^HIm"Go)e+pgOZ:5XjkU>0j!iZ:=5]^Mc60b>ViUkg9!M)Zq.:5X)@Ko*-28OAMpP9Q/$ +,iYVU3%5bFc9#+FTeV]*s.)5uIh(TH7Z`XHMU"nbR4QRlR;F$qWB5=lbhY$h>W";V +NOr,J-KRokdrm(5U7BVE3dUDDH=k]4R1l"#GQ5/Hk@"-M;Ind.asl&,_8$mtN1$/<1W<`UX_-Zaj$F=EAc51c&#oeTeOJ'@h)skj$3"HHSq?4':E;l +hYV@J7kfP%mE?o@Em!cMT6S!-537O$c@I"MRBhFPZYe=[rnU;GqFe_[,Do!.!UQ=,4;g"hm$>u+46?'@ +SGHGoC-3Q@pc$GUR8!UN132chB.2>Hk]X#t>$JFun2Hoc5Fa^?lADJfk\lo%$XF+LMb[tc?X2HHeT5ck[0q1r( +.B<+ab)SM7$[gXgRm"B?]'tTL6+MEs+YQ@(8J,r?Paa:!S97F:Ji'C.]k%N(VDE'naG.rGg.=*Lk$["J +\)]0D\%.Fa/aP&X9kA5"b(';S7(%q5hLu4%:8UASqlNL(mLUkguZb"W[!c8m6X6(1_QX'6:8X_+j' +[QV*t%Q$VtC3!1bmkAW3>W076O1*f'qti028-^%YH:<^IPG6iS@H+@eOuVtSeSV9@I`D6))C=1,&@d[\+bAFJD)GMZ>[+@XJ]7p0c#Tc4Ig(_5pg?K5^O`"#?NEDi$j/khdX8eQO\ +dZ*@4_6uH"k%cX9oeAnEFAk9X>ht>4rV[_%'+,B*8.fi>Paa:!S97F:I14O^L#_;JCi=5s+&eCtI:RJ# +NU+K^-eH[(^*/qfpq5Q`3U%ZKL_LFK7Ch["`*&u`dA%hlf=BUS[p/#(?TWZ,l;WT_X\HI=D?5#SM,T7%=tQV@+&MSWZCT+DoJTDskKbD_H>q8njddjL_75 +1`H%8B<`Qk-UksF;/Ws!ERlR1A.$c)TDUcacMYB,J*Y"DICrEaGWEpCdu1+CIoK?rO.[76ihVAi.EY@$ +-I+XuM6oPuIrfOOBAM3i8*r1+?1=l_]H$<*Faijs4>'*unRfJEGDf(TLm'%BUaS0-QoODr,iV,To\UTP +p?5][mG%*oXQ8JVf:$$YIV82YX/l!R1]p#f:5XjkU>0j!iiH(A\6Y1so\Us]]P[OaHfhO2H[NIGmF,l0 +RnT^0cJg-=Gk!$!%_*2\&ogf;S.t(^$PR0DP/J\5RT1a'DU;Sm-BlQ?]hGJUbLV/=NL-EXZIpT^?!FRh;2PO63%6%HMK71"M6frK +@P14%iKfZB=+S*E[qQ(3T.DWcLTB-3]LaP9KT@Su,iYVU\5Q/M58)L<5Q~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 0.07987] CT +[1 0 0 1 0 0] CT +N +-1284 0.25 M +-1284 356.5 L +319.75 356.5 L +319.75 0.25 L +cp +clip +GS +0 0 translate +320 70 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 70 0 0] + /Height 70 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Rd;?`c&4KGtj#ST"g[Z/WUmn_W;F)#_/Nb\VUFBS.RZ!7L%fcS0zz!!(s5m(gTIIhb/3c'*3;/N- +Dme.j"Jo=Ohl'=aPeLB#RmY^(8lW5[o@\Y9k/-g-@jCn@.IC9`&)Y,K>%agn,]Rj[BV>&9e#.r)>7Eiu +@[k/k0.(7Q]7MHXhfS1A=lA$hlDo(:[W*(Tl4/5O#4qB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj +'JITuG;:;8CW-)]W9rK>WJMgid=lM#HXdq7LPlYeJXc`)B&bntMqdB>:jNdcsjG>@gX^RJpn``N*JB(b +]m\]+;UEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ +ckh\Q0Rdr1.nam$9'^Z>%AU0hs;c##/bs)1]J\Pilk^ZkL3oZKZ!I51[UdI[55!A>Agq#~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1283 286 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +0.941 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.48 0 0 0.48055 0 207.31862] CT +[1 0 0 1 0 0] CT +N +0 -71 M +0 286.5 L +1603.75 286.5 L +1603.75 -71 L +cp +clip +GS +0 0 translate +1284 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W6&8h.rAX4^rgS,Z9f?:g$7(9s8>$B%@4>JQM^V,m1lI7pWCRDkpg8j(64&'EO;<[$LfR,5L]j]e +-t4EYOCB^aV,W(Lb*sME#Z2q%@>;Fm\TPR1bWY^Uqg6UKcZ@Cp_kA?>M-`umcaggZ4QkRJo4AdK*&u`M?AOGK)EihWMi\o>3#BYdWn +55T/#s(KEJh]QXO_r,Q'&qWNA'G2]]D:=23e9kr%*c6u+egX2O&>\RMWaB,`+0!02ars1^J05FO7Dqbp +gN;D[jLKDESWZBA6)ms^I3#a^S>WGd7(EGW3%6%HMV=p8QQj`k4Ct(H6uG:o\EGd/%-O[lX4\=]PUaD('*R70H.AAeR7%=r'rYWV[,DVDo#ATp*fD?3h0` +;FHa%gcf"'Xg,:l.9CkZ(i#.K&9hie.AC=(7%=r'V$jb.(J\P3/U5FgSg4L[,TF[m)/tJsOrt>a77?CW +mF-cQW8I=er2f4!:D/9jOq$C\$pI+%6]d+f,,02FR4.?^MWPY^'.%"4hX)*IZG#kf)GP^o/Zms*uf#- +-Sb0+Obt7n-UgEtFQ''5Ci!@kIf8E2;V\&U06llb_m.^"SJ#=H\7pRT9SuA-OGK)EU3,>o?.dR'n_r3U +Z`pGC-Vj=4&@+!XS6oLtE<#p`pKV@]f;h-<^8LFTR40R/#sfb1M?s()NL(l]Z#sJFH&&"p&J2_F%>bPR +eOU9D#SM,T7$N,YJE=B3p7!1co'iC3cnRVc:TRJqB5H5:.u77L73j$I`(:P<'4#j*bB;c@T&/_%=Yjg\ +)tt^+2.`>ALp:Aa2Md\D73j$I`(:P+\9OC&Q*0,!*si@Kc_1K +06fM..>\l\lId:t6Ae<#OGK)EUHiBn("Ogj(`O6r6+i9nOb=hh-UgFkYraY"dq^VH7FioE +(`OC!6!Qd784&DoP9QaFfGth*l*V>_3pW"uN"+%b+J))\84&DoP9QaFfGtgsl##()7lrFPA"Ad=Z!gcn +Lj,%,Oq8nuUkgETFpEbHHF;[+=f3NSCfl5tiZ4hN,Qrr`.3conh19"*?7iYlegZFm)cgR'GafD0KHLjS +77"o$5%,nXWSgqjN[_-[MFLa3+[3%6&oN89&J +:TkP("m_C@N"&?8q$=M3?P92k,Qrr`.3conh17=WY83W?l]aer%1/#;K34"%6BI"e,,3<6*@U=IC;JT. +:.`CVOOcBMLo#nQ"Go)e,0Gl#JT=Mh(7Zi>PXLH>D&D#SC.T"/#SM,T7$JS$"!sID/N@Os.>\l\g,#=) +VPT(o"Go)e,0Gl#J\4d^(0gk*PXLH>XW#;5do&RHJjc+C&S8uM5d%"d$SI0V8g;90%Ldf==GfX9OGK)E +UkLa3+[3%6&oN89&T:9PF<,jU[_N"'&2C*T#gOq8nuUkgETFpIRt +]ioBkXA]`d2KZ/%QK]#h6nCbP+\9OC&Q*0,%eP0q[G^%IV^,_fkf(=qLo#nQ"Go)e,0Gl#Jc8EJ(-E/Z +.>\l\H8M!Y/4Cg-8An(3;.f/)oPo2bi'FVC8]99.])a`t.3]'a,#4#O11PO$gXE2,&!1p+,0#@21jK[Q +[T@/l+pgOZ:5Xi_@s=THe#NW_Q?>eD`eJl*Hc;9pOq8nuUkgETFpH_c^#dO=1h*#=Lp8*^-!n9]La3+[ +3%6&oN89%C:N))2'MGc-8]98CVm3@j3k/k%$/8An(3;.f/) +UcuJgbY'3kSW\f6+b0;piMD>U"Go)e,0Gl#JTReKru$Yo=JqoO?1Z=%74[9J:l:dHp/49kJjc+C&S8uM +5`4$[o3=]LXOC%7mD&=0Ibn(4IPj#APU4X+^D)f`+;#ti7%(;A&3IRNc9cWq3ZWk?$G:3TD,QA.]K-lK +9%'XOH-2Wm]DSr4:qe[@1').4X,YnO`V* +@JD)S5pB']Lb`IaT\8S[JsjeC*Kmj/Qm_ +opVa'7DmRO?Mc#D1457<+;#ti7%(;A0`Uc[_),Vl%`#s3hVM/frNDN]+^2;dY8fL=-V4RVEDf2h)7out +SR*n8-!7(AX5=ETAuSGN3%XaU>YSOc^74)F,Qrr`.3coV[&VBqbmP=?>e+ldn[EuGoP22nbrRF[9bHf6 +WCtX[/Gk"Kbr:OiT-PRN`(:PtnSI1bmfWCtW@/c5@o +$TEtQT-PRN`(:P +]im'q>Plb\(3*LdEDf2hl"EXIDVV1Zmn5=I[/H8LNK*3RB?m$8qfh1H@4dj9B%Q;65)gfM,HE#9e(($Q +XL;9E0DnSpN8:VsICQ`:s+T0(p`W(dki#GuQLsn*Dcb3Xc[YrclkdG0X`$B%pu?GqcWK3UR%P67k?gRZp;[-$4%Yl:H@t`<.2dL\.rhlXVf[%lHOWU9`Ie:5s]K8Q2njtW!p5<;1pJm)o +^[T9b*)1]VF#Y-W*i>\:/mPojloieSr9"#S#72%F.sg.XYD=Y)aC4U?&&ZqhJ1AP$Du2"QoA1$V,A+46 +bE4a"E!-["OX#=&Pp0"0%8>Y.leK$l42*KIDH],$J(i"3A&q]_N<-aKd=hmr/FL@n0oNR^Obi7X3%8%^ +h07cTi.DPXs*!@6.d=gg=f4Y*5)Gb#dJ;NDP=9DsV%L$mQQm+t*>'YfbBij3=0$ZEqqqF&CMW6g%mDt% +r:k(chG"e5c8m8on4DE<'MVq2]W`!K_gIHe&W)m8Lm+PJ4T4p$LCP>n^?tO6P:*P>]2nOJ\hJ-T.8bgq +io[%M@3`3Hbh'?EObi7X3%8%Uk2th'n%W5eJu<^!iiH(Q%%kJgVFVr/.@27l0c^iR7;^M-M6fs6140_d ++@T!CYrY!j/k70r3[.=Vg855kB:TZHpa!"^MQ4+#K:'2SN_%*17%=t!9c#/%3[Mr<8_c'MZ9c]>/k70r +3[.=Vg855kB:TZHp`sbQ'\,-O)oNu_E^V+K,N>MtD7VE@egXIN:Z>DLdDB+be_tLf3?f'a&TL^u2I2\0 +l]`g8-ZO\Ofc]E-/k70r3[.=Vg855kB:TZHp`qJ^+reJuLW^9dR#ZudU:0='`*'!+)*WmMOWka_\/^Wg +l%h;JC7?hLS97GgiKLcn.C)6%C+`"%@QG].MV=p8'5*jKAb[N'Bo3[D&?E^$j`3a5VBb)rW,9+"ERlR9 +L/0":(Y;&(WnAM<^[tpE*0H'ALc?huRk@C)F\7nWPL[n3^.jq<)oSN3E^V+K,N>MtD7VE@egXIN:KC;u +CR[@Roc(j7n=[92Obi7X3%8U3/khdX8sJ:uV:9G7[Z_p0994eM&W)m8Lm+RAl,(AAVaMCdBs.G@T%cr$ +qkZg!jbQ;u8&RtrgN6l`XAbi%T/`M@nF@;W5[7qGac8m8on4C_Qmk2EgW +HE^Y78AoUM!9^T.;G>)9Up5tPi7fh2;&Jrm[S^$aSjNh +jDmX4VBb)rW,9+"ERlR9L/2u.\/i5aV\H,J6fW2T,,QUEUo8.A0QGYm;2.kY/\M+.cWB3$Q_KqhlfH%G +EB,3Tdh-Rte2CU"39ueW_^AAUh%[Oih^Pis4:loGM8:kd&LU"adtNXb:5L`REc_&GU3F[$7I'@RA.U#@ +_/r)8'X^XO-I+Xub4%E3OmV(DH)*tO/m]Sb@K9(l`(hn*Ua0!cZt0%1DP;UdU1QoODr.)Y6#,e5@ML\t'2Id&'<(F0?! +3[.=Vg855kB:TZHp`,Pu2b!1sAW#!rnmiU`8n9t,:5[c&$PR0D&nUhXM)Mep\)j&&Fgs-9#)V?,Neimn +[ctZI=f4[4%\M3e6*I4,QmOCEpj[V0'umk"PpGB4;&7UmEDk:E?'mV;Pc39o;^icZQ>VT:+o;W,h6b=d +I?5k2l%h;JC7?hLS97HRSm>GI*e7:0MVb%[$KHU"!gF6AMI8Ih:5[c&$PR0D&lmhCm@TZBK:ATZ.&=_Y +_mq9C8EA*=7%=t!9c#/%3[O&lDu+EiXfa^!`!,DfVW]&CWA*UVf#e:3R55iq-Uhn$Ko*-28`Ds#RVjJ' +mjG\d3bMaT'YVDEG@8TU(u@t;?b.qJR#ZudU54joqWQ(e/+qep$PR0DP/Pt(Ztq0l*dZH::!l&h\C80F +8m1b'>1l.6iR3EO`,nD;'Wp`MV+X=>*e""[fB^1G(FIEg-&KM42&@oP4F[3QeT5kA3aeMA6sYn?C;`_aB:AbudTNQ=pW6[nLHus+M_7[IVBb)rBNZ8n)c&WieD\<# +n%JI%?'oM3V^/N,88asC9)T]>FoVJ.ad(:E%qe9H-5"]=EDj0FX&lJG4*KudkF\r,TaTt]N'i3@Cfm

alNC!rL`P(NiT?bJ(jW!*5;H#d.s"U^%um/%u5b)NeeWqMAEYVNK&pu1KNq\$i`Xl +Y=Vu-TkKO('=T6f2oo1`^F:XXLXem*pWU?*M3sB!>M +nuEH>?7ANr-a:JP_B`SO,#4#1>e5%Aobj6gS$RUE$e$!ZPU'-2a_*:r1jm(+bZaMcc4lOK.8,-V-.@Q> +`,nD;'Wk%FVDHdP8Ycl[cX9pWq7Gp=UoE&eq;JVmM/W^dMUcjq:lZ!rbboY0774/o,s:nLke\TS3[.=Vg855kB:Y2Rf0NS`S%+tP=D)Rl_)j2=;9>*/jMtR@?'n#R8&Rtr +gN6l`XAb;G-V2Lo@F:orL&]BVrYocYS>,l+>0?_,>TN6a/kdId,N>MtD7VE@egX2_*2M`O`3Z:"n*2`q +VE^hiMMc89Kn!oZ$$9Sg0c^iR7;^M-M6frK,)[QqS1Q%f$G>;\pdC]UmV$P8(E!(FIEg-&GghN!%ZOh.2q3hLBg_4=1H/AH2_SSO=j3NR6ITI'Iddd/khdX8eSZ>.6a0gDT@*ijbKbhlp4^&2#F2^0EK(=;PRGoe2CU"39ueS7N/*!ZCFdC.7:BT +eJ-3n]&d#Yh\EcdLYVfNkdKC3XoA@Ufg?Q&@&-S5,UGlrdtNXb:.[d6LeF/1L%e7'qSS3WhH[YN7W=]4 +Fl)U2l,!RJ;Bnqog855kB:Y3=+;'r(7E'-UB2OGNC;J_uab:7$(FHq])KA[)2I2\0l]`XtM7?Sa<9=C) +T65D3gS+L-HSn%Ud"MeqUdgjg[7qGac8m6X5pEJN'G\,DgquVmB6ZL['D'M6/kgdC2<(F1D7VE@egX/r +&om<9W5&l%mbV20o@orTg@J#&_?>*],>O-MV@+&MSW\XK&_BI/(2a[rf(E4M_kjj#D7e,Y/kgdC2<(F1 +D7VE@egX/r&oeqgBOUe]XS9,#LE!rU34IY9l[ARNp#4V.0oq`-Gu>k^MV=p8'5*l!KHLlYBo3[T9PmOa +39dGGuU*Gd?dl(\US'j-+nN +/+=*Sn5M#+U1@.ig^$icrVY=X?'r,6Y#F$s8AsVTU88cJ+4[[n\,H65AL_ZVrOk"4"c5!+&TM;RdHhBX +]:fTG2n!j$9*C*]*)kAbNMehlDr#jcE,H1;)`\+ZdRXsiUIrpo]9Tt$$PPJ!F\6u`M7@q3eGO`nG&J/: +)sD,6D>kCGmJCGODj7XPR[s3EQ+_eKtEQcG3$_/kKAA/q:kmQJTq,e7b*,2rg!U3T10(T&=-?`/!5OGGDB7EaOOSbDjT4*T56-)90:/i#t.7Z*!@E/Nq!P4Cfb'P,#FPV*7T +EFl%]eQ;FH?@MY&mT#$\Q6%=B+@X3X2+@161gSspU^Ihpe9+C("GsWSV.e`OJ"(Q@H*8l=2IWQ%K*i5S +OWrchRA-dS@WZ&<#.[LnSfeV(MA4UR7om4I.B^4.D%(Q3'LAI);FK1)]K_4/mS2Hj,Y"* +V_1Zu^t'r_&L?A3.'7bZ]c[0aBaGDl;FK<%;Uis>dHg,aO$tEKk50:0ieFg8l&d)# +-Ul-gFYd67"Go)e,&T(`lKkM2O[gHQHM$D?DnN?_[<;<9jiM%RqsCk"\o_ZCpZo]7%;)0Z00pdWMV9c$ +3\3h?8J,s[85X"q+r_I?#7hl'k*]kj^N/GYQ)a48pY9i]re`=%gb@;.]b;[NUo5SLX2BNWS.lQ:HA_"F +H6W0#D6@0Vp=a;c?+t]3q^\/*RoS3ko#W9Br=rV*\C/tXR1LY'Uo6EsO#6eJ,iYVU\=uod_[fUOI.>1U +mCrJc]CWa&(FH7Ru6#6]lC.3bqM']ngpnc&FS+$"B&Tna\hq47[]RS5'S_Uk>;Kps[0uWk5O@EHT9]5CN:^GE3e.HJa/F +Xfnq[GMsmV)JV*c.kDr:`(GpOROGK*pcIBLSGQ@J2$PIGW6sNkDlM/*W9:%9E5%AYTF8GRYVb^A'pPC8hB^u\]Ko%aHlXc[F3IFCF06m7BX+Lm+R= +A35)D&ogf;>\>K(eU>iOFCF06m7BX+Lm+PGAN5;K&ogf;>W4)MgO7JUFCF06m7BX+Lm+Q"jgIH+8An(3 +EG_U%[qfpVFCF06m7BX+Lm+R-jg.6(8An(3ENQ)dT1a'_FCF06m7BX+Lm+R]/UTpfDT4Iu,#4#OCG/+d +^+Xeu/khe94\5nlEDeUnp%Rfq1T(NlEDf20)_#f7oIO:83*1Hi&1$82BJB;=+;Q6^UklPA.<8ef%_=?6 +Y4a+a8An*IOS$%K/4d]N&TMM`jM'j6FCF06m7BX+Lm+Rl=a3#8)]KZH7%=r'QRYGlMk3ET-I1/5,Qrr` +.2o4?[nN\?j91jY*LOGK*p&Sbg.=HJ?&,3"n\Oo(kSTF2FrC/5Io +7%=sN;t:B,5pB']LhiS+pI28T(FIGXSg4Gq3%5cPK^Yld8An(3ED9?l^?>i.QoJ/oOq8nuUkg9<\g0hb +8J,qM;rk[V+C1?o=*p)A,iYVURjg7O"c52f,-F*umYd+SKo'lY-OE'_:5[[>fcV%UihVC?Gq!c@F=OjU +EN]!\+\9OC&[PNp+;Q6^UklND;rk[VKQLY^>jY*LOGK)EG$AC-=HJ?&,3!dDaF2HMl,(BLg6714&LU$6 +C"Wr)5pB']Lmq,$qkBRlG4"!Fn%;Ttn&"PCKo'lY-OE'_:5Z!tI"PO_Lm'%B[Su8$k_=!1\FJdXNun)E +:Oi5iqF7Oo'Ba++&qWNA'N)'m`eklN-UksJ;rk[Vk'liY?eg,-m;K]2(Z!m/k2r@)LRrnaamH:%5(3:c_1Prr>P%iO/khe94\5nlEDf1l +oc(jHpO@(t,fo)'T^""+?&RJS]p+M9NA&$@@f*W70PkZ!a5ABB +YT:B^Wr-28G4!MA1ImHFq;(Lg=7_uo0QDhCOq8nuUkiU4^S-t2iG`=M.)J]kp"!?rpWiX7pV6asJ,d.C +3tElV17:64?[(MJM7t&56Qeq<8J*J?9[9iDI5p\%7#/\=aPj)#;7CF$38aHVZQfhcn&EAeeJY]C<;lO1 +N>qoma$9"(n%N;PNn)Wfqng9/M(:,&8An(3/$hP.E&b-dC^9EPp?^Jc*S^8[p"eMHjiWj@#7mCeF_]bt +6l.2qn4=7E7%=r'0muEkM-#!a;B/TqL`bA#;M98+FQq6>1H%0NY?nnGrSl:%5Fqb],=dbVr:%VqrZSj, +EJ@8P&1$82La3+[3%6%HMO%/Jj/UitpY9iE?b(R[EqP^Trq>NkG2MS(4\q?G(OWh06')XaTorBCiiE(FIEg,mcj]:5VS/OGK)EihU6jlg*kt4S$SeMhpi4kFR`! +rj_+fF^a:2;j%\;ihVAU"Go)e,#4"rF5t9"$PtF#\T29?GO!&`=f3Mg8An(38I,rK;FI#h[7qGac8m6X +5pB']84&DoP9T"^W,9+"ERlR1#SM,T6kY)>S.lP_7;^M-M6frK6BI"e,*EK_ihVAi.;G>)9Up5t&qWNA +'Tn%k&LU#c8Q"5a@H+@eOq8nuUkg9!M)Zq.:5[c&$PR0DP(Xqi-Uf:(8An(3EDk:E?'mV;PU$Bg3%6$X +5pB']Lm+RAl,(A5-Bl.c!^e)$`(:P4#SM,T7%=t!9c#-OMF^'qLm'$G&1$82Lc?huRkma.'Tn%k&LU!m ++\9OC&TL^u2J8C:.3]'a,#4"d6BI"e,3#DsD9ahT;FD.L7%=sRKHLjS7DqbpgRM^3Ukg9!M)Zq.#sfb1 +MMRIj[Q27D8J+N"&ogf;&qWNA'G2]]D0(GgP9Q/$,iYVU,Qrr`.3_NEg?&eW-Uf:(8An(38I,rK;FI#h +[ETR9:5VS/OGK)EOq8nuUkguZC7@"QS.q*=+;#ti,U#^s8J,r?e2Cs,3%6$X5pB']84&DoP9T"^W,9g6 +EDf1;Jjc+COb=hh-UksF;;7ULihVAU"Go)e+pgOZ:5XjkU:*)"`(:P4#SM,T6kY)>S.lP_7;[+#M3B-H +&1$82La3+[3%6%HMV<4R!!!#)d&6uh +jm;ILO>MkKn;Jg8frUZZ]r5V&$;>m6Xa$RV>cYK/%qJ"gtKHbj:d7:f=CEkc2feZ&dm^/R2k*\Sl%NW( +@rql/f7aSjk&VX@p?[q^q7h6Y'3).d@GMH&td(US9AUEp_#7hn?h"RS:8?]c(lIstGoQ#'Oq!kiNI_5& +=7$2rkX']^.YMF9G4W6%J\TI)adA'3=J,T&n0,hB(b3fr]\Wa(+L?_I(4aV?fC[:G=s*HU]4S6ufI6T+ +eo>i/;<(lnD2r:K2FSYLha^fGhmC-0_\*q@+WN%IHF^Lmf0B0!Zg)RQjN%p;0J)P +#kFS`+o^^0Y=3kXu04+SEGJALV?$Gu?TI<9pR;/J!+uJ%`N8,sqcP.8-PP`0,Idk,T%rD:>O+7(;Fj<5 +IjHKQa&V\ojfJn-Fi%r\AZi-/(m&X)a%2(ff.a%FfJ,QDt0.0Pee:h&AIJ;QAil$]@hu=5DDggs +A`ujd9+Y`sM^A)0&X*(M3hh^%GEKnTjT:pSo@lFk9pVq9F1ZSN>jIH*!Y +WO:E9lo]olaqCd?o9/V+"Xf_QgB0Y_Z5uE04iEj9JrT:4C1YZAtgb^rC7CsGJKAM"Ic0>H"G[HRX7uf_ +*fOK=m,YIh1qsRSune)d?nA,>O9T)U;CL*;Br97",WZlQUjN2c;^%\@i?@)1a-`bG<4nmalh5"2iWDf] +)I-Jp@!U +2]_:&peW\\tJ,$qKo)$]on*IP;&Xmt>iJ3I74'%?6)]K_404$b\il'R)RQ=,`\hJ+]?.d5h`:X!dh690 +XYIsEueBQ;s4-*A?L +OieoIi/(/?bo:$dC'kQ(G@Z9*D?sXX-Ei1[@g&AO'kB;6lO3h%\l-lQ2j?2tN'7E(V9(ft%%ij5[(.3- +bJ,F;OR9*aDFnUa6NZF<-Q88E4b)S'd+92,V%mTu2?Or"QltA+CDH3*&)1I%If4S8765S +V<0*)tZKuH`7W+O"4%2X,jJF$srVPLPIVW`bK+C.*If0!-3e`8??bTc>#^>-Nk:9muI0V$"NRnQER[7B +1\U%.8IJWTU`l=iJQ[\S%+'$X&?[VB4hiE]4r8]4iRD*";RZ=DG\Ha1#i:boDs4>$bebA*u(qX"2Odfq +L89KI0hu;4$K;'WAE7XSk2th'n%Wd4!\aUY>?`1to[2dDQhu4SP\iH%H2D7f13*H +szzz!&,o&+5.R)MZ~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 207.31862] CT +[1 0 0 1 0 0] CT +N +-1284 -71 M +-1284 286.5 L +319.75 286.5 L +319.75 -71 L +cp +clip +GS +0 0 translate +320 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WD2;/&)o-G:,X`iPOGFHNaKY5fXHY5nR+[7N$n6O5M*YZ(()%@<+RB9b%h#XCCa$J'b9A.Y +)Fhm_;c-DkbE=Ee/A2ntLrEE<4I=pNF1?F#N\c%<1Y6pJs-O>'1M>n+F#s=]pDNX'.Nt0gZ"Q9'Uu&+O +X/q)<;uQ[nejD^fdB=;]Kc^1rTjR"@eo.7t/_'008&qGfLCQ3-5:poKZ$mK@iYM;9#m1Q(^\!HVU0mac +H?&q"E*Kl9ILu_?59#\rknbMiY6"Jg+S9H"ILuka)PCF?6GE`g(P0r:!W#5-oP8u+&IA=RTO.gg^BWm< +J^CS$qF)P!COgjG#lP[7?N`^1#3ZkSd'!KH73M5M>9"@Bj-kSJfsY6"Jg+S9H"ILuka)PCF? +6GE`g(P+EmDm:P)Q2B.M6%TdXKiJRshM235j7nNtkgoh5+qXT.5edSXILXXW!his'oSYuu +f)Yan&Ge7L^BfLA%F?a0TjIooMa6LX!<")QqDCOQ#_[X9cn>Ho?N]qY5j6f"r?;gL2*en^KROBW0*7hS +"8%L:kgoh5+qXT.5edSXILXXW!his'oSYuuf)Yan&Ge7L^BfLA%F?a0TjIooMa6LX!<")QqDCOQ#_[X9 +cn>Ho?N]qY5j6f"r?;gL2*en^KROBW0*7hS"8%L:kgoh5+qXT.5edSXILXXW!his'oSYuuf)Yan&Ge7L +^BfLA%F?a0TjIooMa6LX!<")QqDCOQ#_[X9cn>Ho?N]qY5j6f"r?;gLD6ZH`8gdYcrN9CN.:I,L(3H>/ +QB?3!P=r*7(4#k!j/0=De8MTom)O\l$7E!gmQYTidkA1,2ibX)+@f`ulpb3MI^t0Dd;Q@V`@JF7U-k*J +gb,-'>G"0#9D+9nRjU8UXo.+A#LKlS0UVkdMG4(pBT>kg=?g,n@7\b$:+8nNeIMZU)F(QNm6B?R3 +@)6nr+8nNeIN,,O1]-APHLtE+rLDP2^X/b53X(p-qreiBoNRAQF(QNm6B?TUKq1$skSJR;)^G-Jfgig( +UYf7D0k^4-0D+J;;Y0%p55KsSrK_0_SCpEId%arTZffOrqLp!h3j"f+qreiBoNQ(hSCpEId%fK!cC'+Q +$b\[sg\Nq:O+*dhgO;lZbWblW?9\7oRPXj)(OCAmf65I+UL36HWaG?XZ]BR/Q_/_j0PC+,0D+h^DfY!a +55Kt>k5.m&SCpEId*$UI8XhO5kSJPJm5LGSAfpt<09c>]G0]HL?g)()h^$O9IImo\/G5M13O=cqTpM0r +;__qYd4b-td;N4NbWblW?J>9Nf::Ma^X,Vr^T!\YqreiBl0UEhF(QNm68+>sVK^>PUL35]Y2<4)R=4]8 +]l`q:p,TI?I\E*oILYJaMh!)CGKts>/Ja9C((,a/f3S-CWo_C`*?D%hJ`a)JCl8=KA8b^1TCn#C^jOF$ +^ISjZn[HIMni\f-,!"*"DfWR)VI(T=.f)#p1]-APacZ(A'/F4od4^Y-ao*Q!cKq<5,Y)>@:"5]dQhc*N +`l:cG5$di\dF$A3naWia4+I8-ace)bAnFBCTRMtW>9IVmfP^iSbeD:Q0+\$(rk*OJk2G:XpYUJ8Xf_!QRrA;lYJ3^$du_:S;`sZq +1Qp;I_hVeX$"8"#2L:K#.opK?[-=?H5'l\2pZ0&Am.Pi&A6?KhnLLfU,/5OWaL\DF4#/Q8O8II#cKj)c +^Z*uG613F47i(FXm?PY$8po)^2p$DdOU;sno^'B/J;eIEp?a/E"sS?8>EB[!pu@Fs6gX-B^R"!Cm6j=) +(#=&fD2msM!L`\OME/!r&,f+dj$oqe.qf6qH?Z`In^\2?&;c?Wc.rmBa/`tp5^4#n>^5T2\7HE&Hn;A)9qI^@N +o4Rl`Lh6g.)"'q3`N\56c'a!*kdTpV;d=-b0fU48"Oj]\JOMqlAAn6ns&6I)**f#*BB3M +:W]lFl84%N1LchH@M%n853IgHn`/!F)G'?sp=S<2=e +p[$S>ns!Z5pLWJ%W*p^J+OpbqfV.Os8D]>;spHeek=-LVgNTFO,s1]`36l@6`03g@Vji0d7o/I6/4^(b'_h^ +9OmoN>s8_dm-Jg;%h9*#g&eD(idOfeBD1G\?eNPr+"&E`AOh1/NAjHEVj=r=muA0I6`M,br:AgGrCS2Q +7qSIB;mj>hG2@Q2/NM,!MWDuJX +7*CJcn"NO`ZJY<"R=0AE=5K_%+YunSnB&UKg2VVWdJmsj]=ZgGTd9N>"=6Q0q=0qm^560lC;4e=a,V22 +3lAkQEajCfkg:>e\07a,g"G%)99r+j]tM,Yj"Y?D#J#9[G54uF`3M#;H,G@GbEa`HBp0h.e@_, +p:Ke-CMR^,hf&CEQXs^pP'6JQ7'c=Qjicc%Z<<'AQM$a43O:@LrQ9"2F(TWAV)<&>6rQoH%11)R8W[.m +`JPE0>VrgZfFP#cfj',Rl@rD*VK^kT*`mM +>4da@47gkId71T^E%U94"*8']k51-/10=CC$$%__+snW`)LZTSCM[f\3WH1%mBRL1QiGZ1l@7BAr`n3' +p=X([Y?p&KBUb^KrFTQbm)r2>W(N)t14MK,i.1Jc/no(8R@,l.kVp!381g$HRC#J6+jF8UqT[Z_fgif& +[Vt';4aZlr:JY1*jF;55I]g"Re:5F(QNm+uo_uS)HB6O+*32U&*%-SCpEI8<+hkAq&%lUL,:% +ZJV1rR=4^cjO!OgT/j52oDPQbqQUFWN42ib)4.2MoUDo!DC"iPVZ$@6$e('D2@e.7oeY8;f#?dK[IuaAb^T_S!egj`sp7&O99DRd1 ++kMd79M\I-n*\i5,J&\$XfK4V>SX]mX3S^<-LWI2O&m<3WCFYm(Uf=VHBg!\rqTFJi[s9Pqr`W;5IOZF +arJi8,j3lsYMHEk6aRfa\ccTgV((`Cl>6B-PojluK.PR:2u^\&0jCjP9e$,A7LnORh]uGk<_aKCFe,(o +EZ-R:maD"`^=KmlCWodelb)M!r*j79`Xf@SUYkhSgW"'Cb42k*qK83gLOPG+rE)##N(tY;JHH.Oh72%d +?Vd1O;D\]Y0^#A&'.NslI,\HQ_R +rO(5:nRJ`nXd/Q`cQK"+chcBArkc,9gH9cF2^-e6mHs;SlMoSmgZ98%kFp;.[FWl8CHW_'juVC'jnWim ++T9L0qV*:p_RB*k4IPXsP(`/AcHa_X1!Z=n<`SJNY;'+4X`M_Z^U5R%ZL%.goaJ-$o\6>BoS_e*haZ!H +P(S3&7DDA'qJ?rF+uqLY(5p02r1RICL[:#$k-H++da9_2KDBS`('."dn!s#cV18J/)#tG&I]^sMo]Xm6 +rBcQoXjA`nkgp1?6B?StcnG%Tf*_ICO++4X0D+J[$N3;PI]^sMo]Xm6rBcQoXjA`nkgp1?6B?StcnG%T +f*_ICO++4X0D+J[$N3;PI]^sMo]Xm6rBcQoXjA`nkgp1?6B?StcnG%Tf*_ICO++4X0D+J[$N3;PI]^sM +o]Xm6rBcQoXjA`nkgp1?6B?StcnG%Tf*_ICO++4X0D+J[$N3;PI]^sMo]Xm6rBcQoXjA`nkgp1?6B?St +cnG%Tf*_ICO++4X0D+J[$N3;PI]^sMo]Xm6rBcQoXjA`nkgp1?6B?StcnG#0lF0kHp*5[%XtK#`+E7aB +_Q!etn&#/\qDnOANIJ"V0D+J[$N3;PI]^sMo]Xm6rBcQoXjA`nkgp1?6B?StcnG%Tf*_ICO++4X0D+J[ +$N3;PI]^sMo]Xm6rBcQoXjA`nkgrG#:p:uB;NH:/YiVS!?=\aA%mBV1.r9LpYnpPOb7#+T7?k9l`\&=$AGm'f@HDQTi$=>rV+-\#Go\#*dXc"^LtlaPmtHKk>(kB/MVV",EFt8e^YE53& +s"0DV_nM5QBMJQl5P:++MB**#KNS4-fcefuh2C5Q&A+0>.2M]C_"bC@+H1b`s92*BRjp]F:!":>*^bbP +qkuqsE_8]B>c4/mc1]^PGa&004CLV#:#<='s(0Zut]c]C*7'-IO\+1T#fLVTOA&rUsI)n`;t5U)mlmOW +8Z.7lgC.H1g.6LL5K&t@h +ti_ZDueH7l^N;4Y$1MR68Y;I=6P7iIC:N\]5b + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 173.19932] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 357.5 L +1603.75 357.5 L +1603.75 0 L +cp +clip +GS +0 0 translate +1284 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W^M4I0*6)FU+ZEdT"9KdHdb/pM2ATNL=_]>@Jt6:7&.;]6@k"iQ5\r#'h9J>19i,kbZOm%uh9=1A +rU&moo.\-L',-,HDbaUs`j7H$zzzzzzzzzzzzzz!!&\32#mUVphO\^$ZG/00k)W,KTO9SL_<#uV]p.2C +<[bGV]n_=M4BGO+XD(X,,_0dggP4QU4eWLWJD0R,,ZW*.Vd3K-Dr%[#`5(/-m9jV.O(dbf%a\S.\_TXa +Jjt.66`F=4)iSh40"i`eujA\MR+d#\83>Jr6$^5X,.3TsW.\_TXa +JjucMH[??aZ68`&/$u<;*mh7<('JMXkCSPS5W#cM4BGO+XD(X,,_0dCghA0.4'X8:.:BY&ig\h;c.>=0 +KJ<\&jde?,"`,tS;PCeW&7eHS9%:/+d#\8-%nZ]7H@FtROq4Y3MhCjaJjt.66`-iq)3N&)Q&ig\(6:g0:6r$1Qftug?;b@=PS;J^<,]Y@YV +kVaY@<0XB"$D'^2OjYc*?9H3r($Kc0T=$a-%nZ]7H@E)bj"BV]6E^-IJS'M>M?sTS5WJ`F7V#eX&mWM> +e0XpI`ipI(lon6#`5(/aJiiY-RU8P*^&*2h7`NsRl>8/UNjc4gVW$A;\h!U]dl#0l>*Y\NFLJuEC:*VO +])iP's6g$rVQKhqet?(q=3LcHp41IB%O4ns8;]aZF@;saZ674<',I(N+.^45!4iR+XD(X,,_0CS=Z>?m +d<8eqXj\e^3ob)XkdX9Sh5:*>F:q%3ar3*_g0ng^%;j-1,43%*)T$njA\NB7;M_qp$:qqYNPNQq6,2Gr +k2OJNupSmp+Ed-M8`:Ps,fgSnQk7GpVN!>L"'C68:<[lQ\ek]QOT/j&ig\h;c2btM\k4'4[)&9p'.rZ_ +^ni-U[O%^NSqBep9#I@h#m]TAa#^='3iEQ`g7.!&/$u4F&ig\hRrUsV&3`9AhV[5*IJ``[aiWg"* +.B$-_e[(%^K,`;F,<,RHTb^_p)5[Ko)F'RR?4A?,'#.Ok32#BDf>+4h7K_CnI]_84mMXY[P4cTfcW@XRfWbgMMGGN'Ybp^.$6Q^20HqON=DWS;PlQT?m9*p28]tFnDa/n)#dF(L*7kXh-H\; +bF,+P"!TK4H:g5,]Y@YVkUT3HM-R?+9)3-iPT6q@LMe6@Jdo%V8RnG/7O(gEC:*VO])iP't%W]Z]Ju1k +EHU36aDM.kA`lr+d#\83>Jr6$^:#bkEHTf8/=nIZT;/-G'7 +&,iiQ>[7oK?"QMKLAR(jAaWt/Na3=?QF2_.FBuJ92C*TLr[?lL_<#uV]p.2C<[M8\'9;]W,kRf%>87(O +.OB(-%nZ]7H@FtROpX&S,Ab=c7(%t(6V*K@NJ%(&/$uES0]o*TNYf@Tt#K=iBIl^V]#e&jde?,'$Vbk +W`KG?*+~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 173.19932] CT +[1 0 0 1 0 0] CT +N +-1284 0 M +-1284 357.5 L +319.75 357.5 L +319.75 0 L +cp +clip +GS +0 0 translate +320 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Od:pKp'L]cT>a7M&QE#'g#`*U_17>-"ES2SMhZ*WVzz!!!#7;nRX9WP8f?PfpD>U5I.#Z_E?#Za6 +B`bHnOLMT`?4U5I.#Z_E?#Za6B`bHnOLMT`?4U5I.#Z_E?#Za6B`bHnOLMT`?4U5I.#Z_E?#Za6CKH(O +[Bpj2e=>h;t?JZl!/MLO7C\"[5QqQ8tJSUN*cAZ+FCIbEtpU0i*?Xlu+BIOk8+Vr*A\g$,u??Zu:Jka. +ih/,EhThhlm9l*mBoG5A?)Q^rRUqIQS^MLO7C\"[5Q\,*f5SDuTo-h'+YH+n1m]8m'/\GUiEl*mBoG5A +?)Q^rRUqIQS^MLO7C\"[5QqQ8tJSUN*cAZ+FCIbEtpU0i*?Xlu+BIOk8+Vr*A\g$,u??Zu:Jka.ih/,E +hThhlm9l*mBoG.7>H!A,)A%f~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1283 285 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +0.941 GC +N +0 0 1284 286 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 286 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 286 re +f +GR +GS +[0.48 0 0 0.48055 0 381.51931] CT +[1 0 0 1 0 0] CT +N +0 -71 M +0 285.25 L +1603.75 285.25 L +1603.75 -71 L +cp +clip +GS +0 0 translate +1284 286 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 286 0 0] + /Height 286 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WIq[X3IJE=t$)JSLN0Z69.%u,-/3F/G6Y?rt?!;u^j#JYbg=;/"#SA<4@GH-XGSt6&U9p#*'H8". +j>?H>F5iu2Asa-"`-"qH-?VJTA2-Os,f2*RL+mcj7>JWrI,>n[l0e3(p&=0(4W]+;55OL8M0Qa!^/saj +F16(kLCG1\XNU(+S,N38^M`]fcjbXrS;Q<fLf70ubeh;$Vdpqri`*9gPn>uM\18GJ]CIE/\qEJAMMP2. +dEKJG=f5dR,iVe1E[uDZ\2",pqFFgU8AqK`3#*1Q/52RV8es`;:.6\3JG/7%B-j;c1rd_SfTTA0E5X<)hEG9u6'uSd94#8AqK`3#*1Q/52RV8es`; +:.;X]:;Pd!]L"q/:.<+#W+]dOiiH(a&/&tG?;ZnZ@sL*,Wq:O,A>(4,<)b]\0QGYm=U9koSup'JS6+$k +6q/XM:.7T-$PR0DP(f!RlbSII^EVC$K%Hk)6R"T,&Z;@tR>$L(:<>gn`:[(u9s,)![\rT&q>`TZ:PD2& +<(*76MImVNc8m6Z+VE$L(:<>gn`-$JsBaQJX8uCO@FaZf^!u2EG +U8"'0M6ft!6:f%;ECFRKDKLF\;fHnc*"qR,7Du1(BP(]4XAcD9&ol`2;3NH]eZk"YXWas@<(*76MImVN +c8m6Z+VE=S(!XtHe@e,5:J**9pP$Q^;\="!.[jM-39ue[KLB'(j#/[o#i\5CN017glQ1k.O%VV75G;bB:Y3>OGN5@ +MIpI4"pYCH.(ZO&@S!jF'bR@ZA\L7Oa#=heBFO+Q-iiH(a&/&u">EPh9 +4%n=_6jHR$E@ORuh7Ajueqo6?lB_8Aq6]8Yn`*8dIdF>[*dQh/lHNt5Gp_o=4$23\cHX5E=f5dR,i[Oa +U:Qs;m-3GOet+&4=]@eZMNW&DcD?Bjm+J_Q)`M]lI.6(1\i]ZZp&3eR5PX=nIsLn_l##1`IJ_dVDK=oP +@H+@eZ4IXgm8f@IK0*&EKg[:d&TK9mVP^6=`0uG9g5Z(0rD;W)+^%^B!Ep"BBWDk>f +KhpF9HZuM$JSlSUinCE,rVQ>=p9UAs1^0d/BmZlkNFfIs89?6p&_G2@X_ej1[r:0TX]o.[n\t>"q9,*X +Va'+JD;1M<_k%Ykm\p.u71MX.Y'he"H^Uth$[U1Fg*\.,Q\pSt;)19t]u&AlVr;jIqhVF_^Y[SopVS*P +YgK#2Ko*-2a[/P:[6mB*?Pi+c_4@DC6DDr.Lc?.*rqK)g-Vp==[&*J2=qr^(o[;0#OMCIjDsc;gYL/o1 +Ko*-2a[/P:[Ci6FQcVjpEGLVmeS9A#F9**gVkT&S]6100%smB3g%l'E@N/-72;#MPA+\nG3i>A>eYIQikW>pFYl:3rqsjYaJDOr.Q8Gr +m'4_1L!uX"Za7Qe_tW&#g^:7<]WI,N;ZjS;Oc^\6As:%$R`9Up5t(.'4J,o)E#O1S(dQZ5&1:.<+#W7Ji&_M&@6 +W`/DmHT(m\V$aO_ERlRA#`6(aY:.Z.9*\m$#Br5,,N;ZjS;N29(+.< +o]p0BV^(2#7%B-lAel`=3[G.W&Q_dtF^GsP%DlDpl%h;6&Z;@tR>$L(:<>gn`+?-NmMgi.QFstdG2m[4 +S;N,$;:3V)`*'!K+XH(<<`Jf&_PAZfOYE\,X_DiPJV]t,"<&Cp8ERlRA#`6&k$L(:<>gn`7;!2-)4TpV^(2# +7%B-lAel`=3[G.W&\#-A;Q8_L=fdq<&TK9E:.7T-$PR0DP(f!R>?'"u.qPR)\5gR=#B;dK:.<+#W+]dO +iiH(a&/'!5MHhp*#cAp,<(*76C+Utf9M\Gk]68&E4j2sX)fNCZ]ANQ>IF3FPBLf4n*.]Q&"D&QSRS2a@uQS2XUPU>1JOcd5%:/=Z`4ibR^DVi&%hL"^V8u;Odr[(DeRtZ^*:<>gn +`9lHsh]PU_dh-RL,#1QA4*Ks-*ZhK@4E]pgRUKi@AsVl\kbF4,hS$OIoBj$EhR[jqq=Ebgh6U7O39ue[ +KLF28cVAN&B\(2fLc?+]VkW8'X]oLKSX>eCDOI,h:Kri$5JCgWRl>8_m:X_7=o"VXKo*-2a[/Ndq!97] +E]PC>7Du/R3HsmrRTRn-4^5O$l:#9$#gFQ#.egX`- +M)FnX8`\uT7%B-j;c,;-23fN>7@1gTn*`J[i4u'Mc^["Zp)FthHMHHS-RL13I.GCdgHuK7eDtAm)ceCN +\i:%h[!VQ8+NUb,MD;q4$l+$[JNbEg;B4*U,*I/.fH +]/9s0B:Y3>OBC)>+!Rd1Z4IXg.O%VV[2'Mn/mPp5r:>Ljb2r2`Dn\4n41jjNAms]#;mHrIbq6V>o]YN& +U4$!/(.'3?7Du1(BP(]4XAcD9&rkE>m%j1AnYLT4Lb+3IS;N,$;:3V)`*'!K+XH>VeV=F2Lb+3IS;N,$ +;:3V)`*'!K+XEARq]Wh9#3";d+ql+3V]t,"<&Cp8ERlRA#`1tgI>Dj++&NVQ#`1MU&Z;@tR>$L(:<>gn +e>bS90DeZt+q#P+V]t,"<&Cp8ERlRA#`5SOnaYb6S^`IQa?iG9WJEMK(!S:'S97F>6D@q2FEDAPn8/o& +M3*Nl&TK9MRTZ";l]`rR6rGM"((J,pbK><>7:[Pei`&=9MMc!]-I+Xu$R)eIZ]djK7osgD.O#6D&Z6i4 +<)b]\0QGYm=U;!nebp'OLeZP%hTj)+4'gg<(%Kg,#1SFWMjKB@H+@eZ4KnMX#gSW5!,_U'p433Lf4n*.OpliQoODr +/;2NTdNEqh&k*5G[oGgC66]/n`2T\KAms]#;mHs4=/u-@_?CCc[oGgC66]/n`2T\KAms]#;mHs4pSg4[ +(]#(N6l&*5:.<+#W+]dOiiH(a&5ni4+!30"6\Lq*&QoBr3#*%'U8"'0M6ft!6:fpCFEDX'#N=De+ql+3 +V]t,"<&Cp8ERlRA#cVU(8u4t/E8%T]^KO':+XH&H7%B-lAel`=3[G.WFbcfcSMWAg\o,8g7:[Pei`&=9 +MMc!]-I+Xu$R'Z(\fW+Y5!H]Pa.OW^&/&t4,#1Qq1CFk/SW\\g3a1?*\@Bj[C.(L$+XH&H7%B-L/M&GH +:JY?0G&;s1ccs_Jp=a<,pG!$hB:Y3>O?1uSIu&%W.O#6D&Z6i4<)e#$aH=3N^(#;Qfs<'N\]aunhS"7p +D;*Ke+1!Y@egX`-WC+)>+FIB^A0N;Y<(*76.OHMje'NF#`//NFI-K(UJUrB7OcecjP'0Sb@MWPr=Jo[Q +Ue\;h6\No2gqr)K&QoBr3#*%'U5BsQFpnll=)XZkFXkY?1G^hHp4)D1\,-"!LPCQ-_Q3k*PUqL;EE`#Y +_G-tb/;-E]MMP2.EpM;Y1pKeL9N8LFcd/L@a,_=G]77@rIcQV]*e4+X["DMKF^Uc'Koe>g$U7%h#o/SV +OGHgIE@OT+SNE/bf!B#8eo'9UH1Sk)#OCf-kg6$,IeBa#DVQoRZTbHhZ4Kn=C$gZ]2k_G$&/&t4,#1SK +eC7Hd=OTZC]4p8HJO?182fNA!Z@-KJaXZiof +:ISe;,iVck>VB0cL0=<6c'o5+nA(*7r8YpocO7'CNr"6+j<18l2`!72f+P3sA9;*,+q#P+V]t,"PY=kK +YOFEt:H\0cQbN*g[-,pZhu2teqtg0;(N6tV7\`%($PR0DP(f"O>[)W";BmY8$R$+[,2uRDqr92Ja/Bl^j +qWXn:D;(5g8D>*>ZWWYTl]`rRe@+*#Z]c;)&QoBr3#*%'U5I(7aY^@G4*I]1/ml89EF<1I3Z=VTIstp= +jO0kt97P+]KioMK.9QqU4Y=?XFP@;B[oGgC66]/n`2T^#nDV95I=7C44:_l1c-=L5[\#O!,&Na:RTfP4 +Ams8GF\3E9C,[q)1N!$B8WB[)/;-E]MMP2.XbDI_$PtEpVr0,M_SfTTA0IbW;KiZn+!5FoE@jOHA0E5X +<(*76MImVNc8m6Z+gmB-Q],gkcVA4C7:[Pei`&=9MMc!]-I+Xu$R)p7Nqht:7T;bk66]/n`2T\KAms]# +;mHs44'6gNkVNRSqOgkT7:[Pei`&=9MMc!]-I+Xu$R)o_B3d:(/esM&OGHgIE@NH,=dV27PUqL;6?TJr +<@X8;I9`hM,-hamE@N/-77B!?'5*l!KofIa%DkcAPh#c"N35fBA0E5X<(*76MImVNc8m6Z+gj`$?8B)E +'p433Lf4n*.OpliQoODr/;1D0l$L(:<>gnl_\I#PdQa5a?iG9WJEMK(!S:'S97F>66HZGIhB9IeMTA+T`2SPP'c"tE9Up5t(.-0BGMXITLl08?8AqK`3#*1Q/52RV8esb9 +V=#]:GO$3$(JtcI#`1MU&Z;@tR>$L(:<>gn=cAE&pXC.8(JtcI#`1MU&Z;@tR>$L(:<>gnED'XTh=Y[t +?:74m+XH&H7%B-lAel`=3[G.W7J,pcKGRJt=U9jD'bR@N>jfWp)G%l9LEf`]/9s0B:Y3>OIE-FFE$7(i'gU5 +(.'3?7Du1(@WHKE%BV!QS1'`IQS)P^(`4)-)6(72rr)`'Ct\C-9/!,)B4kjMV:,>:huE,Vml%aiZS6CM +.9QqUOh-n$lLF1/2khM%&/&t4,#1QA4*Ks-*ZhK@4E]pgRUMOH$C[TZhmd[dOX!Ar>HV8DQM,^kWM^Zj +ZWWYTl]`rR<0u3*YDS4Y6:f%oLc?+q[r1"o'C3TT7!2.kH./)>a,_n-F)Gm`o]ai2Za816$O[>VcR5?4 +ZI,54Ko*-2a[/OVbfn;dGMdg$PI2ki+q#P+V]t,"PajgZ/krE[c^q8p\9[].K*Mo>qm0'_rH.nh='pB` +d10bsiiH(a&5oXA:BqC?%^u5Dg7=,B6:f%oLc?+q;c?V/@SIMUmDf/)rW)SNJe@`'[\ +8u87\82E$jr[S$ka#9S>teVD]V<(%Kg,#1SFollX*p!"$ST/sC"&rQTqUV$n85Kc3EW1idGlB-qU?dW +a]arNYD?G:7%B-j;pjunYEX2#jN0hpC9OL7ld2=ml+[ +.O%VV't\YVg:MQIok'uF<(`/m5G#Shqlrc$L(:.E95g$Sdo(rhQ2P"KLD>fLf70ijQ'hi +>)s[c<\B^l?G&6O0MidLc_!Ed1,A_29Xm +OGHgIE@RtLI/%Yf?XMo(c9jrdF)uDfm3bZ!86_sgVUF'D:S+Y7n`M7f/2dkLB:Y2S:PG%1h5-_am-3H< +SuRV+#`1MU&Z;@,$O[=Y/mP>M_-*.ZM6ft!U.7P=R@3SeE,g3",)R]mi`&=9MMc!]-I+XuMd?[gmM,$P +i/DuU,1_=,Lf4n*.OpliQoODr/@mp=7N\&ZZ\.l7mYY+*66]/n`2T\KAms]#;mPW(()?W@KK.r<\u2.#5Y/hmLc?+a +bq)PYF\3Di*E&(MfScKU;b'nWq[aB[.O%VV75G;bB:Y2SkD%Js$ML4XFk4)9#(RXu2OE(b:^7C43]&Z6i4<)b]\0QGYm=`d\8VVq0t=V]>mKLD>fLf70ubeh;$Ve"1(+Z5[>#6tKH +AnG4pJ3C#5.\b/oM)Z4^VkToRKo*-2aeI$IC3"GJ;fMVIo8;`G66]/n`2T\KAms]#;mL)kOIAt]/ed&A +g9r1dMi``n&TK9MRTZ";l]`sS0I0S9a(`GKkX7`]:.<+#W+]dOiiH(aBWI6C'O3,Q9YhaIXX^&d$&LVV +&Z;@tR>$L(:kB'*-66]/n +`2T\KAms]#;mL(.8BX1/SA++VJiee;&QoBr3#*%'U8"'0M6ft!@\#kip5*0UE$oZe+q#P+V]t,"<&Cp8 +ERlRAXA;C1Nke5c<:Y(n%cqe#(.'3?7Du1(BP(]4XAcDQ1o+\i$gif/mbi1T(.'3?7Du1(BP(]4XAcDQ +,,CJd$[gQslXGj7O(\L[X?WU-Z4IXg.O%VV75G;bB:Y2SY)k.)mn-&W:BYk`V^&JY&ofBF:.7T-$PR0D +P!q(#ap:*h34Qg;fK4\sP1("G#`1MU&Z;@tR>$L(:G0jq:>o-CRgF+6:f%oLc?+abq)PYF\3El6`LX2V,D#]_f',c9uF:2!r!h8,I.jnE@N/-7;K66 +YV84'II5arAbS"h'5*l!71-"Tq6'"NhaL@I +nu.MM[79I2eJS3dAOI!Q\]on:o.S;XY^n?Y7%B-j;phW#\ofMt^(V.neLgCao4cDt[20e^nc.Soh?`nk8+rr'g.-L#Ol +2NWWSLf4n*.U&+:WMq]^Vhqp,@t0".JHH,qrCEt2$No5OZZi5:d^F#I*kW^l^1l^M)]LlM`9uHnQYqIV +;%Zq:M)Z4^f?4o2cRf!@^\<)-&e&..$Ec&j'5*l!Koh*h&QoBr +3#*%'U8"'0M6ft!6:f%oLb+3IS;N,$;:3V)`*'!K+XH&H6l&*5:.<+#W+]dOi_$%FWt0c!+=,rG6l&*5 +:.<+#W+]dOi`&;]66]/nA0E5X<(*76MWQQcS;PBS8AqK`/;-E]MMP2.dJS]T;\<.&_^@RnT)`LeI)B9_j/%!h37 +;(AeGOMD'Rp(4SFJR%#a_fe@CB=iN7:OI7%FsjS +RO_BAl\+sj<8_j4*Ks-*ZhK@4E]pgqO>gTbd?eokV2W"o,BW+4EBLRF\@&5R4:f>,[,FlFKW+0H7KFX6 +UO4Q@FjAa7j$R#H[#/FgMaj?FmIWMkKgX,OQ]*s'U0YL_A_@,Vc26TP]=m86":O_kuo^CDL%mR^?M +Xcdt'l$^1LYUS=\ofP%!@K3@Wa!mn3Ggd*r-/U1MU!t92g>,jl.ObClB[-XKiHH`:HmIKUS:,@bN258H +0"M9+&?MmZS8G&oB+;>ni:dR9WJ\WNq_19O/6npp#\'V4Hn3nrKe#ZqS3(-^TC_Lrl`(S9DYB+MXcdtQ +"s?]LYURR4*N=U!RI?#>J"nHk04,A*c0@Md06+jqWZM[X097kl3!aO*'/3*X'bhjN>jh#_Q:R(YIp#n5 +-Ma#H4"BLcCJ>Ans@D:`k3OW2+S7>qYL&;n:Mh\ns@*b%mII>g\1#QJ%O5970c?sm`h4[Dr2FFC8VmZ\ +QQI4'eSMjGL4jh<;H\L\unSRJ,b'%#?l7@FLhhr/u405"bnnEGk#6SO@`fKKis$aB?m<+L5(F(Ie_lYM +2FFJ]O4Pqrd]f45ZjO?8BCoesKkK.L>RiIs,$$k3YKQ)qe8:H\0D07>:?Ni)7>mWZG#f@QA'GiOcB>(((+N +0djsWH.C`g5R.jZ<1XZe`pqmjg'qJp`']U>qCX?`1p5hA&b1lVcSijPsMuSLVC)o?W. +$aE!j,^\G5j21b_o!GY0u^A;=0>e"btLl-eo'9SFfUC%Q-0,+=nEEb\%_O;IJ``GlB(3hMl%=%\QOQrZ +`o5?H0s=nC[^,neXJ2\c5IZeIQV1?H]<5OXbDI_q&3$nrUcs]S'i*eH`I[::,4dg=Sq6[X&lL9;CeeJq +4B._4S)6hbKZM8dhuk9Coi=\7;#aa`H]JKC+ZR/6gj:j+8gQRfWfJ-I.?48JUrC+FUH<\SXiK9&@QN7r +k^$_Y(-Xjj8Hirc*P_[S,Q8HFQh*Lr;=1q^AH/nIP^r7G5_:1J&/B+r,bEg;-B]m[3Q +S2]`ob#SsDVVaAi4so')`Lg$,Q6>3YI45"kTk:Xhc4T0>Cm1%G25cd0n^0&>LOeZ7"ZpK[R_s8:4C/A8 +S(55OKq:9J5o:&0-fChkd`'#k2ZGN0Q`l@ +G\qsMUFr/8>bMtc+YIr2q7!!!"^YCHN:Bu'Z$^\unDiMN)cF*%BZci8C=4nj?oMdNQG_a!t=:XVH,oj@ +`!pZC3-c/8HDk%=X^%^C\cpJ=4IPh**!57`s5Q,:71Xg0PB_sH3nV^ +1kl;MD%XkXm6)>Wp&5@+6UU75r)nlb``ro?khp9h)(DuT(;rbFRWs8MKe%NYTM!!'^s:]L&_^\Gc$f)8 +XK<(c3?s8MKeIf8S>s2#SdOM:_:=2+7D'2L0jRJ-RGDh%ZIJ,\VF3_gXkqtm"4m^_VRUq&?WZ>'2K54b +2UHhXCaX)&ju*PM4(Hde%O4.2P+)QDogrpK7'Sf`@L6(31^*^*]?cOW8<0PrglkYQh[(B=F8\l?4FY'o +kbOYkL`On8D3TDsuPplFgs\)2W.hra@^>? +VSJ(d(@0p.4Y1`*&]2LT@eH?sOLn=G!p8U'aN47Cs/c[GQDQhF*;S*jg-EjGDkdUYKt1fXB[#QOjTDY* +l)n#%o3TDn)Il@-2tK`:VA5J>nF2:=.lpgVr/%DnC"BtbW0*_s%\RZW+"pO@\]hS%I3\oQn)m5)C*I#W +6ZkO_^;?tqWt+hH&,%fcS09t^)C4))q:hY4`cpU_rGKg5@jK0T:O5Q(HB5JM-o^,3eTrG5Ps.$jfY0ITE"_Z%mKbrjYMtGp7\1RpYUIM@U^P;9H- +4p8UpD7IoZsJ,BKA.ogNq>g`IJ!I/cOU +NsVpTY5Siu?(oYl_A^=_XNf?V6-Y+8S';V&*_a/V@+fi!/S/H>bN=Lk((r9'?mFj]o4s7_"=g]-4Srh4 +A=aXmA4/$F?^.po9)'(P?N6hHZr>@qM\5P;!OrSp-A@s.\AldsUWX*qa%dbis^Q[egteXJa$#ljsUTWt +c,1g9n3TDdI7cTe$+gXE&rVbsq/V=LGOmA@5Z>Aq3?^.u$"47Bg\/sjnS3HNM0p\b!Is*aapnF1B8LXY +bid75:Td5^NWDZ9@=^THA7h07cTgiLfTrV#!jO0J1im2u;j8*U#&orDHUHgeYUUG2`K!3AJTJ%'7IEnA +@[Ykc!$I4l&$eK(*n7f$a^qq3(knK1h]2GLf1p?\aed:ia?[r5X8IDj5D^@6jAjectRq,HA"II;/jHgP +mpNBHaL'@m!5^4#nUn%YLX^Dm4RJDU"YLEHNnk + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 381.51931] CT +[1 0 0 1 0 0] CT +N +-1284 -71 M +-1284 285.25 L +319.75 285.25 L +319.75 -71 L +cp +clip +GS +0 0 translate +320 286 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 286 0 0] + /Height 286 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W9k[en)Sg>9PV,@]i'L\)\-Q'G.4K_.7AU_8(CI"u#7N@Y&Yup95nk$/],%5R0d%2s6!B22cp]D7 +;DeRp?+?!M+9ZCO5u^1u@Vs27Bt-OSSBRPoREUjgI_XFWSE]:-2pZtc:?]a8!e&O62RAac\i2Z2R_=@` +WmPKj07e7A^KL2%S:7g.QT4qUd4]M9c(\S)oUA:a`9B4=IYlNlV:ui[m-jX/VVb"#C1(k#KRPEhWe%G* +&c/V?W!:7B8*IFFTc+bP*MM@_L2(+4FbG2o)6/7meIhY[A>f^P<_)A_dY8dBWMukbR@.ra3B]P(j,ZFE +J&`#1eU=.K8fMrPm^V;;rVQ>5o?=i7]'K1TT9'#4)g8:Z6GJdDlqlqjIF2u0%6IPc?!m5.Bt/r`qXs/m +]^a#?pYS1j19tWCVFM\QjQtR+MhdVl]6E`#aiVYk?[d:l/`lDs9:&g0=mWVfVq`]`)&jOllDq,fn+k]I +pK"IUrDmYTW\;*]U:rg>nA,@Q^6Vs)G3>E?*^+iAbaC77:7TtJFmJG(hgXtH[=s,(>Je&Mm^o*=o#[l1 +W=n-A%mG6qIJ9!b>=V>X4*U*DB/k)$=K/]uoC'HomX5"O[[QXK[o3*"^%Kr\KDE\MI90I`oF%(ZTAR>/ +pNuABf6ZN=ackh#Vb`pkB^>ELHgbeT4$,N.Si/;Xd5kI">n"/aP*1+Ro67OkFAKZ:g4sEVqH`COeZ5TC +SU07K(Wt4LNJ4fiQX'uc&'<-]qeYS3H(Spo3d:!slQdLrhp#3N4oJDr+0&c7\(PS`cb8G_ZlIo-A]nSE +4a,QerP&=2qb)FtIr8s@?2F/noB4H/:=A,!h/"t)dr2)VTdj-+fb^so;RpgTqLr1cFK%Yh5!D.j3VhEJ +^Qh;]=Y'1Bat'YHrO!/!o[(=#qcZK?qsAe1DRQ#%l*o(P&(Rppa):e;e``\oLK<]L]QLZSG3-l3Ocbds +jOV`$IQO1:h%4Mt!9go?&.d=@e5Z8/%NIG^J'1>(I8iDiQ:F]A4LrhiDgD8gf3Z(?5VPg=nqfff^P<_)A_ +;W%Ke!?G^#\(<^.EZ,_M).S'lo]9b +.#XWDc4jipo>I&k/9[UE;j>8H'5*3.fQlIM;&25hS12TikD:u`=mS7jVC7@n-a'?rlLeM=#?KUE]f5ul +zzYeaVXGk'BmJ#tgi2`LKu\).[?4fh/=;`$^'J)S7u++F!YgdkSud(=]$o%;UIR58J*O1t879A?ml)t9 +"mG:KM(EcU`u7iu%7lO;h6j'R92A_-F,e]c62g"iV0S[[B+(LQ9Fod?AZEqo +._E\4_Dmp854m^r0g5.c"*HM?6>([pXUDCh9O;lmkQL4T3n%A8]_[iA&HhQ +5HDnl8E!tRLjDHs\j+a9KYA]pt7o?TWSGDo&WkqJ+%8L=rl'J+:Jo/,qUb4&6+)VWaqMIO] +Oh-RY6EB?-#QVPrsLmJP4u2fC8;iJ/M@cd1]BGs$;AXC^c`7G54mkCE^?>'I/V]iq"(0#o_J2pK]d*grC_-nkRd0401r\`^uQa0/d\QFP#0UgN+2Vl1$Mf\"j9p",cuHY-l%ZBban$hNu](GB ++-Eofpe63$uczzzzzz!!$Dtq(EY1K?s~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 347.4] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 356.25 L +1603.75 356.25 L +1603.75 0 L +cp +clip +GS +0 0 translate +1284 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W:InH%)o,lJ+sOlmX]gA\^(D(lKh+a*[$-OV$3c=hN0X-r#.SFQ)1bhUDaJ,TQNzzzzzzzzzzzz!"*rDm+IhrR\?FK$J1 +n3G:m)a3&k%(]`J06JDurM!<<,PB["hY/c#6[gU;6JG*_#n4psT"n=[J.4ho +\(g=kmO5C`[FXSi2#!'h,c7Rf:ShOKS7f!+h7In"rr) +^g9hj<,oB4Gdf&pi6lK[[#l`X[1IeW9d!s]I*dE=o/@&!.Z8`4F(uHqk*WLfop:No]];'FoErId*O +I15+_drH@#GOdA,C7QS2_.>OjWV%j%7.R)lc7q!hkGY$K`#dS%KClk]X%!/`Ha0[SS/4(i%er'#K$XSc +i//DS_';nfu\^H^Itl-cF+l@Ng*g=j]a:ofcX!WW3cEFrfpL51PjOnR(k4(lj/WN3'MT3LL)_B2!k/H> +C"M2?%YCMR^72.,tIl-o2\(G8t62ElEWV"4Hc!!#7r^4-%7dEthf!it&/g;_.PqYL'-BAptSampCmY%A +XImbO@>?1<=nc'a89fe3=*!#4D1M%eS;H2RI-^:q>?iSj%cl(>_FhaM/9dKb>E$be7+paf[lIK9EK+M# +nIdIPn19Ci+L[)afrkg?0roa?4TG`c.=_YjN9!'kQX?G+?kc_dS:nr:tf5L85r4>8INDj`ft+$4YI[!l +V!-N>.OOcbcRbEB2NL&2#R!.a.90fK&fmcDWY#A2f%EF\=cXZLapbVhY6r&A%AIhp>ehV@#Gs7#!bk2t +gmC.Ccal"69PKe(q3!!!!5iUuNgq1&(^AKtqs?XG^NAs]]-]pc-m57?A_ICl2H+odfZ>IN>"H*dE,/,? +hs-2RiG!.a'fNfI_Hle'kBOt4`=gh`S,#3>\tYDV0i!!!##Q3CXe"TSOQFj)BF!!'f(@ho7K!+;Q>*?? +.=)B'\>?gToVzzz;?$Ur!5P%#[9:"#4(94/&P37b3#.R/OGHgIE@NH,,>=Vb84#kcWJDgQ7%B-j;c1p^ +@H(6S&jdd;73j$%i`&=9MMgO%';m%;&/&t4+pfD)V]t,"<&@+43#.R/OGHgI8I1/4'bR@8I1/4'p402 +Lf4n*.Opl39Upu26:f%oLa7XAS;N,$;:-@li`&;Y66]/n,U"Gm<(*76MWP0XV]q98&ofBF#pBnY,2uR$ +1e3om'p402Lf4mW#`1MU&Z;@t3Pr_6La7XAS;Kj(8AqK`3#*1Q&LL7A,U"Gm<(%?c,#1SFWMjKj0QHLI +mAkXO=:^'QS;Kj(8AqK`3#*1Q/50;mGP>k0"kEc!&Z6h9&/&t4,#1Qq1CFlZ4=(:W&Z6h9&/&t4,#1Qq +1CFlZ4=(:W&Z6h9&/&t4,#1Q=Dr//&o&\'SCWsZ63DW458AqK`,_SRUMMP3YMLG9hiUGgDK7ef70/EtL +k<($s+VCUrOq87G.O%VV.l0(Af%f&Nc+.FpWk`mP&jdd;73j$%i`&=9MO&$mC9">8C]FD4Vb`pC$WA(k +;j%\)E@N.=+VCUri`*9ihnFM#Eo]be:JZ/M/511G:J!lD`2RDpGl,[6BDjomo69T:CK[P$g!Ohi"m/Vn +E>uH_Ps%B+9hg'9rr)$H:i$oE3^!3`#pBnYU33+ckE[IMI$h%>YHRc.`IA61h^/%o;Y_lV/D9&\2uKh\ +X_kK7!pbE!Lf;^YjjpP<=0>g(c`HIn`qJfS(Op2,j8o>4LT8V9_&YhN(Lfp$8AqK@/URBgD>mFO92!HC +5KNt79aa/ER"_LX:Hh?:8AqK@mE"2MSA31VT]*BZQdY'?b!.-+\WD:kPPof]/)a9'7,187./4Wi(8bh/ +q'67i&Z;ARQ#QLM%Cr=72LB2'Ip#g\Q_OKk+VCUri`*9oAmq^ZGtJ]9,#1SW=.4V?4S+jChL+%Z_Pk'l ++VCUri`*9oAmq^ZGtJ]9,#1SWZ/YRYJ(O0OLQIo2Z2b+~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 347.4] CT +[1 0 0 1 0 0] CT +N +-1284 0 M +-1284 356.25 L +319.75 356.25 L +319.75 0 L +cp +clip +GS +0 0 translate +320 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0R5n1^s&-ThB$'%82>(-XT'+1YB1g[^"_.g:DLX-V6zzzzzzz!!!"L-[GkRe,CgZPi^#tU)(q9F%% +[H-g-@j/5OP+[90#j.r)=O3EO^>H#g'd`U`(p;2aPX\Y9k_PB0/EQ7(jQ=tEIp't%/8S1A?Z4?6Km@X3 +!sW+E*]IKJQo\)fT0p(J9=^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV5\ZLj&kLq[&4jeaEmU'E, +dQqLuJgqYbH*[9!/2N9C4^MuV,ao\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.\(0pX4Q?4u~> + +%AXGEndBitmap +GR +GR +%%Trailer +%%Pages: 1 +%%EOF diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_2.eps b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_2.eps new file mode 100644 index 0000000000..772e51f015 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_2.eps @@ -0,0 +1,1548 @@ +%!PS-Adobe-3.0 EPSF-3.0 +%%Creator: (MATLAB, The Mathworks, Inc. Version 9.2.0.556344 \(R2017a\). Operating System: Windows 7) +%%Title: I:/Entw-Technologien/Studenten/temp_kai/blend_case_2.eps +%%CreationDate: 2017-10-06T19:21:34 +%%Pages: (atend) +%%BoundingBox: 0 0 774 521 +%%LanguageLevel: 3 +%%EndComments +%%BeginProlog +%%BeginResource: procset (Apache XML Graphics Std ProcSet) 1.2 0 +%%Version: 1.2 0 +%%Copyright: (Copyright 2001-2003,2010 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/bd{bind def}bind def +/ld{load def}bd +/GR/grestore ld +/M/moveto ld +/LJ/setlinejoin ld +/C/curveto ld +/f/fill ld +/LW/setlinewidth ld +/GC/setgray ld +/t/show ld +/N/newpath ld +/CT/concat ld +/cp/closepath ld +/S/stroke ld +/L/lineto ld +/CC/setcmykcolor ld +/A/ashow ld +/GS/gsave ld +/RC/setrgbcolor ld +/RM/rmoveto ld +/ML/setmiterlimit ld +/re {4 2 roll M +1 index 0 rlineto +0 exch rlineto +neg 0 rlineto +cp } bd +/_ctm matrix def +/_tm matrix def +/BT { _ctm currentmatrix pop matrix _tm copy pop 0 0 moveto } bd +/ET { _ctm setmatrix } bd +/iTm { _ctm setmatrix _tm concat } bd +/Tm { _tm astore pop iTm 0 0 moveto } bd +/ux 0.0 def +/uy 0.0 def +/F { + /Tp exch def + /Tf exch def + Tf findfont Tp scalefont setfont + /cf Tf def /cs Tp def +} bd +/ULS {currentpoint /uy exch def /ux exch def} bd +/ULE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add moveto Tcx uy To add lineto + Tt setlinewidth stroke + grestore +} bd +/OLE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs add moveto Tcx uy To add cs add lineto + Tt setlinewidth stroke + grestore +} bd +/SOE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs 10 mul 26 idiv add moveto Tcx uy To add cs 10 mul 26 idiv add lineto + Tt setlinewidth stroke + grestore +} bd +/QT { +/Y22 exch store +/X22 exch store +/Y21 exch store +/X21 exch store +currentpoint +/Y21 load 2 mul add 3 div exch +/X21 load 2 mul add 3 div exch +/X21 load 2 mul /X22 load add 3 div +/Y21 load 2 mul /Y22 load add 3 div +/X22 load /Y22 load curveto +} bd +/SSPD { +dup length /d exch dict def +{ +/v exch def +/k exch def +currentpagedevice k known { +/cpdv currentpagedevice k get def +v cpdv ne { +/upd false def +/nullv v type /nulltype eq def +/nullcpdv cpdv type /nulltype eq def +nullv nullcpdv or +{ +/upd true def +} { +/sametype v type cpdv type eq def +sametype { +v type /arraytype eq { +/vlen v length def +/cpdvlen cpdv length def +vlen cpdvlen eq { +0 1 vlen 1 sub { +/i exch def +/obj v i get def +/cpdobj cpdv i get def +obj cpdobj ne { +/upd true def +exit +} if +} for +} { +/upd true def +} ifelse +} { +v type /dicttype eq { +v { +/dv exch def +/dk exch def +/cpddv cpdv dk get def +dv cpddv ne { +/upd true def +exit +} if +} forall +} { +/upd true def +} ifelse +} ifelse +} if +} ifelse +upd true eq { +d k v put +} if +} if +} if +} forall +d length 0 gt { +d setpagedevice +} if +} bd +/RE { % /NewFontName [NewEncodingArray] /FontName RE - + findfont dup length dict begin + { + 1 index /FID ne + {def} {pop pop} ifelse + } forall + /Encoding exch def + /FontName 1 index def + currentdict definefont pop + end +} bind def +%%EndResource +%%BeginResource: procset (Apache XML Graphics EPS ProcSet) 1.0 0 +%%Version: 1.0 0 +%%Copyright: (Copyright 2002-2003 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/BeginEPSF { %def +/b4_Inc_state save def % Save state for cleanup +/dict_count countdictstack def % Count objects on dict stack +/op_count count 1 sub def % Count objects on operand stack +userdict begin % Push userdict on dict stack +/showpage { } def % Redefine showpage, { } = null proc +0 setgray 0 setlinecap % Prepare graphics state +1 setlinewidth 0 setlinejoin +10 setmiterlimit [ ] 0 setdash newpath +/languagelevel where % If level not equal to 1 then +{pop languagelevel % set strokeadjust and +1 ne % overprint to their defaults. +{false setstrokeadjust false setoverprint +} if +} if +} bd +/EndEPSF { %def +count op_count sub {pop} repeat % Clean up stacks +countdictstack dict_count sub {end} repeat +b4_Inc_state restore +} bd +%%EndResource +%%EndProlog +%%Page: 1 1 +%%PageBoundingBox: 0 0 774 521 +%%BeginPageSetup +[1 0 0 -1 0 521] CT +%%EndPageSetup +GS +[0.6 0 0 0.60069 0 0.20001] CT +1 GC +N +0 0 1290 867 re +f +GR +GS +[0.6 0 0 0.60069 0 0.20001] CT +N +0 0 M +0 867 L +1290 867 L +1290 0 L +cp +clip +1 GC +N +0 0 1290 868 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1283 286 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +0.941 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.48 0 0 0.48055 0 33.71863] CT +[1 0 0 1 0 0] CT +N +0 -69.75 M +0 286.5 L +1603.75 286.5 L +1603.75 -69.75 L +cp +clip +GS +0 0 translate +1284 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WnVbokS]C9tCIARD']Ju+5d/rRV'JSjlZTSZbg+GurL*J1B-%T]6+=]!kE1[E8DcJlU^2^2Q*,KTO9SLn'?+Z\?L]V_$cZ27FCMPR2.6s.@&T((P4jT:Mlb +N[Q%rE[i92U=O-hEm?,?S+LRB"+/+J#pGF0&NLT6AdafD:-$hR$#C!qA+i^I7XhXJ +N[Q%rE[i92U=O-hEm?+4/A-S&O&>h>b7Q%R6r$23<7I?g9[5Y0//*7c2L?.M*)T$njA\NB7BP7[jIB/g +)YXY9j!X=;+VgJ/L]@)J`7e'#-%nZ]7H@FtROq4Y3G$]neMUqB+!@@+VJbAc>Jr"cKTO9SLn'?+Z\?L] +V]>CNaI+*A/#9U#%,j&_`@@U8ON=DWS;LLVKul?Q)lr\SN.nVYL]8(_OK"$K66`KoI-10kruMF\;*mh7<('JM$[D[HkDt#BLECt)l()["EmEo1RH9R;NTQ@bqV[IMqrg0X;*mh7<('JMoD8%$ +0=uLVq=o_u9Q3TkMJB%XjCbGZ:%ouFEE!VC^QX,%BT,,ZW*.[ntK)`P?R4(\=2?ji0>:5A/> +f@X6G"PtN%5&0`-iQ\=i;eaKTO9SLn'@b +p%+gUr9W+C5Q0hBO$7^Sgq!=Vmn\6/,P/MShpl;HlC5TJVLF$`4'$J3L"(V$^J2B+>PEVH6jY$!;\?Q7 +Y%Y'lr5&gp*'\n"CMW7eX]sD[?0hK7qXj#*^A)0[(1-?7"T&t[Q.BPr/`7@k(\h-")Nq(LX0TUsV'8M; +M4BIEWG#TsDVVaKp[8#&5Q16'DXG12em"?ph\IR%SG(+tB0>JLK3O$d5Z8bA^QMfs,"`,tS;PCe./1V$ +rp\pdMj+-A>F:q%\oQ-"OJ2$u;*l2$WBY[BjP-CQ8dL9DMTDc1/e9$f]RB\NJL?TJ[d^+dQR;YU[P0`) +FB,2#ld^M4n3/!)h3@)2>M:K+&LE)u:.=5C1mbmG6smKFAcF=m*]uE.o]XlWetdf6(+/470%9I'EF9.b +MicN*iFt3I,"`,tS;PCeBO2K.?9E5e`6Fc1]AMMXGOOnOZS/u\_P9V^-G@/G9[5Y(K%VL?;SN66:.X\] +F:q%gjI-kj,Fe54h!T@<0Y-X]\s3G]!0C-m>^nn`dK-.)1Wf+XD(X,,_0dCghA0Bo,u!!dH:En3/"t +^%#=:F;1d3&jde?,'#/Jbn_YhS1\XO(BRpprB!VB)e/^V/m?1CU4eWLWJI(%>-iq)3N$qdnnkWT^(=0c ++`3_Rl)TUQ66`"*@7Kj`'e[<.2>]H8E<`<:.;;<_W3WESBg[<^aAQ`2D?Cg1"Y;< +WJD0R,,ZW*.Vd3D-FY/]FI&6n[XSr0&eYSAAJ(u!&jde?,'#/Jbn_Y8hWK"#(BWIJ_Q1DU_m\DF;.ZnK +'>n9aNl\2US;LLVKupIS3ShGGIHW`Qn)Ln'?+Z\?L1qOAW'E(fr)QY+SfZ!)q\"W#>(jbTFaaJl*q>F=4(cKSd7pH)THV&-dkq"IgD#a30ImRVSVH#JG +YJulQ)]8XB,B>7XVkVaY@gmaTrLW;*/^0lu +i0.BHU;O/iC`&K`E[i92U=O-hE\5T$$kq9Ae90f':G/WfeV8K)8*!W;:.;;<_W2'Mkq"IglqZfVEks,) +;UP8TNX)4n&sjh`.O(dbenec8iAmIj-%J_J:#LV!Y4Sp[->[+KGQYGqO])iP't%W]P?[YaoF?-Q@/G/s +\DWQ$YA!hF[YWP+b7Q`LO],+m[P58FcfrZW"`+6Y+r>WI%4h#/D4AK*Ln'@DWMh[YR,"CuFQNnBSD-\4 +\_rZU7R\q<)o1QkS;PCeW&7eHgt6[/r?q!fH/r*\puI`BOc.psF#bGB,'#/Jbn_Y8Inln(jct1N*6JGW +gp/H!qQ'op]d8n`3?k@FE[khK(+)!=R@d!Y0MLGW:Ms_Y9mm5]e_DDO/k=u%6r$1Qfto:PLAgFe+S0Th +-b&)mVS4V:.W6/FE]G>A,'#/JbnanDZb-FF4+8a#"eT$A.WSs&"+:H3Ti2O`'t%W]1Q+V-!nc%5d0N'6?n+IEj8f +KLB\h'ch>VV$@2:&j<-:E[khK(>]UXrlePdGK`(DKTO7aX.nfr+_bXm+jj(8,,_0dCuI60_u;eYLDT!@ +;F3q8L0B^.gbV;,M*=cK3>Jr6$a5t$?M:=6"94X-8dQZ')g4XcEGl7I,l0eqjAaWt/\D:"I+b:KrB&.^ +V1arOX[>Mu38F]&&/)%@,'#/Jbn_@pfjFHOdJ,l?3MhCjaK8H2;m5Gd^2[q+X[6Jc<('JMXkCRDY^bPC +c?bk[&kf,HE[khK(@J(lO)$rOf1J$#PRnJ<[B4+F5P)a`[1.s=;@to( +[:Tg@`:T\hS\BeZc8h_SON=PJ7>HATTZDG7`,_f4S;LLVKi_jBVg9E?_L:I7;F3q82PD=QGR68O,[++Y +3>Jr6$STc;cgs8lq_l_K-%n[(UFI2M=,m46W/t[\7H@FtRP!=9!mgI#gq,;d.4'X8D?DDXpdM!N%jW'L +%1J)6.O(dbenj=#ZZeS/p6nLR3N&)Q'"O"(B]9f9L_mA%+]1d!jAaWt/Ng6$hohShf/dnH.3TsW.QZXk +&[N&^3>IZT;/%K.Q#9 +#`5(/aJl,GNS;8@5)aci/\LBK\5dQE.3TsW.N35HIV>X8ON=DWS;J)gZa4`9cCM/QqWcH%>e_l'n7.hu +hnlGn:[B+bPS-;,VkQOPrDL,i)qQ<$,,ZW*.U#+38NV+^fTE((LMR[_M&BpJ,J5W&%]/qs8Msgn)&!j0B`5@F+89ME)%i*,]YA +O]0Y4^A6o\DVMo&m^r@7g"tTRhX\rRmnERJqR0D)55nGDhu#_%PS-;,VkSkco!Nb\#`5(/aJii*[r:0l +QLE(f2b2bMr:oe@hgBLn5I.;kRXG%6oZXjek>m!E=9h/:VkVQ1YL'>aFiu!TE[i9BaV?D3Kl>eYeSX8_ +&WaZr`/,.G]^a!<[dX@ueZ@P8\\\A6,,_.J^/>>qS;PCeBJ.!P_JB_FPS-;,VkR@Mg^.(i66`i9OSk#6#E_\&,,]YALiK1ec?UmF,V]p.2R`jPDA\IipHO&C_qJ9=?fYZ^>;b@=PS;Q#\jH3m: +77cK=6r$2B3d^SeCTkF[s2PkgV@oB93MhCjaJihj2II4,M4BIEWG#ViY@$b\mFuQo]t:oRaX<^N3HJeS +)F)C\-Kgms6:g0:ZrHA'LkS5l7H@FTJ?>rUA&jTMH0Y>'o8aqURe"b40KJ<\&jde?U(peN74]^).O(db +`U)sXI/NNTdoZ#[>F:q%8dQZ';pi`]I9"6dhmg3'M4BIEWMh[YR%/4i#pGF0dHZ_G_Ll(266`-iq)3N&)Q&ig^"b1&`4lU^?&FR@Y((rVBO],+m[P0`)PS-;,VkTlR1l#-hMM4Gn +7H@FtROq4Y3MhCjaJikBqQYj16r$23<7I?g9[5YpKTO9SBMGRS&IXGcMTDcq2-ZH[ +^BTm`aJjucMH[??aZ68`&/$u_Yp74]^).O(dbenec8c7(&m6r$/i@d5'g(r\fcV]p.2C<[M@kF>F:8EJr6$^5X,.3TsW.b[SXMc`O2<('JMXkCSPS5W#cM4BG]g%Z`'cjLKpM4BIEWMh[YR%/4i#pGF0&JMYC +4H!<4jA\NB7BP7[jIB.@#`5(/fJdCJcTI)=6r$23<7I?g9[5YpKTO9SLb\%!`'PZG,'#/Jbn_YhS5M[E +jAa&co.4>oO])iP't%W]PB0JJ+XD(XiXiFZE*>fn<('JMXkCSPS5W#cM4BHje[R3D:IsPG6r$1Qftug? +;b@=PS;N][KIsk+$PUEAQQpN#C<[M@kF>F:8E3E[kO,PLm$9bT^;P +aJl*q>F:q%8dQZ';\:12Qms^YjAaWt/POGMV1arOWJD0m8/F:8E<``O5c!cNt +WEbJ)WMh[YR%/4i#pGF0&L@p?1iWn-`(]<66r$1Qftug?;b@=PS;J^L.AS[cfI1>5QQpN#C<[M@kF>F: +8E<`F:8E<`9L/D.:M)3?kAq +enec8c7(&m6r$18=>6PmMqH3,D'5/iE[khK(+/47;F3q8<()QkUiAcS$PRG$QQpN#C<[M@kF>F:8E<`< +M1Ug9Cgi6HF1<3P;c.>=0KJ<\&jde?,"`Q(2"n>6NT*[jE^WYkXkCSPS5W#cM4BGO;&dsU3G];2/!hK$ +.Vd3D-Kgms6:g0:7#=AV=i;8ujbTFa>-iq)3N&)Q&ig\(U1m049tVWSNl\2u2-ZHJr6$^5X,.3TsW.\aj:6Q3^nNT/o<3?kAqenec8c7(&m6r$18=FNg$LTj30A1gC5<7I?g +9[5YpKTO9SL_?`saWb%-/k=u%e=%s_c8h_SON=DW&uuOWas&a$#@d?@0mkHcbn_YhS5M[EjA\O(*ee,: +%p"`@@^&h]ROq4Y3MhCjaJjt.]aJPk*;1:EIFc#f(a'R$AdafD:+7>3E[meO:ICSTKe;;5l=iZqMMbTD +Kul?QPRnHf'iC^o+[%W7NT)#EHUS?oMH[??aZ68`&/$u<;.:([9jY^QEA%`p,'m!QXkCSPS5W#cM4BGO +0qaUZinbSM7J)n'>-iq)3N&)Q&ig\(@W3Jb?c5G(8dqjHH:UK,ftug?;b@=PS;J^Ld#cK1?5[EFoSSN0 +Z\?L]Vj%`*3>GD#kTPp'_HrUSl%WLVIA!;<4TG?N8&"<,%mQTVcF,DekEHTG,,ZVWX@Gep\$/<6'^JU. +X/2P9IJMukh7UFKq==D-\i3B1Ip8;1F%Q`^O])iM(-;Cn3%bF: +8E<`(rV^SAEm?*[KLAR(PZ[O=/E3L.o86WW(!Z(-5QCQ4ZY+da +e"7\0lJ/pkp[@"3o);/AG87imbA>]fJTW(\E[mg%-A43e&[2&.XbfcNLt4JpU;aS1$ST/3r:ofaXfYg: +DI'ktXo%/Ce#0tUieo6iGd2`1lruGN(aUGJ.\aicKf!ZTHPr[n)cREoW+_(UmHoZRp>1&S`f7TSn*[s: +Vk8FlXkgJCH>Cq6Z%^:[Vj%`*3>GD#'13\IKoP*1-<#3V7<@./7.c`p=8(P"mH(*5O)AfO:&7@r@<0XB-%nZ]7?eA3[tcPEU\IY8,,_/\?+bFP +`/,-dDr0i0J+)7-ZY.UunA5Qo*^,tarjFL@kEHTG,,ZVW#\fo"]J((_;pi7hg&(X3Y9,oRFRP!@?[:au +cT_5Bb59r1jIB.@#`5(/-m>C!:#*TlR!8ZIO]0Y:[^W_1qWO]X^]+)VpU#Xr3N&)Q&ig\(6:d;6gqjis +1K6b;7BP7[jIB.@#`5(/-m>BRN>]SKnTUsgS;LLVKul?QPRnHf'i@:gZ^N_+NFEW"5t.p/_W74c8dL9D +MC9XD\QQks:_>]&[:Tf1F&"/-,]YB/KTHs[&W]a#3>Jr6$^5X,.3TsW.\_TX,*Ni57_K"iMTDcq2-ZH< +F%Q`^O])ibPt-<:^='t%W]PB0JJ+XD(XU4eUXN>oa(Y_Z:I:.;;<_W74c8dL9DMC9XDH5!aV +)D>P7O],+m[P0`)PS-;,V]n_=WKF04GhkioMTDcq2-ZH=0KJ<\&jde? +,"`,T3S&NhX,*:'>-iq)3N&)Q&ig\(6:f9VIL$JaKo&et:.;;<_W74c8dL9DMC9XDPo.8i#Jr6$^5X,.3TsW.\_TXH=X8; +-<:^='t%W]PB0JJ+XD(XU4eX)BP<<+7H@FtROq4Y3MhCjaJjt.6L(ao>I^:Y_k\Js.Vd3D-Kgms6:g0: +6jY%0h1RB8X$s*6E[khK(+/47;F3q8<(%$9%E@$:-go#',[:Tf1F&"/-,]YB/KTHCKl5F!j,$#:9 +'t%W]PB0JJ+XD(XU4eV_W,ISr7H@FtROq4Y3MhCjaJjt.65m$+I%+5,jAaWt/POGMV1arOWJD0R)S0We +GZc];U`sh/C<[M@kF>F:8E<`<#pHF1M-ILg,$#:9't%W]PB0JJ+XD(XU4eWZbkN@:GSI37S;LLVKul?Q +PRnHf'i@:gCMV'4XrbP3,,_0dCghA0.4'X8:.:BYde[aoLfHi<7BP7[jIB.@#`5(/-m>B\ES.aWPNSK; +&NLT6AdafD:+7>3E[mf:O>bDF/\LR'0r]u!MH[??aZ68`&/$u<;*mgCCYTJ-m,a((6r$1Qftug?;b@=P +S;J^=0KJ<\&jde?,"`,t;2^EC'p2El +CUPoWkEHTG,,ZVW#`5erftuWp4XLo+;c.>=0KJ<\&jde?,"`,tl)&=V^]%+C3>Jr6$^5X,.3TsW.\_TX +R*h]QI..q'`3lO$>F:q%8dQZ';\:=/`7tin_aZBPF"M.O(+/47;F3q8<(%$9Z+/IVnWYDoL4-3nXkCSP +S5W#cM4BGO+X@C7[Y56?<@2Z:C<[M@kF>F:8E<`<#pGH.:.8*jY[,HuY!oJG_W74c8dL9DMC9VnEoSZ2 +C^V3$N,gLbbn_YhS5M[EjA\MR+d&7#d0sgC0#k;t3Eql@F%Q`^O])i`2dlKul?QPRnHf'i@:gjJj\u +Ba`\7CWa&`2-ZH`2dlKul?QPRnHf'i@:gjI0.,gD9LIe:Zmee=%s_ +c8h_SON=DW&jdeOe;@kp_PZk-\/)Ja$^5X,.3TsW.\_TXaK;J:H)C(<(U5&=mOQn`enec8c7(&m6r$18 +&/)N;<&EJJp,$%Qdt`h3Rk7=Z3MhCjaJjt.66b%!MWPkpeo;5mYbF!b$^5X,.3TsW.\_TXaS!$-0KJ<\&jde?,"`,th!cdsF:400Fu'kf=4,IP]6E_EYJ*'e`grZk4*U*Tqt=[U_W74c8dL9D +MC9VnEpHP!Y_C+^#+IWT6U`RiGjsYPaceZ$g_bHLpIRGim+@Gu?@DLs7un]cq$=D_3TlV>66`HV8D[_'.up[6l**A1X/2!'AB_W74c8dL9DMC9Vn +Egom&jVFHTqp)8A04)(]W+Z4dgY5f4>^>bJk?`W5jTA`^`>;moE8e\5+$]PYKC0cJhnMD?6eT?;S5W#c +M4BGO+XD(I>>`j$]ZAYFZAku"\q^e%<-pRBP^r671XFFS]4h08VNZWk]1>MaYgSOjpG(+/47 +;F3q8<(%$9O]0XE/(qd'r\-S9P`E>/qrGk_DkE'Ld4KbCp?h:/2fIOmIHTY2O2(MQS!-%cOni::mlG[\ +go>8olTe#%<(%$9O]0Y4HFUqAr7-8o=oNI'^_XY,lW3c#D@qrDc\W;_hK\(?.9h%_Ih>@oe_baP5tL'9 +6jY$!;c06rgj20`+WpNZmHs:r,7]B8$e#?`p66+$3>GCX8Er3>IZTd<#W%E9HT@Z`h(9gAW2-V@lP[S5M[EjA\MR+d#\83>G9_lk[r3>IZT;/66`GCX8E<`<#pGF0&NLT6jtf\+7?e>r3>GCX8E<`<:.;;\%$OU%-%nZ]7?e>r3>IZT;/8A5 +jA\MR+d#\8-%nZ]7H@FtRa3ZJ'i@:gjA\MR+d#\83>Jsa1JCP4PRnHf'i@:gjA\NB7BQ@oP>`&>KLAR( +PRnHf'p2ElCHfYs<(%$9O])icCI%(IJ)-jD[6HR]tN6%o]`9GY53mW%N[cZoVXqPC"9\E +U"\>YrUZZ]r'15Q5M^6YI9RPm]ru3YI.>1UD[:u;n(t`jmom?Hc!87hj=YYYX&lJG4*Ku3;m3q3C3LJ9 +e[L?cH2RI-^\l9i*'JUiYG1K6msEjs?1j1dfNrpl[HF-.DUl4J6+VODqWZJ,pF\sKcCDFnqt@%.&VK`, +>ArnY?2N$1IUj$HQNn$BS32)f`f1q4Dfh>8\NF)Y]A2MZq[D:no%LKe]=Y\!n)!I"2I*h8C"5/"JU.4e +f=RgSLHk^1X6B0$5QBE_IeN^2cZePre[LI1lE3\6FibD2!StG@/tU7Q00aaQjNB3kln1dU:7Uf*)!:Ku +s7K`)8AG,jDnE2gp[6l7rqF2-dRu6qqF31V>r1;"l7SdX(#po*EuA2E]A2LSI.>/`(G>(SnUh?9kW\2( +n)%L29k%55\$qD>4aQedebut!G<fm^4*cno#h8=N4[lhBMq:eEgQ[cRr>l6i4JHl]Dn2b +PMEk5STipniN6Qk*^+iA]ANSTo?TYYJ'6-_:05iHIeB`VhC8_EGB`f%5Q:1+J,.:U.V&oCY:!,;F;LS, +s8Mbn^**pA`JYPECXN,'J+K/eFD6_`V1Z#o5(%V?e>ZgTqUaPA7TYP\grsY67IG2_]7'R"pBeKXoB,6U +NupUck*p:ll6`b?hL>@hY,aednW0`b+Ul]Jf%*(bj$,_Vr7Dcs!IJ2'55OKI[J#4Se^8S/GTemW(+q8JOD3?1-(A>IV1>i.:&+ +3H=&s)fNBoWggrRIJiWU>s`uCL';tqaX;F&VYe%YbQ<7nqqlT(SNhW'jd0>'pYC%CorDGBQ_PZ$kXV4= +RKZs1I/1ijrSRoFq!uT]=+,uDOom5S?#CE3I1*[1ST?KaouB;rn`7fU5P`Ds*^>+dlJ'1k4F-MYqqqGQ +J,\V\metNKAc][Dh=%p.C-V^fO8$]`LECtii,OT-MdQDA(T1$!lL'7ADf=OFGOO=tF/Mg+2M[]k/PL') +(3Z%V`8:*&HGB_2hLGS(DS2K(J)7/8!WVk3s7Z2:s7b@2kgi]4qQKpFm+AT22h1huT3$Hi(lU]Wi=C:] +rB'jFpGSF@2gu;8+Qq?`J)+Cmro66B@thCL@6(Q$guW,KPI@^-gf=&sD#_3ChE13BItc1\[^NWUEdmi- +?`s6[0N8i@>W=L!'uTB%rUeRARHrJ.O1t/6WDb9U/pi)IEo:/Hs7B+eoS[)S@T>@D_)\`/guW,KA%/\S +gpPG!4o1Z%GM[VCb@^B,hI0_P4$21AhL>@hkFR%'qb?hYBJo3AYtViD?.rrcr\u +iE,tpC'Ts(dB[t@Xe4/hf`AV[^NWQ\$mGAGjrj+YB4.'^Adup + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 33.71863] CT +[1 0 0 1 0 0] CT +N +-1284 -69.75 M +-1284 286.5 L +319.75 286.5 L +319.75 -69.75 L +cp +clip +GS +0 0 translate +320 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0VDeN2Es2XXaA;!JZ+VDU26&]7/YR=r48hIXuO^_l1)%fc!)A3VtLkuP>5Y=EWKTI)f.nW3FR+\+( +_&*b16R:-V(e]V)@"h&#`V"Ya&"_\K>j"/[Y.ZL7Hb4Yhkf.aNkNn_bFZRj>F"V[-;@3B,WN"!d#WGR] +=4`$-$S"m,_Uf;GSd5]q#!%K5GXB'r`?l0[rr]fUOOI!f0hgD=1@6'"g54K2iP +V+[/LWCj?j.$Uat2gaN6p\X[=Demd:YJ"!.S'N,Rk3lpM=#OmITG_jQ//F-nDT.JJ$PtE8)]R8%hgYGs +Vl]48Xj^3C4b!==k1m7R9BlB];]WG(RZ,=bpA>Jh`2UQak[4WCSCA7S)(28ck>gCf]*(1(Ren%S[:;]8qu>;se95em,1 +m!c"2/)9+BiQIQ@VbYbS;o^uEPHfdjrqU2=$Z@<_Rm6a<^!L]skSNnY^\OX3e-l6;O+'qZoUA)A7D7&d +rob0k6GN>k?doJ^k)Xsg0D(_uqF/*EDuLZehr3PQ`&E2j6UO6(c_%E0l6[i):7aR.3B:7"`Fcc@=bV=r +f@^$Krld?>_1Di5s"!*F_*Mqa'.6P>:V(f@@W-2RbSH]go08b/[sN`daGFMrVH$V +gJV1h#7hk\\o_Y-F0k"Vm^_N??Ph-d*BSF\gU:r,(G=2GKgJ'A\^AnI4*E/YZq.C(>.&,6hS#oeC7[NT +\3g@#]mB?1hg?*'H0!$Y4k1'XFeu&j)ep+3hKc#*R5]%j7n:U$?[WXui/K\PW+N!s]^nmc1k5N&c'pYI +S*cHD]ejk9!*']pm-3bkDGuW6QklX@naBfK@U-*i00:Q.DuLZh?gWW>SXlC%qWUKlIAApl]'HNg*Su%U +]6E_pMI(VnV;oUW%h&k[2H:<:,u=tP]QirK%m4gpd%Lt6pcslTEnI5]RmF1Dg9jR?b=>=e^&X#BoC^.F"i9*PE8+ZmAA2<;Jg@Zi_f6dGca.n?ahWlND+Z=d:='\6qhf%Kj +bP0nDYI!#'YM*3!]lh;O($5C+$oRr<8R6IJmF0qE2DHo!.5e0lANaA+AkdAgh/`.W^P;29IS)'4Q7Y`L +J$\nI;WW?p9kVbAM'dqW`>SqTWBbpT$G5H9+)JW]`%@Ak)`RlbRQ7\NkpL*G+6^^T%13Ni).2&6nDjkL+JNL3)'f[2ZdSk*tK2 +Adig&E`/*Em+;nE$J4P"IJ7#VmF0qE[_1L:0s1mpD7#9!JC/<7)SY++WB3K4FRf;)o,ShlXf\_ami1;Z +5DQj=WWMRXC.'H%H9*f:&hTp-]cqTaWo`H;=Y(>L*BSHAmC-0^\,8.jnP"[lL@g+eV`7,c31WD%'nqhg +5fQXnj7DH[f<56-iUlB`iFCru[:eCL[_,tl[V4&4/Ko*hPK]:Hq4lj#Cg^X9(TR#OV=W)3A]"a'Y4]Y8@%h*W:;X?eEc0(]&h+ +rHb/Ef[6l=qLosgHD+]udB=A_KcU,jg4lq +]6sHdq(.$`/t[Sq50KS1bNJII0783P48SV:^TaJ?0D#W6qXm1iYbPPYoF!bWog6EtUL,\H$Tn4X6GKb" +?SD$Z48SV:^TaJ?0D#W6qXm1iYbPPYCjF[<(O9H4_iDFO$X4ZqlpY75nuuUWo"p]J*IEnb[QNp/MnH:U +@E2^8Ke3fsp*)/+H.]A97tS7:+X;Y[]?W_NortH]],Q4RC^Y]WgL+a'Rh\/J#I^p7aGF +ZcnF+jT#8\zzz!!&C]cCI%@q=19k]5o#+k+'r7kigEhpYC%#pc?jGPq0WJp04`Z2(JRMe:uN9:$q]8O$D2@qF.jB]kDY8=KfC:qW +iY0p"-$5Q_&[&l$Mj/Vp]9#C3B47e\->(Wm)o_GTsQQ?[r#S)uSB%T;>moP-GgGF`hhNG=PG!7n8n!?T +p;'j>"j_1bg_Ym3rN7*og2_\gp#:H0CrWp?^HGT$f9lOk>RUqs6+%eKn<3XKHsDMH]]m@q/,Iq40TkG'%hRrU +neB2l3^]Don.gH>rTE=l44.2G8l7T@'s829"X"j(VpR][&og#_XNYAa%q55VO558N(Df>8iI!f0Tk]4mWFH%jEoB-D2^S +jWt?DNct]s;S655k$o)]RPAET>3,/-#D)&'$so,l34#l<^9WVbedpl`[^3f:G'Jc>.Y)AtW6W.bij1s4 +r#Ez!''itABo#@~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 0.07987] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 356.5 L +1603.75 356.5 L +1603.75 0.25 L +cp +clip +GS +0 0 translate +1284 70 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 70 0 0] + /Height 70 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W0l'_b+3/@CNC1ZNEU6%(*..m@%_=at/-I,Si8>^`JfPk">WRbPF`;@8To4`6GRsNJ0/60sP((-[ +F40.Bi]GqKgb^sN +eYeP\fIer^$[;4C^hS":/hd-$NmFr-m4?1%Mp?gW&!FNTjmFd2[>.igA.\_TXaJjt.66`+Q?ba1AY?nn'I:TfEGPAatq=;1rnd^j^&LE)u:.:BY&ig\h;pek+LAUT*cTcoGiosb +Dj2[5:c1g\a:I"XNp$5DF4?;l00(=Gb"TG5)8dL9DMC9VnE[i9BbnVh:'[Q-_+$P)9DKZuhoODdo\FK@ +aj.9P$R4e\+<(%$9O])i]'W.$$`Jjo66`< +O8dL9DMTDcqhom^#nscrA]QnSpJ

WJ",!tmk?a:;*mh7<(%$9O])iP(%n'LKfaMHgj/\aiTX'dV\0) +.&LE)u:.:BY&ig\h;c1<9]C=\6??a_R(GCf0O$A-n]Xtd;]c*O)*@@[.8dL9DMC9VnE[i92U4-a!iQec +BcZXJT:B(6fqtKP$*Zk#;K6N]QMC9VnE[mf:ON=DWS;Jr]kih6ZVbd7qn-SP;3>GCX8E<`<#pGF0&NLT +6jrGeH,"`,tS;J^<,]Y@YVkVbT_rOcb#pGF0&LE)u:.=5Ce=(TKS;J^<,]YB/KTO9SLn'?+o+$s_&LE) +u:.:BY&ig\h;c.>e@<*ilKTO9SL_<#uV]p.2C<\h6DDIW=r=[`c66`66`66`66`ik,qY+iWn$C1 +L-%nZ]7H@FtRU);NF3;&EH#),=QZ7e_ic7En+d#\83>Jr6(FJ=8'%t?kC_5@-i@`IS<11#SX[["]-N^f8dL9DMTDcq27kP&kEHT0KXB.mT5?OGE^U3WO])iP't%Wi-Kgms+q!S_mYZ2 +E=tW=0&/$u/-?NcJ+f2&LE)u:.=5Ce='[1S9%;Z+[R)pmBlQsbiOWn66`!u2M.%3WJI(%>-eU#F&"/=A4Z` +)?Pbk=Q!JX6*LYJ`N[Q%rE[i92U=S\JjIB.@1_"0uR_%"`pihM,i%EgQ5D],?N[Q%rE[i92U=S\JjIB. +@e:s3(WYbS3dV#30kJ;dl3MM1gaJjucMH[=oPB0JJ;%hdDr7C7@=.N@>`L3U,]Y@YVkVc/_W74 +c9&q.$pg`'a?Zl&18'%_pf9O<^J-B2<-m9jV.O(dbf'L97S5W%es'0g$ht+MWQ^R`=2(NZ,U4eWLWJI( +%>-eU#F&"13AeuM3s#pBn/MlSXn1;iGL_<#uV]p.2C<\A)c8h^hh(H>EbAZiB>\OeY&[0.7L_<#uV]p. +2C<\A)c8h^h'%4A[^N6Nk%U4f,E]KkION=DWS;LLV$^5X,.5;jk14.p?>CLp$MMnta;*mh7<('JMXkAE +M3N&*$$#,feDaX6U\X$QZa*s^tb2nEl66`^M!q%!P?%q#~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 0.07987] CT +[1 0 0 1 0 0] CT +N +-1284 0.25 M +-1284 356.5 L +319.75 356.5 L +319.75 0.25 L +cp +clip +GS +0 0 translate +320 70 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 70 0 0] + /Height 70 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V5tEJ"!!i"K4s&0_\u>s(N1#XT^r6kBqN,/.h\6jF#/O-#Ie+KdstF8c-k_mZn2mPo]/Am]I6NJNCHLkpkCLkpkCLkpkCLkpkCLkpkCLkr"7 +Iu*J96-Op?.e]nNkNLd +Cj?Z6?=Xk\4GdQ$Z3sm.`3C-3Ke6*[fSf+5Q7$\=7W<%>;TEcK=,HHsh)gt^]:^C_A)*DNA.,1R/3%*[&DlheY10cH$0/<]>hn^6Ce +>[CaFUN.fN?hGD2hqm5H56`J_-XNtm#8)`WX'K(>7m6]4dYc7,/<]5rrdJNo#TjiR91BBF*61*a1I?Qf +qYEleQVOGue_Aj5;B!oH?g09'hqHfLl00'))e4?=U^Qekp@a6A?].#tH$S)Q^s$Ds +PEV3O%SH\KIlne(O3c%om-a:N#KaieL_E+Kd*9gpZ,jLsS9B$t'oU'-qtj5/#R%@oDtj@H]Y*0mrV_02 +0Y^eHGKr*_?'jH,M[Ne_naS@-2`E\#^Tk&+9IBG3ouP3FV,BTV[9=Rj*1WId_Q4]QiG]WHhVI"%L.,u, +PF7f*^e5eN:JXd?hs4X-b&NlrkJQcJ45oPm%/U;Z?Ld>oJarL-oi:kpo$>X#CEFGc/$M#r^KBcmIl]_U +?!1=H:l$MC8&\pL@inaQp%.eNI!oaXI=7*'KNBh,eS5pd:&3>Rbb+2ies5:&iSe4pW>?_s.)LdVd;PH1 +cWB(;g`2nme'2NaPm[s-qTUO=+P%c?LLoW?oX+q/[S=[((MEe9DNT;_2PRrELt1q=F(4^Mt/-\QCVRb@Rfdr*!0>qI_/;fC&CiWOb+)Qr2o7 +9n.%85!G"N)NS:e7;=N%`lX-sMct2t[BA;aq/,L8RGXlMRU62emro.rj?XHBc;H[C[UdWdhl +d0g'Y>''MIY+5;F@X_P4c2!(HfiahqZ*IE2+`Y>+&8PVN:,:DAQnr6_MB>]a9I=cAg=i"hGP\&]-rLqD +`s[TQWQ:fSk1>FbD-<4g([G!nN*S'&IWs!(pD`Q\,n^Nff;f_H*))ssorKlrfk_Egie1DbAh(*bSG>$O +ITtt$r&+j3ge9&IJ2AgrLm_#9)uel6S]-):YccbQQs]NR?]p-=RI.#eQa(!9cGW`fM:`mNEjBMHY&&>/%\G~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1283 286 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +0.941 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.48 0 0 0.48055 0 207.31862] CT +[1 0 0 1 0 0] CT +N +0 -71 M +0 286.5 L +1603.75 286.5 L +1603.75 -71 L +cp +clip +GS +0 0 translate +1284 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Wd>Lueh$nd[EDf2p'U>O'&(N3G33+B.VaD,6P)K>0g`Yju +j('-l6BI"e,3#Ds1gP>[T'a)"0T/JI8Tg_F;Q't=`1pc]O[>.cP9T"^W&.eiFYbp_?'mXjgS6Ap:Q-Mr +#SM,T7%=sVV@0kHhc;i'.na9@%a%US(Z)JCN.c1=$:,k2MMRIjdWFGKbkmjt10LNmOVjkh^K$sMl,'3) +*0ERLg9\RVjD;7f>CF_idWFH:4h(&Zr[u2O*L*WZc$s^^g4a=Ub)sn-,)%/IVtV*@)q%arZ`#]d\3bPn +@f;583ZX7QCsDR"ap"cQ,ss]4&Ldj^0tgX9gVC74Qpmk9_NeeUJc:FI3Y:O!S+@R7%8tG$Ld]5p.0QV) +]iQT9^p\!F7A]7crL.:L\]`Oq-S;@$01WgrNUe/0VQ-!baUAnf!PXhS"AVaF:`#g`EcQOosi(E0V6N^tVj+b>M2&qZ;-1J>Vp*mcH?OB+P._tN$D4+ +;Ic_slr)=M.2jL<36Nkt\#-QuaZ:FD?'mo3N^tl5Te'.D-lJbcM%4'hhB+Z<6)d6,)%-sVTP;Z7?*G(ELNrafkgP& +f'*`.=*!m#%a8Ip0QG)E.0QV)O'T^OtRTJJ<^cQmtV:'K&@;`i67V]X1gVn!MBjL+5[uF'.-PGgKgIf>&3Z+`Z7% +$:/E#Q+%r8e/0VQVokV$>l45"YQ#qj&mM7AMsu8,D'4NT25\MV/k(b^U2d4"),IMc5i)RU,gL#NQML1; +_Na5lJJ<^cQn%:/'K&@;`i67VHst>Zge$sa*lR$gUrq*$XEU9j@VG2u,,TKo'm*5RKel9U.tMMAd^.j&F\fI\8j-n4m]39c[9XWZ3&'C<99/eA&0\JM4tE +d!LWe#!lutQ+%r8e/0VQVokV$I8,!rZ^64>kL?uAQPY@Vn%uBc[,+a"0F1QdWZ9noe/0VQVokV$h7Np& +%A&;tp%$m4r_%S[pjtU.]r-Zp&>[Dm.3]b0e%!FQ$kgF6dR3&Ha.HNN$rA +92T2pBUgPs`kDD_D4[>r/cSshTaT>P)H+KR#Z>04]G6R'`^nK:p%443r3bD]gcSA4]D?jA-N(QB%"*nL +=sYVlFb8,1.L3Es2+EJCBt;RbZR'hDqk%Y]+]DB$3]L6&:Rj%20pSPjed55aWZ9ele/0VQVt/1//?f!& +(HlBmq.l3I4IK$TX.C$Q1dPkMPuCnE)C%]2<9>'jnbpcqO/-2aNXcPr$Vr,!mA_O&1ej&M;ruMpU;faO +39<<`#JU5Kb(pYlH#6Nq:Y^DKHW:/TXrCA@MJ/>!25\NY/ObY]U2^N)j5'i#G+RV]l,!Z;m2"6<+rVau +NN$r)92T2pBUfid`b0]!F%pACN%.=&#LeA;,2+WK/O_KgO;_'Z]U#;38C;s(DJn=EHhY/@^,u+`1I'p+ +^]1jA"Ata<^.#_"CFsYe`tb*lk#GiIBTXDpVTN=!7?*G(U$,8*b0%lu:]0KZ^O#P9dBlD>IE#o9^6POL +jV!.\@0XoJ7Q$cL1fnAIAh5Ff5RaU@46cBc4nYG6#')YZV-/o,=V"spV6aa2;\p.pgE88)]KaP +=0>f=1@`[QH,^A2^,:Urb09P).(0Jl)Ns\[[0+2$o<\o6B;>4ng2&\%Suip]?I[V7n'SQ1g#dMAcCM-6 +If/`V]>+-9+8tEj\)3eV]MI78kP'$(G3OLQfOA&2iTYfc[j-lH\QAqRr)f=:X']` +OaHN,VuHFi*;,n6gWh^l]6E_hq]>"inI,7`Ya]@QJtAQYPm8`lS#\BhnP"Etm"/5.jXHo=Jjc+CWZ6dEM_c8+gW9.P[gleFOZp,RNP/>8Ppc\]aA#n[;-s8Rb,fR2nOB_ +q=(=8ZZKX8XA]nd<#:!GN$"cOf6r0%bUDpM_5(`F(GB*"=]sGW +miS)`HhPj^7oLJ1$PR0DZFDg)E,uiEVp'JrP9+)m\S*0Ue5)0^PG2A?pPIhmUhDKPS"'0?h4*8Q\*:cJ +=f4Zqn?]HE1jrA0E:83]R#qt+ihU80)Z<(nFAc),/khdX8sCs@*"*QkY7!-AJjc+C/^;#5!^E:Gn$m4s +U=+-liiH'FWiT1K27Uh*/A0m;rkU)1JoO0c5pB']MAj\dJu=g,dWApWXA_G(]QuHFb>0\%a1:r<15F5S97GU7XfO3NqTWg_**8@O&pT6g.Dt-&gZJ4.,?"1&NlTuQ)4p&B:Y2N(G8DuL7&>\ +EDi$7^n1n(:5VZmKo*-2M5mj]*N5gJNpj#$ba8YKEDdK-WqShU,"8?m_SfTT7(%;cCMBtp@@2fm&LU!T +S+9nLRP$SC+@W-k_SfTT6oB=RCLbDkp@?nGVU]n[`XsX&M)Zqf^ngO*drf9%Uo6+UQoODrW%)$%N#L>m +>o;]L8An*I>UI-#\u$uO&#?-\?'mV;Pc0nOGn',P/due6,iYW`5X#d-+=Sd7(FIEg-*$Kn?e)c\\9#58 +UEN1H8^S59&Nh9e.SU^+c8m8OnOm9?]:ZfY45ZsY_`!NG+\9OCdCsuPGV2lnRP%:(F\5VuX_KK]m^k([ +o+ND/UhC:2#[W#qQ)4p&B:T[crY;]p,KS]7lJKs#*0ERLLm+RR4qSb]7A]7c`*'!+%H>f_QXQWK&@9Aa:&N>jeq*]1!,=tanFCF11;kaJ[g!mJ29u>_Hm=er1hO#SM-08Wia6pLl60 +Wg582S97HR5VK0$^Fid`S(DsBA/p^uOGFi@MPe$&\u*;.A-5A"j`FqjXAbjnCfPJg*KA[)+\%QLCH=o@ +$XJUd;q(qUQoODrW.A8dq`1b>r2mB`)];/A&1$8(9@nd[EL!HC-\@ga +H&;ek(FIEg-#1fYi2%LJd`uC*G>?-i.0PL;$<-"!_9Jr570B:m'5*jK2=ugsZKq"#<;EU58#-\Aii5V5 +:BWVVWY>o*/khdX8qK?9Q'Z]6b<1ba5%V_G.0PL;8lF^`_+gsU70BCp'5*l!TZU`/BGh%6oQ)u"j+nD$ +6sRJURdseK&RQa9@ht?,$PR0DP$eD/BjGriN$D4+&okU\aEsTc`[iT6(FIEg-,Bkif#8/>"R#8>`XsX& +M7=k08.-IaZ`#]dQoODr'`SuFTjP#=iR3Z0;@!nT91tBS4CDbuBt?!f@'lje0rSN/SuZ#SM-2;WPO&4@!7o+F4o3u3-=B2mZ[A#GMdO[pV$=XS+K3m +@H+@e;Z%n5A%Laa*:'!;fR\u+;@!nT$`$<$Ae24]/rq&:\om>ilK[Z[=#Mur]=.V)]C/Mk,J#RKegW%; +hLub1gbGA)!uXV)&1$7I:QZ".,\Bp#jia"&[en`jT1sXIoCKQs:HNX[9]*It&8VScpDlNVHaF3.mWfTC +%V%S1dhtZO&1$7I8^[/G]S)iOEat\0nKNALhRt.(NkG2MRo:BgF^a9-iX2$lpU]k&kRimEN(m_#O[>.c +S!"sK">_:/BSqe/NUM)EoDa:5fk3g9*e!_3=4c$SVL0%_QoODr'`T8N;;Ms;GdsM2=&Tr+U_"g2p.^Yb +bZrAd.B[!j])/VQ]0#J@)`J6K5(*(jg$(s#YqA]Sl]`)Z"_iAY":O&o\Vkd&EC:.",&VU^<+$JO(/=>g +iPU_"g"bBG!c9(L=nkF47iiH'Vke`85(V-)_RW1_&5pC2.79VAWB$n8o>e.0=j/lp@]IlmJ +F\7OR^IS.j!lmIT'/bJ`6jc4(8-L>o=YuJqe^_Qm&"bt0o#B.mpu73'Imb)"c8m8jhUdHOH.+J)EKBcD +9jo$(6BG<:(/,pYBJ.o??f1UmK(SckDbp3S\hJ-T.4KAtD7X\>EC:."@U:N<-(%,tl,(AAVkh[nGKfe> +Bm)4k80LOJ:(*p=Z`#]dQoODr'YT@+N^tMf(Ilb`6K +N^tO''5*l!65P*:/j(fIL_PHZZZc?^jXfkSQ6P=aW&.eI39ueCH\8-[*Va72&YGI_ +N$D4+'"dNP_R"_9=GhouU=+-liiH'Vked5Ns/*u]RD:d+KconFr'R`bLc?h59c#/%3[McEmuc2rT%JB: +S='\B,>nfqaA(p6ZO^=j,#4"fdtNXb:._,e:\WA\E+UFNN^t`-h&TL]JRP%:(F\3!HGS[m[FN+nA"DK-5T*V#`MMRIjdWApWXA`"j%$JW>a0'bHk@P7b +L_PFtGU]6jSkb0!PG6KI@H+@e;X>JNYCSn'kq;>plt.6"6BHpOpCkBl8J,r?<15F5S97F2?$I^B']GUd +4n]N"EC:F*)OP/Eq(:1#R./aZ1QG!L8Ae:#'>`-h&TL]JRP%:(F\3!HGU?A8`q5>/V/2DQN^tqjh+m +7%=sVV@+&MSWZ;U51j\L@`i?C#2b>u,>neFF"//1;FI#hQ)4p&B:Y2P(S8`2-_YVu[b5K*b-rG&4!IFW +9%/KNZVF(H/P)/^g%6n9*dYRG[FoB)j_U"Dpn@QPNeimndWApWXA`$\_?[%",kQPPCU;]1;mq"^4!IFW +9%/KNZVF)c$Ql%UPL02$YK^[cD/U/0G=7r8Q)4p&B:Y4FJ07j@e$C$(55*YH8#-\AihVAi.4S%.-I+Xu +Ke@`QB4(q`&Ldj>3%6%HMFu?OM6frKTaOXPo#%)fEC:.",#4#OPG6KI@H+@e;Wu!HN;W0XAfQq"OGK)E +ihWM0?'mV;PS=?-C!;!Ebf;mXj+nD$7%=r'-Ujlr_SfTTUsOf+0[?GbPArtt5pB']Lm+R]FCF11;c;hr +'92_$k@Ycs6BI"e,3#Ds1gQJ.l]`*pn3j$7nG*qlU_"g2UkguZ.SU^+c8m8jX?db5+4Q!WAfQq"OGK)E +ihWM0?'mV;PS>nWBsb!]Cra.9j+nD$7%=r'-Ujlr_SfTTUsOhi2o7ol0TJ[I8An(3EDfc)/khdX8d\jK +Ij7):!98kLJjc+C&LU#Dl,(AAVkf^?o>79fL1<=%(upDdJjc+C&LU#Dl,(AAVkf\)8E4End[ +EDf2p'U>O''5*l!_X#)9(tHH70TJ[I8An(3EDfc)/khdX8d\jK+/:mL%)rl."Go)e,#4"fdtNXb:.[1t +H_a0[&BAjf8W4ll#SM,T7%=sVV@+&MSWZ;UhJ4BlRE@K5KcgsT7Dqbp`1oh`56(QPl&X5GM6frKTaUq!>@$8[]5+\9OC&TL]*`Q#p; +pUbNS=^PNcl(saGr9$:_5p![C*F"s8MaWrpc%6rqPML +T(q.1I^cVgdf&1+YJ-It9lsh'XA`"J&u;4E`h=GTH,^FF"Go)e,#4#mGkgde8%WSTDOrq6DVn7bSj.^R +dPoR-M6frKTaO(rGXo+T`XsX&M)Zq.:5\2^e_uKn/ml>?q!d81(Z![jr=N>&39ueC]'t1Xnt;;f3$O&! +&LU#c8Pt/e$aUi(.4KAtrtsf_/W4U:$:,k2MMRIj#[NhZYYIcp@#tTG*/AVeb@MZU6BI"e,3#Ds1gQJ. +l]`*A#AH`REKJ_"M2WZW#SM,T7%=sVV@+&MSW^kU_Z+7VLU2*B&Ldj>3%6%HMFu?OM6frKTaUmmJK<%a +)+UJ6,iYVU3%6=O(FIEg,t04QK5$@WE/\d)$:,k2MMRIjdWApWXA`#M4tf$Wr]$8Il&TgP$:,k2MMRIj +dWApWXA`$\_S\BqrYS/$&1$82Lc?h59c#/%3[I5e4jjFGOi'/9?5Z8/&1$82Lc?h59c#/%3[Mbt]C5e1 +T`W4G.0PJe.3_NE9%/KNZVF)c$TEP@'ekY=+\9OC&TL]JRP%:(F\3!H\G8LjJ8msa5pB']Lm+R]FCF11 +;c4kDWTb1t$YM/R80LOB8J,r?<15F5S97G]BTS\NP"kCm9oL;p#SM,T7%=sVV@+&MSWZ;UDKY,X@N:Ls +OGK)EihWM0?'mV;PS?.HO`%O'[_(^f0oedJ8An(3EDfc)/khdX8rB,][Fcc?iB5*l.0PJe.3_NE9%/KN +ZVF(HhUdH#hE&Ldj>3%6%HMFu?OM6frK1"%]0g^_r\ +3$O&!&LU#c8Ps350QGYm.nd[EDf2p'U>O''5*l!+tZ?&[EH#P[X,q*.0PJe.3_NE9%/KN +ZVF(H`\-&nXu=-V,)%-O-UksF;/*TqERlQfFCF0f,>nd[EDf2p'U>O''5*l!+u;clFNdeZZQ7$t.0PJe +.3_NE9%/KNZVF(H\h;dV::A/%,)%-O-UksF;/*TqERlQfFCF0f,>nd[EDf2p'U>O''5*l!+ur3e2pM3n +YX-8@.0PJe.3_NE9%/KNZVF)S+/G>eVBT9"EC:.",#4#OPG6KI@H+@e;N0paiQ?AJ=iM%&"Go)e,#4"f +dtNXb:.\=DFl@))VCl,.EC:.",#4#OPG6KI@H+@e;O$I`(&#meGH?54&Ldj>3%6%HMFu?OM6frKTaMNH +*0ERLLm'%BUo6+UQoODr'LdUQBBuX?Y6Mg&,)%-O-UksF;/*TqERlQf`mTPkgnb$+N^t.cP9T"^W&.eI39ueCV1MZN*Nj7/L;c+rKcgsT7DqbpBt?!G80LOB8J,r?<15F5S97G];0:d;hB6*iEC:.",#4#OPG6KI@H+@e;A9O;ojQgA:*8YQ"'QIrrGmr^^=^39ueC_sbI-m)G\98#-\AihVAi.9aO3Rl?BM5Q:Qo-Vg2AF6Ci5#OB7F +=f8&D)l+BuBFXhmFN+nA"Go)e,#4$XFnb=[Y23KpE++0CrS)8*II4GoNZ:(Mh07aNHFtk(S97G]JTp)0 +G@2DL,>nd[EDf2p'[>so='n+3k02+hs7NRF*BSH2dn`3!oQXZ/39ueC\hJ-D&Ldj>3%6%HMWV4Y^>="e +%mTu2p-58Z'5*l!U3MdrD54]3;A_p"&1$82Lc?iPbFL_=\o.Da(LLF[Gk':_;a:*":G,n^nEA8p.=^FS +q-ATMSN1bgmp0<,8dB'S)_Ln&n5pB']L`:Dm(?iUWIe[@'Bkt_Se#A=4h"#60ddT=YC+8MfD,l5pB']Lm+P^2E!J-hkg4mX]r9GDL;6Pl(.DBl)1.s?g2n, +Ui2t8'5*l!@^'-?&RH[AL_PG/S.lP_78mI)H?JsdQe0g>DVr1arSlP=CV([3jN*JTkKcs#-FtmNegW&6 +'VshL;dFX)N$D4+&ogf;S.n]fnDV9GG1k[+I.6&[f55H_]XdoJa,V1hKdk/*-,oG,eCTVq\o;r7&Ldj> +3%6%HMWXTkM_Dg?k*nHIrT9Sf]!ctIb*>PZf7f'f:.[1af7d5_jYDie+\9OC&TL]*YA;amFnPIf`JYZS +)\4#Sr5C=e +4BNCHc8m8j!5#HN]QD.*.0PJe.3_NE(:1EVeQ(PXe^XaDW='APSW[I861Uc>&/e!6&1$82Lc?h59c#/% +3[MaCK=%/7WWb&HEC:.",#4#OPG6KI@H+@e;Y9IiE7$`57H$EI8#-\AihVAi.4S%.-I+Xu7A!jF]rij% +4Y_sf+\9OC&TL]JRP%:(F\7N4JtrbbNc@Go5pB']Lm+R]FCF11;c<6$UdCnX$grjk6jc4(:5XjkU=+-l +iiH'V%&=8X7CZF=&Ldj>3%6%HMFu?OM6frKd9D]QgT)s^/)uF"Jjc+C&LU#Dl,(AAVkiN486H*s&al-. +6jc4(:5XjkU=+-liiH'VoGjZOHS,)HWL8`c6jc4(:5XjkU=+-liiH'Vo\Y?I2-(*fK5j9d5pB']Lm+R] +FCF11;c6TEL$.j0MJtG3,>nd[EDf2p'U>O''5*l!U6Zoj\hJQ5,t0XcOGK)EihWM0?'mV;PSCM.Bn9`o +#Zmde'[\m+,#4#OPG6KI@H+@e;Uc>T\[o"h7ouRR'[\m+,#4#OPG6KI@H+@e;H+@+4eEaD[c\_,!.%O? +MJ/D#&LU#c8Ps350QGYm.I%8gZYap/7Q'I'&^:q6ihVAi.4S%.-I+Xu7?'Uem^Xfd4).G$%2'rGUkguZ +.SU^+c8m7?NBu!H>>o$hBTO>K-UksF;/*TqERlQfWXb1-gk_t/6otO.3%6%HMFu?OM6frK+\9OC&Ldj> +3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%H +MFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?O +M6frK+\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM6frK ++\9OC&Ldj>3%6%HMFu?OM6frK+\9OC&Ldj>3%6%HMFu?OM3B,]#SM,T6jc4(:5XjkU=,Q?ihVAEJjc+C +O[>.cP9T"^W&/.S3%6&*+;#tiU_"g2UkguZ.a8hX:5VRc8An(3.0PJe.3_NE8oo`DP9Nm1&ogf;$:,k2 +MMRIjd[Xb*Ukf_:7%=sR6BI"e,3#Ds1hW18.3aHg&LU#C&1$82Lc?hebf7ZBh7ImGq<".K+6a1I-UjgB +,iYVU'S8`C'G2]]$i%U]rc%)JU$&%mq;ms@2e$Ee3%6&*+;#tiU_"g2UkguZRb)oT^"8g^CY,_+D/J+D +J,c@3?935oe3%6&* ++;#ti`(A@h?@VqHe#-"1r:^*`hs\kLlK[Z1a#h2W8J*BSM)Zq.KcgsT7DqdF]^FRcZa-n-a$9RnDh%Z= +X]r8p+$]bj?iPGpj.Dt$-$YkXO@#%bP9Nm1&ogf;gmD$IE#lZ3iPJ?.4aHVZ\)2Wr]aHLk(Vr!Y,)%-O +-UjgB,iYVU3%:;*@q4Qr?+.NCem!&54*KtcO$;k.hu!0:T/guTq!mBClEE'Xgd/UoM$+r]J'7)ff)?;\T;&fFD5;tjnU9QorDGB\"J0<&WKpt>Pmf+Is +^D+U$WQ-pu74f$LN2>?cUWqqo$?L,\";ZgXHZ0u*P/Dn;ckqtBD07BXl$keMcOs6WLHPl0FR+4$G,_3\8Yl)IJqo=^[9%gOX!B/"N.n/HgeX#8Ct"Ml`P$tn)&WZV3 +arJ3nT\6h&UVSU"qjgfl^ +E#CEhMitk^WTpJ%Fst/$HUkVo?1-Ak%t^jH_m`^]49$mKA"._$<&/ktZ9nPq,q=oXIu0jjBhn[r1#*?! +Z:^b3%huQ_J]]ICI81U/>-!>Gk8hIcm8G\gaE_1U$KEoksPPpu.!Vmg.0brUndS=g[eTpYUJ8Mq7;H*U +N+hn%V)1m+>\EnsAb\]3C4uDItgV7;fZbh77U)4+SK'r:02.rocj:HMd*=qq<>#i:?!Q?*6Bgm'FEMK_ +AZ`Gk%[rrTW%iRP)ec[P)B&I.9WAUf40drT#=j\T$MZ:ZSd2/k[]F.t@KW+92&D^NDK-a,_>Boril$pJ +0+dY2QOeYIFjT-hea/mPpUr-t8"H1K^.^[6B/BZZh]j-H/<9^fi9\` +557EddZ7B1J"(IJ),lf@JOLhA7*:^:sTp9/$8>c[XgNXY.g!DV_nS?.;WA5.smD5Q16'4mu$s=hOFY>K +;aElY]!I2E(!\^OPCiRH)>KX2E.V(FF#l(O!um@f,>*HG]q9DVVbLQ'\gicWZSbi:k"drJuI+j),ibo# +1!GpUc]UQ0oGqIJ$Ut]1+SohnHf&Z[[1KdG6/@lH8Re%s%[=_[kQkqU;)9eDjl_Q9?h>/N4')e:l_!BA +*::rq5U/pO'qETeim1(GBsRH0:`BqVDffFme%qkP(aj0Y7Q#*d!g\H1U1d0-32u9T#4\p-5SDGmWr+^\ +m1tDnX]Q0%$1P/]KA$$eVor]B.?2o:Q'\NAo_Hq5kb.c2t.R\^JH/pWn)>H#7G:DX;E>o&[cE%sPeP^> +AUed?5;QHu\n9VnVuh)aCp8#b]G9Pd*_og-Ep=g2*SiuC(5J^U-?ba>D>? +cLYL'V(PKlu?7HM$Fp+?FZ1eG]:=qq`ROZaR;crquRZLCW_L*9kd6s1IfkU*Z#[hOHMi2` +L3]]4X\:0E><]>?qD=?!ZiE7qgZNh2rd$9d7(%zzz!!!"Xb5VN/E!_L~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 207.31862] CT +[1 0 0 1 0 0] CT +N +-1284 -71 M +-1284 286.5 L +319.75 286.5 L +319.75 -71 L +cp +clip +GS +0 0 translate +320 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V>Gt6o+33oBoY6K`>e#k'2u +0^\m!X(OhU^9/ +\DOW*cc(eYa.;\V^\?s>A(e/!l+tW"n@n# +:?#^;H2[S^9Z!5UCXRDQPItVYVX3"%-rKS6Ci4-nm>VRJiVQ>ME[q'ikKTLJ;6\-fS2h7HY[PG(1Bldk +jCQG.Ek,GdLWD8X+0R\Rr-7#-HhVFS<%R^Jr>'_Nk"WL4d@s(O:1DDsX&mdIKdps>jd<+HB^rX]chP[L +G#[]"nDV8RP7WHZ+$P,Lo^(6]rLrjfK8/hR4cC_b9UV%pX`T[I4ErP,TmQP%s7q/NB$EqlP-!2%\cVWn +gD>(n.%*9lp?gU,rSG,.CYZF>nPeEN]mB>i2/6+%N0E^]dW\^Frl[#tq9_gLgbK)e;5)lX'%/S+`/GQi +i_Z40H_hgAIV[="r>(tfL*8p='HNu)1.c[fFp0484F$?^oATu\UeeUJfo]X]#:7[9KmZ)dWN6*h- +]Ut"$g3?8emCsW^Pom-,hsSiti;Dr7l!?GCqP[]`f<,6--krVPXA1q&o6E?]Zo +4Op2sO!lEJ]mp(]J,oa]W#=?ra'@^r/*aU45t773*>2_3Kl]"^cCFZdE?L9c7F#o(h7In"j?+T4p5748 +%2ut<=C#plh"9(R_1U2m]u!2ZCO=U30c"*932TbHbRfZ(H7/hGq,"qCu(Od=+*sGX:$M)2#"'rRTn6dP'l- +rRQLJ;((S:#5!g76,,:M?doDYXTABp:=*ngRXhtHgD^&MpZkiO5:6OjTO/N3KoUUfkSH;VFeN1+kgtN; +Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[I +UL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo`] +>ea!-gph@)T,[`Lj7`6(ka*6t^\:44gpme>S9kO?43a>J7GeNpf#mM!1AZ-e?g-_* +Kmo_j?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWP +I\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN; +Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[I +UL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo`] +Cqm+I9lde7HO-Q3c(3HTh[ITWb4-9'qK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t +(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R +3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(c +qK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;V +FeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kW7I:.t3[e0'=8Wl:uE`FjW,MZF+`NKoCIdkSH;VFeN1+kgtN; +Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[I +UL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j +?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPR>6f& +jMK,.a!u+a$Sr;6X2p!_LQ5Bq_TYBf.WSt[TjnhJ0_t+R:rm@K2%j!ih4-i`$]XG8WQBmgE(G:r#G5:^jK*De]_aD:JVsGloH1L%;OX$(): +,lk0QttmG#)X(G6[gFD.N"4ET/`Dr5a+g +_jtSSis]%^Frp+L^@FoA-r4/P)NBtDr/-3/sebLq=XqTtp'gU:sWCU'9`BC]9jhlcUqBB$jd4*Ks]* +mu^A;7)3UT0=8+itBfNu0V2o&\'prFs`X9`kV1N'!(A4n=UHorMgA?+P,,* +ZdIj[.#qm#[Y2j[:=<ZLB`hL(.W-rH:Ucs5V.\D[#k&h&GP'E +@82Aa,,+CqHK3Sj2\?*Yp+Vkk09B?6!TF17b>`(rr+psoe3=B\r,M.3O\;.[4AuhgY1BDc_!F6ba8n_4 +r=OXOQ[LlZE#Ah:J4>REr@k4g9`5'T"'L[TEb4>5-fV0lBQ]=z!1<]qK$Uc=~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 173.19932] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 357.5 L +1603.75 357.5 L +1603.75 0 L +cp +clip +GS +0 0 translate +1284 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0VY>/>J*6)EiL6*Mr\,p40!)ZUG_Lh,++IeJB<7GE>o`u2?!@6\Z'c>A2Q7N?1%eJl-H,%)qB:^J\ +G4uR#_rl6t2a]YJ!rr<$zzzzzzzzzzzzz['BuGCX8E +<`<:.;;<_W74c8dL9DMC9VnE[i92U=O-hEm?*[KLAR(PRnHf'p2ElCUPoWkEHTG,,ZVW#`5(/aJl*q>F +:q%8dQZ';\:=/6r$23<7I?g9[5YpKTO9SL_<#uV]p.2C<[M@kF>F:8E<`<#pGF0&NLT6AdafD:+7>3E[ +mf:ON=DWS;LLVKul?QXCPgNp.Is;<(%$9O])iP't%W]PB0K%C>Sr&5uS;PCeW&7eHS9)8ZSd/us6sU@e3D;pa8!FPt3>IZT;/ +Jr6$^5X,#Oeklfk +cIfhnnEY+j=aD8!FPt3>IZT;/FYZIgY7&p`=&iXKul?Q6e,lVf?M]C6SaFBal +A0i66`r3h.h66`nKaC&ig\h>N^n7+ae]d]s +G%dG^-aV)Xe'_`Uh6>qF@^i]Z(>.b]rEa/r=\\G`M^4(OU6-U?#BO&/$uR:g$T:X==rl*[=;:=o0Pre9im++J`o^i(/?+XD(X,,_/acHa_D\$lkX%MfT6Yq=2W?4Ng"dqNQ732 +a[D@pVk%c0H + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 173.19932] CT +[1 0 0 1 0 0] CT +N +-1284 0 M +-1284 357.5 L +319.75 357.5 L +319.75 0 L +cp +clip +GS +0 0 translate +320 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"/k;+.!_*68)B,eJlSb]NBssjD>ssidVMc]kP,+#3X(7A=-:sAe +bTo"5^>&,F3'FtD=e4`$4&=3"rRf*ZR63n`f&4-XGKFgHU+bUCn#U+j463n`f +&4-XGKFgHU+bW7Ca+TP9iVKO8Ts3X8J(TZa_3QJJ@4uX)>FtVF]uUq)H/]q>,$2#1M1=09>eu^Rra@I# +"^@SnLV:bDCjG"^p**SBjb\K>Md4)a-iRO-EWo[=pkQQ('l80T+8-m[YiU^kfSa"NQ7)Z?06.5#T(n)S +a;&N:iZIk<=,`)s\.4)FGr"(1rs/o?G'u?frH0+^+b-0"(=6D$''QM&oTt46qJ--1,p@j=>!RR/8>*^5 +csICukcY0^Q\5Ms182$`ZJl5oNoLCsZMsU@%FJ +qtF;u>*:5g]6CH\XY&2jk3(pTSkD^L5o&TJ8A,1eNfH.]3eE/'+b%2)'6 +Bn?7e5^rk]K`:8`hro'fd;\+5rpt[.qXWak2fIR6k2tgej>8\M*$-.nP"1MRaiT^/P,t`9jF$Xce`l=t +9#=R9$O]*\4o+pLaA>on=GI#(6W]dkT`aGbbD5C]]VYu8T0K7efPrT"`e^D95o +?hEOHlKYpj\Z)R-==dKP<-3SZ?:A8,9*MuNugI$]niHa0rWU,Yq\Z!eC9`0Ag#G6 +[t"NmAnLNDgYCRPIb]qj#o,UKo#.71"U4k4+S,Lu2\?hlm-NYZj[[?`%@DqP$_s<)C4C_?AWoI=e?<5o +jA/m:&(u==3O=JqZOQP\;<P?8g')(`RT1mPi5^3r?]1rM+\c<-@TH#r@&8?#6V,Qt2cZ!t\'4@t^e +C[`.f_HEt>EW#b1F",6To[e]9qlA$m6"t&4B4s)CIT8p:Imq'&D+s0b_Ka`2>E[9]*tmEX;$dE-RVj#_!OC:/Vq'SkiD$TK7a8Je.3Wb,d&[MH[:!;2@QQ% +N:cd];L]hq%PS8j8ZCQ2$fg:-(hZ\Xsj +$O[Id4>?PWEH.,1Rg\Jl]=[b-L>);CS!of8-O+&0"e@.n.e<3&eV^c0UE_EF&+akd7R#'e.bk`"6.BL6'A@SYY^sM)Z7R.:fon>6@ft^bu8W3 +L^]lY1`L2]K>]!*9UBS*.K+B)$A"cu[CWqQ4rr4,a"p"Ed+!d?c?sJGmA=_>3:tgSB`"i\1B78a!X9kd +Z+n&'e&#PEqYIeLq<'VN7kLZ4s*Df[;[)j)[aM&,`MKEV,$1O+:g>fZ8kB`!/.pQ)^,*Ok/S*S]"_rbOm-rOqOof@c:H>>&5XIb)lMf$Ib3k\ku> +gL'pBL\eHd5s]uPTYCMj3W>lXqJ/B[7WZ;#WbKJ(TL8;Gr)U9:2 +^?2QV^1M"jCMIUG,es@:"R4Tg^7(,t`o/@T%aq]^X?NmDDes3&]qDlt^V9e1nU3M~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1283 285 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +0.941 GC +N +0 0 1284 286 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 286 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 286 re +f +GR +GS +[0.48 0 0 0.48055 0 381.51931] CT +[1 0 0 1 0 0] CT +N +0 -71 M +0 285.25 L +1603.75 285.25 L +1603.75 -71 L +cp +clip +GS +0 0 translate +1284 286 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 286 0 0] + /Height 286 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WcYHL(s8V'TY7VY7&eXCQ$S,Mm:dg13RtV+SQn4-A]$a;%=D/;h%02jcJjD^-^P<;imkhdQnp6kNS=/\HRCSBhL[]O1kLoe0pi.tRgUD)srdX-Df<8P"pJ[_(#V#1+ +s(M-?Bc@eL6:f%oLc?-7bn_YO3@0B@K)X5nh9Vp7<(%':&Z6i4<)bb[QoO8n'LLN]s/a];B@Jgh,)),7 +;\="!.[r"`S9%:0+mmSO+`%NROH>8s&TK9MRk7=(F%Qc_m=GYWL5JMF,iVckS;PI$$PV]nP!V]$AI,K; +,#S]'3#*%'U9`Ih`&X`k&COps;+VCUri`&l8>F7D5PS?H!AIIse +MF\m9i`&=9MTTN='BbmK6:ah*n?/aH'LF!=7Du1(Cpl$;c7(2qe6iPPW/)-S,#1SFWMjXA0QGAe.>7?@ +&.VVN.>1*ZMMP2.fkb*VS5W;kl(]gMVBSnDM)Z4^VkUKM_SeI4U_&\4*2auY#`1MU&Z;A/fttZfVkiPH +O::(^4^J_'KLD>fLf71(Cgl=n;c<748@EJp*j:AN66]/n`2T]clK[Zigt^YnDa/O/b0!=rjm1O%5C'(W +9Up/rMI$e7&C/_I'p2$X`2SPP'f?^JMH[-jmr.C8T3kc*g?ctSIemdYrr%g,:S0haocghuM/uE`dBau6 +_MNTI,'"!H3#*%'U5#YHWFE50:/9C+onA*Rrr2nZOc^*Vr9^uNMCWW%]NJM8m+J^Ta,Z]QH#ei;C^Up> +ScCJabH2I=&il^H1T5XFpD.2Fo +;c<7"8;;)dhQ2NT66]/n`2T^)iPUF[Ytdtd1`ok'=XbC0+!4j>nV%>ecC?oDI.P@;)o\)#Mi4(,Oc'pXrDrE;I=4iQ!rgOW?H7^D7'J;bVS/'ZtU/m[2GIt.(Q83_FmBT%(M +$PV]nP+iaZ15*j%$PfoOKLD>fLf70Oe^`4Gq>'.[roB>Ks#-\X>F7D5PSCeBOtBqI?DrZC+=,rG7%B.W +Ad]:8Sh5hB)Uh@L<>.O%VVA[@)IF&#=:3#*%'U9`Ih`&X`kkjFOKiP!Xp +'sVD)7%B-j;c24a@H*eU;KQnLL@Cf!ielA]66]/n`2T\O[P2^H.4Mhf,[5N>FE'b9,p4S#k,%$XGFP; +,iVckS;PI$$PV]nP+qh?15,C.pJjUc#`1MU&Z;A/fttZfVkiOjODNmhf6nC](.'3?7Du1(Cpl$;c7(3R +/0mTm/\8#-g-nQNWJEMKQ-C'kc8d1>Q\ArX[mcW`7(W_]3#*%'U9`Ih`&X`kkbaFMiHFI_MJ+4X&Z6i4 +<)bb[QoO8n'\sId/_hu7<"Wsm,iVckS;PI$$PV]nP+l_Y15+D\CFZ'lKLD>fLf71(Cgl=n;c7^38@EIV +F\P<3Q]R#%&TK9MRk7=(F%V>X#g!WqYO8?F[M'9n.O%VVA[@)IF&#9Up/rMCo1V(2g`:W/+1k,iVckS;PI$$PV]nP+q4b)8_r;l4q\i +KLD>fLf71(Cgl=n;c6RU,X](/<4X87LQIF7D5PS@++aUmks?dISkEH[G$crtJ5aAC+c6O[p<(*76)N&fEN7Q+maa1Y7YcpqTgl[_]%Bl:kRk(YSHM-Ro +o(&8/DpFn8f@SW``M\l9@H*eU;H,7o>IXc8Xs,ojOGHgIE@P]_ET4p`cqs>Y4*U*,)tq7/([J7eHhQgA +^d.M9^]+)qnDV9kZa6.7B3U`_kEH\2$HWkI9\$/!N[5t1<(*76$6MHZ-l2-e`'j3ZQ'IWoOn%u32-2C` +0,K],]6A2jn(\[*c8d1>[6h0Q4&`O:U14Ld3#*%'U;G-u9X.\A4F#6Li5($N042H+iK]HG59:gc4aHVZ +(LMQ0p$1'Z^9dUphck?'Y/A!`$k0N?.F8MR'N46.DYJ52?Rf"eSP2SmWJJ&jY,F?^Vl-GN"hNhd@.j3c +?+P,lc*4m*mp:"qi8ENp?[Xdd0lcU7[GnL@Vb_7$e"hCuR(2J]RhZej'Tu6!GH;-O6miW\E@N/-73?`Z +L%!m3>?lI9baIVgSXlHY/$WfZM=%$$oM)Z4^VkUKM_SeI4V!-m$%,n5MjGKlI+VCUri`&l8>F7D5PSBZ. +OtBoRSQlaA+sc/I7%B.WAd]:8S(f;6MMP2.fkb*VS5W=-Ce2!RKlh-"E(+nK79#V8 +Lf4n*.Opo>9Up/rMTuOM&?`1]WJF.[&ofBF:.8a#Ko'kG8^TOAftu:i3>GD*66]/n`2T\O[P2^H.4I;' +,X])i3i!t":.W=&Lc?-7bn_YO3@.+LKObHl>)-'FeHq'l'bR@=qkQ4'p:.<+# +W,/mrER$!^3[#bQ\EZ,K,.`;dLf4n*.Opo>9Up/rMRJ:N#^:j.=%%6D&ofBF:.8a#Ko'kG8^[6nCuI"n +p58&D66]/n`2T\O[P2^H.4NsiOs/U$gl=oK,iVckS;PI$$PV]nP/7t]cL$E%66]/n`2T\O[P2^H.4Nsf +Os/UdX5-hK";EQ]u5/-"Kom6S\R57pI>U_;0L"QRR.4OsG,X](G\K-f!8AqK`3#*0?AnHq>qV]2Becu^, +4F$\mjUJC8*?G+Lb:hbfJ%s5[]MmG@hZa8hgAM`L#(%4clPA?`s&8WAgCYlf7%B-jlfF"mF*2eT\lbWE +,&Z5@I=6O1/mVYqD4`q9;I'.pn`/WT4tk.:\F.+7O^ZR&nk%dmk&s-"+VCUri`,Pi-X1h@H26*tA&&$> +CZA,nR(_IKP/7t]cL$E%66]/n`2T[h2`J4jYEAhS`udPGr:Apjl+d2d_[j.YXn]f_Vb]6ukI*'ac7(49 +-6tsg]=ArE6:f%oLc?,tr9#G=h7@`Ebp0>T9rh]9BP>fY^ig`qFfYYS;*K`ZZ`2SPP'fDLB +F_Z/G\G>9!^JeihF`jP"f\"i-!sa]J'T[2+So%oqS5W>P5tH`Lf5j"tKoh*h&TK:8R$sHn7Vf8Glh1+< +-VsU9qjZ&ZG'8%>A7T+W@K#*SrPIn1kEH[h0QCu#U_"/Y.O%VV9r"JY[9B^&2:$o-K1ej1NQ3Co-]:Kl +Vl//H'jI'=iLH(*m#VY7?Joup$6^"Z,2uSOC3=ZAIX]#AJlC==`&X`kY_@pT4!KB?Lf4n*.Opo>9Up/r +MGcesKR8C,&LiB$S;N,$;;#boig`qFfYYS;*K`ZZ`2SPP'p[#Z-I+Ut7YN^t<=i`&=9MTTN='BbmKU83kZiL]%P6jb(LV]t,"<&RsJ39QM?[P2]=8#-\/E@N/-7:ifZ +M/uE`;+=*2E3F7C,)),7;\="!.[r"`S9%:0>F7CZ,>nf(3#*%'U9`Ih`&X`k8.WfRmQk_\U_"/Y.O%VV +A[@)IF&#;8$PV]H6jb(LV]t,"<&RsJ39QM?\KMj-]ufYt'LF!=7Du1(Cpl$;c7(2Y@H-(&80K8<<(*76 +b%;"pkF@\Z0dK,rB=T%F6:f%oLc?-7bn_YO3@.*p8;;(:F#tS"OGHgIE@NH-/PM[U8d\di34C,1#`1MU +&Z;A/fttZfVkciiaUml[DF^K6&ofBF:.8a#Ko'kG8RVu?kB96#+VCUri`&l8>F7D5PSEYL$t7qO80K8< +<(*76b%;"pkF@^P(+.>oL_N0":.<+#W,/mrER$!^Jfu#KHuqFoM3*Nl&TK9MRk7=(F%QdDQoMQ";@&+= +'bR@F7D5PSC_>aUk>km>)HC,#1SFWMjXA0QGAe.;WGPgl=oK +,iVckS;PI$$PV]nP%&VY)8`b$4!KB?Lf4n*.Opo>9Up/r$nf(3#*%'U5#YHWFE50:/9C+onA*Rrr2nZOc^*Vr9h2c%NRTPs8MuVb/M?=F6Chsp=X)^?G5(Z4kmil +3N&[WKo'k4,)),7;\="!C,g4m`6ck-o?B@@mM$W6`JYOJ/mDI]ftW#dlNchEgUG.e^]!l\B(+Cg-I+Ut +`V)YSK\;==&LiB$S;N,$;;oEJ&&i"KR$_PN;Jb4LOn%u32-2C^/`"*eGB%W-q8hjdC2r@CCY(IkkI*'a +c7(3N)3Q/e(>Op@+XH&H7%B,A1G^i+@X[b(60j..%j&B2q=5p5mb$/onTQ'=I(7i5qf_Bgr7f$&5eEWi +h-c]q[;Os/T+[FLG!lZ\&Z6i4HF]d4>b.IX/f&-*Lg)fS?XM]e9:*(._XmBj`FYNeB5qu#Ir\B'eunmg +(aQ_SeI4V"eIkL+o7\G!lZ\&Z6i4<)f6S +S(m2Z8Dh.0qnRabK7ee\&)iSX0H$BZ;V8k)\K-f!8AqK`3#-l>k9jis\ohf5,HX1)X4?FtB^riD\T27C +n(\[*c8d2iefbpJCkpaW.>1*ZMMP2._l6S4L51PRDf581PmZR3j,G.![E\G^$PV]nP%'?2$t6Tdm>)HC +,#1SFWMj6?C8r,Q@q0$AHMtsL-iO#Bc"T:%4$,2QZEUQ0r-<:%n\B\Xa,_=WrGR`=5Mk`igVUlhVkgPN +M/ta&Koh*h&TK:8)BBj#eS:*$C,@n0\?rWmG^OOPZ?To+HFinCX]r7mr%1HDHtT]99Up/rMTnICK\8Pu +,>nf(3#*%'U/n=qX7PrSbnZbc(+.%m>)HC,#1SFWMjXA0QGAe.Fs/_#dXnV8#-\/E@N/-7:ifZM/uE`@1*b3G!lZ\&Z6i4 +<)bb[QoO8n'WiMr_X*<;&LiB$S;N,$;;#boig`qF1da$Ofep5`;@&+='bR@F7D5PS?PWL+ko28#-\/E@N/-7:ifZM/uE`@1*b3G!lZ\&Z6i4<)bb[QoO8n'S\;]DF^K6 +&ofBF:.8a#Ko'kGM)0fViH@Tr+VCUri`&l8>F7D5PTW>\/8dtR&/&t4,#1R9Up1H0!,*$BH[tc +,#S]'3#*%'U9`Ih`&Xa^e:uj*/0s4S6jb(LV]t,"<&RsJ39QMHg$FK*$&LVVO[;UWWJEMKQ-C'kc8fJ5 +[eQ!466]/nU_"/Y.O%VVA[@)IF&(DcXabb^OGHgI.>1*ZMMP2.fkb*VS5ZlVPJ9Ui8AqK`'LF!=7Du1( +Cpl$;c7,CXaW^RZ87YO/.>1*ZMMP2.fkb*VS5ZlcT7>VT8g+eE,#S]'3#*%'U9`Ih`&X`3B)=sjUQEQi +,#S]'3#*%'U9`Ih`&X`spRcRlq,`5;S;Ki\,iVckS;PI$$PV]ndVA^fhTc]3-mURHL_N0":.<+#W,/mr +ER$"-]3E)\66]/nU_"/Y.O%VVA[@)IF&(EXF29pV_B4J""r37e8AqK`3#1i+lK[Zigt^YnDa/ND/PM[U +g<3ApE.a7.;\:I37%B-j;pd33+1,4R[dS.tc_"QJ>2%tF>F7D5Zj1G+JTnST)h^VLMF\m9i`&=9MKW`L +C9!Z1PEV4'H@941Tte?lA7T5/lYU(iSC8Oa`Vti-iK$;LK8N2C,iVckS;MLT`/,-p6UT*Wn(bHPXPF[G +?>Oeu/PM[Ug1@_!GnVQEPE:T!&LiB$S;N,$d>iVLn=eLC',)&(7oiYkMFY\,a#ElG]6M^2d$/b[F2KA# +7ALJ-E@N/-76<7\1K,j^#7hl_5'ZST_98/uDt[8drAkjqV!B1*ZMMP3YNnD36>l",oNOe!]V]o"EM)Z4^VkP3"S"&0s +f1>el=i<=uU_"/Y.O'S.Lf4n*.b^$):Wg7kldhLp>(>c7(2q7%B-*+XH&H7%B-XlI;e3_o'C: +2uQJtp4.3V$6^"Z,#S]'3#*%'U7IaARO[Ief[s<>?@VP,8Ul5>lSMqo(+.2%tF>F7D5PS?G.:.:ri&ofBF:.;e$]"0pe5JQjuDQnhOF&#:M,iVck$6^"Z,2uR$ +2HuP.kEHZI,#1S&&/&t4,#1RF7AbO[;UWWJD6T,#1SFWMjVs_SiF0 +'LF!=7ALJ-E@N/-7:l'&-;I"*&/&t4,)),7;\="!.[uJQE@N/dOGHgI.>1*ZMMP2.fmJI0;\:I37%B-* ++XH&H7%B.Wjs*QiMF\m9i`&;Q+VCUri`&l8+(=/.6jb(LV]o"EM)Z4^VkUKm#SX*[;@&+='p1gR`2SPP +'p[$6QoGhC6:f%oL_N0":.<+#W,+Z7`2SP,66]/nU_"/Y.O%VVAb-K\:.:ri&ofBFKoh*h&TK9MRu]7C +.O'S.Lf4m7KLD>fLf71(I"[>T&LiB$S;Ki\,iVckS;PJO,>=Vb80K8<<(%':&Z6i4<)bat@H(6S$6^"Z +,#S]'3#*%'U9a.g!!%P0dI*8.zzzzzzzzzzzr-tcJX)i?'Dr89GpCj7;<`W7YMOm%^hX2aX2cs6eHM$D +u]:T7,EjHu0s!K2Zpi3!]R'89d%NhAs8IB2K3iT),O6lNOult>R\BLI:/4R8lh,scL#Q[]aN-oaT+i%3h07`k8+ll +[qaEr3Mi6cDSbqc[c\h>KmcR#='mHJ5^17VX$cT/u,+bi:Y:_I]^NoFJ\/k88p%]:148%?H^D.pJ]Qpj +YGO=(iB?-#Q4*gC-]:Te&;)D4aU+R'n>qoZ+ZQgDij<]#]'.6N`%1P2P]Q<72nM,O`i^FfY'%O:nZEeu +[rp/1>2]jJTFqtf-0-o9HXJi:g04,LTAT2V8(1nB.Sio.h0[P4=YJ:&tI/*3tltdW5&lhodJr1A4br9R +u^0UZKjsq)?UuFhjJ,fE'inprC^A;ThMXck!D;)'80=fP[q!b!P!)?3]1`n/1aMfCaj5Y!(<`I8jdaHs +`G8D&3RuaYRXqhaX]6:<9c\dU[fB[(pS@N\%8oYZQL"WHejlPLaF77u!G!?Qgn%sbd`P&5Gk)C<=MR6R +eV1T6Umsk@!I/*42rSRViIqW+B??3B#RoSMqGIj31RGn>Di5(&s#7hk?pda`Aj2XSZ-Vp?3ieoIm?G5Vco%8YjV-:"po?TZ$n])`jMpEVXlh!Sd.HsPnGundF%,s%qCZ'be?5];e]lFd(CBmQ(j=l[P:)>5;uG; +aAf'mk]2p773&p]`='nZq9O,2QXgfCB5(2-\P@bW6?G3r=+30EaJ+Mes]:T[8Z=_(#aiuGS]nO/\NcQn +rp$gqX6\c/B4aZo$qtKK,/l^'co('%qa+"![f>a;g0Y;;umbQkE[52I@1ro(Om-L1E98)]BGk'eP_hX/ +:TIVNCc^m9=(`55QjG5h?p=[=$D6\f.jA++tH(2NUK+te5h_)pli8C8,aY*Mdj,]g^rcrKFRf;%P5Q"? +YeVZRRIJU>,:QA*LRW5Y!UoBlq"T)r:$^,MLRhUS?!V/!2E#a:^k(JU`T-?7C/i=&UISP +'H0Y(gf7$UJ44X+e.Q;8#BKi`7]Nj6SY$Sfq`b.`JXC!\gEjJ^1/+Sib?/l`N(G@l9p%jk":\stc>L+a +Gdc07qnI"7Mn`.+YXmm'd@!@"50Y;Dtk0;m-[F*CLQ'IV9Y?lYUp".0tC[^,nC\RFp@P1oqd.i(Kimgo +rceQmoctAu!p@@=gG!@W0A,V;TP+/"G;j!F%,q3.Ve7uD2QQltM%f5i3m)Z;5[l404DY90^m9 + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 381.51931] CT +[1 0 0 1 0 0] CT +N +-1284 -71 M +-1284 285.25 L +319.75 285.25 L +319.75 -71 L +cp +clip +GS +0 0 translate +320 286 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 286 0 0] + /Height 286 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V^M@-:"39r[_+@n%r!,"@U7n;j#Ciir<(,dk%dIG7'a-fjE)I7@E#1ge5uB%2"VhUa8o[[H&8Dpo +M'=-t#V$TW^(1\1;"EXUr$6`3_s91EHd9/^IId#UT6!f$=^>Uj9W-A5&@QcGd%U.Ep<_79dcHg&Ie1fC +^]!l\kKfbBHes2Fh=3(X5KE)0keRKG>^PY!='k^dUQ8aX[F\c8rEXX'[=7cIqsRlVoZ8JQXIWnqq)8[o +HXU>+q/ZHlrDpT1qWm/\I/j0>fCqE%giM6CHdb\JXhX8$T7/hKq!`P"qU$nlSULL^oK/uC>B0N3l1#3( +5CS"AU/tZHjbbRk(jk+cl`J]3rJpi;5Q:GMJ,]9BpYV$9l;^nk[Cj/bIWWb^B:jZ0rQZkj[QO)u'8QU+ +EH1NTcT\u-ZHJhclD`<.rSE##mGeY20"Uc1CVTlFlh-HXfC/(tDl2DPV;J#^rq^Ec.+b%d\$KE)2rB!= +Sn`KJr+kpdVt@UID7ILO&cL!(rU+b'AHr,$F!jbT4EKZ/?N2VRSQ#Ar%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EF +Di9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>oL^JfkP",^^hcsD?XikK +d4[NY[cj'JITuG;:;8CW-)]W9rK>WJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?- +I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs;D:(!srO1pVS:4]7 +92<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>WJMgie4[me'mo\6*I +E]`Z4atB)U-LDt\mk!h\*aKYNDWJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q` +^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs; +D:(!srO1pVS:4]792<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>WJ +Mgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.\'of(m#oJ$dN/JecGIlod?T:9VXD4;o\6*IEr'Y# +000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.n +am$9'^Z>%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG; +:;8CW-)]W9rK>WJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIo,g@(83 +17:hi6X\eSfXX/V1,.J4rKin#>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro +/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![ +d4[NY[cj'JITuG;:;8CW-)]W9rK>WJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?- +I`^i`7DIpUgS/%qqJFV53W6DMQeY@5S[(m8P=^*,loPS:[msL(At-C3'%:cWI`^i`7DIpUgS/%qqJFV5 +3W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.nam$:Rb`fAj>0K)hX/O"]PI$-MbWIndW5ZuEZ\hr14?9I@`QXOP +MH\4d3NK*tb`fAj>0K)hX/O"]PI$-MbWIndW5ZuEZ\hr14?9I@`QXP#d44u/^DnIGU^]P +*PGMRDrIEZ3!/mPpUieoIM2XeBWmOe>7m^qr-KKo+U6i(@U1KMo:pYJ!"9\).Ppu-uQ7uj*'IC_/W"6f +[Yo4'skHKh9r@WT(\mt>i0>882l.E7N\))Gsp`R8_]NcKD_6gc10>IF*D=Y__rUNa#=@M8q4#Y-sfD&VLg2! +lTs8;KK=mXEqn+9QB?+9WncY^/Ip;.1jXDR6hIq$;FHKUiC`o_LK>7Ym>q2RV5jk^gbV@>?]bVRT#E5N +,,IItq-(Hq`hdoTn)oltKMm+Ledg!;RXU(9KdE,<8ak*q^OBAA(\HgJ#WkROm-1]Qs^%r_:0"O0%bCfj +-`If&Nb4E]r=V=-EbjG6gClScubhgbZSi=[fYr:9X\4C4!Fh/T`mD>l!,/[sMUr:ogTc(BLbH+[QJN\s +:MODXgbkB3AGq'S)(U@I2floB(F`45Rq/M5e&hgJ"U*rYK8hhm%KORu1`b>gA7cf3sDg@Nq$A&llc44b +Aln1UlEXGgtFr:.i$5QCcazzzzzz!!$EPr""0p]0Z~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 347.4] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 356.25 L +1603.75 356.25 L +1603.75 0 L +cp +clip +GS +0 0 translate +1284 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W:InH#(r0RBLkp$!W^TH$&JYn?85r!aEtL]($]0[Y'.mAk&J;-sB][BrZk/'0+V>;%/L3ZWKsViS +[#g(L60&e26hf@GGF4k3U[Rs%>kE;B1WRg_L6(4X?cG;pIK0?Jzzzzzzzzzzzzfpt?W^9+MSI!iTCobT +5c.+Z.\q(h9:!WW4NI*BG-!!$glAmkZ>!7mu][JJKN`IhM]oXe7NIX]"5^[tLuZZ'om\[f8IkA94j!!! +"\ah)ccH@'utD;*M5;t8eiR5<=@e$TAPZga51=13&)rpf$h'.6P>laL`cG5M"#q@f1$n=rL7/mc1mT!l +>G]f>2A04.b&>lXj)aC0+0EqAG<`HZai`f8s9iIQQ2mdBLP1-KmO?u8X:/6pK2]6I+Z +os;]N>iku)AH\Yc-sUU.;uk09Cin)*$;It)I6(nXlP>lXj)aPldUdn>2iH2$a_`PKlm5F504cGEjm?*g +lDj[U+'(QTOr;B1)]p2,)N>]\i=J[r>%o#URohfnMD=WFNV2;H8q!<<*BLU"+hiSia&%[P$$X&lLiT@V +?*6UO66lUOH8Mh>*7?7hd%];Dd8YPo&I20)t&!!(*<>-1On9GA`=]mKM!SNGHNd8<-;/tI\\IJQqCl#U +oe`PLEfhXYBkk5"W3okQ0e^0DY*p?YrBf*)-SJ/i05B);E%COa."]tM+k]=YDUF6-PLIX]$1,fS6li1< +)Yb,Oe/)5/VX6a^S6)Bg9n2XDV_n3FmIU!4*D;CT0@[,/mC@EQTs=h3]d-4n`-B7Ntt +Ogqtk0,rSoHai,o&#!$I'kn)'K3e[JM:W$P=Vq,=J;qQ"$ig:C7T69c`lH +,4B4kHI63$ucz!.a]b!rr>:s$kILiT=C$F\G4L+XH&H6kVg1:.<+#-oX4]S;Kj(8AqK`,_SRUMMP2.DK +_C,'p402Lf4mW#`1MU&Z;AYlnEE/+pfD)V]q98&ofBF:.;_H@H(6S&jdd;73j$%i`&=9MK6&@`2SPf84#kcWJEMKj^s:);\;S,M)Z64KTM!g&TK;CbmkDb,*EKME@N.=+VCUri`)/3*@=NM,U"Gm<(%?c,# +1SFW@0\e9Upu26:f%oLa7XAS;N,$;/UQE3#.R/OGHgI8I1/4'bR@6kVg1:.6EO,iVckS;M?n_o/O1,_SRUMF^')`2SPP($0%_M,P!X#`1MUObAgP;\="!b0:D0V]q98&o +fBF#pBnY,2uRdRiNhnE,`&307id]8AqK`,_SRUMMP2.DJ!>:gnuD@,iVck&jdd;7Du/R2mY2XmSf8[&o +fBF#pBnY,2uRdRou+M)Z64KTM!g&TK:XR$sK?qXs0sCWsZ6\PGd`8AqK`,_SRUMMP3YRS?RR+$ ++N-`JYN_0/EtLk<:0u+VCUrOq87G.O%VVCGRh*FmRbc@a6h=dff);Oq87G.O#0B&Z6i4<4#g:$O[%="8 +hfZ4+$\&=d\>jT9=khLf4mW#`1MU&Z;ARR[KR(I/*3Xcqs8J/51,p:J!lDs%dJ3!6qp[Dd!nl(;>4op> +b=4gu'`r>(cF&3J)(2,prD*#i>V.mdkl^6c6EK9[7%(1ec>dK.erCj=dZ'@_N9G4R] +4hEiW&t-/,4OQ=dZ'@_N9G4R\9I`N1*F/,U"Gm<(*76f$#[=e$r1F6:b7]^Thuc.O#0B&Z6i4<4$7L@@ +G2a8eOJ5ADPSHoi+ + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 347.4] CT +[1 0 0 1 0 0] CT +N +-1284 0 M +-1284 356.25 L +319.75 356.25 L +319.75 0 L +cp +clip +GS +0 0 translate +320 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0RYt?6<'F%qWfg8(0$S)gY74`;ob!N>bb0E=)`7?m:,uRhNiWE:bLffIhL]e*X5\qu&ln'bVAhOKY +2SnaiGKn7fpJjfQjjkkj>05%FImlW.#Q<6cP/gqH1I +At.(d+'d9hIYPM[d:S)46m/*CHbDj5^rDtI-+7c_%UjHi2^CJ0+*jL;SYQU6.:97A>N\$k@aU:9rX\\1 +baj\-A\P'NqcQVLmHY-c;5;tVG]=qeJ-?K`Pkg8i8>`S2:[)"H0JYM@^iD%h^J'M1T_UAI=5 +C)>&#l4jXWhk8*QDie'k$9^QXkFAROuqn'b],4$2P=b3XI_b9ACthYs4sUDq\q?WNOo~> + +%AXGEndBitmap +GR +GR +%%Trailer +%%Pages: 1 +%%EOF diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_3.eps b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_3.eps new file mode 100644 index 0000000000..d80c371859 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_case_3.eps @@ -0,0 +1,1567 @@ +%!PS-Adobe-3.0 EPSF-3.0 +%%Creator: (MATLAB, The Mathworks, Inc. Version 9.2.0.556344 \(R2017a\). Operating System: Windows 7) +%%Title: I:/Entw-Technologien/Studenten/temp_kai/blend_case_3.eps +%%CreationDate: 2017-10-06T19:28:14 +%%Pages: (atend) +%%BoundingBox: 0 0 774 521 +%%LanguageLevel: 3 +%%EndComments +%%BeginProlog +%%BeginResource: procset (Apache XML Graphics Std ProcSet) 1.2 0 +%%Version: 1.2 0 +%%Copyright: (Copyright 2001-2003,2010 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/bd{bind def}bind def +/ld{load def}bd +/GR/grestore ld +/M/moveto ld +/LJ/setlinejoin ld +/C/curveto ld +/f/fill ld +/LW/setlinewidth ld +/GC/setgray ld +/t/show ld +/N/newpath ld +/CT/concat ld +/cp/closepath ld +/S/stroke ld +/L/lineto ld +/CC/setcmykcolor ld +/A/ashow ld +/GS/gsave ld +/RC/setrgbcolor ld +/RM/rmoveto ld +/ML/setmiterlimit ld +/re {4 2 roll M +1 index 0 rlineto +0 exch rlineto +neg 0 rlineto +cp } bd +/_ctm matrix def +/_tm matrix def +/BT { _ctm currentmatrix pop matrix _tm copy pop 0 0 moveto } bd +/ET { _ctm setmatrix } bd +/iTm { _ctm setmatrix _tm concat } bd +/Tm { _tm astore pop iTm 0 0 moveto } bd +/ux 0.0 def +/uy 0.0 def +/F { + /Tp exch def + /Tf exch def + Tf findfont Tp scalefont setfont + /cf Tf def /cs Tp def +} bd +/ULS {currentpoint /uy exch def /ux exch def} bd +/ULE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add moveto Tcx uy To add lineto + Tt setlinewidth stroke + grestore +} bd +/OLE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs add moveto Tcx uy To add cs add lineto + Tt setlinewidth stroke + grestore +} bd +/SOE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs 10 mul 26 idiv add moveto Tcx uy To add cs 10 mul 26 idiv add lineto + Tt setlinewidth stroke + grestore +} bd +/QT { +/Y22 exch store +/X22 exch store +/Y21 exch store +/X21 exch store +currentpoint +/Y21 load 2 mul add 3 div exch +/X21 load 2 mul add 3 div exch +/X21 load 2 mul /X22 load add 3 div +/Y21 load 2 mul /Y22 load add 3 div +/X22 load /Y22 load curveto +} bd +/SSPD { +dup length /d exch dict def +{ +/v exch def +/k exch def +currentpagedevice k known { +/cpdv currentpagedevice k get def +v cpdv ne { +/upd false def +/nullv v type /nulltype eq def +/nullcpdv cpdv type /nulltype eq def +nullv nullcpdv or +{ +/upd true def +} { +/sametype v type cpdv type eq def +sametype { +v type /arraytype eq { +/vlen v length def +/cpdvlen cpdv length def +vlen cpdvlen eq { +0 1 vlen 1 sub { +/i exch def +/obj v i get def +/cpdobj cpdv i get def +obj cpdobj ne { +/upd true def +exit +} if +} for +} { +/upd true def +} ifelse +} { +v type /dicttype eq { +v { +/dv exch def +/dk exch def +/cpddv cpdv dk get def +dv cpddv ne { +/upd true def +exit +} if +} forall +} { +/upd true def +} ifelse +} ifelse +} if +} ifelse +upd true eq { +d k v put +} if +} if +} if +} forall +d length 0 gt { +d setpagedevice +} if +} bd +/RE { % /NewFontName [NewEncodingArray] /FontName RE - + findfont dup length dict begin + { + 1 index /FID ne + {def} {pop pop} ifelse + } forall + /Encoding exch def + /FontName 1 index def + currentdict definefont pop + end +} bind def +%%EndResource +%%BeginResource: procset (Apache XML Graphics EPS ProcSet) 1.0 0 +%%Version: 1.0 0 +%%Copyright: (Copyright 2002-2003 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/BeginEPSF { %def +/b4_Inc_state save def % Save state for cleanup +/dict_count countdictstack def % Count objects on dict stack +/op_count count 1 sub def % Count objects on operand stack +userdict begin % Push userdict on dict stack +/showpage { } def % Redefine showpage, { } = null proc +0 setgray 0 setlinecap % Prepare graphics state +1 setlinewidth 0 setlinejoin +10 setmiterlimit [ ] 0 setdash newpath +/languagelevel where % If level not equal to 1 then +{pop languagelevel % set strokeadjust and +1 ne % overprint to their defaults. +{false setstrokeadjust false setoverprint +} if +} if +} bd +/EndEPSF { %def +count op_count sub {pop} repeat % Clean up stacks +countdictstack dict_count sub {end} repeat +b4_Inc_state restore +} bd +%%EndResource +%%EndProlog +%%Page: 1 1 +%%PageBoundingBox: 0 0 774 521 +%%BeginPageSetup +[1 0 0 -1 0 521] CT +%%EndPageSetup +GS +[0.6 0 0 0.60069 0 0.20001] CT +1 GC +N +0 0 1290 867 re +f +GR +GS +[0.6 0 0 0.60069 0 0.20001] CT +N +0 0 M +0 867 L +1290 867 L +1290 0 L +cp +clip +1 GC +N +0 0 1290 868 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1283 286 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +0.941 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 -0.40068] CT +N +0 1 M +0 286 L +1283 286 L +1283 1 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.48 0 0 0.48055 0 33.71863] CT +[1 0 0 1 0 0] CT +N +0 -69.75 M +0 286.5 L +1603.75 286.5 L +1603.75 -69.75 L +cp +clip +GS +0 0 translate +1284 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Wd>M9(IE(7D2+#@i,9a!(hQo1HVNhgDa;7ueQlaa](gp?%i_ +pAGP)#Q$jahY3oH!se\Z'Y7^M^93Ss&NLT6AdafD:'p:tQJIX79OO?E6iC@\e$cK2R%(9pjA\NB7BP7[ +jIB/7F6I/>Kiupn(>f#Qn>]k'?eA,'#/Jbn_YhS2+g@YW-"lmh$2CALa.H=#Q#\&NLSg;c.>=0KJ<\_nP)O +Y"9c^E`#9sHliH[_<$KCMTDcq2-ZH=0KJ<H8G"Dbj8Ln'@DWG#.9I.9Wa +D_GJ3r8*+pNRQbA(+/47Nnk]PG@Ck;,:%:[X(^c62N$+`Qn)Ln'>ISKU]6:=RmHN)t5QCZQ)-_?rrt+O"3`fLSqmCrUrE$c>&e__U66\A8M?`ZNq93QG+YbGr,,_/h4FHqnp?gW3Sj^0E_Nul\ReEr; +_W769^5Au_1SLQO']MGFkXT>$^5X,=6\)d +l,bM>/j^a@r/*&b;&FaQ%%iEuWG#-F04$b\il'*22nNGS]6E`1fsB!fpY;N6O/BV*/POGM?%Meun@7q` +Kg]fH's6RH?5k*rM8<(%3>F-GS%Z#>qWcJ'7sA>?kbXVPqXi64B=tu29[5Y8B:bFi?CREs+d!^L70fIH +Oa_5h)AugO%$6@fWG":r>?`1upu,:]IDhCBN)&894;/=0Y%WQVt7P(YWL1LL4hrn)d\Xq +>\1f?U4eEFWMh[YR,!"[dlCb@G]sPirF!Z_]gH6d<:nGZ7I3&#k9MefHDSlYsd")t$nJko-12)bGH8VBK,7H@FtROq3Np#gGfVT]0L)_UpH<)dOpT3X`RjA\NB7BP7[jApG! +VqmKg^[&]>T:Ce]D<<#X&ig\h;c.>=0Y+(DFS9S(^+IR-\_rZM;'Uj'Lt@QHaJjucMH[??agF++F%kNb +]E-bt0)4L,eCoZTM566SWJI(%>-iq)'qWm"?38a^?<8cSjA`K);n)8-/7Kcb;\;#)[:Tf1$d\'"0*,kj +DZU`&E[k7hK88r"/7Kcb;\;#)[:Tf1>OJ"&q@^U;n"t*@-`<\fXPL5Y$]1C2aJl*q>F=4DkB-.D!]GTg +OUF?5(%lBK(9rtBWJI(%>-iq)*I:KDT"qCP]F:#44uj>e_YC>*66`-iq)c$ngJr6$T!i2?NPI32_^2W$U-QcNQ@1tZ&m&+Ln'?+Z\=fZSsKEu_As#"_Zc[&MT0>\ +*:E(WMic#ZLn'?+Z\=f(rsJj'mU,.A&jde?U@F\`L&4uj66`a!8hJk+C>/8E4-%nZ]7:UQ2 +&75/V((rVBO],+m[dXKinXX>$:#J1[.4'X8:.8qCqQ^eK&ig\h;c.>=n:ip`#tb]]_XhhD.3TsW.U#2f +/7Kcb;\;#)[:TfMYrVOHr].V2kEHTG,,_0C[G"&#,]Y@YVkVaYiAEF;b8upci4j;-.3TsW.U$noqSm4m +O])iP't%W]YCS]@p8E07:3')Y+XD(Xg(Z?Xi8l%e8E<`<:.;;<_IGG&b8rMZ^\^!gPRnHf'lB8)J,#L\ +?m1Y2&NLT6Ada[:hA>khs&mam:H58:&/$uF98_nJ5@ +M4BIEWMh[YG_Xo/_n>1Q?=V80-%nZ]74_jCf5)Um;\;#)[:Tg8>4+n,B5HOdig`n58EL +,B>8KD54Q/U>Nu'ckc'Hs'21d;b@=PS;LG*bklq)*us>$E^WYkbLtb/^2rJ_QX>2ek^0Ji(1-iaR-3AD +IK*Xo*9WP!66`0E\ +]5@Gj47DOLASZ(^Hct/^^HLsk);8,nqY2T:I.RB.;b@=PS;J_7?q=)NYIO+1#+$QT.U'p\gUFfVHh6CE +aYt8*If/lj?_@%qoX.\_O8gO/F7.ho^)mFB5Y0s87#;d)c+]$;jb%+%>`QmBE^`W;iENd/F^)@[`05+% +ET4oPYIui[/\K%%CO3HpY-5k%;*dVY):TF6F#^(^&/$u<;.BNd%_4r9K=Mta;pdr" +gpscTpIGh[H))<,kEHTG,,ZVW//lq[Ip9]XE]G>AU4qqt;qASL^:WhcPL$,6O"F&N3MhCjaJjt.bD0U? +o2dP7jA`LbZY.Uuo#`J0^[:PKQ.R)%.4'X8:.:C<6h4/d\/\@37>na+rH^D@hg4UjSXl=45Q:H%R5:&U +ph@/j3TlV>66`8;_UY\gNu^<*jBLfnT7?Lsd>W:XKul?QPRnHf'iC\bUmWmB;>D;C +86h1rWG"F:8E<`66`Jr6$^5X,.3TsW.\al(6F)F!R*-(%'t%W]PB0JJ+XD(X +U;VpVo%1fqalS>KMH[??aZ68`&/$u<;50D=Ao1stG_B=^aJl*q>F:q%8dQZ';\:>P%I)e&m"L;:`oW#H +MH[??aZ68`&/$u<;.9eS*B]!,ipdjP7?!dp>-iq)3N&)Q&ig\(@Q5N*h80rti!lS.`_/b&ftug?;b@=P +S;J^L`/or;Yo.">N/`]uZ\?L]Vj%`*3>GD#cllQ1,;JD>(^UqbAdafD:+7>3E[mg%#XXuoLqO#A5`D?[ +##@[iftug?;b@=PS;J^LRZOC_@BW9c"J_rP!E2j2[P0`)PS-;,V]n`l0;PM#nFqn$^5]hW3A$(P1;,M,;0KJ<\&jde?,"deTUmZkf,$pg;=2[A&W&7eHS9%:/+d#\8-.X37 +fo+K2?@au4Lu,,02-ZH# +2-ZHM(-;?sP9lPuQ8%/POGMV1arOWJD2Q*R`FhC_<";mtt3V't%W]PB0JJ+XD(X +U;Wp^1i3^A`>(;-,$IlACUPoWkEHTG,,ZVWCg9LHE<@.]&Ik\;2-ZH3E[mg%7Y`g3%XSY^7;[-8'N%_C[P0`)PS-;,V]n_=l2Ma1KtT21r)?'Z7BP7[jIB.@#`5(/ +-m>D$3G8\&`:He;3>Jr6$^5X,.3TsW.\_TXp6iXL/`Na/6r$1Qftug?;b@=PS;J^Af)o%2/6r$1Qftug?;b@=PS;J^C4Rl?4)6r$1Qftug? +;b@=PS;J^=0KJ<\&jde?,"`,T'rGg+ +[2Hq5,,_0dCghA0.4'X8:.:BYd`Td=_rG(A.O(dbenec8c7(&m6r$18&CTjGh]c)53>Jr6$^5X,.3TsW +.\_TXqQ`kG9roOZ.Vd3D-Kgms6:g0:6jY%0C[)HPeS?kGCX8-, +[:Tf1F&"/-,]YB/KTHE"i#t5X7B@\R.Vd3D-Kgms6:g0:6jY"Ge>O.fMTDcq2-ZH6LG]epH +7H@FtROq4Y3MhCjaJjt.6=0KJ<\&jde?,"`,TS]kEYm7+@F:q%8dQZ';\:=/ +[!l%?LK-`;7BP7[jIB.@#`5(/-m>BDG468bnA<*n,,_0dCghA0.4'X8:.:BYdb6d-#a[/Q'p2GB&tXOF +fd#DI_$;&UHY6QMS9%:/+d#\8-%rWKVJb@M7B@\R.U(=D2G'RA8uP)GBK>Hr=E'966`KeXmFntiDd@hi +q!iq(p7i`.ki0R>dlrR>9[5YpKTO9SL_<$`Rd"U4rutJU3>Kg.i5($b'=PBZ+$P'sqtBE*:EnV3iCfLi +:&?'A0KJ<\&jde?,"`,tfk)nsh,16tE[k86mWV'+\T29M/_s_sHM$FOX&l4t/%.,lkEHTG,,ZVW#`3iN +Z\Td`>-t5d(GD@>ET6-*S)(::h7In)m+LQnHg]:V8(6>P(+/47;F3q8<(%$9Z&kqhGc;K+L-;\.Ms=pC +X096`o]X]Y+!Q0uV3aCEo[=]HD[VbM:+7>3E[mf:OBBBL/Sl$*/D$WXCi+$;pu.!8IJ)-amqE/nF&"/- +,]YB/KTK`2dlKul?QPRnHf'i@:gjQ1Aum9[5YpKTO9SL_<#u[p2?;0p>k#>-iq)3N&)Q&ig\(6:g1SMC`5N +E@kqO(+/47;F3q8<(%$9OXmEpi$%H@M)U0);/66``2dlKul?QPRnHf +'i@:gjB>N)n.mp>f9^8MCUPoWkEHTG,,ZVW#`2g@MCdc3i*+X!/POGMV1arOWJD0R,.EGe/E1g8>1Aum +9[5YpKTO9SL_<#u\%7H//7PB!jBRUHZ\?L]Vj%`*3>GCX8E>)qkb'NoAXGs3[AYreR%/4i#pGF0&LE)u +Y.':cCgi6HN*M9EZ\?L]Vj%`*3>GCX8E>*"kb')8&W_rf;/66`3E[mf:ON7m8,,9C)#$B9AZ%^:[Vj%`*3>GCX8E9tZ +dB"qdAXE[6YSF8L>F:q%8dQZ';\:=/6uMc7:#3Z%nR)d?U9V%YaZ68`&/$u<;*mh7`_t4#6eEe.L9X]7 +?FDOJkEHTG,,ZVW#`7?H.[mc!/`QbgX.h>SKul?QPRnHf'i@:gj@3C:;*5[?Y%T(s@<0XB-%nZ]7?e>r +\^=Jh&J)\@.iLNSR%/4i#pGF0&LE)uDLF"'15EOUdgE0P771VoPB0JJ+XD(XU4eWLX]n6C\$/`bcFR"4 +4LQq(kEHTG,,ZVW#`7?j<]"@n>Jr+pD+0(c$^5X,.3TsW.\_TXaK9YT1Y8j.>\:Z")?O-1S9%:/+d#\8 +-%n[(6RjjKS(`s]H=RDIF&"/-,]YB/KTO8l<(2Sg@F%-NP`;]A(Uc"XkEHTG,,ZVW#`7>Kr3>ERM-si!m>-iq)3N&)Q&ig\(6:g0:1s=*'lj9WbW2MRXR%/4i#pGF0&LE)u:.95l\V6#P't%W] +PB0JJ+XD(XU4eWLWG""B\+%E*@MQ:_$^5X,.3TsW.\_TXaJii5LV?aX7ZCjl[:Tf1F&"/-,]YB/KTO9S +-tF$AUS@Hu@<0XB-%nZ]7?e>r3>GOSA-fC3dnm*D2-ZH%Ia#9T +a7AS$.Vd3D-Kgms6:g0:6jY$!;c3SWhSOp$`@>WI(+/47;F3q8<(%$9O]0Z3bVt312N:_dROq4Y3MhCj +aJjt.66`>%_-1X_p\G"t.Vd3D-Kgms6:g0:6jY$!;pd!E0SK=JH>42&AdafD:+7>3E[mf:ON=D7N#(9e +:]2tfmGh&@CUPoWkEHTG,,ZVW#`5(/f"p?M\Ft]<IGUUIUAWHj!Z@S9%:/+d#\8-%nZ]72r15$hsBX?))K0hRt^Xc?*Q>EXQ6hI_C*O[;4B;G'<<1^"^6< +V+[.eHk9MLS?"e!KLAR(PRnHf(%G#t4OoAi]lW*2O&ob*C[9-./'g?Lb_n-g\8Ma>s8;I)/mR'+mJH@e +hn?r+qtI":-sN(/X$%b^6r$18&/$u<0hp+K]]@ZClN?JI1Gbqa%j.B(Fm@SnrcRsnaY!G`f3`a:IHR/F +p>3,[c.uU\If&MH+3G?GkA!d!+d#\8-%nZ]74b4;/Ng\,5N@sW/_h^LSp^.s.%]52[r:0lIXC\Kc,k/n +If0!,YJ39g6QK=bS+'_HcCO:u+g;H06r$18&/$uISKm^[2g,jSPH,ET4CX +?CS3RkP5Y)$uc!+:FZX,S;J^<,]Y@Y2h+9^\qRW%[N82rp]&q_AdDZ/.3TsW.\_TXaJk!Nl7*_;(;=tZ +g>quU5CWNTnknl1_KYG'=lOe(X$;kc**7JZ#`5(/-m9jV.O(db#7hm2R'=9;4o)k\gVUSKPkFIVn*[m, +mTV$19kthTR%/4i#pGF0&LE)u:.=5C1u#_rgc9`Tlmo9PZM.S,WPf-'Vj%`*3>GCX8E<`<:.;KP]nNXs +^YZZcXgd0@g"H2rj,_NV3AW[TT4SIpF&"/-,]YB/KTO9SLn'?7H[6'DY$Sefm+AT!T.`BXrqrN]O(NbB +>F:q%8dQZ';\:=/6r$23<4&1_mHoruqIoR0hIR(YF&"/-,]YB/KTO9SLn'?+Z\?L]Vj%`*3>GCX8E<`< +:.;;<_W74c8dL9DMC9VnE[i92U=O-hEm?*[KLAR(PRnHf'p2ElCUPoWkEHTG,,ZVW#`5(/aJl*q>F:q% +8dQZ';\:=/6r$23<7I?g9[5YpKTO9SL_<#uV]p.2C<[M@V]n_=M4BGO+XD(X,,_0dI"obBL_<#uV]n_= +M4BIEWMh\H@<*ilKTO9SL_<#uV]p.2C<^CYS;J^<,]YB/KTO9SLn'?+o,/mQ,"`,tS;J^<,]Y@YVkVai +L<6EN&jde?,"`,tS;PCeW&:2UE[mf:ON=DW&jde?,'#/Jc"EE6MC9VnE[mf:ON=DWS;LMA)()7*8dL9D +MC9VnE[i92U=O^Haf1(/66`Ep[11mC[IFWKcF3W>OG[5^A6or +A&jUWlfFI?mVY>fIRK.=j2[5\j^8#Kkg?0;'n-,ZhsXA +V+[0OISc!IX097k*S!CPi=B3_T7-Fd5Q0%q4o=TPh>-I8EA:MM'reetfto;6].<+)\U=5DP:'/?pu75" +Ecgl[]mE!=_%0jsiJ"uf'Vj&5e(mlF#u.Q>\(iOq^l$ofQ[A`-k:^)M +YkQoHhg`E3cBbe2?/pI3d +mHtX4oB+<>2]b24r:SZKn*;.%#2X[lXfe]hrpc%t57kitlIDs8n.5Qgq!hiuQmM*J;>]k#&%dp0Gi=@k +s80RGI/1(?aC>N^9?3&5qtGU?p](6]s6abg:S0iLciM(NOM;@@LoF2b(Gji??]('h?r->3'-2cN4rS-6.h;<@AUKcXPf73bNA&jUP^\IDms4m?9 +@l42SLHk,hp[@/^h/0HrDZ&r"6-c[JqPq.OrPsEc7ur^gES7g1giB`SBj94AGEpu+gOJtRGPAei+/-$> +X&lKJjo"\9^[1iK0NJuB>W?a8.u*kJo#W7b1Y8g;T7#[u;lqd[FL>ROKaFA +=r5N,P:(Q-6nc4B_S( + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 33.71863] CT +[1 0 0 1 0 0] CT +N +-1284 -69.75 M +-1284 286.5 L +319.75 286.5 L +319.75 -69.75 L +cp +clip +GS +0 0 translate +320 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0VDeN2Es2XXaA;!JZ+VDU26&]7/YR=r48hIXuO^_l1)%fc!)A3VtLkuP>5Y=EWKTI)f.nW3FR+\+( +_&*b16R:-V(e]V)@"h&#`V"Ya&"_\K>j"/[Y.ZL7Hb4Yhkf.aNkNn_bFZRj>F"V[-;@3B,WN"!d#WGR] +=4`$-$S"m,_Uf;GSd5]q#!%K5GXB'r`?l0[rr]fUOOI!f0hgD=1@6'"g54K2iP +V+[/LWCj?j.$Uat2gaN6p\X[=Demd:YJ"!.S'N,Rk3lpM=#OmITG_jQ//F-nDT.JJ$PtE8)]R8%hgYGs +Vl]48Xj^3C4b!==k1m7R9BlB];]WG(RZ,=bpA>Jh`2UQak[4WCSCA7S)(28ck>gCf]*(1(Ren%S[:;]8qu>;se95em,1 +m!c"2/)9+BiQIQ@VbYbS;o^uEPHfdjrqU2=$Z@<_Rm6a<^!L]skSNnY^\OX3e-l6;O+'qZoUA)A7D7&d +rob0k6GN>k?doJ^k)Xsg0D(_uqF/*EDuLZehr3PQ`&E2j6UO6(c_%E0l6[i):7aR.3B:7"`Fcc@=bV=r +f@^$Krld?>_1Di5s"!*F_*Mqa'.6P>:V(f@@W-2RbSH]go08b/[sN`daGFMrVH$V +gJV1h#7hk\\o_Y-F0k"Vm^_N??Ph-d*BSF\gU:r,(G=2GKgJ'A\^AnI4*E/YZq.C(>.&,6hS#oeC7[NT +\3g@#]mB?1hg?*'H0!$Y4k1'XFeu&j)ep+3hKc#*R5]%j7n:U$?[WXui/K\PW+N!s]^nmc1k5N&c'pYI +S*cHD]ejk9!*']pm-3bkDGuW6QklX@naBfK@U-*i00:Q.DuLZh?gWW>SXlC%qWUKlIAApl]'HNg*Su%U +]6E_pMI(VnV;oUW%h&k[2H:<:,u=tP]QirK%m4gpd%Lt6pcslTEnI5]RmF1Dg9jR?b=>=e^&X#BoC^.F"i9*PE8+ZmAA2<;Jg@Zi_f6dGca.n?ahWlND+Z=d:='\6qhf%Kj +bP0nDYI!#'YM*3!]lh;O($5C+$oRr<8R6IJmF0qE2DHo!.5e0lANaA+AkdAgh/`.W^P;29IS)'4Q7Y`L +J$\nI;WW?p9kVbAM'dqW`>SqTWBbpT$G5H9+)JW]`%@Ak)`RlbRQ7\NkpL*G+6^^T%13Ni).2&6nDjkL+JNL3)'f[2ZdSk*tK2 +Adig&E`/*Em+;nE$J4P"IJ7#VmF0qE[_1L:0s1mpD7#9!JC/<7)SY++WB3K4FRf;)o,ShlXf\_ami1;Z +5DQj=WWMRXC.'H%H9*f:&hTp-]cqTaWo`H;=Y(>L*BSHAmC-0^\,8.jnP"[lL@g+eV`7,c31WD%'nqhg +5fQXnj7DH[f<56-iUlB`iFCru[:eCL[_,tl[V4&4/Ko*hPK]:Hq4lj#Cg^X9(TR#OV=W)3A]"a'Y4]Y8@%h*W:;X?eEc0(]&h+ +rHb/Ef[6l=qLosgHD+]udB=A_KcU,jg4lq +]6sHdq(.$`/t[Sq50KS1bNJII0783P48SV:^TaJ?0D#W6qXm1iYbPPYoF!bWog6EtUL,\H$Tn4X6GKb" +?SD$Z48SV:^TaJ?0D#W6qXm1iYbPPYCjF[<(O9H4_iDFO$X4ZqlpY75nuuUWo"p]J*IEnb[QNp/MnH:U +@E2^8Ke3fsp*)/+H.]A97tS7:+X;Y[]?W_NortH]],Q4RC^Y]WgL+a'Rh\/J#I^p7aGF +ZcnF+jT#8\zzz!!&C]cCI%@q=19k]5o#+k+'r7kigEhpYC%#pc?jGPq0WJp04`Z2(JRMe:uN9:$q]8O$D2@qF.jB]kDY8=KfC:qW +iY0p"-$5Q_&[&l$Mj/Vp]9#C3B47e\->(Wm)o_GTsQQ?[r#S)uSB%T;>moP-GgGF`hhNG=PG!7n8n!?T +p;'j>"j_1bg_Ym3rN7*og2_\gp#:H0CrWp?^HGT$f9lOk>RUqs6+%eKn<3XKHsDMH]]m@q/,Iq40TkG'%hRrU +neB2l3^]Don.gH>rTE=l44.2G8l7T@'s829"X"j(VpR][&og#_XNYAa%q55VO558N(Df>8iI!f0Tk]4mWFH%jEoB-D2^S +jWt?DNct]s;S655k$o)]RPAET>3,/-#D)&'$so,l34#l<^9WVbedpl`[^3f:G'Jc>.Y)AtW6W.bij1s4 +r#Ez!''itABo#@~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 0.07987] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 356.5 L +1603.75 356.5 L +1603.75 0.25 L +cp +clip +GS +0 0 translate +1284 70 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 70 0 0] + /Height 70 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WZ"atg(rtT?NCCcPZ-4L"(1)QbL%\R(Kcm0'/1^oi0VsJZ9hH#0'NUMDU_/^lc`!AB;sIJ`O'E*[Yu!!!"6*?6'cm-Jg%n*D[>\$suFo&@WHj5]0\+$Xr;E<#q9Se#iJ!!!!g +UM#/Vn`.Zb7uo90^3o7tX\404_DcAf\9muFn!)G2A!pKup-`P +fj4C/]6E)#Dr/-pIHN+^Xuq+,n@T:>`&FcAiGY`hKTO9SL_<#uV]p-t\Y:6?nYoghmG#,+S)$Orq`\)b +?@D>$6-h/Gj:afi'i@:gjA\MR+d#\83>M4n$Squ:U4eWLWJD0R,,ZW*.dD4m(Z+',?fErkD=8YbJ;P`u +,]YB/KTO9SLn'A!S_Aetr1pE\041<`_k1.FJ""q;XtVjN;*mh7<(%$9O])iP(%jYr$[6LF]Qru&]otDR +RnBc4MC9VnE[mf:ON=DWS;R0fn%sEuHeg8LT0>D%55t'1_p6QQltSZ/'dfh&8dL9DMC9VnE[i92U42^G +E9HcOa4oA'J,]DA4*U*4*Zk$&"BUt\MC9VnE[mf:ON=DWS;NVRFEI0SrVQRGp^^GnS;J^<,]YB/KTO9S +Ln'?+F+q`8L_<#uV]n_=M4BIEWMh[UR%0$^+XD(XU4eWLWJI(%>'#D>WJD0R,,ZVW#`5(/aJl*q=.#M8 +-m9jV.\_TXaJjucMHY(Taf1(/66`Jr6(FJ=8;F3q8<(%$9O])iP't%Wi-Kgms6:g0:6jY$!;\;#)[:U4%kF>F:8E<`<#pGF0 +&NLT6AeufWS5M[EjA\MR+d#\83>Jr6(FJ=8;F3q8<(%$9O])iP't%Wi-Kgms6:g0:6jY$!;\;#)[:U4% +kF>F:8E<`<#pGF0&NLT6AeufWS5M[EjA\MR+d#\83>Jr6(FJ=8;F3q8<(%$9O])iP't%Wi-Kgms6:g0: +6jY$!;\;#)[:U4%kF>F:8E<`<#pGF0&NLR`\$]e+0rA?W[eATV@uN.^XCZdY@>lq15FZ?R(Y'@4N%pM; +?a:MYN;5?kYkH7JN%r>>$SoH+0ub(%O7@$oN%r>>NLSJfZa7aKHi91rHq8%&gA'8uaP-+4$D%JM?P2<@ +,]Y@YVkVc/_W74c8mK`)c<:8c2/p$pq]mE?@B3Y[MPqjIE[i92U=S\JjIB.@#jFaP?$MK;HX%NXE%7T8 +ON=DWS;LLV$^5X,.C#iqD(WIkNaNZpq\0^KW>2[I66`OpYEH'EKc=KL/jiS)!l?IiR.& +W#GA8j$iE&cgK]EKIUHTKLAR(jAaWt?'q.'8r789P^IOp4_sNE?W_b1chqj7eX0B+N0]RHWJI(%>-eU# +F&"/=XcV7R^>lNKYG(Er1cDIn(bHnn;\;#)[:U4%kF>FZ=s6lt^Cq:L_QERlBiW)b$&P10aJl*q].c8. +PS39hBq`4V1,!cVNGKDKYnVOEU4eWLWJI(%>-eU#F&"0hifFAIA!Z!U/@u]7h?=E[PRnHf'p2ElCNa-, +c7(&?&CS@:biW,;n6T@Hm5u@c6:g0:6r$1QfnIO.Vj(j2d]dXbDHVEml.q850<9jQ8dL9DMTDcq27kP& +kEHUe+V!:X(L)Pa2?'od%nDiq&ig\h;c.=R@<0XB--AS)fmd^LWP*PWpNOTo&LE)u:.=5Ce='[1S9%;Z +3Nr6p>b7uO])iP't%Wi-KgmsilmM0 +ghuJn(Rh1fp,oO@;\;#)[:U3Z7]:g?Z2aCIMHG7agW`dc:Wp7;~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 0.07987] CT +[1 0 0 1 0 0] CT +N +-1284 0.25 M +-1284 356.5 L +319.75 356.5 L +319.75 0.25 L +cp +clip +GS +0 0 translate +320 70 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 70 0 0] + /Height 70 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V5tEJ"!!i"K4s&0_\u>s(N1#XT^r6kBqN,/.h\6jF#/O-#Ie+KdstF8c-k_mZn2mPo]/Am]I6NJNCHLkpkCLkpkCLkpkCLkpkCLkpkCLkr"7 +Iu*J96-Op?.e]nNkNLd +Cj?Z6?=Xk\4GdQ$Z3sm.`3C-3Ke6*[fSf+5Q7$\=7W<%>;TEcK=,HHsh)gt^]:^C_A)*DNA.,1R/3%*[&DlheY10cH$0/<]>hn^6Ce +>[CaFUN.fN?hGD2hqm5H56`J_-XNtm#8)`WX'K(>7m6]4dYc7,/<]5rrdJNo#TjiR91BBF*61*a1I?Qf +qYEleQVOGue_Aj5;B!oH?g09'hqHfLl00'))e4?=U^Qekp@a6A?].#tH$S)Q^s$Ds +PEV3O%SH\KIlne(O3c%om-a:N#KaieL_E+Kd*9gpZ,jLsS9B$t'oU'-qtj5/#R%@oDtj@H]Y*0mrV_02 +0Y^eHGKr*_?'jH,M[Ne_naS@-2`E\#^Tk&+9IBG3ouP3FV,BTV[9=Rj*1WId_Q4]QiG]WHhVI"%L.,u, +PF7f*^e5eN:JXd?hs4X-b&NlrkJQcJ45oPm%/U;Z?Ld>oJarL-oi:kpo$>X#CEFGc/$M#r^KBcmIl]_U +?!1=H:l$MC8&\pL@inaQp%.eNI!oaXI=7*'KNBh,eS5pd:&3>Rbb+2ies5:&iSe4pW>?_s.)LdVd;PH1 +cWB(;g`2nme'2NaPm[s-qTUO=+P%c?LLoW?oX+q/[S=[((MEe9DNT;_2PRrELt1q=F(4^Mt/-\QCVRb@Rfdr*!0>qI_/;fC&CiWOb+)Qr2o7 +9n.%85!G"N)NS:e7;=N%`lX-sMct2t[BA;aq/,L8RGXlMRU62emro.rj?XHBc;H[C[UdWdhl +d0g'Y>''MIY+5;F@X_P4c2!(HfiahqZ*IE2+`Y>+&8PVN:,:DAQnr6_MB>]a9I=cAg=i"hGP\&]-rLqD +`s[TQWQ:fSk1>FbD-<4g([G!nN*S'&IWs!(pD`Q\,n^Nff;f_H*))ssorKlrfk_Egie1DbAh(*bSG>$O +ITtt$r&+j3ge9&IJ2AgrLm_#9)uel6S]-):YccbQQs]NR?]p-=RI.#eQa(!9cGW`fM:`mNEjBMHY&&>/%\G~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1283 286 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +0.941 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.6 0 0 0.60069 0 173.19932] CT +N +0 0 M +0 286 L +1283 286 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 287 re +f +GR +GS +[0.48 0 0 0.48055 0 207.31862] CT +[1 0 0 1 0 0] CT +N +0 -71 M +0 286.5 L +1603.75 286.5 L +1603.75 -71 L +cp +clip +GS +0 0 translate +1284 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Wd>M70Sd067)@6_&,QoG)H#R=9A/PskOki,.!sqYS>m0gl?Xt8e]*PPN#`'%Z49ugh&K_2hOapt. +<+Hi-+aWp4e'h;:6eoALma[aTZjeG#JS^O<.E4$2ctkbF3Ajm+V57iqL' +1&^h6++l1O\l@Uo0RDE(<,.#h:>&N5UBHE5.3_NEC=@l,@_2K'/]'U3.>;:?OtPOaCal)UJjc+C&LU#T +l,%k6AWpu&nCMqYrVQ1F^Yp*(8OR+US+W'.AB[k7%=r'-UkH-_O[)p +GcP/jS#6s_3Co$H.l+#YL'SE3;FI#heYW^uVVVgecXEDC7-5W&45>_;.A@u;7%=r'-UkH-_B#$86c&(s +Sl.u=+F156.l+#YL'SE3;FI#heYW_`lX9`5l<<"+V2-IMU^JR%7#K&]OGK)EihWM2?+<>rILF_AbYsC4 +,"NS`*-3s!,4ZBmihVAi.B6)YDGGp*_>BUt'[]=1P"[oKBd-",-V20e9[Tr"8;C^FUG'2t/,%E#08pbm +R;oX46*19hh[Q7aXe^d6"GsXI.2oig3BB.rU'i@CaQsuUc6M@a:P0Pm;U1]O:/`Sc1I33;=,I28:l`h] +?Kf7?9kS>#gY(Xkm:fU*60`GWkWrE%+tZ))/m!o7Y>1,'NaCqd#rS']>cU$4C+`'!-g!BGQQgG_,iWj@ +.0%gCLQS=bFQ#@Eo\\_db;E(0r[I,>54fL3HEACRal)5>+Do> +CG\Ij,=oc78s,+59i=r[/j-5t?4MC^>k=1l[2u.+AoMQIXUkGBt<@Tc"]C4h9>)[[@SS*&1$7UV8sG&-7Prh(LETq?\Hs. +&`1Vf:MEi760`/j2l_KEOb"VeQRbMuV`%+DD7]4_FRsP:6fjc71om)gFYgF0e4kf;Q66d+.n0DBI?&po +eYW`+1EDd6DtbO/4Jc3:"KBGTU)Dd_Lg^T&EOetXB53VEeYW^UdeAaZI<*S2Rak,o`\?`-l6\q.99=tQ +Pp4s\S"uu@?+?t'IjfQ>pX`+$R\r!C^EE24nT3tK71;MELs)'X*a?i*85X68E'^81(Ha;$4Jc2O"_gqo +5TZJmLnP(eEOetX:MQ(-eYW]Zc2"'kn]3R`Bi]^Ll>b:oY.kZ*#XKXe=,LfbD3CWL)`MZi^3tA-a6:B+ +j1Gu::Tet8ZapRnQrf#.FI=?(:QUCUPp3h;S"s_/2fIP8ET4o,++6!G_fXiPN:VgVkK#MVBi`8?C1Q5c +6,tE`SHOeZ>UaEf`NVjihj"B.!UbWo9:ODCH9XH%q__X>g.N`3kW#nh6g;IgkaP8#V7cCI$E:S)ssSTcE!/CK5b +hWAo>Q9$->f7I@ul*;@YVhh_m,iWj@a`rH>IjBD'=0O8E46]+tJ +5p@p_MJPLoP+n3nf7h7>[gOos'U!7JYJuO#C*Y7&2F/N>=,Q?-D3CWYl,!rgVuMd"8640g]HFOc$[uZ! +9I6H+&bQDBEOf!.C9:3.Znbp3NkNU;(lX_!"1?K9J +O<>.%.n-n72G$bgNB`rtqc$bg0FW:%QG";$s"ilULG=c\)0:Z`8bg/bh[eRGt#u+pUctpZq6.TCZ45GT;-bLl#\p=fW"f'NHO1tbblR4U'O%/ +/m!qs/o?cGlI%m:589WYB;J8/T@UpBF05[r;!-6LUkg2tM7?e>EjaIF.B5#^_jPWlR#JVH(F-D\D`f'8 +._->miV:,oKG7::+;'qj7J+$(%KHLkn.n0/s2G$bg3?HJ[ +T*!mD7oJYn?B!P[:T40-#FJVoOWoYo,iWj@aJRB4.B7:LLNB^"p,C!^o:0r8rD0?tc"9M4Q0Y`,AdK)i +\Pct=D3GZ62r);2oNq$ICVT"R3A3j;W+T%d%V&I=OR'3'AdK)i\Pcs.Bp0622pfJ46c&`jr:NGo:<((# +/(\>7bn;;]Ei$j#dO$ECDN,1icl9gh"ZNX.Gqi%r5*]#&QVgmjY0.$<,tPu98s+P19i=sH=rtu*rnHIOS4O7<)1MMEfRNpEq"3hH(ic:m:cI!sfWpX:+BDN5/HQ2s]bBYfTH(MW1B@7%f +2G:3r#+?i/`8sURD50g'<8H>bj$k^5-Q2?,(jep?gTF +s3SY)cG:I9mKGCF47e%/UjD#uSP2*RfKm&#)ebN[&XP":p=V>qd0_4QG4b"G7t:*q?iTujh7N8BqS:!g +0::il8Y]0HEN)^1*QIH=>aGmV99=tQPp6*AS"s_#++3RoU]Oo:H?FR]qbM_Z?[1O`H%4eNLGSmr +g^%LP/^+S4Om7crFY`>GLhgK_;4e%q1PP8=mEb4Cj3UFI6/7I,1l5cuH +).Yh$VAQs[\LnhEXH3pI,MtdKEc#RUK6lq=[7TugjHEB:htaUNrTr'9?_.4iG/)t1Mn&s$)Lb\1XEk;G +S\IgB8e%2;8r#TsiLMuMB'<0<^u0:hiG^dM*CI#g/0W-42P$N6AT`9jIer<)X%jT'oK/*U5Q8eUYKZhn +PE0)89>Qjd=*;P57k/4KX:`Hjc_W@G$f0c\39D2;.*8@[5`QI)G-rs8_A@e/%)0A9<]EuKHXc@rKY*CP +2Z2Che(io$3;*h#2!h)_G`ECapA4IJbJ$GL(FCcd`@!7C=bdm3?+H7GcUtNa"c5-/&TOe]E.5F6.I4\A +hV7drl//AGO"O+reXi_JQJ/F!i_HrDaA^Ng>X>)Y;FK:_.;J]:"]%A]o_J+Ap\ss7IoAoT-I-@%<20h= +4pEdoEod%0RuDpB[2aHUR[#Bo +Zq6.2>A&h^^)]?'il-gGNum-UpQ*Uk4aZqkh)QRY$PR0\-`Vfa2iWq1A?P#bYa^n.8@1sGL`G +l`75'Rbr^@l,(AAm>:+?nP*EP".$/)BU@X<`]TQtQ6X4`G5!1bdkk?t33,2>f&\q^p^3s)bL+s3^E^:@7%:!.\:Q.Ji?c;aFCF11p-`&SWu9[=k$!eb2YnH9 +'dVKY7Dop4dIcha]g&eq(FIFB:R'!ue#5_pf5`^JU$kXW+OMMb+tfH2[tEkmKo*.i'4mT7BiLjsCH3<` +d.\hfOD'gl&Jn$$gVJfpKo*.i';[H`2P'rp[2b0=gq/b#8@1sGLg)l.D?hgc(FIFB:R(O`^6+@oY2s=B +KANA.6)%(O7,5'sR_B0rG46\t@H+B^7&^UfNF[uh/Oq$'oH**$&oEdUi`>B^G+Q\;_SfUEM6_*(9D;qd +CH3$oB&s@4D3?+&(/3,i$>eI"S1[L*eXcgnheUXJjc*(Lq)!H0T4D6M6ko7L3:/Cm`.<= +En(&,pFB=,BTj,BpWsVb*?:j_'5+_NLqa,8C0k9)A"7][\X'"N#J,,iJjc*(Lq$S:E\Fp:?'mUP4[hK[Krsf$FZdIMQD9HF\p(]:7%:!.\:Z7$:F;@' +$PR0\Vhi7&YZ8Dt?3`IAEn'D'O$Kijd3^:dDI89+*u1[]'5+_NL^/NBXfnqO1![0HFZID#O=Nl/8W&>Y +99IuFQ0?*'=m[YIIus;YqYD/e>g&6<2_.skBTj,B\!nSNKBpp=M6ko7`$%FPkZ??m`gU['Ep_;V>X>)Y +;FI&0_6rOE`*)IVip\()'/fIWXl`m3;.kMC-Fn^rS"tk5rq>Q/FCF11p(T6f$h*jcBLSKZZalBsc3AJd +EWq:X8J*Io4*YUa@H+B^'JV733^lb+NGbo\W2GauR9W%P9iAoEg^[XLMUj_4L; +naY+O1T]VH?R&='=Ot=C_,=7%/khd\8eDR?P$1^c+%*[:E\s>i6bDgp"!k7uZEFif74`?-DBhTXl,(B, +V\J[j+K6)5oeF%Y/4XcMDm0`;_TfBi&LU!uB]!,N]jjT1`/P]pDVr/ks'p.Zilk>Y==?^T75]M+%XknL +BNEY8?'mVCPTlFW_YQ\A*=V%BGf@9Z,Y24jKs'%Pm&!NYEbJ$H9'J[ejhh[PlASl=& +^@g&]a,V0ep=X)$@Fi=YV2,jj%&GgE\r4;6?mf"PFlD31f8,V7%^je:P,)boGOF6HH1L#Xn%:H%6Pj>t +bYsC$r?j`WWp-3_ijY$T+5nE9Vs0lp2u=M5;T(aLCdCt+XNf#7HhZsqr)AOim)$rW":mP7k022M5S'.i +ddBS&l"!8$HD_h\>a@/n7Je]'ia6KqPK`i%A\T5NA7$QVMV)QK&ROP%N&a>C;XD+OCD2aZm)8o]*(k!d +;!4[R7U"r(:AJ^lI>p38@b9HHYel,;7<<9_DVgeen,M[iXGqKt&^`54DA:Xb\1Z9bbL2)kA7$QVMV&ZS +<0?b=\ +s&U"KD9Pq9.+![5NXFJYdJYLtcRT64I0m/pQ@4`P.2oRr.BEW3%5cD]d:G7_Cn>.dtLpDo9:Gn6..j[BPU!X8C9KiTF:M5^.+V,NQo0V_*uq(.9`,lbhAG^ +'N$2J8k0$o,ct^Sba.5:4%(0&.'9 +)KAY3&iFP3mua?uFCF1Q;i3!\Kb]]DRMa=;]'qcJqmrNS/Gg?*;D\UN>4=-nW_F9Vc9.qZ_ElU68o(0R +4j\o7:D`GQI>p38lO-m[[o?_MJCUoM).pV +U1+qi6jh%"U)D6P([cm'NArBPd_Qb!<$au@l,(B,V\Dul8=E'QGm?4WRa?M[1+DM( +i![(0eYW]fB:`!hh&r[B'dTf*s#>X!bL,77:]r$N'$ZT6Fq&m^G`NQhV@+&MSl.uL+R(5LV&7SbcQLVU +P[\`:;5,2\/1G>]_1)SQ([dbF9UpB#&aK^#T'`Wr!tXRG0:uo<'"4)P$51UHU?:OI*P=n"C=@ln1J]H/ +X[,Rba`TlcSk0`9Y(a_d62noR!qcZ<&VC"SQ:5\U8?A*Nu +W'4LS3:i@YL?udF2o9a>D]i\B--Hlh#SM,TPZIe<_0_uV2I2\0o9:FsFQ(G*@LI9dqB/>I8PrY#D>R41 +Rk@C)GtJ[d#:]J/fSJfo(NZcfL//Ce&1$82.%;!eq(5%Cl,(B,V\Ck5OP8D2Z`6*E&,Y_2Vhcr2&ogg6 +9liaJ7uHT\Ko%T].:W&p[CAr.<,j%'@Fdc,Lm+PY>.blg7uD'1Ko%T].3_EH>21\#WH-Oo^aIBi+rVh" +`(eYW]fB:`!hXrqkkKB?!B+;b#.\Pbi%"Go)e)G17DKD.o6D7VE@js`a[$Bd#H +aPJdGoX!buXH=V;-Um>[h1GOV^nmtf?'mVCPTk,OWMcjTJ^A?go9XNpLh6r+EDj/UICptS+"=3a(FIEi +V2/ABl>K$1eEWAK9E)>(.ABss7%=snc+Xu_pU"j3;/NluETS]=j9h&&>["'60.^1^BU0bQ-Um@KjmCth +;RDn;c->S+YCHH:[h&F.?'mVCPTk,OWT0Ol3(jeIo$9Y*'[\Ht,#4#q3quJtFh(dNdBYYg2I:k.jWd9U +4Sk8^++JZ9MW]B"/khd\8eJUcU^PeJ$e#5E,#4#qlMGbo*[t:D9/!u#il$\75Q5nSHi)Br>JL$bY9&&W +Z(![jcMDckQoO]%,'!j$$sW'0Q5J5=Sq:2rU03(9S.sMXY6&p_92!Z12E*RU*'#c;BX_p;T![=_hgTq' +?dXRa1J]GD=-,nWb77sSTj;g=F/=\)#nh5U<5Wran1H.nb2'[R5Q(#g>ISM;]k;+rCXRC`GiOcdImk/# +c9.qZ>X9:Y?FtB=G2_QE8eK;\,iYVuTMaMQSgHllQS2^;[,'%ms*WB&4M&0un=VRMc4#+.oH7.qVK^gh +c_XG7Y%jj6=(Lf!V20:$&ogfKd$u\_DduO>]Ej#[eQ9eP++N7'(Zp+(qng8dM(@WodKmVb]QQ*GaWDAt +>UcCMJjc+C'f""g_0_tkNm5hn4&bkOhp4Oer5=F*cThI-`l?#g:SJ41B:`!hXrr^LlWq.$@iQ?Z>UcCM +Jjc+C"\o#\8Yb+$V.bmi=0GB6FEtIh^An30a,V0%HhQh(,.sZY;hB9e&a=m(k1t6pr]/EcMk5#>U.p5- +S.nDNr-%QN.@T;Zc'oeLg"P06IK.9E(FK[d[F\_hjHJfE`0mM;OY025iHL6S9[^= +fn6(.=/$(dO!##^7C0KE]Jq@?9c#/%4Jdl#6*bQ513Z:`>r"Qbm'EUG7L0WH;KS#T_98"9g2pc_bYsC4 ++@td:=c8:&oMs@-a1d*=d/lTX:5Yo#qi+GkH]r#Nk7H>dd`(?(8 +*%+1_l,(B,V\INVODO;MYlUA9IFV5Q85,,$)fpo1rK7CijiYT5U@lleM).pVU89!ci$1jQhQ1(_eP_8q +9TY&LD2R*Y/nhU@=rUb73:i@Y)@M^FIV"4fmbiMKajF4Ja@_PM)e"W$QfT7&,HHJ0VJ+4r@H,L0MJ+b9 +G5>d8rZ8qm)\ASSigO,487mN(Z47[0_A',?/khd\8eFc%ad0]+C=c%4hBRc?27X]hb]m$m\)+\:>hjR% +1_a5_3:i@YRLGA9[?Sg.ojAU0bcmN_QQqY2:PsNAZC^T^BILGGETS]=1`7Q?!/k()5TT]aC!R42C2uO+/XD=&D;fqN<9RD4j8up,IR<4rChf;tkN`20q/khd\8eDL;ajn0o +Xg\Z[qIR?TR`..s*0E^o.C/!9;m.Xm6Ph)p4JdkrKB0=GQ8Mu/*DFk^=&"%QR%sK=L]mQIObIoUqFa[D +\hJ-d.7LTa,Zb2VXS9#7..UU"Wh5aJ#pS=Zcr:=c*j?Ko%T].AHV\\O8F,HH5*?.<:oF75Vb! +O[(iSZ="0@RhV]q$PR0E;FDluEeuJem8m&@UhELDp?]&bPN@TdooUGVjKSJO;.dBnETS]=oFN:7$e'hI +NMTOK'JiAmb7R*W4[iDj$sHG(I_%4Qjs``@&gUNeHd[,s?1[Bd-Zbe==cO=ulR>_b1MAp?q<>c7RAUbH +:QSgEljWSNJOk[\h^0E9P;tu./4U,Hd1dW!9nNkQd1eH+(FIEiV20SPUco5H!q?sF]J$>uO&/'a&]8<> +797'#)GO'rFCF1Q;i9cg8?O3BHJes*1oh"Fr.uGNY*L::dT28!I05QVg.G5#FCF1Q;i9cj8>C#rK0JcC +.It@?&aF3n@L-hIaNBfI>:FH>)O)0uW.&$P4n%\ekbE)Y?J,I?:b\tG20QH5( +7J%InnQ]Vc\>!/>MItZkV?;6!8$#eS'Y,7>MTdNll1NY8J*aG\Y$JXtH2Gc<^H?Z(e>*T#q06243:i@Y +\dd(YC/U&s3(Sq^,OS@:b7SNS1I]$1f9VpU8o&3Xlfc`kDnPV[hKpk]]QiMYOmVQ'=arjV-I+`MLn95U +L22ehO)Cm1\OYGCSsL\Q-"2\d%W5%Y'IC6!\bY9H5OSJidYfL?=;Rhao9:Hq%oTSI+.;EOi$k,fbI&<" +LEkP>IY2QRNLH*h]6E^M\T6g,n`0ZH^]-CY,9L4J-$[e)1M;G+"a\O>OJ]3GL-Crep0Ci2I6(M/2#&B" +_r5RY3egr`=[J9GD%Mg^I-oXqh;8"3s8&pKrTdhRGN$UZJfWaTU6,A,[!NP9R:HgX:Po@kgiYRrNe7RR +ag7rm[(3_mB?KlHRf)^TrVM*Do:MY)rqVa*:X5rW\hJ-d.7K1>83L7@4Hf64cEa9[nEXD9_r960BMQ!1 +/X2iSH"1@Ei5aWeIJ_*J,8Vt;R.X$m@H,L0MASdD*dQh=s(u&a$`m4$M#%723%6%HMWVEooB&PA +[^NX4gpo4dj88;[TDl(Ta,e"1iet-,M(VJlGtJYn$<#"^\X]`3@72Q=-:&hNMMRIjV+fV5Ut`W^p=jr_ +\TR5hj?7Fqq<"cnU'hWISl3L_K%0Xtee4VQLf-W8Jjc+C&LU#j3e./,n`.YSrUeR\gGa$*Gl$[&:RsQ> +_[euUqKQ;53:i@Y7Mhl=NUe5*TD@\K6lgkIS.lP_7EaE;fQcYWQhGlapmGcdq=`Q]kBsop9UpB#&Ru5I +i_hd8mW0FZMPre%Lm'%BUhDX[n)(m/rVGp9[ibI/\hJ-d.7K1P8@be_"'Tq`pIJ4NBnAaC'G2]][HYQH +RAU`Rl6'Z?QN,H$&`+CNMC:uVLm'%BUo61WQoO]%,"Aab\NOWU.A@eJ,#4#OPG6cQ@H,L0MAUJt +pAWn,S:+ ++;#ti`(L6S9[^=JonNe+5ksjWC?+;Te3e9,.o-h'I$)d8J,r?<1>L6 +S9[^=K(KTXHY[g?G`NP9KHLjS7DqbpD7VE@js`_)d_ef:(LFPocFOD=;%_ZI+pno-ihVAi.B6)Y-I+`M +#XMY2r*m:fd2+7+#XKY0MMRIjg2pc_bYsB1Uuc:+LQNd<&1$82Lc?i`9c#/%4J`(mai+=X#j5pB'] +Lm+ReFCF1Q;i2-^W3Q'RBi7>2Jjc+C&LU#Tl,(B,V\C:F`(:P0#SM,T7%=tAV@+&MSl14a&LU!]+\9OC +&TL]JRk@C)GtJ[#7%=rgKHLjS7DqbpD7VE@js`_)&ogf;&VL6S9[^=Jjc+C;1p&(-UksF;/NluETS]=#SM,T6kP#=S.lP_7CDBs +`0mM;+\9OC&Ocsg\7. +""BrM5pB']-pj#OP9T"^BE]2*?MqbqI^-/_Z!\ASeg5R&<)A5j&LU!]+\9OC&TM;Ud:u,Fl`\'p%,.%T +IQ;].I::\oMC:fQLm'$?&1$82Lc?iP@rl__cBQFtmbPML`"PqZq<"0Y+.s"J*`>Br2fIRN +BZeoWf"#kb%fH^k='g/`egE2h`(:P0#SM,T6kP#=S.lP_78'&uiQWg<04-ndm+CBYano`L'dHDB'N'N+ +&LU!]+\9OC&TL\_iTGnt%k44Y5pB']-pj#OP9T"^-pe_^4f!lX"Go)eU'O%/:5XjkU=tP[EDf1:Jjc+C +;1p&(-UksF;/Jdi3%:R-5pB']-pj#OP9T"^W&>koS.nhR+;#ti'Hp#c8J,r?<1=IH:5UGdOGK)EM@_&m +UkguZWpS/_Nu.is!!Y0"9*P@rzzzzzzzzzz!!*"4CMW84p$9)*h]DV^B\;J0js3CQIf&NZ-UJ@3hS&g* +(LIT!(:7h'Hi':N^A7Ams#bR:k]op87&uUFaJK4:Oi5ijKd`u(L@sTUhItF +0>031WZQ`gnK^Q4DVb0>YC?/s@uj@Hn'L5dX!T@FA6mW;=i'1!?kT%=(G=LTGOL;/I[DZVa,_OeA-$O@Dr8fTXSNF=Ta$9L\&!JaJ +b?t6(i$m'>c-=KX7ugicFD:2,PKV6CMC',P".g7pqX1ua-KX@UfsA7D.sV'hkKfYYZhMMTrq3HB:S.LN +Zd/h<-!E-.JgM;%H[gGCrV%]:RTCd*:m6g3\))FHZ@>%7m'HRmN(\%(1b2!I\Vn(Ls8HO/hgP7T^%Y2) +H1IOW=0;sVZ\+\[b[C%k`d2R#:1#Q3^W9s!>kFR%g[F\`SX?sKPbIVodhIQ0Piue:\ +oSLr-hu)UqPI5)mO2(SJmj?GNhW`rpGOF6HH1L#Xn%:H%A%KXhjCWD<;WO"Q\80^!9a:*N:Hk1qkKeVN +ZWCL^Ff865".(ZJT&f2.cT`NJ6tjocM\gm5MdL::D!8a;@!>QHo#i[66$hgqVuGdbroM"N$[s:\'hkoT +o]X^&rpTmd^]42Gai3GB;lBWd5Q>fgPG/)AL3rCG]q(e\b]Jb_aj&gWVp:iZ`>;lDH1L%W&p/29o>Zb/B?m#-Fm>2Pbi3_QioY>kQeY$rrk;)[ +fV"&phg^,R?I#-*CK@`mhu<>Y5QCQ*-d0o85C*5#f8gt93dKbTDn[+ce7ckB++Ls#]76a9l"ZDn:!2q= ++6<@i`ucZZdf96krsX-QZt3U&EQQH4Ap%OKgnjB:f`$6d]smGn^4jkd&M0Mc96Ij +/V_5mqsCkVb^Aor#Li-(H1IaFZ'='_WpI?Op[A.]mk;EMi_ZO@F8!!YQe\\#cn=[J]<%p&Ies!ApmT\\LU"KV(a +_Q;no^]47C;nI;Eio]Ccc'pZlmGIn[ZEO6%j,ZR"(QJ78Dgo%io>6]lUWGED!f*k01*o26)in:YNrUU[Rjh:EF47oAOIKYB2?'pmO_K4_TM;'cB1.)) +2E"lejcq&\NpQ]ckqBdG@d'#M-*pq6lB7XUm7WR7mWu=M[_DV'.oR(2%W$'%:Fbk9>IT>N&_a[?Fl_.E +;ijXizzzz!!%P0rBH8AK'W~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 207.31862] CT +[1 0 0 1 0 0] CT +N +-1284 -71 M +-1284 286.5 L +319.75 286.5 L +319.75 -71 L +cp +clip +GS +0 0 translate +320 287 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 287 0 0] + /Height 287 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V?\lu&+33nCg[@J`VHgWu>e$10>VHang@aW$aPS>8UhqRH6>gs>85cUPPaWO7$6dY:J)b07dY9$P +G,7#GBi7;crg`-FcF@.^]*cD+61Y%%j-N:+22@3Re0mH+$WM-QL&/+QneDa5'_DDjN2Z?gpqJZ?bRL< +CtQ15gX8$`?b\dr\od17d4jRnZY.V@]mFt7V5pXI??tMHL#gp]!Q:%:D/F_"qK:KYrVc]JA$5Bt`TN,l +`G;IkA6?*G7D'q;PB[pgSiqE9POQaK^SL:nmBSf[eVSh]&%Me+]?5jAc%[GB@p8'J/g] +B@"RCCu2i#Ck6ghH!.G:>V;B2+nCTDa(nG@3Ycl,gprV4'hc5V^P/7DIie(WiJ/M3R4qEo]bb_H1)moq28n +Gk'd8-coOdUatrm_1Q_TNfrX*04);ieF.=jrV#&kho/BJ(OYi!2q+Wu9:'Yh4])!X9%uQljsiT!l'GVo ++9sCgRnQ.pL6G`LE+'a/ZY-K66d]fmR_I[rZc=b._M"Y9<(85!:q1+p53Li_9p9;V$J*HRJfXf1.D5n6lX2jo?VJ,d3$cSK1WQoAqip?\')LOSR9o_ld5 +8fp6FqD9hUP"WH0OXF4L)`MY.?P\FH3.)EHs-?rAZ/FZfY$JZfVtTK1;mr%R,W!,q$p/Y!8=+='?XF4L +bEaaEZcPYqeZ2cpDSLJgB(hUmr+JOtAe/pT6GHB8d3QZ@G9"HNX]q,THNPSX[gl-iDg]?&^i=u#`ufe) +R51VH[5E7J-s5P=p$1)4c=9kp!A;rNS7'CT]6E_@4/oRgisIO7'*kUd5(E"U\P]+kQ0n>Bp$8l2mFnuD +:C\C+'PF>KV5pXIGsOPMI/*1Z.%\+rn(sEd@q)ASgU"q*j[Uid\!fTPR.210?\=&RZ?5?Zpk`u5Nd!q; +ERm9^`/*J:+a`p?cu0qn^>E$uER*A1anYDddke\S7365krVH3/F^GtKH?A*Ib?0>c`^i(HLb5mLrVQ=j +3a^^I@Df=a[C[es,qiYpuo0soSWo>^NH2V1^Nr["9"r[cu0rg(CbtFld,\iN;D,p^U[^TU48[Ae:E'SkSH;V +FeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8 +TjR$If7e[IUL+i6Ks'!U4#'9H\X\!PBK0k:)kq6TAF&=O$Q8oScnBM5lU&A5dBgoME9"s? +EGkElNSBeHUL.oo^>'\+EoZlThY@&7$bsH.Ndb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN; +Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[I +UL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j +?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+C@/JDK-Zf,\VG]idoO((qIW"$:6*)NdBbe/9.BK0k:)kq6TA7Obq +@hlO.o?ab">aTg#=@Fjd4ERYK(M4^L"*T3 +j%KG?C?WAOzzzz!+2ZpNumufp6L-,YHREGX]thpGjm"/]8H1Tr54:#n)(l%G:]M(H9FDSmHs9W04$4,l +q]kE1iD*YIs=NZifm_;h;-mcVbTtcF/8'LVuQVZYHHhAX8'u!M2gaOg[F[ma5\WF7CKM&kf/T$Z"\Eab-GU%2Jq/iq!__g74"4";&jZ;iVE*.]u+Vpn]_r84RhTrTX'Wn)cB?nH5\-,HWN@Wo=&hX"#fURh2IbV;J;l37!EokGn5 +MF&@s#tsm-gQF>lUN:V +5l">Sn#t#]mJe_^SduBq= + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 173.19932] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 357.5 L +1603.75 357.5 L +1603.75 0 L +cp +clip +GS +0 0 translate +1284 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WZ"m;8+3.gi)@:R8.1R@=`C6:X34QIH#XZ/fio4"cThst4crh/4EKGco.+H@-3_9DtV,3j^8kXZm +kInj8->i>QS48_*iGd3"'5`f6n!AJcYrOfFzzzzzzzzzzz0#HK%hY['9]DioE#6Fl*!0''"3*99b\GWN +8%j%*o!<<*:DblreR@^!V>?gFmG]RqIG':;U%ZgI=!WW4NVApTs?+FueEH1O@OennN?tNO\(f6$5OW[U&4?: +Lr0Jl]+XD(XU4eWLWJK=T=-UqGCMR_CkLLS2DLDH]\c44Nr'kY",)QY_S;J^<,]Y@YVkW/:md;,9*.7\ +ZEGf[^lIDq``aMr9S;J^<,]YB/KTO9SLn'>h4$,O+]a.GPD\p7W&LE)u:.:BY&ig\h;ph93p\3k6k>1" +8kglk?>?b<[c?FuaWJD0R,,ZVW#`5(/aJii4#6tM"lg$Vo4(k/L$BnC#-%nZ]7?e>r3>IZT;:AcYE[mf +:ON=DW&jde?,'#/JRO_',7?e>r3>GCX8E<`<:.7luKupGCX8E<`<#pGF +0&NLR`bnVS',"`,tS;J^<,]Y@YVkU'!_W4Za#pGF0&LE)u:.=5Ce216oh"(%,W':F)JWRsPL_<#uV]p. +2C78:mS97F1+d#\8-%nZ]7H@Ft1p\krl]`#K,,ZVW#`5(/aJl+PAmo`2.4'X8:.:BY&ig\h;c2#f_W7d +s8dL9DMC9VnE[i92U8F?4a]YO+&/$u<;*mh7<('JM9f!0@ZVDqm,]YB/KTO9SLn'?kAel_+SPhdFjA\M +R+d#\83>FEr/54>MV1arOWJD0R,,ZW*.VbDTR%/:k#pGF0&LE)u:.=5Ce24Kc3U;nB66`)BA,,ZW*.VbDTR%/VdO>8!FPt3>IZTd6J0P%pQ0eNu)YGB;[iYY,n_sHAYjmS9=]&lMhVj(OV^[2C*L%-^3qR&jde?,'#. +OaPPe1ldr=%S^-IE7n6)*Bm4Q:Z.Dpu_BdFu!a3\kmNupS +MccKd-qDd[:7-g'MURHm2`8edZ6jY$!;\;#)%;L$.p%u5-3p6[)OtkTYYHWe_''\Oa-,hG&jde +?,'#.O8>jq)R[Ug:rVESpg>(KMob=_d58iH&Q^gbeb^d^RFPG&jde?,'#/JRTZ#M^A]3 +`$F!8S1%LE"`+*39'epb_6r$23<7Eb2n0IUj5Q&Ho!!!kHrrM99Z).~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 173.19932] CT +[1 0 0 1 0 0] CT +N +-1284 0 M +-1284 357.5 L +319.75 357.5 L +319.75 0 L +cp +clip +GS +0 0 translate +320 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"/k;+.!_*68)B,eJkXA^\Oj-IYC-R3_$/P"B+0>pPPCOr6iS>sr9[]81!\R4`*7,g#BsYHF%!-BUub +7$;*Ch08bp*N*Q=H`H["n'9DRIQr#G][&Ua3PXbDI%GE)o=gc+Oq9qc+bUCn#U+j463n`f&4-XGKFgHU ++bUCn#U+j463n`3j$"V3A@R(?ih%1_U'VGPBR/2QOO1i6pU7l'-iRO-EWo[=pkQQ('l80T+8-m[ +YiU^kfSa"NQ7)Z?06.5#T(n)Sa;&N:iZIk<=0UeCJ(TZa_3QJJ@4uX)>FtVF]uUq)H/]q>,$2#1M1=09 +>eu^Rra@I#"^@SnLV:bD',)%^6Qik0D"dTf6:-<1+3nnV?&(\dDs4A.oTt46qJ--1,p@j=>!RR/8>*^5 +csICukcY0^Q\5MVZaH[C*8:JW;3dEtgJoA70-BEZi^q=A60`kd]X5Leds.HAY/hHn?R +ndhD.(.@"do]Q)IqYBs_g"G&'4C22pH@C=7]W;>869Fi6pHttl+$Led7(Bb::kb-uPgn76IJim1rVQ=K +L<.g7nRT_cboI`B_hAX)Ff"_SARu&.FmG`BF`mAB[Hg*g%1OK8^q]rU@7jd0>`="A8HE9sWDc_oLp4:p +26"hcTI4UX"b-=?#nO<6ol]?9mAQ+Cd=VZ7/@nYScsI/)mk8br@DUR!?ZEjO8&n42R"tbNURo&3B4j_G +P="3Wc_!G`,:l;67c`Ui@gMu&j\'_>FA=cE7MlssjVYlc/sPqO[AtGEp\/Y6(:,K[grmd[#`]LdV\!)C +_9^EPoG*$lrf!b.RMA@11/b`9r`TkYIm**fr*M[6ng(36+cBB[D@7@RQ#'>h>?dRH!I>qSnmETX!E)L+3t(XX(0r*Z!(BlJc8gaq=ds^ +/Nt'l)l:k22:-Kn#S8KS1+b(0WY#+TR.;&W=<+7W/AQioS\MeK]1@O.36'/[X)^4=Y$D2[FfC4NJ@-t0 +X>l:h8]l>grZYN*M`!AP+$Foq76-&#=qGt/oiWT>W2T1oD2-?>$;'KcW401BV35@dRB@qh5OmsihVQiT +QtuFYA3@WFW3*1e$*XitqY6!EYRIB5J!5AJiAEJM,4`t%9Y)#72pbn>,4@0G\b`hF[tfu)5!GS9CM>dg +32j,k8B"u4YCBLI$UJ`^=f=qbp\nWB+ar>m1OU4`qbb*2'ZOc^s+jjQ$Gt$kElL<*?$# +Xf\#iSXZ+475QLBhaKVXr`TkYIrU+F"#E^]I$hF\*Vk,YrbApCX%QbITasL=N032`Kde +\@g_MK1@!4%^jiR7*$@+`Gj6]95t%3[35&>U>'2'/DHqbQ^=a%W3-?CO`,gs>=hDC4r6*OM[Q5_Hu4N< +0uBBWii!G,4?mOI' +)u==NrQS(k;Iaa-8\=gbRWI\r"nX`h)&X;0)B(Xr3,fnZs3P*\kV!5Ks2NrQBL.,>8<@2,KDRUJFmRDH +^Eb(DrbMW6,p@j=>!RR/8>*^5csICukcY0;p@l#a+)=@nDTtNq[\]h8(.?p7gV/0.*LXFFjQ5cA%1R(^ +rO%1T#"Z-"ec5PDhj-/KkcY0^2aF[hhb6:b/P+9&c8W!8D?Hu%llV_po/B8i+Wc[hB5VU)F`mB"b)2c6 +jmK=,o7iX9H#'pn\[f8*L'ApFhu=UHoP?am[\X]I)r$fcmrH1<^Hpf5_*Ir._u?p6XfW%69he>*($1[F +Q(b6c\bg0RoB)FDH#'^h-7":bGY4?UoP?am:Me5ogDX6K-uS,70A:IAc2~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1283 285 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +0.941 GC +N +0 0 1284 286 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 286 re +f +GR +GS +[0.6 0 0 0.60069 0 347.4] CT +N +0 0 M +0 285 L +1283 285 L +1283 0 L +cp +clip +1 GC +N +0 0 1284 286 re +f +GR +GS +[0.48 0 0 0.48055 0 381.51931] CT +[1 0 0 1 0 0] CT +N +0 -71 M +0 285.25 L +1603.75 285.25 L +1603.75 -71 L +cp +clip +GS +0 0 translate +1284 286 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 286 0 0] + /Height 286 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WcYJd>*r"Xbf8,BA7"i.m_BW?r:dg13Rmdo%0Nf*`bt9pkO@"k%%KqH*5VNQhU)7-+&IF&b+=\!2 +,,.mBLrb,Q"pl10\Y%0rH)CCKP0`=:bWU#Q.l6?HB,Ft4ESaDFBf("NSiqFDjo:*BG5:^j:>Z5$JWSNq +J,N=3Fqp-+8AqK`3#._'(+.63+EeeU$73iHXE@N/- +7:ifZM/uE`+f'RN5(jk),)MD;;\="!.[r"`S9%:0+n^mO&JB(5,#1RfLf71(Cgl=n;c3n#L[E@uDQ&@:66]/n`2T\O[P2^H.4Kr"PBPp^U>)Z''LF!=7Du1( +Cpl$;c7(2qe9c@a#WG^N&g7jb,2uR$2HuP.kEHZIl92aAKGuBbM3*Nl&TK9MRk7=(F%Qc_ooeJ4h;og8 +:_?9P7%B-j;c24a@H*eU;@'77m/E?(>?!ut+VCUri`&l8>F7D5PS?IL-+gpDRoDc_+VCUri`+EFAS#Fj +DXQ>mGFuNgH$Qs1o$*X_T=iSf-I+UtKogUG#JVJ[#6tLS`l?TCo%.5u/Zua8 +rH`JFoB4G\*^+i/`/.2PZRnk@F%Qc_&JB(5,#1R\NupS%MtPf3CTUg,Q^B/>GZQb$`.]9>],2At<;@&+='bR@<`\t(*5W."O[MaYWJEMK($=Z@[eN#6`VSiIJS&)E3M3Z+$=d%2E!HX5MuD.gpqLNT,0,_>Zb)F)&YGRi4uBp +Y;Z7$[P2^H.4KpL$/I^s)_*%gOGHgIE@P^kS=H\2C<^sX*ZcA/>[2+%5!;&7gbLrQg\gQYLC`G*j*UOg +D7u#&UIUA'q6[RaER$!^K[ft3HjDZ16kUXTV]t,"e8Ai/jiN]6XVLcU.l[KF8kl`JER$!^K[f+pIg@u4 +6kUXTV]t,"<&RsJ39QM?6L&0J(r?^sO[MaYWJEMKQ-C'kc8d2)OLeImL2]LK,S:Z/.O%VVA[@)IF&#:M +V!_3U+l#m+8VBL%MMP2.fkb*VS5W;kW=)^A#WG^N&g7jb,2uR$2HuP.kEHZIl9_p5rCKPuWJEMKQ-C'k +c8d2)OLe0JL2]LK,S:Z/.O%VVA[@)IF&#:MUomV7+l#m+8VBL%MMP2.fkb*VS5W;kWI%Td#WG^N&g7jb +,2uR$2HuP.kEHZIl4VVf_@76B6qG7qLc?-7bn_YO3@0B@\j<0JE'm&>&JB(5,#1RF7D5PS?I<"hd'P)_*%gOGHgIE@NH-/PM[U8dZ`, +XlkIY6U`74&ofBF:.8a#Ko'kG8Kjm-otq>YW/)]c,#1SFWMjXA0QGAe.>1YM>8XeD8AqK`3#._'(+.b.k>[^F77u!qrF%8HMuO'B7KjL8+DAZYI'"?F&#:MUltAr&8nG&,XN\M +7Du1(V0FD'[G%4eS3;4BEH,un.q`I+%Ndmi]Q.t1nG`C`Rs\CDQoO8n'LF9:?ObtKMF\p:i`&=9MOKEJ +>E%.tl03N-5!M8=/DRM8Z$!7?S!tq2S=#[G>$?7Lp?gI=Im#J>YfW5pD.2Fo;c3n#%(7kVJ<5Rc&TK:8 +3e%#%aN-p<_csTf'cZl.bEm"J4EKZ/d*PVT_a"2!o?a0u%j!jckVLu1`&X`k&5p)g)o<%!O[MaYWJEMK +'g@*E==i$=f<2N!=^GDeDOHr^>'8rTdSb)^SND&(rlK"cig`qF#Urgs%cN/"80T>=<(*76$MX#H0,Ele +HhZ64gqS@=KaS^J^%1^Ra-2^N-Vg3,pV6_k0eorT:DPngg;;Te)?+>`BW0C(JR0Ip7.!@Oe+[ETWJD6T +,#1SFolER(\N2f6>:e-$Dh0c"0JG[S^1uM^qSq52@-l%j!i8p&3FAfYYQeVkal%QG+BV%2G!n8AqK`3#+=M +IV\/LI#mpU]6B/`Y@#'$IcJ#?\]#=2c8d2)OAb`B&8nG&,XN\M7Du1(nb5+Vh7.H7eLbROgABmKMrOGu +R@3af%im]A$WAP/d'p1mT`2SPP't"bI[T9FdJ#gudn8R\P,u%02(UqW6cF0mGR[Q9^n6kk1 +'.6O;J,XhZ:MU=OkF@]%8.gSqPDVmS,iVckS;Q%&XK4O0nmq@L(Yhunf8BsJYd!W=.rA_SS5W;kBu^Y@ +();ru8AqK`3#._'(+.F7D5PS?I<3^\-:.O'S.Lf4n*.Opo>9Up/r$6a8K+hS*rO[;UWWJEMK +Q-C'kc8d2)OLE4GhLpbW+XH&H7%B.WAd]:8S@@eU:.:ri&ofBF:.8a#Ko'kG8Kjm*15%k`7ALJ- +E@N/-7:ifZM/uE`+f*@#_HW]X;@&+='bR@fLf71(Cgl=n;c3m8'8Gul +WJD6T,#1SFWMjXA0QGAe.>5&?6[7or6jb(LV]t,"<&RsJ39QM?6<]^!]IbTWKoh*h&TK9MRk7=(F%Qc_ +XihL:();ru8AqK`3#._'(+.k`,.>1*ZMMP2.fkb*V +S5W;kBbais#?4(u+VCUri`&l8>F7D5PS?H!B*dsbMF\m9i`&=9MTTN='BbmK6:`\_n;aK('LF!=7Du1( +Cpl$;c7(2qe78hTW/)-S,#1SFWMjXA0QGAe.>62]$V..O'LF!=7Du1(Cpl$;c7(2qe1cQn<(%':&Z6i4 +<)bb[QoO8n'LFR.j&ADR+VCUri`&l8>F7D5PSANeOs-?icRU2tOGHgIE@NH-/PM[U8r>eCaVIY!k7s+J +8AqK`3#._'(+.6oP!i3Cgl=n;c<7)8;;)X +^9*2r66]/n`2T]3GUR=7g7H'L4aXWi\Y&dMUSFU]p?gW4)I&RgkEH\Z)3Q/5X$8*YSWJY+&TK:8R[KQm +7dK^=FM2"QT:[2F*#on:g\e5h(DhS?(1n@BVkiP&ODNl\rE0`sKLD>fLf71"n`.Y[7RgQ'hR[j1fW[#9 +^>.rK=0Gp>$k*OT&)4tf!p>d^I2J+jS?=FcE8AqK`3#*1rhnNMLepI/Z[GSKDs8EBB0!'EujQ,CbT7?k!_hTg] +iU+2bs"lPheEOS10BfHug4sXFmn6I@X,8CTbt.c5$ep=N'SGG$n]cI!g>/[XJ#EBlW\' +a,c-CqmnJnj0J\s7>AM`aVTY(dT7/\*[),n,2uSOfZk7=>$T7g_\Q`;\="!C:CLSo&RoVed>/\$PV]nP+m_!15'jG%p-\e&e]16,#1RD +,#1SFWMjXA0QGAe.=*Wm6[8@u?:AshOGHgIE@NH-/PM[U8r=YtaUmlr*ik)l&JB(5,#1Rc\,g7G>QUb6tZg_3#*%' +U9`Ih`&X`kk`(Z4iQg_JMWd#C&Z6i4<)bb[QoO8n'Km"j+l%'#:_DMK8AqK`3#._'(+.tA.*Td +%obqU&/&t4,#1RF[Ip;]16uoU8nQV3#*%'U9`Ih`&X`kW+\_VE*'#P7fLf71(Cgl=n;c6RI,X]'L%,pX+r=r0966]/n`2T\O[P2^H.4Mhm,[7eU +FalhZJrkde&TK9MRk7=(F%Vb.k>[^F77u!qrF$cbKIki +04)fLf7/ZDJj@(h]mR@k08>Op'hNCDa&gn[P/Sh;c7]q,X](#GWI%& +OGHgIE@P`$rD$KSDVS?:9Dc>8[@<\V/H47Z(GB[fPMLf4n*.VbE?R41VkPq,r$p%m.8:Bq+0>LWA`K*Pmr +A+T<``Ldq^(A@.MGO3teS,`-Un`,TmZRnk@F%V>G&'4tB4e@[)+XH&H7%B-<-7:0V[9E12)@[2f0.nk8 +DkEVm/oI)JSLshgj2R($Iga[O5?Ve1'BbmK@[BIQjku`kl%g$?,#1SFWMnXqb0#nq:Kf8.Ke5ugF%V>G +%-_.!Zto]VPg.Yh.>1*ZMMP2.fkb*VS5W>P>"FBe&+am2$6^"Z,2uR$2HuP.kEH[m'9X*cgH6L=6:f%o +Lc?-7bn_YO3@2WsKObA_4ASJC&/&t4,#1RF7D5PSBsKA-ho9 +:Ih2l&ofBF:.8a#Ko'kG8PrebL2d1^F@6HZ&Z6i4<)bb[QoO8n'P2sY/Yi3&V^#(FM)Z4^VkUKM_SeI4 +Udu$<&FMoWN^t<=i`&=9MTTN='BbmK+u^c7\3G7",)),7;\="!.[r"`S9%;[`/kDd^?+U8'LF!=7Du1( +Cpl$;c7(2?6F)4?S>RT0#`1MU&Z;A/fttZfVkf-(A-h5[L_N0":.<+#W,/mrER$!^Jfu#K1cue@6qG7q +Lc?-7bn_YO3@2Y)6i,CQ\T`u>nT7/++VCUri`&l8>F7D5PS?1eaU4o9V^#(FM)Z4^VkUKM_SeI4UtBQU +^)Y@#SrQ>b,iVckS;PI$$PV]nP%%W=)8`Judh+$^7%B-j;c24a@H*eU;J\'5/!s%U3[09>Lf4n*.Opo> +9Up/rMGJu6Fp/i%XQ&K#'LF!=7Du1(Cpl$;c7(2#&'5/j.m+i76:f%oLc?-7bn_YO3@2X6+o-.4>Nb7, +'LF!=7Du1(Cpl$;c7(3nL1CIo?(R'I+XH&H7%B.WAd]:8SF7D5PS?PW +lV:hTR'#t%6jb(LV]t,"<&RsJ39QMc/!q$BN^t<=i`&=9MTTN='Bbku@0Mk9dh+$^7%B-j;c24a@H*eU +G$pL?C^%.TSrQ>b,iVckS;PI$$PV]nZ?7KJV=J^r$6^"Z,2uS/1Fjt+F)uEkhS&gIQ*rAdc7,7;I/gWU +Y`OZ>&LiB$S;N,$d1-j$>-1i[o9ZdrcRph&DKr+8m`Oqb5 +T!$]2oCMR+K7dZDA"-_A$PV]nZE\H$D^nJTOa5&i%rKoh*h +&TK;SQM^FF(X)R0]mK-*mbbeZ_M&@:rKU-m7L"1cjW,3W)VWkOKGG;b6:f%oLq%o:BEqadG':=H_Sm^: +^N!Xh@!m@W('_pfKBK;CG4rQRm8QI\+VCUri`*9:[P3fGr26^t`tLXV.O'S.Lf4n*.b^3!\[h*VO0nZ, +,0i1pKoh*h&LiB$S;N,$dC(mh.I5!5)lmjjIlFK0Cgl=n;c3mX3#.QB8AqK`3#-;^ace)B7n6c*qOL7F +!p>d^66]/nU_"/Y.O%VVgGLcC;p)c2qc'd3N&Ya&ofBFKoh*h&TK9MRk7=( +F%Qc_&Z6gN#`1MU&Z;A/fttZfVkak:E@N/dOGHgIE@NH-/PM[l80K8<<(%':&Z6i4<)bc:0QAQdKoh*h +&LiB$S;N,$;;"(K`2SP,66]/nU_"/Y.O%VVAb-0@V]o"EM)Z646:f%oLc?-7bp,oPMF\m9i`&;Q+VCUr +i`&l8O"bQ',)),7;\:I37%B-j;c241_SiF0'LF!=7ALJ-E@N/-7:l&r';m$P#`1MUO[;UWWJEMKQ-COI +S;Ki\,iVck$6^"Z,2uR$2NKP3.O'S.Lf4m7KLD>fLf71(4Mo%9L_N0":.:ri&ofBF:.8ac#SX*[;@&+= +'p1gR`2SPP'p[#g9Upu2+XH&H6jb(LV]t,"<&TdkE@N/dOGHgI.>1*ZMMP2.ff[-iWJD6T,#1S&&/&t4 +,#1RLopt9C?&O041m4C4#r#p?^Jl-"s4F +&Jaor_h[]pqeklZ^!2Fp*'$oecq`IIWblj"FPjf\S\>n>d`fqZiPUG?:Ig%iKD4iln+8c;P(*?!a4A"@ +VbWegLtS*GY?rA6Gjr3"IVZ^G95_1LD+.cOV^l(0'T_>IoQ7cQT:_`/?8db%NHP(m;)-sWqe^=Pn(t`^ +<4&m6nFbC?+#n5435;#u?W:%qN)Vnmfu!)@nZ&7=ZBkci!gUG/NdD3PJ0>Xku^]+(F&^p@N*;Ei2!Jr:n@S8F+/t[r)qAq9,*X4*[*jQK@5i]mKK[ +^3mJ69C]dW\pF/@@AiQ" +eMB3Gq>'3GJ,&NL(1mf?s/8)0r4!$s`/,0-rqF/T-78XY*]m3*<3+PuqWXp8J,d3,nTjp!iGZI+VUKaH +$7H\XdaD%RY;t(G@_Cc6lSbbV/MFYeAnPb7#B85jM@9n%N/s$Dl7PqV:S'Zt'(R8$m(h)J:Ra_Kpp2Qr +%c"U9VatKGGkd1M^:8@Rg8.H/MPm4JVbVLrBm*ln@Ud[@NZC4Tp?gU0h]o.3eV`m[RiU./$[]u9k?sJ= +iPUEU%j!iSk0/Sk+$+gWW0jEXV5$PArpn5cbn"Z"G2W)]gVlBlVb`q&NSasO*j#4P7+dCgkKb52FJsEj +]"G]Rr;#rXTqTAZ&"CIQSLmOj-.@'ait<3["Z3ScI2Wqr>R\5 +XLcB#6>*nW-RXgm/mc3CkrV43NJrh_=cX<8f"H=uU1e.NAu>OH6\c/NLi$l*8!R(]_Y`W!YKib)hl44U +jfDW???3@SlY^,a\T=<(H#e"K[.U+OG\78YVP]5eg>Cp2[VKGcA\$'N.p)H3gK4r^8`5G%KmhOBYG/\^Jc$$4ka@krCc!UBS]^2`OlVF-:*?6&85H"!B +$O[?].thEQNnsW2juX^%cM2ZnG38Yolbu)'PEV3RbKI(r^]*fI08k>3hS4Qtl2*ogS3(qtq=DJBX/"[H +\WN#:!s]=KHM+;epP[0r]4/G6B"M)Dj<]"AMUr9E>-7VfLr7-JIf0E(EoY0!Fhb?7O2)<8$aX>349lB< +Cd&H%3-!sRoB2[Y7o[fi%j/g3a7o#XgNjt+7un^6qG'm6)guq2QeZE5Wf"IM0$h\_b*+2K*(^-V5eUJW +-Lemh6Ds8Xe^f+jjR&^6j + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 381.51931] CT +[1 0 0 1 0 0] CT +N +-1284 -71 M +-1284 285.25 L +319.75 285.25 L +319.75 -71 L +cp +clip +GS +0 0 translate +320 286 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 286 0 0] + /Height 286 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V^M@-:"39r[_+@n%r!,"@U7n;j#Ciir<(,dk%dIG7'a-fjE)I7@E#1ge5uB%2"VhUa8o[[H&8Dpo +M'=-t#V$TW^(1\1;"EXUr$6`3_s91EHd9/^IId#UT6!f$=^>Uj9W-A5&@QcGd%U.Ep<_79dcHg&Ie1fC +^]!l\kKfbBHes2Fh=3(X5KE)0keRKG>^PY!='k^dUQ8aX[F\c8rEXX'[=7cIqsRlVoZ8JQXIWnqq)8[o +HXU>+q/ZHlrDpT1qWm/\I/j0>fCqE%giM6CHdb\JXhX8$T7/hKq!`P"qU$nlSULL^oK/uC>B0N3l1#3( +5CS"AU/tZHjbbRk(jk+cl`J]3rJpi;5Q:GMJ,]9BpYV$9l;^nk[Cj/bIWWb^B:jZ0rQZkj[QO)u'8QU+ +EH1NTcT\u-ZHJhclD`<.rSE##mGeY20"Uc1CVTlFlh-HXfC/(tDl2DPV;J#^rq^Ec.+b%d\$KE)2rB!= +Sn`KJr+kpdVt@UID7ILO&cL!(rU+b'AHr,$F!jbT4EKZ/?N2VRSQ#Ar%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EF +Di9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>oL^JfkP",^^hcsD?XikK +d4[NY[cj'JITuG;:;8CW-)]W9rK>WJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?- +I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs;D:(!srO1pVS:4]7 +92<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>WJMgie4[me'mo\6*I +E]`Z4atB)U-LDt\mk!h\*aKYNDWJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q` +^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs; +D:(!srO1pVS:4]792<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>WJ +Mgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.\'of(m#oJ$dN/JecGIlod?T:9VXD4;o\6*IEr'Y# +000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.n +am$9'^Z>%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG; +:;8CW-)]W9rK>WJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIo,g@(83 +17:hi6X\eSfXX/V1,.J4rKin#>P(Q`^Ff:.Vr&9gP1+?-I`^i`7DIpUgS/%qqJFV53W6DMQ^iM/oLhro +/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EFDi9(dl*j*rjRh3$?ZQ![ +d4[NY[cj'JITuG;:;8CW-)]W9rK>WJMgie4[me'mo\6*IEr'Y#000!>ka.fg>P(Q`^Ff:.Vr&9gP1+?- +I`^i`7DIpUgS/%qqJFV53W6DMQeY@5S[(m8P=^*,loPS:[msL(At-C3'%:cWI`^i`7DIpUgS/%qqJFV5 +3W6DMQ^iM/oLhro/c$ckh\Q0Rdr1.nam$:Rb`fAj>0K)hX/O"]PI$-MbWIndW5ZuEZ\hr14?9I@`QXOP +MH\4d3NK*tb`fAj>0K)hX/O"]PI$-MbWIndW5ZuEZ\hr14?9I@`QXP#d44u/^DnIGU^]P +*PGMRDrIEZ3!/mPpUieoIM2XeBWmOe>7m^qr-KKo+U6i(@U1KMo:pYJ!"9\).Ppu-uQ7uj*'IC_/W"6f +[Yo4'skHKh9r@WT(\mt>i0>882l.E7N\))Gsp`R8_]NcKD_6gc10>IF*D=Y__rUNa#=@M8q4#Y-sfD&VLg2! +lTs8;KK=mXEqn+9QB?+9WncY^/Ip;.1jXDR6hIq$;FHKUiC`o_LK>7Ym>q2RV5jk^gbV@>?]bVRT#E5N +,,IItq-(Hq`hdoTn)oltKMm+Ledg!;RXU(9KdE,<8ak*q^OBAA(\HgJ#WkROm-1]Qs^%r_:0"O0%bCfj +-`If&Nb4E]r=V=-EbjG6gClScubhgbZSi=[fYr:9X\4C4!Fh/T`mD>l!,/[sMUr:ogTc(BLbH+[QJN\s +:MODXgbkB3AGq'S)(U@I2floB(F`45Rq/M5e&hgJ"U*rYK8hhm%KORu1`b>gA7cf3sDg@Nq$A&llc44b +Aln1UlEXGgtFr:.i$5QCcazzzzzz!!$EPr""0p]0Z~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 0 347.4] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 356.25 L +1603.75 356.25 L +1603.75 0 L +cp +clip +GS +0 0 translate +1284 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1284 + /ImageType 1 + /DataSource Data + /ImageMatrix [1284 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W0k_5M)o1uA6q!i/PCjRCLo?,cV21"b>6b`S*/]PC(NFC,6q!OJEU)&M30YTs;FEpQ3#%K!%`L/F +E@<"TKb@^g/]%2NGf.4ls-VsE?0]1R5#ZP$oe%6A9DX_srUBsizzzzzzzzzzzz!%?\!oB3`s\u)C'>?l +[:_M)h)bEjkT]iP16!)O`C0E;(QIiG^J!!!!<;QX!scVN41^O5@>B&TS_V/1S?Y)9Q)[^U"Zg:5I0NXG +ge!$upbmbQgZi205<\phf93-46+r;:qj6#d[SEokI;mFsjSr-75>M1#91D;/sc^0TN&4psT$o]Y;RMtq +8&]=YCtIeiPG9udD;!.ZiKcH^=a]K$pLDqqiW5!D.c1=G(;T1u0f%j%gU.F,g+S@`RgY.]2V5'ZSTiPU +EQ!s@5B9:%9]fB6pqH?smaHZ.cRH2mmBrVQ>%>I*dE9udD;!.Zhp5(1$4n"4gmhOSN]r:tD_ZZ-cKlg% +b[>g0spZEeN3aH:TF=at;rcou^MM,LhcI!f'lO +k#iN]Ymiqgd=a_^"fX&nn^X0*TF;29$cBm"oae:lmdBN3n*fZ2D*A=\/M.]]P24>?mb+ +oo^U`Y0<;L/ADWSA.!!#iM;lgq*F`,rM*ET0.Bs>efS$Kq$<'/KP].!!&1/oB&b*HO^Gm'Km^6i^VLH+!ldjXh>BfGdO_oLBoP)%@E>Hk"]QnOW/'hRL +CV"BnY5h7ZS6C%;j%\)E@N.=+VCUri`+F!\[f7\a,X_PY$%,%E1N/FOGHgI8I1/4'bR@<[9?mC*Z5jK[ +r:0WZY.P"FGB+G,_SRUMF^')`2SPP's6$'_SY9LLW@ESZ7jG=FGB+G,_SQja"$?U!(t_sZh*bYo!`PpI +f(1C>F^E!C)$[)d*U-chZYn>+VC':U@j!7#CsQpHa@[^f7iIV;(q*pJrke(US@F1[Kce](&WKq/54)$@ +7W^U`3i^pH9]:L!6kXr+#")O0h*"C&/&tW.f,p3U\VfQ3$M*d`2T\PAmma?F\395,-J0pF"V>!E($4RL +f71*beeI\l]`ZJ6sRDLP:mLhNegUGE@P^m=dY4hSW\YfLhlNZT\''Ynu)(u7%B-j;c2;n_K:b&PU(oMK +_^sf4<+s2,2uR$2R>)WY#CJp&oBE+PEptKi`&=9MTTNH#2nCa#Rg\PaaX?B+4)k~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48055 616.32 347.4] CT +[1 0 0 1 0 0] CT +N +-1284 0 M +-1284 356.25 L +319.75 356.25 L +319.75 0 L +cp +clip +GS +0 0 translate +320 71 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 320 + /ImageType 1 + /DataSource Data + /ImageMatrix [320 0 0 71 0 0] + /Height 71 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0RYt?6<'F%qWfg8(0$S)gY74`;ob!N>bb0E=)`7?m:,uRhNiWE:bLffIhL]e*X5\qu&ln'bVAhOKY +2SnaiGKn7fpJjfQjjkkj>05%FImlW.#Q<6cP/gqH1I +At.(d+'d9hIYPM[d:S)46m/*CHbDj5^rDtI-+7c_%UjHi2^CJ0+*jL;SYQU6.:97A>N\$k@aU:9rX\\1 +baj\-A\P'NqcQVLmHY-c;5;tVG]=qeJ-?K`Pkg8i8>`S2:[)"H0JYM@^iD%h^J'M1T_UAI=5 +C)>&#l4jXWhk8*QDie'k$9^QXkFAROuqn'b],4$2P=b3XI_b9ACthYs4sUDq\q?WNOo~> + +%AXGEndBitmap +GR +GR +%%Trailer +%%Pages: 1 +%%EOF diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_radius.png b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_radius.png new file mode 100644 index 0000000000..889411ede8 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/doc/figure/blend_radius.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/original_trajectories.eps b/moveit_planners/pilz_industrial_motion_planner/doc/figure/original_trajectories.eps new file mode 100644 index 0000000000..23215b12c2 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/figure/original_trajectories.eps @@ -0,0 +1,4376 @@ +%!PS-Adobe-3.0 EPSF-3.0 +%%Creator: (MATLAB, The Mathworks, Inc. Version 9.2.0.556344 \(R2017a\). Operating System: Windows 7) +%%Title: I:/Entw-Technologien/Studenten/temp_kai/original_trajectories.eps +%%CreationDate: 2017-10-06T18:32:09 +%%Pages: (atend) +%%BoundingBox: 0 0 1152 564 +%%LanguageLevel: 3 +%%EndComments +%%BeginProlog +%%BeginResource: procset (Apache XML Graphics Std ProcSet) 1.2 0 +%%Version: 1.2 0 +%%Copyright: (Copyright 2001-2003,2010 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/bd{bind def}bind def +/ld{load def}bd +/GR/grestore ld +/M/moveto ld +/LJ/setlinejoin ld +/C/curveto ld +/f/fill ld +/LW/setlinewidth ld +/GC/setgray ld +/t/show ld +/N/newpath ld +/CT/concat ld +/cp/closepath ld +/S/stroke ld +/L/lineto ld +/CC/setcmykcolor ld +/A/ashow ld +/GS/gsave ld +/RC/setrgbcolor ld +/RM/rmoveto ld +/ML/setmiterlimit ld +/re {4 2 roll M +1 index 0 rlineto +0 exch rlineto +neg 0 rlineto +cp } bd +/_ctm matrix def +/_tm matrix def +/BT { _ctm currentmatrix pop matrix _tm copy pop 0 0 moveto } bd +/ET { _ctm setmatrix } bd +/iTm { _ctm setmatrix _tm concat } bd +/Tm { _tm astore pop iTm 0 0 moveto } bd +/ux 0.0 def +/uy 0.0 def +/F { + /Tp exch def + /Tf exch def + Tf findfont Tp scalefont setfont + /cf Tf def /cs Tp def +} bd +/ULS {currentpoint /uy exch def /ux exch def} bd +/ULE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add moveto Tcx uy To add lineto + Tt setlinewidth stroke + grestore +} bd +/OLE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs add moveto Tcx uy To add cs add lineto + Tt setlinewidth stroke + grestore +} bd +/SOE { + /Tcx currentpoint pop def + gsave + newpath + cf findfont cs scalefont dup + /FontMatrix get 0 get /Ts exch def /FontInfo get dup + /UnderlinePosition get Ts mul /To exch def + /UnderlineThickness get Ts mul /Tt exch def + ux uy To add cs 10 mul 26 idiv add moveto Tcx uy To add cs 10 mul 26 idiv add lineto + Tt setlinewidth stroke + grestore +} bd +/QT { +/Y22 exch store +/X22 exch store +/Y21 exch store +/X21 exch store +currentpoint +/Y21 load 2 mul add 3 div exch +/X21 load 2 mul add 3 div exch +/X21 load 2 mul /X22 load add 3 div +/Y21 load 2 mul /Y22 load add 3 div +/X22 load /Y22 load curveto +} bd +/SSPD { +dup length /d exch dict def +{ +/v exch def +/k exch def +currentpagedevice k known { +/cpdv currentpagedevice k get def +v cpdv ne { +/upd false def +/nullv v type /nulltype eq def +/nullcpdv cpdv type /nulltype eq def +nullv nullcpdv or +{ +/upd true def +} { +/sametype v type cpdv type eq def +sametype { +v type /arraytype eq { +/vlen v length def +/cpdvlen cpdv length def +vlen cpdvlen eq { +0 1 vlen 1 sub { +/i exch def +/obj v i get def +/cpdobj cpdv i get def +obj cpdobj ne { +/upd true def +exit +} if +} for +} { +/upd true def +} ifelse +} { +v type /dicttype eq { +v { +/dv exch def +/dk exch def +/cpddv cpdv dk get def +dv cpddv ne { +/upd true def +exit +} if +} forall +} { +/upd true def +} ifelse +} ifelse +} if +} ifelse +upd true eq { +d k v put +} if +} if +} if +} forall +d length 0 gt { +d setpagedevice +} if +} bd +/RE { % /NewFontName [NewEncodingArray] /FontName RE - + findfont dup length dict begin + { + 1 index /FID ne + {def} {pop pop} ifelse + } forall + /Encoding exch def + /FontName 1 index def + currentdict definefont pop + end +} bind def +%%EndResource +%%BeginResource: procset (Apache XML Graphics EPS ProcSet) 1.0 0 +%%Version: 1.0 0 +%%Copyright: (Copyright 2002-2003 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) +/BeginEPSF { %def +/b4_Inc_state save def % Save state for cleanup +/dict_count countdictstack def % Count objects on dict stack +/op_count count 1 sub def % Count objects on operand stack +userdict begin % Push userdict on dict stack +/showpage { } def % Redefine showpage, { } = null proc +0 setgray 0 setlinecap % Prepare graphics state +1 setlinewidth 0 setlinejoin +10 setmiterlimit [ ] 0 setdash newpath +/languagelevel where % If level not equal to 1 then +{pop languagelevel % set strokeadjust and +1 ne % overprint to their defaults. +{false setstrokeadjust false setoverprint +} if +} if +} bd +/EndEPSF { %def +count op_count sub {pop} repeat % Clean up stacks +countdictstack dict_count sub {end} repeat +b4_Inc_state restore +} bd +%%EndResource +%%EndProlog +%%Page: 1 1 +%%PageBoundingBox: 0 0 1152 564 +%%BeginPageSetup +[1 0 0 -1 0 564] CT +%%EndPageSetup +GS +[0.6 0 0 0.6 0 0] CT +1 GC +N +0 0 1920 940 re +f +GR +GS +[0.48 0 0 0.48 0 112.8] CT +[1 0 0 1 0 0] CT +N +0 -235 M +2400 -235 L +2400 940 L +0 940 L +0 -235 L +cp +clip +GS +0 0 translate +1920 940 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1920 + /ImageType 1 + /DataSource Data + /ImageMatrix [1920 0 0 940 0 0] + /Height 940 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"-Q5m[ZK&4Jl8OV0*_(+G@Y.C0eEKu^JqF`*XtIa8)r!'`0W\G*^"53L-Mh`kI`mCg8:"TSN&!5Q@u +T)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7 +!<<*"!%@'Wz!#5'>S8!!%P +$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\ +#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!! +`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!! +&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2 +,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT! +!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol +!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*" +!%@'Wz!#5'>S8!!%P$rX8c +,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8 +-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)h +o3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56 +Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!r +r<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$) +#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2 +cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'W +z!#5'>S8!!%P$rX8c,!!#8 +MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii +""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!! +B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!! +#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!) +V%7!<<*"!%@'Wz!#5'>S8! +!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:! +.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF +!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5' +>S8!!%P$rX8c,!!#8MIg,l +Q!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW +'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7 +CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT) +eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!< +<*"!%@'Wz!#5'>S8!!%P$r +X8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ +ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`* +B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[ +b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,u +L!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!! +3$)#sX:!.]\#$ig8-!.`a+;ZZ.&$Pj6eoOe.(!!!!+qD/*O!!!!&r>>S8!!% +P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.] +\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!! +!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ! +!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'! +2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT +!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eo +l!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<* +"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[r]/+NCf:N +'D!!!!Ikg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)e +ol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V>(F3` +AjzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzJ:DhF +b7^kp~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 921.6 112.8] CT +[1 0 0 1 0 0] CT +N +-1920 -235 M +480 -235 L +480 940 L +-1920 940 L +-1920 -235 L +cp +clip +GS +0 0 translate +480 940 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 480 + /ImageType 1 + /DataSource Data + /ImageMatrix [480 0 0 940 0 0] + /Height 940 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzi,?jb%4q~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 0] CT +[1 0 0 1 0 0] CT +N +0 0 M +2400 0 L +2400 1175 L +0 1175 L +0 0 L +cp +clip +GS +0 0 translate +1920 235 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1920 + /ImageType 1 + /DataSource Data + /ImageMatrix [1920 0 0 235 0 0] + /Height 235 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0R5n/K4!!GS<83mS\ed55T$<"C?^a1"AS#Z3Fk2#b@zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!!'f4k+$?sz!+8+I[U +%MXp'Uj#!!!""UH\h\!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>S8!!%P$rX8c,!! +#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!._q]rqp&^1! +2QXz/E0NVSoa>s?JY\_!!!",&c+9`z"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!I +kg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"FC +rV8F5o/qMI"TSN&!5Q@uT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii" +"onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B +&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V%7!<<*"!%@'Wz!#5'>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!# +jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!3$)#sX:!.]\#$ig8-!'ii""onW'!2,uL!rr<$!)V +%7!<<*"!%@'Wz!#5' + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 921.6 0] CT +[1 0 0 1 0 0] CT +N +-1920 0 M +480 0 L +480 1175 L +-1920 1175 L +-1920 0 L +cp +clip +GS +0 0 translate +480 235 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 480 + /ImageType 1 + /DataSource Data + /ImageMatrix [480 0 0 235 0 0] + /Height 235 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlYNTE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzJ9M*dVuo]~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 0] CT +1 GC +N +0 0 1920 940 re +f +GR +GS +[0.48 0 0 0.48 0 112.8] CT +[1 0 0 1 0 0] CT +N +0 -235 M +0 940 L +2400 940 L +2400 -235 L +cp +clip +GS +0 0 translate +1920 940 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1920 + /ImageType 1 + /DataSource Data + /ImageMatrix [1920 0 0 940 0 0] + /Height 940 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"-:! + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 921.6 112.8] CT +[1 0 0 1 0 0] CT +N +-1920 -235 M +-1920 940 L +480 940 L +480 -235 L +cp +clip +GS +0 0 translate +480 940 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 480 + /ImageType 1 + /DataSource Data + /ImageMatrix [480 0 0 940 0 0] + /Height 940 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzi,?jb%4q~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 0] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 1175 L +2400 1175 L +2400 0 L +cp +clip +GS +0 0 translate +1920 235 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 1920 + /ImageType 1 + /DataSource Data + /ImageMatrix [1920 0 0 235 0 0] + /Height 235 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzi,?jb%4q~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 921.6 0] CT +[1 0 0 1 0 0] CT +N +-1920 0 M +-1920 1175 L +480 1175 L +480 0 L +cp +clip +GS +0 0 translate +480 235 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 480 + /ImageType 1 + /DataSource Data + /ImageMatrix [480 0 0 235 0 0] + /Height 235 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlYNTE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzJ9M*dVuo]~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 -0.6] CT +N +0 1 M +0 310 L +955 310 L +955 1 L +cp +clip +1 GC +N +0 0 955 310 re +f +GR +GS +[0.48 0 0 0.48 0 36.84] CT +[1 0 0 1 0 0] CT +N +0 -76.75 M +0 309.5 L +1193.75 309.5 L +1193.75 -76.75 L +cp +clip +GS +0 0 translate +955 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlSlTE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz! +8q],& + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 36.84] CT +[1 0 0 1 0 0] CT +N +-955 -76.75 M +-955 309.5 L +238.75 309.5 L +238.75 -76.75 L +cp +clip +GS +0 0 translate +239 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzp_ +u9P$uu~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 -0.12] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 386.5 L +1193.75 386.5 L +1193.75 0.25 L +cp +clip +GS +0 0 translate +955 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WTS+'TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!!% +O*SPYH+~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 -0.12] CT +[1 0 0 1 0 0] CT +N +-955 0.25 M +-955 386.5 L +238.75 386.5 L +238.75 0.25 L +cp +clip +GS +0 0 translate +239 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;! + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 -0.6] CT +N +0 1 M +0 310 L +955 310 L +955 1 L +cp +clip +0.941 GC +N +0 0 955 311 re +f +GR +GS +[0.6 0 0 0.6 0 -0.6] CT +N +0 1 M +0 310 L +955 310 L +955 1 L +cp +clip +1 GC +N +0 0 955 311 re +f +GR +GS +[0.6 0 0 0.6 0 -0.6] CT +N +0 1 M +0 310 L +955 310 L +955 1 L +cp +clip +1 GC +N +0 0 955 311 re +f +GR +GS +[0.48 0 0 0.48 0 36.36] CT +[1 0 0 1 0 0] CT +N +0 -75.75 M +0 310.5 L +1193.75 310.5 L +1193.75 -75.75 L +cp +clip +GS +0 0 translate +955 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WDeR\qh>d"3DiENij!)0V0m8OSW$BS;+gNsOLi)AcZ#^7If.E5>"B!*gUj1_ZUq.>(NCc3g==@IO +#Vqt<.T/\46]-uM207&'ru`&E60a%i8XcL_r]WQ]oN\<7S%6I_YEBrGd#Z4SFXK%s&k-1k?@b.QU,MC7,/t9ln/EARVW!ZL(YY^V$XC1#.`eUQ$ +@9$kmVlWeHi_FB!,X48F,G)pdi^?e>We,r)$t]YN7lEP1<=#WTK/mM>WX9NA17/e-*81*`I25M\B*AWX9NA17/e- +*81*`j!reFf'3a7lTo]4rrhW^'Pfmo@](3AY:pRl5,` +oH=WT"g/XFW[`[%Q,/uc]QSWWnSbJZ9(*Wi&U_Eb"n.dA,7\`aDj*H;U?86SLjOb=JWu<<[*ejnI#G[i)qPYZ.;mGRJZZl06=e+[h-d$=QqWT99!HqXT(LZObfB;E[LO\nKFk5<>@&aAM>W:33r(JW^9ff +)9gX]pd/QhYJ-B$=QaE18?+pT"UeC(Nq95ZIsq<>j!f(633YW2^D0eMILe;`PNB\JbU6.Ss-]6<+5rJqYEg+0$fDl-:fumq&4ifd<%!W<-'7/6$17.p+d:nY)sL=B/iObY7,%KQXF%JYWWPR$l+A$gV%,!_F-*Wh?CR4["5`il)9A0">7f^F+Y:Ja5`JeuBZICj>U"8$?41k18HX +pO2Y&hqYecCMWqAYc7mgR,%sY.Q9#^F#=4KQla+'6!/K37"@ +)p+.*rqQ6&s8@0!IISJb$AFM#NffHd9(usP.r`0M`*,#;bYL@k`9?DuG0?4ZIeiWb:V.SGns@+is7EOq"uM^8b2.hpXp0HQ +$<1#%<*d1`F\be/n[I,R?XL<^iT\gh/)?i6XMFM'&8Tg40Yd5)4/FmMs8Mo``f1orWrM^qWWk2Fh$3<4 +]#7SA0!'+O#UEECWe0uFj5]0;!$=@M[.Y#=0Iq+U.)]:H)pH4;_kf&>?Fn]B;A]15AIkuiFH$AV7*foQ):ka8+$lT +DL"IpY_@5&#(`QW<)k?IgAG>BEo`&R$Y@kgqXeKGM]9uB#'9Bc0$8E!ROAhbea+195Q0hBV)L=4 +)Wjg[rH.AVXmTQ%Gk'eN=+4=/XK'jC@HKZK=t-Oo2$bI-TW3VhorDfK.RAj7U%Dh%XgG0Ah%i2')5B)Pg0iueflIDqQ!$=CP\Fddp$PtRc>E%l:+kE[H@!8,ip=]2tJ,bRZr*sqp\,5RH[;1]p +H1]KSh;,+2H`gYdp8r7]bP`Alr24Em(CC=)S)+)rh4.nR2):P3ICXB&gE!/sG@@&C5p3-.<>?3Jh-VQL +2Jaf@p?'VhDA)Em\$+6Y)9`Fm0++sbbm*d"*BTj;ie_J35Oul?^\ZKa9(+S%@/f=Rs7u&#J(oBfV88ZN +^.uQtkNm=0@9(R#0\?*jObpgSGkpYEH*/)?i6 +XMFM'&8Tg40HWB"bl.D,J,fD]^qc:DIT^id.YIPT0>;BEp`&,c`XdOfK38G#VY#!L5PicgRf@l2ATAd7 +FhIu(eoN]U<:u9I$<1":Nf?%mTsXZ\K&5rkfiWnG)%)5OlZ*T7?k9^&-,0O,].T]>'m%*R+9G=+0R\ +)J6C%0K8o!,dErAq4am5,ZUM;Jq^35Ad&Q42EnBHX`6%EpUf>-(:*P8J,RQ=86M%/#BTTs=b!#t"g&cg +c+FW[XQO?4W9PBp'r\-?Tdk[t_8;=o:TRPL?+MjH0Mq]g3Se*2mHlL=ja5n^hG8Xte'_Hh*6^j:I\)8t +XVo@-s2_]PcddD?eN8pD:fY=u<k\3sd\TTdr8SlchVr<)B5G:W6TpfbrH..ccW)!c +H:Lj:W[_,Lb$\8SE+J!mE`G&cf<8OgH:pauLZfW<;V#0PTc;4)hhWQ>F&e!qhN:cgWX9NA17/e-*81+_ +>h';Gd^K:jIf9,\%mU!SRr=VL7kpBhN4_nQ1mi'(\T2!%ZDW)Ql;6"mOt':._*V-+Z\!lU)kW@q]#!aQ +n1T:ebKiEQr5t9T:HeC#GJ<[>ZBm^I#BQdHI +*6\l,&P9Yqgt6"P_.taIE3:ZeeN8pD:fY=u<uuroo>gT<)p/!Q,0P0iQ0#oWX@1SQ!UBI7di]O=6!nqI5'hAPr"r2$<6\YU^C($YY^V$ +XC1#.`eUQ$Xf5Lt3fUFHARsR\Sb:l=j'_T[7O7Zd>ZIGsG9g1]We,r)$t]YN7lEP1uB!*nVND/FBkGeS`]d'KX<>@&aAM>W:33r(J +W^;WEB!9IX[$HW>g"gMb&"dhqS!qJ;c;b%JH:Lj:W[_,Lb$\8SE+IurWS5]5-\A2\Z.npmmsHD<%Q:KP+` +K3+pOS%88]t$m_mud4&ePEr#nghJi +YNuE0%m@;pTncC3HF`U*N)YcF-:fR+JU^C($:fL`- +B[QbRo69Z[IHo=]!s]=Ghfa-bh7S$>df&@QVGJ\RKfZcJ;t>AC&Yc:WfrBbCaf_d>D/FL2rabU"%;,?q +g`>2+np[Aj0O$DJ<:u9I$<1"RX6O7M+$4Z4r;#s'hd4!BeW&E+4*FR@b[19RPk&rR;2V@\#-1f'+W16/ +0\^tU\Wjq1&YQ.J,ZUM;6>NSR?+P.6\oj;;.>(cSlU?B)$J)6&p-pgT<)rEo +p%Ie-!s8X:59duSjp`7f<;I.1ZNAa:!`^!%<3>8[.mNa[iiQE\k5@h`B;]KWnrk;JZ9'7WbrdMp%>`%[q@qcil)^@]RKg*Q2]d'KX<>BUU-RU9sL5,-Q]fYOC)9`Fm0+(ZYaJJ0@K/mLBoo>gT<)p/!Q,0P0iQ0#oWX:>Yg>l5Y0UPd.j!f +(633YW2e3rWa;QCL;BB8Wd705"fa%SW6/LKY]+.56WF;4eQQ:^>l,J&0UPd.j!f(633Y +W2cMklLB_m+pN,2W9PBp'r\-?Tdk[t_8;=o%oj>Y<>AFm_dH4W5e]-I%Ug=JlPYosmOt':._*V-+Z\!lU)]u(0GcBEd)j/X._439\ +H:Lj:W[_,Lb$\8SE+Iurj!f(633YW2`Ze<=;_c_dH4W4%W9RIX`,]):SKOeM +L;BB8Wd705"fa&.WqQ2]5QCZQ@q0!Wk.GZH-rAH\+C;35,6i53W9PBp'r\.*W-[me3OaQpL5+KeCE1RK +'afZ(:oBnVT"H.3KW*V;6K)HVf;0tC._C2kmbmN!pEQ5sqi'%Vf9c6,QlY@i!`coYW<-'7b,RA7QH.%d +T6p":2r8d(q^p,JkJbWl6lE!_al",#e;It,;asdU8?+pT"n,GuJ,T$^rpY[j:7O:dhmRCVk0BO`IW=o; +`C_5h?rl33;4q[A_L2k_)$Y0)W9PBp'r\.j^$==ZCM/#t`TGF7.t%\U=b5m-JZ90HFXjl.h$IO?B,JiA +UT/M`hn>UZ,dqiTH\_439\H:Lj:W[asJnf$Z' +E4mYr[75;P&P6)))9aP)Wco^gTW/'G3EBB<$<4,r/`55_qAPC$(1YKN4;RS2.*j7oBMj.TiXl?rjLNAE-&-#;uPQikh_< +?qqS>]d'KX<>@&aAM>W:33r(JC)J[C6kWO##'7qcl5`G2W2a7!.YCm?_NBfgj!f(633Y'!Ed2Hg@o9172!0j!f +(633YP-%p>)j/W#XJu]kPYosmOt':._*V-+Z\!lU)kSsff)%o^oa7`LK/mLBoo>gT<)p/!Q,0P0iQ0"d +[AotQ@$Ctu.YB`m<:u9I$<1$0:mJku@,]2H-_79>RECr%W\Tr?7TJnJW^'Pf$WS(g$5SY_U^C($YY^V$XC1#.`[CV_?J^L6?=?iF0W0l0hN:cgWX9NA17/e-*81*T>da6#//O"?Q,/tG +Wd705"fa%SW6/LKY]+.5'4o]6VfF,cWXS$S;:Rak.Ss-]6<+5rJqYEgT?+%p5*[p4b*fK6172!0j!f(633Y_Q@Qo.VHVub$[s^V?GMX4hhDP'U96 +qB,u`b,FSG8JS2(TW:+-0T^_3hpS>oY@#%NjdpO*;^`@:6CiFKHLcLj&[GQI:cR.YFfDM1:8OrVrfOgF +]3F-QFCmG3r?u7oZndgGun)!X]r7a +h4MNb_"FF`A(?rQK2EtsF-XS319,Xj;?a'M.&r;eNV2u;'n=6BMA7YE?G0P(rXX;B$0ti-0W0l0hN:cgWX9NA17/e-*81+oZ>-mf +6rL4mc;b'ib,FSG8JS2(@%f'&=i%t;NDF>i-M#)q;-gT<)p/!Q,0P0iQ0#/>rB$hS-eLd +AMC$@Wnrk;JZ9%mW\TqV/0*@Wsn-T;:Rak.Ss-]6<+5rJqYEg+%=`ThM@t6@@aG-k*/`n +4-D'igT<)p/!Q,0P0 +iQ0$:RB#Ma&P9[Q3`d3C.ANne,ZUM;Jq^35Ad&Q42Em+#jb`*a_E(a9bUK_-%-EdO +eN8pD:fY=u<3P]tJj>II?P?\"'MST>@VE +oqIOJjb[Q`_E(a9bUKi5LPLWmKu/EF$Z=b0;0//m2 +ZcXj8f"&pZbUKK/mLBoo>gT<)laoh7%+Vn@X&ZE/QWXS$S;:Rak.Ss-]6<+5rJqYH(HFg)G8[GF#=B-6_.ANne,ZUM;Jq^35Ad&Q4[[#?@3KmXs +ed/<+K/mLBoo>gT<)p/!Q,0P0iQ/GsCSl,!SZ8e+<`K?Fb,FSG8JS2(@%f'&=i%t;]d'KX<>@&aAM>W:33kTIT?HchsVf,TZ9%C/!4-(CD5T``$7EHs0"M"i[G@grPMI7bHb'`% +nUkXUl`6m*XK(\E;IVGURX"&`^AQ8>_U^C($YY^V$XAJ$"(.U,A\R$&S +QhZ56Q,/tGWd705"fa%SW6/LKGjrf*qd_HDq*>e!QhZ56Q,/tGWd705"fa%SW6/LK-luN7Z:>\5QhZ56 +Q,/tGWd705"fa%SW6/NAc4/U4Y"S+lmjq]snHl6GK/mLBoo>gT<)p/!Q,2f[1J]@(PA=UcHAPF"<>j"0 +UT/M`]d'KX<>@&aAMD`:Z'!7*DOuRJ,sGa;AMC$@Wnrk; +JZ9%mW\TrWdJQOI-TBAgob*n$W\Tr?7TJnJW^'Pf$WS(gPr(Udh>c3_mP'g;ZZ*Ur-`jqOG3!H5%.H&0W0l0hN:cgWX9NA-9dei(NP]a\b+!S$V**=<)j4).YB`m +<:u9I$<1$0:c6)*gs"6g281Wp_NBfg<>EBNb$[s^,G_SVFp%oj@]Wle^E)-+?pW<-'7FB,69V?R817n,s[]r0Q"r89JN +INJB7)S$3X(BpfgKN4;RS2.*j7oBMj.Yt+mfs>>ZgpqLo>I.qdkic]F46J\mB3`Ei[df"A\7P+S_Y'Wg +E1&)qn4TW1=\<>j"0 +UT/M`]WcSHg>[$8r:!'4M*Lt2dn9AE0 +EME*Fq=Edao#`JJ5,>;t=6g*1mAnn=2P(N4MZQln_Q(ad:)R'F,H1`E'tdU2Fm>21b\f>*hUp6'Z$Gs' +8fuL(_;*UbVlT,]V>h8;-f9eM:JXdC%j=8=<=#Y*qu#$sf9c6,QsEfo6U=`@8?+pTKsI_p%H].d$-2R( +EPoF/9pD?O6gT<)rEoF`;<(hNqdNpOE3]?)'@'!F.:\(BpfgKN4;RS2.*j7oBMj.X8.c +I>d!V6\ig%.K)0?11oFR,\UF9$<1"2X"&`^AQ8>_U^C($:fL`7I/3X3l\iONraY%d$a(9LW[`[%Q,/tG +Wd705"fa%#Wb-0So'X-Y:/=ZX4*Rj>]=W/L5D[.W_"FEe@Y^2lU?B)$=QqG0W0l0hN:cgWX?>;;c?V-c^q7&Pqrh\"g/XFW[`[%Q,/tGWd705"fa%SW6/LKY]+.5 +6WJiEW\Tr?7TJnJW^'Pf$WS(g$5SYgT<)p/!Q,0P0iQ0#oWX<07 +AMC$@Wnrk;JZ9%mW\TqV/0*@<:fS[?WXS$S;:Rak.Ss-]6<+5rJqYEg+0$fD +]d'KX<>@&aAM>W:33r(JW^9ff)9aP)Wco^gTW3TtWXNJC$V**=<)j4).YB`m<:u9I$<1$0:mJku:ad%K +IL$O[W^9ff6Q+*V<=K+3]d'KX<>@&aAM>W2m.Ss+gXJu]kPYosmOt':._*V-0O!-\dDI45ZIuf +.T"YWOt':._*Z[4!!!!a+S+C7zzzzzzzzzzzz!._XT9hc',%ZZL,jS_=fFSYK=_Kn%!3U+i>X&lLerBR +)43]_T`oP+&Lo6qeoE`G@"jHI4u"]3V=o^p+\(GB%'i-pg_@f`UO#/JKQ2tM=qHhQfo4*U+_qpqiO_dA +iRh7.I"Za98UZ"&&DO;Nq9NP58\bO+]aft"\=nR?_$jSba'2_d&GWm:F_`f1r5%,fb6p$:ecS!WlELr= +lK$g)i-qa)t)#8O\bLC\TOF_*;>]=U<,puB"/;:s0a3AY:pRl5-+qk413a,V1Z!O-%PMUp*cGHnYBqS7 +XamRtapkKgd+A]'63/Ue,5\oQnilfmsqR(__LlZ6E]*'\o-SZBZG_hCFnrU&j`,g#q'Kl,SQD(aka^ju +=Td*RmOX4(qJ]H:*8go"3I)1]Qi`Fkfkk$i5c^AWZsm`)m]=25"F#IC5dj5[Udh/],e4$#Ft:SQ*Q)*d +mWpG\Fsr8EDQk5EZDp@d6Z]8Qt**cqM"NN;66HZ%NBAM&?>0>-ekg9k^S^NITH4etke-$6m"J*t^9l\%B2A%5pK?=[FVdo8O",6M;c@c#cJ?$PBCV;erdNIes"`oZ[;4CLr93P8AMA +Ji5*#ORjIKgIJ``GlnLomp?gW&)L:M:GMFhgb[-6gNZL3$`-q2*hVuPHCKijnciA:3hn=BGpflA!9kX +`Po3B15X&a)jZGh5QAMAJ9CY,`7cHa'E76JT1a5*Z?UIUC-rBOi#B$HcWSpA=mXh5`HnCF^Gk<>XVJ,A +)Ln%#d6F:tO#r:.hq:HWXV7-T^&kKeY>Puh/_(o$(\M#1G!n,@?>0T5;R]@XGRF660Q%jV?sh7511gpH +D2k]l^D2tMJX1$J)pJ,]9,a2Z+9be@oK\8h?8jY+=>;Ytl*4[&_`ct3[p%(K*,6UO6/>[=dSRG2l=[r: +G6(d'j%l4dTgpu;#4gpqKUrTdhjq9OOSEo]bm=4]*SnV^F3JF$s8]u]+0NOa?+k#O[)LbY8ccCR2b:/k +Pj:M;EF1B.E\h@c+j3M6F;W6YIIP&XDLV +6Op9Ku!7Rfl$gY:O#np*H]JA(sYlTb=+&)HF!;tk.`[VbHf\aC7M04)m"TuCX.d\Z9\XipT=K^/@&j4T +J6.(taUb^=^/eQ3P<=>2TiQ1kB;JQp3,hd1O$mToR4of'Y]:!4SFp+:`i?NF`In-,[B$td9Nqtj[F/R, +[ErBMRcc;/Y)QMjlFnRW!Fqf\KGQ3<:$P2)R +6o@di0eoB+;=^YlZ"An:4FfMRodhVR\7LsGNfKl6G?`MHp]\?DJ!0j.gf]691+^$JU>q7l[\Hc9 +4/a2d$!eJA2*d-Q`bhL5.Sl07GRkK\3oGF.`11l9'8M]6^)l<(?&8_MFCcb?Ri%s3G +$>/*Ib`0J%O_L=qo9ldiUf21#d@G-3H$["t@>d#']%aFd@\Ttl:PK(V^Z8\L-^>ETSIh[ +8;(sK_(^RO#h>'t6n\*j%ZeE4=5Za6to9=erm1A@&)hN'&hqtKQ'4l)GJGs@C8\A,%%Dr+a3r0Js;o#W +8OjRPj=VP_Y14j-e/dPN[50fV/6>N3L`2aW7RL3OK`UZq%-bEjlkni\+G`aiSErrrf1zzz!!"-3rso[G +'$U~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 36.36] CT +[1 0 0 1 0 0] CT +N +-955 -75.75 M +-955 310.5 L +238.75 310.5 L +238.75 -75.75 L +cp +clip +GS +0 0 translate +239 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0VH\bAq+33nY8Wk>FIn9F5X@R)1q.&0djA4C%UeR>SZpf"J?/cqT;0(?>KTHJ5?]pRQC&_!^M[bN] +-J'Meg\forIa`[Wf,-U@niF+KO/1HMNoS@H4Iu;89d5SY"KmR-ag2)R8%&"s4PAd`'K ++D)!jf'8b0r&[o5G?#S_Bo,5&i0l]n.SF<`Fs%&CX-thhRtRL^(e?:QBmiheaKXj?>F?BM\e&;Cln-"9$G8A +1M9JG+"N9g2RPh`kc4eZ2d3NGm[d\Kk[P +'6D:7]P,ck9mum1)*`R41$?s[^":iT_Qs+4cTb^J_(K6so]];enHB:9Uqb]uHhYUD,CB+KeZW>pKaS]/ +:J,Q^]K']SU^/F#<`W7m4F&]qI-/Y@>ISL0(Qhj*0OoaWP:'_OHoZORrgh)AkKfc=Cht(NE+#cLWiCq* +I@'-\eZ2cVc1^PZirq1#nTW!#r:of,j*UOgNRo+-Cl$\Yf<2f+iBhhp11J1Xp[@"[Wc-e/%3@WVlI;df +3Jk.bkK]&=Y@#&9b*j0fm9XXHD>Y-rf[H`o;u`FFkLk,s2Jh$Mpp9K$jQP!dnN)Pe^TmB7Lb&(P%j+\6 +WrCa^^38mI;o/HfQt(Gim/IP;5.kFJP.4%P[c)B"=>sgY1@:1Q[M20?3<+3PDGfUE_Cn3AIEm +9f"Q*o'\\!)a8Xolnc=)P)`O^Xj2!"$]MXcZX7bGp%9'7P3:b]oJa^YoLc +I?bBs3?J3j:_IKMjHZj-nRnra,V1.--r2JO6^c/X5e>DF)tSTZbO$_*EH0?V$`,@s3?J3j:_IKMjHZj +0RNN[fH@>kEA&+'F`=8MWpZEc7_QL2na1IHp-9s9Fk@ZHSB")"NVoB+?Y4)Q7ge/[Iib7X'L/f3$ZcL?cp.TdRs9@-"[iDi?skW;7M:O:guoQ&q30()21`Y#pj0t0&\1BQr1AH +T2;+:LYni,V0G*+e=qetJ-ljo"YhM=ZNW'6&@2Np.hK#j0e3gA"YhM=ZNW'6&@2Np.hK#j0e3gA"YhM= +ZNW'6&@2Np.hK#j0e3gA"YhM=ZNW'6&@2Np.hK#j0e5eec]"KqX/g:`6M^.8T,BlB`0FJUQn=+sJXA$F +=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2ql +Lfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r +/VmP\Qn=+sJXA$F=b5`l>O[%&DW(?7aje7o=b)kkLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+s +JXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F +=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\k;-?3I-,>)q96t1 +`0FJUQn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\ +Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+s +JXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:aNEbnVnS[8nj2ofhQ!&X)/VmP\Qn=+sJXA$F=b2ql +Lfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r +/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\ +Qn=+sJ`egnn9jA9i;(U=eARVTJ=%pE=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F +=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2ql +Lfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo;l1L'?rk&0,QbfSAQQ!&X)/VmP\ +Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+s +JXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F +=b2qlLfo:r/VmP\Qn=+sJ`egnn9jA9i;(U=eARVTJ=%pE=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r +/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\Qn=+sJXA$F=b2qlLfo:r/VmP\ +Qn@UhleR?C!thjC_lgeR=to:MN2d'!Y/$u>0^K.*WWg;C^o1EjljA]O"iILYW*1nA$B1tg_lgeR=to:M +^H8I1HpB;l,l2-Tzzzr*^[2Ht[5.H2mlWq`oI`qtKQoIf2TnWi2kPhj +5p!p?p`upHb(RB@!/L;GVOR(^4#lJNujkk:NZ3kcY&=Stc-=KfWg>>Dat/Cq_=Yt)GZ86(')hUqA*QS7feHjEE[>@[_q;5(f<&7i36IgC`] +lX&(GB*b=`WfWW>KnmFN)=X3W8Ia8S;s?=]lVd.upCjal6ebL:XOjmbkp6ldr>DY-+?N@n(Xupg(gS'_ +g>Sp%8tG:0b,;9m_6aZt[uiY?pT[W%sYPF50JGc*7!BrHlbV\Mks,:<_tW]pQQG2FoLC^*Q0=eCN1l3, +g#3jbXWu@[s&QH"&n3EgIG\Lmd"l7>ip.nuXp7jia"24SeF$];ZmY]5clND;;YKI8K%-F3NkjR`%XiUS +Hl#X]mY?2;)iA<8P<8[p[%(oT*-bK!An1QC)Br&$!9bN]HIZZR>A)lkj\jS?3H7HfN*T17!gVNBnn$!! +!"l8H/eDbXls~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 -0.12] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 386.5 L +1193.75 386.5 L +1193.75 0.25 L +cp +clip +GS +0 0 translate +955 76 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 76 0 0] + /Height 76 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W?UtlH*6.MVOV%%LN^S+8LVQ5,7-[+6$`P7:9U7*D,SgtHJjc=7YIY"%Ul1$]"Q;mL6kg@[&s>P( +@8IY+ajTbKo>bKeQ\T#)?!@C2zzzzzzzzzzzz!!!"+8Yo +>,A?siX!<<*"F$4d;?sA@^-ra2He^`1bNCGenleK$lC?*dle'?EE/GA/HP?^bi7Vcf*Z"(h_EfU2#!3h +A[R&d9g_uFLjDc\g%ZEdBQ0/#!Yq!k,^IAL$)p[@"?NH]SjB3i)+qYU0?*\#;5!!)NH'P:?G2p>'o;J4 +:F;2\U]>'0-^R7scbXJd_Oa-2^NC?+Q43IRm6jlPTf-Vldidj`K@!!)MMX"Z&2^3jp%>RsOepRPCH@Kc'jj!8_`2k"hU5*B8"'/aDZfF65Hk1_1'+\msaiq#:6he]aGL)>1A4UdT@WoSk[d2hs^C[41 +9QH6pkL-"B;NcFZ/^Q2:@RT0Ih:A#cT1eP,FibUs5Y!!!#33JH.s28\J3"ec:go('()[^*35Wit]^)ci +SuQr(\"\QrDmb\Gu0q0@0u,*>F2WOVH7>fc?g/@>TJ-HJ!5Mgi_RsO-So?Z:lddZ^mUa +p5e!m@1%8cZHGkJSMYKs3$%1Nc5041<=RG%'_G_Hsr6?_Op1,UqSCG0s1QiR0ei6D.kr`gLOC8'ck<)b +iZP$VDdQ84.]quCeQGJkicuW(1*C"mOiRGXP: +jT_@-DE!,i#VB'&gXj(dZRqtJo,WDr0PiPTE-g5,>@F:AZgR2^@b#VNG;GBJ&=K/XKMn(hP\`FEL4'P7 +=F!-g$,0WRfN;,Io4Ka%_c-71&\5CVD?f][_QZ89;lZ0CE5B!k2&iS1h[n(=n?['V[Uf?2@acT?=qX3U +Y@q=D"QU76@NRB(SVb_<8!k7*bS +S#'$4CIGB[1kaYbjj$8VY^!45s:?<22*rV,m2-'QI&Y@"=2L&?&6b0#UAfibd_i9gRFzzzzzz#_Lk4!! +%NqMH-%%ZH4GO\-`r#puH__D1g],SDR"+nuVJ9!BgG>7.]BWS2r'[3JQUUaj36<%6$(7Rfk)I0 +`#)WY4YjeX"'g=/r\O^9LtGc/AZuqlT_0p_6-`dbX"^ug(C:e;O(WVWYB%O-Ce6=+]ShlV+P/+1/cY_80s@t"; +=\00M=+Ybk_jL9!BgG$?M;kWcgc[JQ*q_O=GreJdH)@In3Rk?)^_hK8`d;,>niD.`$d;.EiDu34&TOWY +4YjQ,4MOUoGGt8g%+?-DX7BQ=.RtocI(s?rs?BjrbmKm>+^CWCeif<=6&8'@*]ZOK&Gq;Pc&P)6%@@U_ +63uJdJDSQmJ,=B)RKa-.nJ4KfR4Fk8<=*iF8mr2b;Updu-'5,0'2kY\9/,>Kq@BPtYU`]\F-)NaG!mg2<$fV^]"5I62`DP2`U]%b0MC@&,( +W]>fTdq?3QO`k4!KqTWY0t_*?#irXf^Em/fY@Jf4GAr"PtdE86(7IWD]+9N)[20;@0-K5pFk,d8AGF+$ +Fq78u3:=(60G!HK)jLO7^,Z=$sm6,mo[/4rmHsB\2V5:"nj!i1bGeZcl<=*iF8mr2b;U+g+hj +Kq%U;+/]'<)a+<@qfqBC"jJDVVblk09C;aiUEWVMc6DHAnaU9AkG.!i\er.ocptO9I#^bnf6Ps'FHgNJ +T)LP:hK?K=h08daI^ZlW9.`]CUkI+sYa9cHa^E++b#cWW]q3V?)^_hK?WCUZZbB''P"Q"hnCb.8]sN%>EJ-BHh +$*L._rBsZfs9lSitichk+[!$F9l!5M5?lWY4YjQ,4MOL8;E"h0;j%=AA)M$>2o*5!u=_[&:LF\>Tc(q" +V?\kJrW*qtBCr*$!21$ADUamhZ;+.`&8e.Ek(YXAh,8AA-eYD]dh8!T#A6W"MX$JjcWP]tM]Sbc+QVBk +aQEVl.8Crq#=!\].R8-1HAG5cPI`8sR75A6Y74joo9I%67KE(@B"bnNrL+WY-RT9dC?QY$>sr)S;iBjk +[Om*oe]n&pV/^+B8_kV+P/+18.[T?U2ojNJh#"F5_g&JQ*r:V>BN0Y7D^=G^lEWK>K0jpPT/PQ!d[n'] +pOeQ)_iYCf*.:MAN8q;O(WVWYB%O-Ce6=+]ShlV+P/+Z9Lo<]d.B0a;/pYXE^F2+B8^e;Z.-ZZhqu~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 -0.12] CT +[1 0 0 1 0 0] CT +N +-955 0.25 M +-955 386.5 L +238.75 386.5 L +238.75 0.25 L +cp +clip +GS +0 0 translate +239 76 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 76 0 0] + /Height 76 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0R!K"\o'L]d;3=g6G>EMrD4UlrpYGC@U9Ue6HfgH:5F0]bR$!WLcHX@"YhCC]HH?iQl!]2&G*ch +R#f.5i1A7rrXAP5X"cg[c@+r6g"kf/lu$mYR?R6e]JtBs')lJoLcHX@"YhCC]HH?iQl!]2&G(L@'4P:X +)Y61%Nn`^~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 187.2] CT +1 GC +N +0 0 955 310 re +f +GR +GS +[0.48 0 0 0.48 0 224.64] CT +[1 0 0 1 0 0] CT +N +0 -78 M +0 309.5 L +1193.75 309.5 L +1193.75 -78 L +cp +clip +GS +0 0 translate +955 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlSlTE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz! +8q],& + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 224.64] CT +[1 0 0 1 0 0] CT +N +-955 -78 M +-955 309.5 L +238.75 309.5 L +238.75 -78 L +cp +clip +GS +0 0 translate +239 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzp_ +u9P$uu~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 187.2] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 387.5 L +1193.75 387.5 L +1193.75 0 L +cp +clip +GS +0 0 translate +955 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;JH,ZM!5fq/l + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 187.2] CT +[1 0 0 1 0 0] CT +N +-955 0 M +-955 387.5 L +238.75 387.5 L +238.75 0 L +cp +clip +GS +0 0 translate +239 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;!=]#/!5bE.WG`:P'EA+5zzzzzzzzzzzz!!'fe!EZk263~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 187.2] CT +N +0 0 M +0 310 L +955 310 L +955 0 L +cp +clip +0.941 GC +N +0 0 955 311 re +f +GR +GS +[0.6 0 0 0.6 0 187.2] CT +N +0 0 M +0 310 L +955 310 L +955 0 L +cp +clip +1 GC +N +0 0 955 311 re +f +GR +GS +[0.6 0 0 0.6 0 187.2] CT +N +0 0 M +0 310 L +955 310 L +955 0 L +cp +clip +1 GC +N +0 0 955 311 re +f +GR +GS +[0.48 0 0 0.48 0 224.16] CT +[1 0 0 1 0 0] CT +N +0 -77 M +0 310.5 L +1193.75 310.5 L +1193.75 -77 L +cp +clip +GS +0 0 translate +955 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W?\h_F]t!TRe!kFE9eObW0HuC5:b76pKO+dK0H^[q&dAqF`'R#"(Eo$i,18ek)&*0!-QrVG#neGU +'T!DjYbVjC$)o*Q3JNV<@RQ>eAH,ukr&3#lfmEFbpK?gohXu8Xi+sdPGi#jZrpE"WG,J8kp$:5]n]r!= +?iTt(h4)bFs80R%s7r;4J,S!i?g5T&7pT.A=I%763]jsr?G10qer\ZgZSO&HWe:XX*LI0%^ADg(S'WFu +9ZnHW19=+3@b$;Zh(h((I%7Dj_5`0bd*!FmFV'!+]-=`qfP;)Eh#C3CpM=HsGkWWP%#_TKkWl3LfuJlN,X?*+1EjWX7Q0,HdoIbsPq\l21\98S +=0j\3_5`.oTfiY@_EFJlN*hW]DgI,-5TG'>>)r/WZPn0,kojU9M2N=+[T06AjV09%3jgGBN?E]PqZrQ0/PTrpXSV(9!Bcg +6<+8&6A1V`+RdLu<[Ck50Lf&Tju']QAnP"n<='8;3\V3kXDA#b0/dTGJlN-;"Vhl6g['#q*>#FZ(Z^7(Ta$!&??W9V/lLp42X,+^Y3CgFPdA@8rta/Xe!X#`&?b'c-/P7jrhLeuWQR1Ne< +COt?i\$dd/WeAKp3]!u#^7(Ta$!&??W9V/lLp428U[R3s=dY3/?Sba +?F?JAorLc9Wj[:eG$iM>U]d_2,#i\m-:Cs=<%W=L`E00q<=)p\2UBV8X4`o*?r]OHWXd7+#V4)tlRog9 +WcbQr!Nk)0%lA95c-E*fWtHUUF^QOa=BS1S:N.%R[741CcrJhGTfMktlqC$U.Nq,Y#A-;UQ%d_4H)lOt +f#XkE$ECORAp.P+cD1TD&gC:t?m\9D=[KgrdA9mGQW4@B<,LC6m(SpZ8I8ED7&]Cd9SfpYg!tQ?>\)C= +=d;fdeQ.*kM>M%2-7)cZ&>?*+1EjWXk#M;cgaTLC"BHgJbf,k'e5m*AKC3CpM=HsGkWWP%#_TGV%g9fWa +U1uqo-.Gr&m$ZOT2*7!7X@e:FeUa,"@:^f#n8_M4<"6pKKGpm,4>XUQAnP"n<='8;3\V3kXR%3AhWHf1 +C.qYSe6=fgUYRKV=0j\3_5`.oO]drGHds4d$pf94CeN<='8;3\V3kXR&IrEQ?eCWf:Cd6Cb'l?T&foS>sO^.D`$;nZ#p1If8]j +]=GAdJXOldj/!@:Ei3VOS&21Y:fSb?@)>H$<@rhp$!&@2;>&5*44D[-B?m"bn3*7"DdNI9/mc3Cn!b>b +#n8q%RPHI=)HF12X@e:L[>^M6>1E\AP[IM.W)[ck\1ocqP=F=."7m$fr/eYOaH7^>/jhEg9k_c^mtBPID^RAGo6j!6;ro2B&/5#V/!>= +Pn:9:K,u&:.V@4#1F^/?K]/ug]6A2ZeueDSpKm^@i.3o5e^N!r_f(/01SGE$9=(MW:.)$>N`?B?X#B2? +=0%3][F%Z?YUX(WB:'c(HIR4$3=d0u%im^*mp._#7*C##NgpqLtoCM4iD;2LIs8D]j^46)bAnPc6eVMZ6Pe&e^OP/;? +eueg`jdkPBD?$bcdfp+b[@sZoLg7@FiLb!]IoQ0n%2-3d6^a0H +U]d_2Z4G182djkSc1PtbF6CiueC:a5a85ag%a9TVa%OmZj\Ems"gBJM.92ip1H2.FK:%m2.BF8P[up[@#.pD5kE'*94.9tpgH-Y1a/=I%7+WXYDb)cX3o9M2N= ++j7Y`5saf:$V?*+1SMgR/Tk7b`&M)G@oYkEX2^ZtRpIrpP7jrhLeuWQR8Dqd2W^+%@u,*: +PfKeW!icGeP8$hN)Ntf88r-l +NgK<=Pu)kY>%ZpVX&dQB"KN.Z<-;Rq5V!f7C8c93E4hFUWe<90XaXl"6e.'^P[IKXKW5O+"CbqHGZfu: ++ri8tYpuFn<@`M(hib!>1+C&>KEtQuQjMGIenMZ.j*3#6<=/&l>326lg/GC7-7)cZ&>?*+1El503\>PE +3*sa;4.5d]TgI:KXOm_AnGpbeI%\.?r]OHWXd7+N%NWr1NR?4E_K*0.Nq,Y#A1i+/)0V*.-W+r +nPUF/'&$TUS!i#9'k1'm<='8;3\V3o=]mD9d8,GUV*%d)[(Im\OMBmW#pps#Pn9Xh"#FBPQ$$7O+N!YH +*ifpT9"_.IWnEf)?r]OHWXd7+nQ+4TH*@Rp96mU2Y\FE1RB;pK8I8ED7&]Cd9Gh8NSX.kAiHQf4<=)^n +au_C`2*7!7X@e:FeUa,B`le,Y-#8BW]TgI:"KV6,4L80J4-L;Y8r-?*+1SN^K3\=,O_XUMFWtMMFVD6qqB%?6A9!Bcg6<+8&_Q#-PZok#fa%L]Z9!IS77\,sLR[m"V<[C/4 +lG-(q\[#O^H#aLWQh"7D@+?E[1c&>n4-L;Y8r-?*+ +1L]Oj3\BM-iHQf4<=)\HacJ0k=Z&:)'!+]]:t:,bcs'YNjbtB'0BKGg_5a7Kk!qP8R[m"V<[C/4lG-*W +Y,p)m?4i3anlV/beM4-c1Ku^uP7jrhTW;D6If8]j]7[l=JXOkj=[/Q)0@)rfWtMKpT#!4h]u]m#Z`6RF_h/OBG+OBn +<[CjjC]iP9QHlORDd@)4;$g8L&60M^(?XcA2K%<[CY(0Dk$"Porjm5:3A"n,=bpHcJlO2GDe;c-Tm\g8h'%!Yr&'T[NcHa/)+jA5\Z1=G&h3S`&+4\&I-.onXGMS`b+KH-VRROuTlF(R0Y'\V) +iJpO7.UcK1\d%<0s)'poA4Qm0+!^&QQ!r3J]\F(@*L1#4!a=-*\M?q'eaP+XYU[Jh)*'KogeeAY,rdTF +_%.rnL1_Z]<=)^NbF'7-N_6N*_1B,sX.IGHr"6cO&Ab@LX(LPd&d3DXJlN*ebY?^XGn2R4:[fo'YU[Jh +fr]RKHY;II\YBe=8oS0UZiR?CHFsfj9!Be]We=A:6.MBK'(2\$WtHUUF^Q9:i"%5mYd3Aq.`&+fWtJ<1 +J&46"G>s/o#na0u0TTYePBMVE/WWm<<[Ck5d&lT2WFk4O9!Bcg6<+9Q;%<+27td-ZO&ZKe-!6GZX6N,0 +EcFSq=HsGkWWVi`L3u9@_TJ1bWW]@8X@cSueX8E*G>s/o#na0u0TTbc9,g<%g%&71?r_eY$-T>01Jc:: +Q!dTYKW5O+_]/UQ*+(k]99gXX@f]HTjLf4k-;Il'!+]] +:t:,BYjkK-&QZ>&<[Ck5?rb'oWm8iKECV`jjfp;OWW]@8eu\.jk!nHm9!Bcg6<+9Q`fOgq/!X@e:FeU`!G +'40NO_TJ1bWW]@8X@a>8W\a89geeAY,t+]s^qA+VkM$7m>99gXX@f]H@JHjOTZN;N;$g8L7&]Cd]M+W( +-R,X.H)sWB.`&-'\<8r- +ROmKZ9!Bcg6IjWsbilAB&QZ>&<[Ck5?rb))lFIP +_TJ1bWW]@8X@hEN[e9etgeeAY,t+\8r4lR\ZNh(C[QIS:=I!8o!`ue;(X6nh-!6EDTk`6_F'YuW79J76 +=I!8oJlN+jeTT6^m\g8h&gC:,G[R]EW^.4t[QIS:=I!8o!aW3f1UB.m_5`.oW'UgPpgiUJlN*hWYRBEa-i5T0ik_F<[Ck5?r]Pd40=2g%&71?r_eY$+3_aHY.Fk-!6EDTkXT1F20;=7Te@7=I!8oJlN,"<@rH-G"3i\ +Pn9X(dg,$&Vm6RY>99gXX@f]HTuO)5T9dU9*L1#46A2@u*ggG4bFKJ#>99gXX@f]HU#)fshY*ifnt +<,Lj7*;kuj&QZ>&<[Ck5?r]OTWkn.P7M6t\@B<(W:pl,^F8X)R&QZ>&<[Ck5?r]OTWki'"*(c6fKs<*N +6Ij'bbjhUm:5kGiO&ZKe-!6EDVeRqBF.B$PYps23Wf)dnQS)RTHhVEtD.qM9gk][Tj71!j.`&+fWtIa! +/RrB&Q*_B/-&bgr`/,-H2JcLqe=JgMmbbe:mlNIHhRrhP?+bEeh`!K-?uF>V0Ki_J<[Ck5?r]P#WWgVj +d?9g3WeB?3Sj%Q`9t'Zbo1tr?\T[Aobfn;IK*PKVc6%N1+arO9,=aq#,2Lo?Df3_5goOG<`s]_`"KN0: +8dJP,5u6OB.U^[+=HtRfeJDP:@GeeI]e;VKo[>Y\hR[iFj`d:VF!gk5J-gLh7L[McR[-#V0+=loX@f]H ++_K9%]MWWJ0p=Md<=)#nIJik[)J`-?4P@>X@::>8o$KrLXo.=l\[gthl/B)prkR=!Bt<9o_QYN+WW]@8 +_H(OAbT&NTUKNj9ACV&(/u5cGm+J^7jR5o<9k-?IFaK"FKSP*I0)Om)M\7NjL_hSc+r9!D$\#3C*q!mD/s-fRP:LA"o[dEt\ +^[Lo`Y@#K>m-M@Wfs^;*/#VKOl[G:nrM%<6>#Ta;;YMLPaJoBD'!+\2?F>Xa9`OS/UIPOtbl.O>Fq&lDhXLUn_lWnLB^Q.:f\kN=nU4>9>eLjA#12-G3n,$ +^YbZ<*LNTYNraF[]6E`Aq=Dc^Oc]k1Ib17Y +6lhX64Y:k??5Q]h=I!8o66iKgCM%HPd`/OA@JHiaGO@S4i'?bSI[?>c^&__]DQ\3,SND%mVP]@o44`k= +%QH.$rQ:EHNu*ON$!&?SQ-bbq%@W]SP-eAn8f59WpOc,qJ,fHp:S(h)H?Eh3VDg_H]Qiu)_1TGq/aqT3 +J'4-0h=kjC\8``fg=s)T+7QOcd0A_;SSAAuGTg1S?r_eY#nkcE`q\sId0f:i=rQ\JOIJ*=$gB7H/:]kbVdrNLhKjdu5<=)\P1ok@9hH8F3UeP8t_PgHN +(Dj8[\ME+-igtjXL!,f95Bu[*c0pTI0GcMsGGDN`2)p+Pn9X(OF_\#[[_'B'4YUs@o\+\$!&?/ZteZK[tZ4=cDlO'i)jCM6XAJYq!Si"KN0(>;bckgt\]f=d9PlC:s^jOje=u`laUf +WW]@8X@d/JeX/.6UQ^FD1^P0nC3RtKU"-M +<=)\P=Hu]JeS%_l'p(6pWtHUUX^tlmHWojU=P<7IQ!dUD<=*59%J<3/16P+:*PqA%s$!&?SPu(B/+>\(18`R$)+[X_:#Nd.A$!N"I/WWm< +<[Ck5:fu6RcQ*ad1^L,p,#k[b?uF)ngki;Ajfp;OWW]@8$EG*(h2-h3%Do,u#ZWAoR+jDKG@IXiH)sWB +.`&-(i.mO&ZKe-!6F/ACZ!=js$#?cDlO'<@\oRCOZn$]a-,m*ifpT +9!Be=e>bC_7@'CL?0E!&Pb#06<[C/4aplRsSX)>l$dJ= +DQ;ei+]]mWW&i&9m'PPDKU"-M<=)\P=I%8(Wl2Lk>FlO<&<[Ck5?r_eYQc_'>S!7;\6A2?`%Er.! +Gt8NJ[QIS:=I!8oJlS/*HtG8c_5`.onfkG^E&QZ>&<[Ck5?r_eYf/2b'*1<%!@_5[X='!+-Q +O_/3h'!+]]:j(<>fafL!:kY)cO&ZKe-!6F?eKGAd1RjV:+ud%<.pF'q'!+^0.`&.&)tI,b +P7jrhLer6I_!JFu7>-pXO&ZKe-!6F?e_pu5^.OS%X@e:FFm6^.[%'dBPqA%s$!&?SQ!i.)$o9]/P[IKX +KW;_:fafLQ.)O,ma/T9C'!+^0lDW:(R8$;X.Nq./m_J1#1%L4+ZK8>@_5[X='!+-K_0>;o-!6GJmFnt9R@/[4%hAC6TDe-VS +fDX=-4N4G*GC:e@?r_eY$!&?S2&Dltp/f^m:%$H+9;B3Ij-aP/f6p#BDoTl>'0F%1"KN0:9!E&"koS`+ +gK*o`>CR5b-eJ;/^Jh%*/\>TYNr^l^qe0eWjGBF8`1('!+\JWcgV:jSei1J+D[BeZ2baAnI@ScRV"0'0KS,<[Ck5?r_eYZSO&HWW]9cn`.Z: ++-!sXn(tl&NU=MTn9?*YQ!dUD<=)^NcD1TD'"Fs"Mi1Z>>T#XRG<%>j4C/:o-!6F?&<[Ck5?r_eYZSO&H +We:XX*LI-F&<[Ck5?r_eYZSO&HWe:XX*LI-F&<[Ck5?r_eYZSO&HWe:XX*LI-F +b*YU[Jh +"KN0:R[m"V<[C/4)IXT-@o\+\$!&?SQ!i//9M2N=+[X`POsEUs6e"7\99LPlOsEV^,Y=.E8Wk>jOsEV^ +,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8WlQO\@?eJd\XR"n(,/q"hK0]\m[Bu=BJ,klHPeWle[_t +gqS@=il-i?^NFHcJPt%%^A=ktXcWC*o&RnOhgG%gf3bH5e@dH^+Pd(VqsV;'CBI>%33co0L@n@0fsA^\ +c&D)G\MHB3IHJbA\i6Df%j+"5m;IJ92)Wq?=.ap7flZ0%]hsd`q!mBDFoD>akg1L!!-oSEE+%_GNHK/. ++Z=8q]`(-l#C:N2<3r,ZIc<"-RdJ9O^KeOBf3a$@mS.K0UIUC=#(W%HHE$bG3?9[8 +T:V[I:Cf%\?+ZD_Q\,MRm%=T.H+BXF%Ptm*__'h5$)Q4o4]h+4Ef(bEa`o +,W^dHnWX!;m^Z6V:I"fbmS.K*^]!P<.HAGjJ"-^n1gE3+R]nU3B6kX/EDj+KBF^RVm+bEPmM?n%A6en!7J(Nht%817Q7lhgV'7 +O1fo73\Y3/:8I-9G='U=Puh;F]mFtlbM0LEDr&"T-g`_dm^qpcdQdZg%s;e]j/!E'H9g*Z1-H7AB7h\k +SGDsMj7G!r4F+5V9,^IO/K]E5;.\KYUjMk(V.8T"pYJQXEF@)Orr)_dIJ-f)q=JPr_d7)iFP,%ZdNSYK +_1UU2WFm-C*LJ4@)SZ.?bAY)e<]Zd40P<+!2`El_?2%Baj3aVlknNuTJ,H)0%4Tu%4aVYRb?q,LX6pF2 +]`%a%V_`)Vb7?i80^ZPH7fqCu$Q%7+GkULWV)N;G[->gohlLW5MNijWF=ui@SX^^f+Wl+$6#-;YQXA=W ++XK\.?$]&)7_:6V[f#g%J$"'=8oX.LZ![Ip]Hq9DnRTDFaf^Pe'rTTsSr9%^bIC="I[bq#F2cX`\G;Y`#aZnTk*^4t] +b0;?%"#K#\Pq-Q=p&4TpFRM8S*BSH"^4!W]c"Xhk=0Hd;On'Yo]$mbN +kN/0c%q<'>FD8 +bN\9U='g16iPUFnDbD8%Nr&P@j0-5s3`Y"IFfR=i,T]*po\=N6rT,\^l$hm4S4bo*?FtB_pZ9Yh2q-IQ +kg?._DGm6Xp=X(-i6*dggmaBY?G*f7?()[9HG#bWSiV"omL:V`l`\'pm[MWr?t!L&`f1qLGOJ:fQ`HXE +]0@J)>;A/Jbh7F8aX:g#fXl@2dk]nMh7HTa;sAj\h&FdMJ,J=KJ,]&3Vt92aVqun"m0t!mUu-Rkbr9c` +>%r0HfWfr+rjoS5WBU@[oBp/pQJUg]B"@j=pV5UuH1U%T@bhu$Ji"W(qsh6&%j%Pp:L&be>`G +Cu)#Q:S0haoZ"";\%8>HnV^cP_@pW9h$8/Ih@B)A?iR^s_E2u04E04Dmn5;aQ^7eq7d8<3MRiB%Egk@G +4*QurRd[lW^.Ys-hG0^u/1rP-/FgO]+)K`B1&],TRO6FQmcNZoHFK]`GA:kB4c$7\X.&J0B:iK+La,'+ +p+fcRI/EePH+AnGZ>r2^2b0^"T'\TDpUs8;K3 +XpO"a\LNRj*Ud4@CcQ_->j(@p?Ki&D`GZ/8T'p+5AcMc.Bj^8ALTnCoDVr1aX#`r6j,ZE"pJC*?7s[Fe +*e:p"lDnV'=F-N0NT]b[s19(eat"M3h=iUI5IkUf^3uJ0Db^g:.c--Ea,fsY)LLr0]q0^/Ea[2Se>ZBI +^!+k[)u%X[t+X&0'U)!mAmqH`_FpQ5FjjG[!thH:Nr#:pu74rO\";4<.^;]:Ss`J +@7]2n;N>j#FnkIbhVnM4o?VpZeQ9.pal;=>Pq,oUQ)/YMr:ofY9:,QI>fE%aONWtDq^cDM:S'\ZIf4SX +T)g*pV`V!V)Q4oQOsEV^,Y=.E8Wk>jP,'#p5:N-Y)Z~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 224.16] CT +[1 0 0 1 0 0] CT +N +-955 -77 M +-955 310.5 L +238.75 310.5 L +238.75 -77 L +cp +clip +GS +0 0 translate +239 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0T_1u%j+33pNR\;"TCqc<+m5%qQ*4_5'ZmdFqdB6J;RS@N/0smlj,dh>@M^7gEM5X.3OcmrPGC50# +VOoVU?G2ftGW81e\7Rq]o=XgFcJ3mH=9fWp>Zd&*_dDBkq/).ONW&Yp\n0'N5Q=(Wq0RR[XdId8rd7'4 +=!_IUVl/9BfS%1J+m&n^=,8k&bS=S^&9EPm'?"-!jodD?LcIcF'k0oLF+otp`*=JJ">Nu63C1k(@SRJU +KpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^=,8k&bS=S^&9EPm'?"-!jodD?LcIcF'k0oLF+otp`*=JJ +">Nu63C1k(@SRJUKpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^=,8k&bS=S^&9EPm'?"-!jodD?LcIcF +'k0oLF+otp`*=JJ">Nu63C1k(@SRJUKpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^=,8k&bS=S^=Dc9g +Za8,^g!nMNHO1Y;BJ^Y)S^>Q5?^UQ4*$FAN0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r# +jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH@]g8`Js\(,S06r$0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<" +bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH@]g8`Js\(,S06r$0dhdf6HfaFXq5Z+ +Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH@]g8`Js\(,S06r$ +0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH +X,VCJG'9<3S!od\04ElYNCIEm'7-)9$e*t#"2nFfjodD?LcIcF'k0oLF+otp`*=JJ">Nu63C1k(@SRJU +KpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^=,8k&bS=S^&9EPm'?"-!jodD?LcIcF'k0oLF+otp`*=JJ +">Nu63C1k(@SRJUKpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^=,8k&bS=S^&9EPm'?"-!jodD?LcIcF +'k0oLF+otp`*=JJ">Nu63C1k(@SRJUKpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^=,8k&bS=S^&9EPm +'?"-!jodD?LcIcF'k0oLF+otp`*=JJ">Nu63C1k(@SRJUKpWOl>Tr540is/k6-L'Q:(VK#QkRDC+m&n^ +=,8k&bS=S^=Dc9gZa8,^g!nMNHO1Y;BJ^Y)S^>Q5?^UQ4*$FAN0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<" +bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH@]g8`Js\(,S06r$0dhdf6HfaFXq5Z+ +Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH@]g8`Js\(,S06r$ +0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH +@]g8`Js\(,S06r$0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0 +`14'cQq&CA3ZS1to5B$dS*sHrbZhM8*.R2rB#gc/`9c,5LHT_?Y\-]NrgH^CY8IXUXq5Z+Qn-+F+Qa"c +-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0`1.t4$8Eoa\O+LH@]g8`Js\(,S06r$0dhdf6HfaF +Xq5Z+Qn(Y$;0rV=OI.PW4i()JN'5!V.uZX-<%Enj_,MqD4i()JN'5!V.uZX-<%Enj_,MqD4i()JN'1UW +;]UM*=Qj;tzzJ(U)@?_0B*o(hn?p-LZoQS2]pr:k9Nanbc.@E:[*B>&]ip?hM:dp';!*?,q:^7I<(@W$ +(d]r6qXh7ImCG3ukjs8MSQfGl,/8e_iQNY^:\Q"\3:C8L9"a&fQ4qXqmY%Be5K:E2fIP8GOF8Sh7NFMhnM9l<_L +Y*_nGbW1toQcn!PtEhS4P)3B;B]o]c(D2R!oN%sQL2)&;c7o>@0Q]uaHZLjAN6DqMfV4^mm)nlnH3`L] +5>DVVb,oPeDSAsi2E[r70tg?;SU4;`6Z7un]sHK\.G+X^'W*;7K-?G-('hsPB0I/Np +h!VQmA0nEei7M2@)ND6mR2k2m-q5.pYFl`e48Gk'dtdqCCLA,A:K_:>SfQTi\9#4CO*r"J2R0F_3" + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 187.2] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 387.5 L +1193.75 387.5 L +1193.75 0 L +cp +clip +GS +0 0 translate +955 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WIq4iC+33n+a^d.Q8Wp"lQ"9o:B7D1hR7rd,aYAWGJ#%mL8WlX;7oEjn0f=ek8WpHMrlETrdN?Tt +T;c.`l?"B(V">:2B2\gku1A.34<>;%U7lOa,f'4rr2ojp3 +'dNYH+^.^OEl"V#^Pr+F,"lHt[YEp$1*'#-;%J5p@\+36d(cI.t3'+$hMH\aRJ]'Xf(!(nDJjLI-QE2?smC6k07Jp;4%@a!)V>,_h +Y?Q*^>+$r'Mi,S?RdoS2iD)Tu"Q2l`oL5:KX!dmUifY$-*jgb_VV&I-]@c\T?rfEnmp8$8VY^!"/&Vl* +9*TGk'e`&)#0@e#%QioI64b0JK_Dc^p#O$?p`\.J8L;"k4eTC:AB8sD;&O$EUVaI]1l]W.#GNsC_OS_Yk09B.GHIWH]mBADl-i[fmB +0IHB`+2M?`^uQ0s.R[eq)XUU3rtOqs +BI1R5o?NS2kYb6a(CQ_M +&Am1%nFTAS";!75W"uDf<,=BjKHKN=,:Hn8NYuH.70cp[@"#+1S`q[:SKj\nn4P[C+Pu:uT:XWd_Uuf@ +8;PhmR-'bT!H6!rr=o852fQefGnrHK=M&[C,+D'@<<8S4]V]rl]^EE9"rtDHKl*XlAK5dG\1d[eWR_/f +jhs5=1#4-XelJ +dkJF`gD^DT?4?g&30geA*hr+Rl@P7G>)EL!!'f13.Lq?(! +*hl[WCWWfaW/a[E.Rqkd5'QWiE(>c=-/,lYC?SNNTJt'kV[$KferOc5in@Z=Ss+>Oc"E19D2D'AXH2-S +Q!4Z#"V`l&7kFkMB&QpM\s']mB?dHdnW%/bAr.d2B$hN,^Dr(68esYPG._Bamm%FKI?_.g_)#sX:5Gb-M_gR@php:@dBld,)'cLJb6p>^t98Z1&-Kt*o2(GB+mWV9%o"onXR_U-Y+a* +0Pq:X3T\@)*\!r$V4/NTom!Soe]?U=j?@B2S?pou19e)#sX:pnRKUpQq55n(=n?e11S#!!%]6o&\&sXf +_$S[U6du?0]r@!!!#9&tMi>s%u)_!!!!o='&I->FYT#e11S#z!!!"L5LC)D!!(q9;6%K" +]A>C<mtaqo<#%_j22$*C*`>u\<"]A>CC<"]G#R8/4Zq+bWf^=GPr\<"]A>C<mtaqo<#%_j22$*CSod#!l;^@t;H4i>"]A>C< +UY`CJQ*@N8"]A>Cu\<mtaqo<#%_jQ,9U6_Nmn;;H4i>"]A>C< +UY`CJQ*@N8u\<mtaqo<#%_jQ,9U6_Nmn;;H4i>"]A>Cu\<mtaqo<#%_jQ,9U6_Nmn;; +H4i>"]A>Cu\<e!i[Xt;B6_sgqS>g.[=3.h:f8PXZ717:LR;^o:eLM$Da[eWX>Qe*K_h5We)GFIJ`_<='%V3n +mlia1m?%We*3,J=+9r6+l@MOZN?a8"]A>C<NB[5eeTfWsV +u#]/X&Z3$2)R6+(pda.l#aKS5H1?MU"$-)&F&iEZY'/7o&dj.oP9ZX]s\Ha$r*AuU8/4Zq+X^:)D +>D//(Y.'d>ISJj*#pM7R/O`O`qTe9-WdJKH]5,gSVN&9.MKjjIGsXpG=8GrWX?=:n(taiI/3?(4*MHXb +@;5u%6cFD2doq!3@-mQ2.70]0`.Cfr$NPVfaIe^H:Ob" +d^^0fe<&!mtaqo<#%_jQ,9U6_Nt/fUskNT + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 187.2] CT +[1 0 0 1 0 0] CT +N +-955 0 M +-955 387.5 L +238.75 387.5 L +238.75 0 L +cp +clip +GS +0 0 translate +239 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"/k?USsb*60-VR3[nIP%'rtmEo-Yh2U!@CNAmuH?!S=^5n]*p*ZcAE.]B\BPWA1@%hIE2[\p%$ +bT`Cs[648_cS=n%dFJ[,TI:a='&JWcBXY0%/_XOK5m;*himm/p!rO* +Q^3rG[^K(GI56TXo4Q55r`ak-#m:;Hj6c69G%Z/K]@QJ2NK*)_k)tl?]A5Y755t?P.#ZMa``:C4d"%Qr +ARupD8"7>L66X_^MC'YsA8X1W0X/'s#U'd`iFG_[cggN[.V%N+m7eX^e/E\^q?bN:b0%k70Sg1\XAA!J +Z,kSl2u_=YB,MiaJ7&`m$ZZaZ1.Ej5rEsnr)'>cSV[j7I[^NUUal+N)Td\I6m.:$L'?LPqipb[U>,Tgu +Ug7/-P!s/%CXZKC91-#+Xpj!cac&KD;2KnT!t0J1O7QhpRT`4"k6!k?NJ +;6_]0sH$amBgP>iTCk$qa3#kC"%a97mcXb$9!EY%I +BlHSX2BoTT8l=`@A.IMc[m'K`h@Fq('h$@4lKXe*RFH`T2e'%Z."Xn_BX?AaIqJnlpcY]A9WPh-Ri>hp\+;Ker@"LheZibM_aJ7MOYe9./#MY]j<@`i/h$8#W[j]iFm"B'1YW^;$Z[0F8>f0 +j?M:rIa7b6h;.W: +[$2FE#e!U1]b02PfFs*0EqZ+\k76$.b`'U&3GZLdBJcaf@c]+/');`9JB-*X`l?p'm:WB(RZtA8 +5kIO7&I\m8eY"t(8XYJA`,HtUaDIGBB?jjOrAs5F-=neZ!eGmJk=['5H#-EMeYLF-+R6$TAL7UYB/P0? +k'mE1;fQ+~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 375.6] CT +N +0 0 M +0 309 L +955 309 L +955 0 L +cp +clip +1 GC +N +0 0 955 309 re +f +GR +GS +[0.48 0 0 0.48 0 413.04] CT +[1 0 0 1 0 0] CT +N +0 -78 M +0 308.25 L +1193.75 308.25 L +1193.75 -78 L +cp +clip +GS +0 0 translate +955 309 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 309 0 0] + /Height 309 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlQ&TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!! +%P-!Re\u[f~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 413.04] CT +[1 0 0 1 0 0] CT +N +-955 -78 M +-955 308.25 L +238.75 308.25 L +238.75 -78 L +cp +clip +GS +0 0 translate +239 309 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 309 0 0] + /Height 309 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;!=]#/!5bE.WG`8*TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!!! +#WfDo,d64s~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 375.6] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 386.25 L +1193.75 386.25 L +1193.75 0 L +cp +clip +GS +0 0 translate +955 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;JH,ZM!5fq/l + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 375.6] CT +[1 0 0 1 0 0] CT +N +-955 0 M +-955 386.25 L +238.75 386.25 L +238.75 0 L +cp +clip +GS +0 0 translate +239 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;!=]#/!5bE.WG`:P'EA+5zzzzzzzzzzzz!!'fe!EZk263~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 0 375.6] CT +N +0 0 M +0 309 L +955 309 L +955 0 L +cp +clip +0.941 GC +N +0 0 955 310 re +f +GR +GS +[0.6 0 0 0.6 0 375.6] CT +N +0 0 M +0 309 L +955 309 L +955 0 L +cp +clip +1 GC +N +0 0 955 310 re +f +GR +GS +[0.6 0 0 0.6 0 375.6] CT +N +0 0 M +0 309 L +955 309 L +955 0 L +cp +clip +1 GC +N +0 0 955 310 re +f +GR +GS +[0.48 0 0 0.48 0 412.56] CT +[1 0 0 1 0 0] CT +N +0 -77 M +0 309.25 L +1193.75 309.25 L +1193.75 -77 L +cp +clip +GS +0 0 translate +955 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WIq[C,q=jDF.AMhW`le:B1&B]JK$l(kL!:4JXW9cF43Q>ph4t/[5m_EC2tr*nB"K6uK6AOh$Dhn$ +*+BkCZ.lS=CiH?J"j1H(FRQ3HqV8 +54OL0H[4Y2G'r_(^&7HEm662L4T>-UgX:+F[&DeN;]U(I@&a?.%;2I>;^GsFe\Y#4]QnS1&"t$[AsBo" +0NE_(L,2_ZW=gt^C1/H"Qu"W2bU@\#@&a?.%;2I>;^GsFe\Y#$0\L2CQr39$_,L`<)U:k[VFnpkWeHk& +@C"Ff0VdE'JuKAV24TdA9U1\_<=:T+_dofV@7If."B-V6C,dFaQms7HWYT25L<3K6_MiM:$)U9OK+#& +OC>HFYV;j-pYjf_,S;i.D\si),djr'/E7i)AZqL +]1ObWJuOJ\;M(k\18JYn-=iK[2(Zs"GElR9"B,bBU]aUBA4Y7f9ZSp@C06j#mOH%P$)SWd8-u.caH3EU +R?1j_eZ^a&gf8s*&kk0QOV1?QO7Crrb$?<\.`#*%)(EC$#oQXK0G?38[Mfon +Pe0OAZ/-"KQs_G&$)fkOWV;=5pM/^/kVPHBRn)*oU6-7Crrb$?<\.`#*% +)(EC$#oQXK0G?38[MfonPe0OAZ/-"KQs_G&$)fkOWV +;=5pMZ/-!XELRl5,P4*U,Zo9M%:Ie)RAamM0=rqtFk[V*pX +amNtR-@!6-Z7r'/IJG*qU%D!p\/lTG#&&S/Be7CQTuusV+[03IJ;Q=cge\Y#LHjO=l7_,Q&;^IfG3$T`:.UedF +2L`S_gU:r4DJB&(^kNLM:@-]BCVm9r."urPha)J3_T_Ls$"RI6\Z5j3-(&F,,:/^,@JZrU?+P.>b*C5P +PPn[q`X=Ksqtlkl*'&%McC?m62JecI,?pNAShGXGbU@\#@&a?.%;2I>;^G+%e]&[`fs524q"XX`rVO4< +\siSCc^m:JI=9Al/[&9]U8+KYnbS5t#7m,t]l;rI.K1lkKpglp'$.qKE`WRE8hee77S5@7_F:<4ET>($ +T>p<`l"P>!_83s.bOG"?j(c,Qp$1(Kk0.Ppo<,^n3dg_l'HboC9^mE9A77s9OK+#&OC>HFYV;j- +pYjf_,S;i.D\si),djr'/E7i)AZqL]1ObWJuOJ\ +;M(k\18JYn-=iK[2(Zs"GElR9"B,bBU]aUBA4Y7f9ZSp@C06j#mOH%P$)SWd8-u.caH3EUR?1j_eZ^a& +gf8s*&kk0QOV1?QO7Crrb$?<\.`#*%)(EC$#oQXK0G?38[MfonPe0OAZ/-"KQs_G&$)fkOWV;=5pM7Crrb$?<\.`#*%)(EC$#oQXK0G?38[MfonPe0OAZ/ +-"KQs_G&$)fkOWV;=5pM/^/kVPHBRn)*oU6 +- +;^GsFe\Y#$0\L2CQr39$_,L`<)U:k[VFnpkWeHk&@C"Ff0VdE'JuKAV24TdA9U1\_<=:T+_dofV@7If. +"B-V6C,dFaQms7HWYT25L<3K6_MiM:$)UgJUur8USFUGQE=B#:A-:U +AqCQ[JQ`Gf(GAk.V*)2qqn/niO/oN$/SbqY%&h7H\Vg!BAq>A%@S +jo]rQ$aZ0MnPUE0(!FYJ5i(8`5=`A2\`Crq)]g%QKt5^qef-\33nCB81YKCar]&`ih4&PLQ7Z=lmR*pF +GP;J^5*5PE^O=dTMe0*G3"Qtd`O +1Wiu4eGA_Z$Zdj8PsD/!pqnC^[VQg'+5ah_Iof]QCOc=3$T#WjKj^n`;[m8G7_,RN/Bf(MPlRYlET>($ +T>p<`l'_BD@p><,ShC4F-(&Ei$T#q2%:io0\>TN_eZlBaIJ`_4?'Q?[bQQ6.JuKAV23Z=%E<%Z=I%$RfSW=hl^ +OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I +%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'H +O/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2 +L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6 +Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23Z=%E<%Z=I%$RfS +W=hl^OG+S6Xp/Vmb`>!Fm\?b>Mb;'PWeHk&@C#P\IGXPVb>4]l3S?RYc_!H!_tNHM1YJ2/CcjoZdg\/hC:Sj2R)? +]`n]HbLHb((*#DZCpFR8qr;dBGAl&$.aM&II0^QC\mgi)B(YUi'?o!TgOUXq`af5K2WKHakigd[b3$q!)k22C*::/N,hu%q&O(-Mb:Ke +We5qVXl&&"IJWTWaN4!\P4;CBQ7Q1D@`;kr[4i7V?O3N$0\&E(AsBnGaMQ/@;$e%4ouN7?6S6hQbN +frpZV]E"_A^V>%eWS_dZbh94/_djkB_G'HH+daaqW@cDVWnB +e(?fH4j=JlGg5jLn8RE&\UF/9eT9lsC#&<_]C%_-q!\t>f"t,MHm+k,_,LM>Qr39d+:C!G73u2TWafOK +qtBEHinMFHUSFT2p[11elIQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m? +a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d +=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$ +0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G +73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b +8CbRNa=u +Qr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(S +C*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=uQr39d+:C!G73uJ8e\Y#$0\Q;i +.`#'d=dW(SC*3m?a$h<$WP8B.lc5&.TDC_eAqK1R_,L`<)T]dSE`W?GQoQcK!DW7.OX$EfSO5b3;r>j: +9^mEQQn#pO"'M!gmV3jP1WMp1E2am@Gd+6LX-@$#F'En'&58?sWdu_F.!IP1Kpgtq&r9LW7nU^HsF"AD4$.*m`c^[":mX0)a0Xlu'$c,dKVAeZP(YW5F]Qh!>MhO0W&#oahe`dn4WHEE/ +Dq7?\[VT*Y&"u=e8hedt\)-t%g\[$J0@RZe=Rbd&T/$Q)V^=^c4O*(cXeAj=E +JuOK/\#qir*aQ/:S;^GP5CZY@Y +0NE_(&knQuWo8o$@&a>;F0.Q3#c:3JjOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk^[f\"itaN3o3a4l"b'mfiOf*sXbkic^5 +eO,*%[JN4hEq8;54aZmes0hei.TB;ojcrp$I2(,*`lH."orilDo:Q%6s(M/385j4:J7HW[[HdfND;$gj +mbHLY/RQ6I0:IHP*')I#ff:lGeF!F<_Z\B2m^o[@\80^!o'a8=pQdJ'V),UXT^#Aq>0A.@KfR*+DnfSp>ebad1G-b/m67['fZ;-),LSA\ +cFibo468c9USGa,-$hRDN6clRq8u](IdXc0g3mX9[RVEf[)Db#P>@JG:s.<_2r/T1D7@dZ1#-fm/\Sj2 +c)YEQjcJ>Dj,Fp?N67*>NqW5AQ`lQrgi8+'3CJ_24LNtOBiFVhnu,m#/LOC7O6T=r2E!I;mbER=HDl,4 +L(,(ocMjHh&9#;YAuQk84ZsSC&ESJMSFST!5i;P.GF'mcf"?>(nDP=)][:ZO#9?Jhj[r.bAA#Rh4'.:p_o%7H5Q%)o'6_$8aElX1\jh[j? +9.>36gX`[.92JCZ;l^3H*TV +AcC$&bHYj7Y4<.8GOOCoX1-o.>Np:rf<=p=(]k-W&,VS:9'NBtkKa%VYC?/R^\r#iNZAKC/GS07IlNVs +d$WsK(\sE$Qtt9SRE<@FR"ZE`Dr8:7W4q+P9&iQWZ"1t>_hSc3qf&]_qtKPt<0?`i$ZhPd$Q%*X2fI`r +NTD\Bi8A"R>/"[Yn`T#;FJ['D/a_fe\`8]3"pP8QHK_'nTR-M&\,/CWU+AI2h+(>'nq=1A6^3KF07N)T +G@pEP>F9+?B#71o^!t`gq;(Lgp[m_!m66h.PXR`^p?Y$Dq9P5hNAo^-B>,?ucCDH0VON&+S)6'*>*u(L +#LW\Z%Q(@13cq;_\j#oIK5E`H/)$"(4%b%\q9F2Q%=-9k4VM.uWCp9`0:+B%D;3Y0"0'4%pY8]]2O+Mu +dF$?o#7iFk[]DSuT#o:5p^()1P/4CWLO80OB1i-dl05H)'94/!5%i02njL4J#BBupi.2-Ir8fSua49/? +m^qr%\T?r"J,JV>s(_;%eRlpd,qE#tB_PA/WW)hY5<"&Y5jODYa"5@dIdr?loB20oNOAXKC/>i>KHIBZ +Fm;qO>lF?!Im%;^d.e3fen77->C;#?`PNU1_hJUHc[5,o\(q9eM6`ab3@F`#mID"@?Q"/MH@'ulpkr8A ++d\tjlE4F)-N&a[;ODl(GiOc9C5B'BorC9<7VOuqS+(HS+8k%%1S!s_V+Xmm>hQdT +YEb#W@1H8.?'[0KJ$YXtdubkF-N%VmUIUB2>FI2mcm9MJ)]NYagY`#(L)Y"Ys6tA0Q_VsrqYU/Z\!69N +1M9I.'&CR&]$`06[&EX87sXOhYK7.Ak*p;c]9`UW:B05MmWZo[cC?mn*`m"LItfl\E64Ct>K4_(p[@"s +e^`4(T66PQbY^up.qco^`JZ,A:ADiTjNR$UgK)]as@?+7"2O.Qbrb^H=mel>!b?+DD +Za6tMl#!oc/TqhSu@0JH>o/5JXeI3A22?`M\Y=joM3(GB+mat'[&j[dD2`qJfSB2t8s4.SN]G%>.% +5DcH=8dX1_GH]m-%5^74rqbracfEu6O=j8&XSVr^,tX7F8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>j +OsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jP,.0qJ,fHf4*D#@$p=L\dmoH?rTG8^*BWuE;br[f^A[c[+2']; +55=&gSND&@fVaoY8WoOnX]p"o2:m5M`V7+@^"-qEnDM,YJ,7WR2nf(QWtVXBg*\J[]W\MWRWi)QIf7eu +?etL9hu)UqhgP7T9DbNJ,Y?G'](=3/9+,%5C/q7Ljm'sO-QdWpO__/6p5<;U@^@lbp\1$Or8Sn!\DYn1 +F8+L7?[(=Ke%+W`rpY^6koP9FdIHbsQX>2eeuW#`hgYIaHtl69,\`^=GiFS.78!ZQE;O#BqU_dN\uKl+ +`mqf;^/'4KPKgR3\9_kd?^SV#V(XE'ZD;d=pV6`V3Tbhkqqm`c!0SU!d`;4,=2+7E07NGCDsHcY/tMf& +s7t!3GM`.L079JkqrXAdIc,b^-n^Adf/QO7#9m-*YXlD^\M+/G.U;Kp=Of3ECAb*=L#7r-no +DV_m,ZY#JH.3E)FGe!3/Ie9BWQ$^7>hKu(?Dr/.;q(^cVs8?u]L(,*;>U2n(UiY_=a2_E!NBBMk]qq&Z +rUeQgqU;&P04'&!eQ8)e^\HfH''0/>f9Qs4gn8'H[:#5KHs-PoO%"AGMauXo]JI= +q$#[0BW=ijn\hse?]m-#1fWmDg2c4g.H.tR]qmEL,5nK:I;7ldQ-K%aFQcQepPPmEP)SsobBoM,Bt^c. +mE'[g,Y=.5jfQ#K.UD]GqsV;X>GkVkhg=h*c[=k2TDRF,m^_MP;Jo-'DVX(s\ttDTBMjZTpV-C8`Z6VS +OsEUkY47[6A]pBFn?%]-:%@SVc^t$D+8Ym3huA/kXg^3;m9@-7Vl/_6GMaDX(arJ/NM$-QDh%Z=NV@0> +bh$.AXBDmPRs+5TktWHhlH]L&&tPh^nd[L%_XXhARIXJ,]9,YJ'Y`TD[atj\)Oo+"^6,idQj +TDe]N5Mp_C;Mg_n?f#JMV'k/AcTTN;C&N`=I%gAZ1sVZlmEO6gG?qbmot/S7H/O5YRNH24il1n2Ce9d8 +c+Nf.J,_0Oq9-$Qa"Hc8hg4Uj^]*o0c/4]3Hd%SQFkqkVRq8ad*,?AAIOsEV^,Y=.E8Wk>j +OsK;7np^X\*>L[~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 412.56] CT +[1 0 0 1 0 0] CT +N +-955 -77 M +-955 309.25 L +238.75 309.25 L +238.75 -77 L +cp +clip +GS +0 0 translate +239 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0VD2;/&"3,=H66\aLkX.`[!]Q>pA0jj60F@SRRT'hQa<7c2Ma\JZAhW3].AT?_(W%^S,Uk1I#f4G% +Q<5P^`FL^!,^O#6HZe!o4?SDJX?5J-?k+:7>FL8G'T7$2\7WkB<^:l$pVCZ#a +eGVY!?@(l,ji]%G5*$dMY2_1=??Y"5VI/DRDn_W'IJWBf]:4a>[tM?(]$BuA`hb&)ddQeEp\3QplT)QD +[bEFGkb9/Jpqcf/NgQ*7!\UZ?o#N&Z]^j5+5(n=R`>;T\EJ4a6*qQog7*Lj+qtBFR`=EtM1j/*5%3%3- ++6u.IH.)ZekFXfm:7F0/L)Y#Dl^6RZJ$`O#f?6f?S^hYlE8eYQDnJq3/7-!alDq++f_aZd0@#tA8"F"- +aX&H=Xup^`7*AjtG3gWc?[VC_.orbHl)1/rRoEm812P'q-#^KieucFIRAJVQ5%?d*_:= +^V:Vbb]L^$Bc;BW^k(LRj\'X;Q7^]nF%pY-]tM,0pDj]ipYC%9[khX?d"(QPmXk`TSoK4$?lH@kj$s!o +ehLqDUhssp3Lf-"eq*\"?[1N5rN(J?o=fYMb.Z+`o8L,Z?+]gDpYOZ5D4@-GHg\GBq6"A&I9,*(X]hRg +4*U*lJLX\J\)LG"bftq-9WKb&A"gDb(D0l,>k`EBis"pP8I/6JRWnoWoT*/$?-UUn]tro&T$!WepOJ[dVtc96 +mO38CWg_=lYoMQVhlemlqF\5ClD^](@=#WNcAS_q>'kCpDDG;3I"B>>1X^^Gh,dp2i.2-_^p.CioX!Ct2'I>@ +mN$YTnTJW)[d++&[mC,QHgeYeJ0O-A_c0h8F4L$BJ$5!XQ7].$qAEdlrUnc"k2VLeY23EA?7i"r@63A6 +]6)=?n!>W6Hd#Z:`g=F&I*mP\\`+JHX8%-8cW\5dUGUg[/Jtj#Vtt--G1U#D'a'>\>7OSGe^'))K7d>< +BY84?IJWU3oB3TA=mHESd:0_6GZlK\,[VSC0GU-!F(`rOp[ZK\hu.KF?[ocGiq<:TS3O(#YMT#3Fm4._ +Re0r8@W"3-)RJj4\*E5Bfs>>;0>IE?pi"1Z*=/iUQ5's6&c\pESeU#K,7 +[i*GGe%$9Wcl$\gA^#_%i4J8d0Im1+_AmBLGc0$f@18_Aa=/W;]YhjY!)4WV"6bZm3k_OMtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf +">MtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf">MtL +Es+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z +&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/3tQH90foqf">MtLEs+3Z&9@t/ +3tQH90foqf">MtLEs+3Z&9@t/3tQH90fr&ck/"mMI[mlNXd2/(p/A0H]36r&9[H0lB4_6TNZ_p=@ZDRC +#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9 +Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e* ++D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jf +NZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p= +@ZDRC#@`]9Ad;e*+D(jfNZ_qhM8hY;37s;`FpDV:Jn]F:<[3uT#A,iFBm3rc,FEWZAi50SG@4GrR!&^H21fOd +R-:W[r'/d66)KtTzzzn/j`;o&RU%^ki^PdKe2D^AdupT7?jN@H4=cV_lM*[pu`An])`P=0J2QCK=M#*'C5ZV*oh1mC1Kl^]dI**jr@gS1RV)4'-t +3\7BC1#YI`j=PXBDmlq!mC&(+i_'H?K(Wg/p_dqD?GP,N@4P;d)!Kg#RN;`*@]Fe,$0d:i*+[,fHK\Y[ +G<@mG#gb]6Vk09Ci`"PqB&$!=&NOehZM, +"s6,9rh0qIr`$:UP%5QJpf]ji`u,IqI44A,Z8s>%scCWkFCO=e:-Bq=;kbRW/7rmA%5jjHEB\fLjVn8L +@pIzzzzzzz!!'fFq$/E;B\*~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 0 375.6] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 386.25 L +1193.75 386.25 L +1193.75 0 L +cp +clip +GS +0 0 translate +955 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 955 + /ImageType 1 + /DataSource Data + /ImageMatrix [955 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WEF`)L+33nk:%lhB^,*kVYpPa[1B"IB-BX]A(!1!L81)e-OX->_dN;'#oUVCAA29R07?[T^P9ar7 +jf2#*Zg"D +d]RAHJY!!!"J_:>SfMi*BIREV-ZDjFo8#7hl_#s0dM,/!6@WMt=Cp(biI0EV:T/0cN;S?N>Z1c3_9g1kj"hS#C4G^= +\ilK,c%YgB-Q($;-+cAM%f:S72Z:=%+e_>/dFiFS33*=XNpCu;sTHhH[S[V9qNS&,R6!3f_Pbfm.n\T5 +r`20OIoGhBU_Y$J[1LJ:H9*'\nbUnkpL+K_ZD\om=>i<,sSVQ>bIO02j)^9b.%rq)_tr6t1:o#o5"a2Z +-MX]oE?.P!$Ug$IMldIO03VW-TEpI)X"GOODP`,'d$BkaQK?a4'o4l+B0]i;B'naZ,o7ul5PB[bS:?dI +X%jQ'imF0jUt-BgH3kg95$mh9`MC"Q(h?UcHf!!!#B868lZ4!2?lDVDJ>(14h**?SeQd)qr^\(90eh0n +D-'e@qF(ffSV!S2_/CtW9U]6E`3l05JPkn^%Rc@'&e$Q8Dne(3$u<)ci`fWi?&4u[t?Z>7H3r?af*8M`*P-e(_?M[uK++g=t@fBJtj'S?9 +n'#GNZk!!!#BLt+WRDkj)&DXl/O90(PPS2tf8gK;L:7`7,P +_m523GeA%*9iAfFdlGh6+\3]c81"pYB[H?sVgUO2+$mNrW[S%lZ2kdQ(5It,3BYh+_mn*92\hb&Hdg7U +,%!!"PdB@!0Wnojs+nV8je=Dh(DVl-GV$XWCSZ*?$-$:O(Og=tB<8Jc8H<1"%5molbLjSUT5diKPO[Xr +r'o&TrY%]ijA.UH["$F>R*j+nFa>@1ZA*?RL"0G#>t9@p&90EV:T//OS:Psl(ZYHN.[ZIu<@Vg"gN8KQ +enkX$1,)r7`s0Gc_JLBVg@Eq7kfA'nZ]AMT[b\iTReJV=g#KF;JGqQdE4KM>C4!<<,JB5hgoB`=(E3qK +k2@e/NtMn/WsFR=NU;q"(:PioP4&KE=E;U4SWjU3Jq*2UZmHko8*d/nhkF=)c:m_Il#4,*+F*# +X8g_&\KWiC_H;j[Rc!7L&GJHJ?G.p^)a4F,?rC2Ig1M^@t8:+E]t=^>>#rX(;e^p@\"MMmCpJ`f.ac"= +T"]:V32iYLS;0GcK.^S$UINk09lEo'GV6UQEYfpu=[SU(lf#JD^lKf6Wkb(WZZ!<<,:2_d%La^e":9AO +%/*7f/4e?)uddTm.?,(LGP$AsVaH:.0r2"T(g1C#*H[DMXD>s5$p$poj.*MYk!5F/e=0GeEZ_G(YJ=;oF)fX +iW%D2Z&bo:H5GrTQ[aTJj%X?Gj/le]=%\$*!]?2r0h6s!uY(Zn'(k8.*5[dJ8![^N +W\H0\YL:Bsu,DJX-J*mO`:GWGfgn#,2q=AYVQHfhKBlr\bGlL$83(Vd8_4!&9C>$C +K(ar7ZLV!M4F5(EQZr;7*51B7CT(,@a)AY!dNAE;VWq"!Oid(Gl!2D=,ma#g&kUG`?-n-NUk"[@G3P]N +:8S9lLNJ'S?"zzzzzzzJ<\^e!!!!M'2)>mQXm_m7]HfkW^*QkUoH\[h@Z^gW@ER_C'\lO+&Z)]<U<4sMS"]A>++k7.dO +?TJ?5cP0eW^*QkV!8t_.ZgYs#cWP4L\)`i"]A>C<Q%5cP0eW^*T@&ZGi2.YNmu#]iNY;H4i>" +]A>C<Y.tk->dt^m;86(4?'hHk:,#oacKb..Y0Iqg\B +@DW8'hD>U<#%sS4!IZIY.tk->dt^m;86(4?'hHk:,#oacKb..Y0Iqg\B@DW8'hD>U<#%sS4!IZIY.tk->dt^m;86(4?'hHk:,#oac" +a-iK?G64)iC!/Q$F9fk(Bm]:+B8SCr;#sI\bYAm87=,#il5h\h#@6C^OQ:Nr>Y@?j(Gt*W@F"0G=8GrWX@0R.EpSqGMB@\>mJ#WDJieG. +ARNS.ZgZS7&iMP$9>(`R(730^,7bBrK_td+VLTN5cWZQW<0%K85oJ`_VEQppX`d-]\Qap*\@+,;KK/T- +\Y]YTBuu]W^*T@&ZGi2.YL`f+$@nEjEJQ:`?X]m'P8@LWsYZ&5cWZQW<0%K:mf^hmu'[MAgopE$Da[eW +nn>e!i[YGR+$r]ManIn.6ad>UoH\[h@Z^gW@C`3du*UP7hfPa5-#$dW^*T@&ZGi2.YNm5KioRP4e!?(A +gopE$Da[eWnn>e!i[Z&O=Gre+bW"J)AHd45-#$dW^*T@&ZGi2.YNm5KioRP4e!?(AgopE$Da[eWnn>e! +i[Z&OK.Uir5W6\6J/WD6"-)=Z`69j$Dcr-OXcHm63sPgQk_s?Z`-6!QK#6N5cP0el5c!:OK(WBWrE&%\ +V`?~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 458.4 375.6] CT +[1 0 0 1 0 0] CT +N +-955 0 M +-955 386.25 L +238.75 386.25 L +238.75 0 L +cp +clip +GS +0 0 translate +239 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0VZ#,+i!(G:/PGos4MfgG0HWQ"4OEYY9GEJWpjZ25]%.O(=\kmF^!rr<$zzzzz!'GYG$Z!L!n&Xi% +FSm^(UZR8k__X%S0XY>*R<@c:b@+>Uj-Wmdnlj6^p0RJCO/*;+p?XZ3/Y#q!dI>%]Y\O+LH +@]g8`Js\(,S06r$0dhdf6HfaFXq5Z+Qn-+F+Qa"c-]#<"bR%a]&9EGj/&[r#jpEgjLq,jr$"C%KF+Tc0 +`1.t4$8Eq+2!0L)R&6j2L&YeWB0T.=6&RC;J=%k*SRhllZ20n05pk'J$~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 574.8 -0.6] CT +N +0 1 M +0 310 L +953 310 L +953 1 L +cp +clip +1 GC +N +0 0 953 310 re +f +GR +GS +[0.48 0 0 0.48 574.8 36.84] CT +[1 0 0 1 0 0] CT +N +0 -76.75 M +0 309.5 L +1191.25 309.5 L +1191.25 -76.75 L +cp +clip +GS +0 0 translate +953 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 953 + /ImageType 1 + /DataSource Data + /ImageMatrix [953 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlHSTE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!! +!"LhZ+\G + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.24 36.84] CT +[1 0 0 1 0 0] CT +N +-953 -76.75 M +-953 309.5 L +238.25 309.5 L +238.25 -76.75 L +cp +clip +GS +0 0 translate +239 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzp_ +u9P$uu~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 574.8 -0.12] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 386.5 L +1191.25 386.5 L +1191.25 0.25 L +cp +clip +GS +0 0 translate +953 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 953 + /ImageType 1 + /DataSource Data + /ImageMatrix [953 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlN5TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!!" +tq=u!uW~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.24 -0.12] CT +[1 0 0 1 0 0] CT +N +-953 0.25 M +-953 386.5 L +238.25 386.5 L +238.25 0.25 L +cp +clip +GS +0 0 translate +239 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;! + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 574.8 -0.6] CT +N +0 1 M +0 310 L +953 310 L +953 1 L +cp +clip +0.941 GC +N +0 0 954 311 re +f +GR +GS +[0.6 0 0 0.6 574.8 -0.6] CT +N +0 1 M +0 310 L +953 310 L +953 1 L +cp +clip +1 GC +N +0 0 954 311 re +f +GR +GS +[0.6 0 0 0.6 574.8 -0.6] CT +N +0 1 M +0 310 L +953 310 L +953 1 L +cp +clip +1 GC +N +0 0 954 311 re +f +GR +GS +[0.48 0 0 0.48 574.8 36.36] CT +[1 0 0 1 0 0] CT +N +0 -75.75 M +0 310.5 L +1191.25 310.5 L +1191.25 -75.75 L +cp +clip +GS +0 0 translate +954 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 954 + /ImageType 1 + /DataSource Data + /ImageMatrix [954 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WDePF!rQ"G[K2T/+#PrD(;Q(hc.NuZt&qXo.#@`f$Y+8j[9S,[N6#l[-Y0;&VA;.'g?9Ph\,p'eE:3=qH)7'&0$c^:fW(H$<1#uW]:S])f&WKeS`rIm**bL^cojW2`*_"fa%KeXVDj9Zsc3FbDZCW#;2-kM^`D$qV24W^'PfW[^iqS;3n6bI&jgW3(ee +-Kd-H>VD3_W<-'7W2`*_"fe$_OHaJ49O`61e(S"&r63X8AU(^k,"#63W[^hFJZ9&V2$qV24W^'PfW[^iqS;3n6b6kSZD#M7m@o(AkY*[4k<.V%V<)oTk!`dKj8BS<@:smE4LM%ss +$@''e,"#63W[^hFJZ9&V2L1dDnP7@N +Y*[4k<.V%V<)oTk!`dKj8BS<@@(ER:c?FNC?6Va7"fa%KH\[c'*4OSoFCB]gRUN$<1#uWWH08 +W+sUF;Pn#Z/6B:Zak?"4m,<8I$qV24W^'PfW[^iqS;3n6b6f:kp+*OFcn<>DI45ZMkHOtnU>GZ`Y)Er1Nrl`0bK;?a'M:fW(H$n<>DHIc9kL,AH`Sp/!p+bg^mK7 +78a;h.Ss,rWX;]_k92;'Z@[js9i"r&F(83E"\BH-W^'PfW[^iqS;3n6b6f:k2?VsD\O!8GO]6n3$<1#u +WWH2FGt?)bXPdMuZ_SJ.)ni6O8JS2(5ZIuf.T"ZcLSRVe$6E7!L"aQ/1,C^dc&ktVWX;^*TW/&nc&%T9 +?bUpXh2WRA?LH*4b6hK*V5bH'b<4d7"7WI!]A?*b2jV'?e$9#k93HCr +bu?Pj;?a'M:fW(H$n:L(-kdF`qr"5C\-Bq_KfSE3WBe@Uh3o +`JLT[<<]BP:fW(HjqLDAgY8&H>IJbEhP$X:JUrCA@;BZEjia"6h1+sB]"5IZ8!UqM.9R`Z?6-_mj!+Et +I,Vqfa6%?5_OZa.QI^^&WWH08W2`*_F6OrU]m=fs:J\P$Q"aBPMoATU)k5NS;'et:"fa%KR +i*oRY\gfculu8[K:fW(H$<1#uW]HYHK' +B[i"TTGe$'r\,t<<]BP:t;'gl."54M%_6`-77lg-e#@&(X!e]Zgf9$4EB=q_X7*s_0N&d6_estW!ZL( +TW/&n'rYlm6W0dm"V3-L7$rsp*^(ERj0#PfjAX*?$\.6o:K;+/G%aj6'30U`$<1#uWWH2FGt?)bXPdMu +Zdm[Z=8?Z;af'Tfg8aMdVuS3F!`]u6W^'PfWg`O@P\F::_ocRA9Y@c-bP,mU/ul0+4#G+?6rfn[!`]u6 +W^'R**g/91.sP0!%glO*]=PR'*0nEl4i_39[jjHrm_Oa[5ZIuf.Ss,rWocDhAQi.(Yd1DGFt_1TFaJZ9&VZ]<1h4S$S,0WO&B:+kj5J_R4e'!0) +%A2@S%CniY'30U`$<1#uWWH2FGt?)bXPdMuSHomLaX^Y]4L[HAG3,H_W]&98%8Kk+W[^hFJZ9&V2o)8G8+-gc@?aToXBQ0[q:fW(H$<1#uW]:S]R?eRMXDEWK>EidV@ZgT1^8aA6 +2H?O/U!_dGWX)T>W2`*_"fe$_OHaH^_8FE-jkd*U8]a]MQ2G0";T;^HgnE-:-`RYV'r\,t<<]Cko8o2N +=i&"uVQVZ[Y?\VYNn8k5DHa2d<5.IlC.(-?]HiIE<>DI45ZIufRe)W*Bj&9ib,2/JojZbq\?K[mNjlKh +K:k:C=B:fW(H$<1#uW]:S]R?eRMXK8tL5)PeY7^%8dXm1.d7UD;T +7TBVl.Ss,rWocDhAQi.(Yd1\EF7S+i%]>W]W\/L/j?'>B<)oTk!`]u6Cj +RNkDh$rn%@W^'PfWg`O@P\F::_ogQ6g/j&.MkfRhI(W^*<(Yo=WWH08W2`*_EupK"/Lu'JffE]V7L/'3c<<]BP:fW(H +juao#>#t-sZ?,X2l>W$X?Sr:i4#BL.5ZIuf.Ss,rWocDhAQi.(Yd/?>^IJZ9&V2u$L"M5ig@"fa%KeXVDj9Zmd2YAEW5*Ng$C=3W\c +]::"#'30U`$<1#uW]:S]R?eRC?eK+a:JT'FC(/agTuW"i5ZIuf.T"ZcLSRVe$6E5+5#eq3HYi]HVu52@ +@>tq#JZ9&Vn<>DHIc9kL,AH`Sp)]u)KX$%mn1s4iHn<>DHIc9kL,AVD%'p?'VhPEV2fK8G>< +lQn:j8_KeW%o-(-W[^hFJZ@ip,\:.[W,Q=WCtZ+-d@q!b0!&[?* +n:ZVW>P)&?L1;]gr]L.;bS)G(ldLB]-+!`]u6W^'R**g/91.o\AX?b1>.H/li]7(@70 +lID=jb:sOZ94ZIDg.4:^3)LEhW^'PfWg`O@P\F9oWbM:kc'ufo_h[3jVPY]bU#hYslQn;(,]/&P)P!`5 +<)oTk!`dKj8BS<@:t;p80eng'Olr%RnSl00-Hi4Tma0l[n<>DHIc9kL,AVI.]j`fU12gHYFQHSf; +]gO4EUbV;aaM)DJW2`*_"fe$_OHaH^TrMh;]tM+(bKS1(iAE@hrU54PDAa<03HO?,*1uju^G7)i*PLj!GkWYuJL.Ss,rWocDhAQi-5C0D+=HKqK=hgYGUJ,)`A>?glAe##jYq^bpX.,8[j +5<^eVdFm3`@)7H3-[GLGLVqb\o7+P%&(aHb4=8>Q%LnJ!o8fmY<)oTk3F1(tl04=mOfMQec?i?aAVI.] +jk[Om5PE`>Ko,PT#9J-cjd22*R:BELrqY_!HK_&HhgK]\AnGjqlpj;:dC6mhm0a^_)#PIpM2*3pIZro> +LPF]dGt(d;W[^iqS1*X9IJ]<]=gIh2N>jelht^_^9ZmcC:!80AKAlS)q9OPFLPNYBo#m3nPadtWhRpC, +_1Gt)Fqnr+EofpgCp1MB8f\0))@o-rEf`,54#CtS22A-15ZIufRStIpI/%XtJ-Q8_AnGXMT0D)^f!]/5 +Q9"BZ[&d*AZs6@nQCi[+q%m@^iIM4T\GuR*f(grNEuNIr&r&GgA'XLX5[p?i@"MK6N6/ ++EMg#R\(T=H)d;T<)oTk!`dJoP!8K]1W`q+n%JI%R"s"[5@_TE"iP7j!0O +Z,[p_ieoJX^#s17]bWfdnF1CV#ONQC60@a)1_3A>kHAVA:fW(H$=@KJ)oJ45;G"bi.DQC?+[PR"3q&kK$<=9C#riGl;';#\>j%<n@LW +d7Ikn#Vm2`;L^bb@]8Q&<>DI45ZMmf8V`%T_XndZk2_NHc->"PD_LiT(=/nZX6Xn-WO&q[l;N,o45VL_ +Cl^g[q"!OiM]_rrk$JnJcPJ4(GAIc`$26P3HXr&3W[h%K +TW/&nc'9R!C0Ie?mmRPHeLXh0B[PteS!1=7"YDpIr:8#aQb5M'`!i]@+MsoaSna:iWWQ4#TW/&nc']q" +)&Zj1L'P09Eq7KS`l?"6/6MDqCTiHLn(G#pN696YOc(.h60=n9Rd&!Q7p@iL<)oTk!`dL5-S>6ihk5GiH?mrU0IUGSlq=^]4;1VPaC*gY`&1GH8L4)]jUT`LeuF +f?f>gC)3bJ5ZIufRStL!kibS:_`b1GQO2D\q'SE&8TSroiQ_?CJ,JgmB?l1b%idQJheYo6,VUQ-"IX%u +Uab`YBV;@SU8e8p'rYkBOgOWG,[lq+R@-'Xl;Utl>KO&"&kpIUMNo2&*DO6a8H[rqaBZV/9kB)L_c9D7 +<>DI45ZMm"UjMk(/\^h!GBZ"7I%DI45ZMkHOtnU>GZ`Yo:t6OW93LW.TW/&n +'r\,tDI45ZIufRe)W*Bj&9ib/T4:k"%OoRW`?a%KD,>c;b%J.T"Zc +LSRVe$6E5+5#aC0dRmLq2LiG[U#HNh)]\%@!`]u6C#t-sZLOWg3KhrAQ$55S''H7r<>DHIc9kL,AH`Sp)]u)K,AFiUmrF"*c;b%J.T"ZcLSRVe$6E5+5#aC0 +;D[[d&E@"qTW/&nc$;k&[&i1oA`H-XF!NVdHY,Ijg#t-sZLOWg3KblA;O&7@oANqSTW/&nc$;k&[&i1oA`H-XF!Oba +X"$1fb2?N)TW/&nc$;k&[&i1oA`H-XF!Oa>W6,U0HZ6,;:fW(Hjuao#>#t-sZLOWg3KbjolS>uur#t-sZLOWg3Kbl1?T!b.CQ?/E(9"5uGZ`Yo:t6OW'cO?WIYUB;?-N[$JZ@ip +,\:.[n?B4gTrL,9.T!NrqKFqc''H7rFr%apnk*S>uur2$A%U3 +q=Ee4\3JHJ!Z-arC[Is-F$g4(K_gTBb_loh4HtDADL%Y?p&6 +do5[u0J+eoYp`H!*(^mDWfUuGL=r9:\Z.npF*/hCGk'd$_Y!Km[;4BKLPJN/&0jll>IJ>spIUa<'t@?[ +W+q?g$<1$HVJ>J$Tu":>5ZMmf8IPDJH@(!s[tKULWMsUXSiu37#h-'\(Q/>pc&ktVWX;m0NH^-^kGeS` +bpehLo^jt!2]k&#,lE+5FL7gBm]NFdm"YH"k#W]=A5WMuj1-Vd?/9[%iZVI]o`('U_,,H1`E +'g,OArsfL!#t-sZLOWg3KbjgrB*iCG(%2/bQ5 +nkh8_W+sUF;Pn"/%kAS@WgY03!`^!+<&Hm92Gt?)b +XPdMuj$Qgf,ZUM;6<$t@4OkEO1P0W[^iqS;3n6b6f:k2Ek(t7oBMj.T!OS3d,15GZ`Yo:t6OW'r\,t<udrZTu#Ad*p9O`%nW1T'g/R6 +I&Le?U^C($TW5lqW([f.FYSSLR!i0#t-sZLOWg3KbjgC;Z+WS?(d7,\:.[n?B4gTrL,9.Ss,rWX@I[236!BT:@oC +c$;k&[&i1oA`H-XF!ObYWWH08;QtjeXVDj9Zmba<]3-@2+LOJW[^hF+oA8s]EXr(-_.NH)BB>[ +R?eRMXDE9_C6#(s<)oTkK!21s52kj?EqWKj$6G?c/Lu'Jg%\Co*(^mDW^'PfVc@MZ_V*17%B!4tbX5:* +AH`Sp)]u)K,H1`E'r\-oK;pqZ_Dqj$o>X-qVI]oA.iTS1RNnd5<>DI4OJf,O5N/-`jIE-J3T2,1e$=R\ +Q&Q;Sc&ktVWX;^*aD4f5PB1'23p%S7e$=R\Q&Q;Sc&ktVWX;^*LnSR#>;]#"B@Gg"L8rOD8BS<@i*g3W +6WJ1Qn<>D7.>4WEQek\@tDI4ck'4CS=HX0g:8B,VTV>)VI]oA.iTS1RNnd5<>DI4cuq45UpI)ni8%LSRVe$6E5+5#aC0;?a'M:fW(Hhl-Qhk<$TQRFYCLD^cM:%jrjUZ@[js +N=T%6O]6n3$<1#uC0C3omn!fs?Z^RL*IY@E6U,";NGZ`Yo:t6OW'r\,t<<]BPPZBA]o%@c,/iQ,;3(9=,W+XGJ]mB?d +ac4cudj$(^pYL?PVI]oPJeY.uNhd"]n<>FZ"WgfV*SYGF7;(l7gf[A2'3&m;kP,HZb6g?7.8\P78JS2(5ZIuf.b%AhhYGiPKUu\2G!AaRH(p=ZNjc@'KlK%3/K[0(38?8C0JG:= +F8GOnE/-#L[`n85R$JJ3#Z@=q3Kbjg\7WMuk4$&WH- +r%O-L^$b/ib?.Tk*(^mDW^'PfW[^D=Fl"LE;h".\2?O$`a_,oI-RY,XCWWqG1,?^A$kFPp=<+!63Kbjg +DI45ZIuf +R]AYeag<%0FED?U]Xde\Y99/DI45ZIufRSuo'Km+n)E8\OUbfm%bVqTkYP/Hn)R$JJs +=;7]KNhd"]n<>DHI:,+=*HhZq[^@QJkG;nun<>DI45ZMkHOtnU>GZ`Yo:t6OW'r\,t<<]BP:t;(kUeJr=*`b0_<&I0D"fa%Kn%oj@CC6#(s<)oTk!`]u6Cn<>DI45ZMkHP'T*BpeXtAWocjP +5ZIuf.Ss,rWocDh>mp]5!9?GsK/3K-zzzzzzzzzzz!!!#W0%8:u,9nF7Y?oKM5U(=Dlrh1DR[TZPkN]U +!kH[:5G`7]E+$Y3:r&G8J$jY?eaurL#Z +N.H.HO(`3P9<1Z:%3a=4?[VSP2(h,?`3Z<=!]M-Kq[Vab7^@h9ZK+Gi0HKikSY-+qseMDS;P4`hVfLmm +2'H.Gl%u9&73,p-id0p+uecuqNn@0AhJ#6cj=0>d70mE$=ohIi`1$dlB[ulf86IkDRYCH9!BAl.a\[e, +X%'<%@ps.^4=0<,1^%^C,qTs9L:Oi4>B_^._DhYCo@Bt4Ul)3.7=d/D24S-cQRl5+5IM`hW*7FZSk>OR +'a@o<&p%J*6YEJ5&X^bNm&X%n;/,/Gc=1fhAUkcD;,0KDREc$,KHHD`LSpRWHZa9Gcf\"i6'e=OKhU&)lQU9-Xrqk,`$4\0fS897/ +\N>jgnhq6iD?b@]/,iP&A#7m\T$rOTZm97"bp@$l\I`&O/@XNS$rV,1[O$(Bc_QXj1-AO``.pVNV',/= +q6d.5K1G.331jo#MB4$&V`UI!PZtuJG\$r31-0q=^b4Y)P_aEYFrqF.XfN$Yhg0T +p_BWgc_`-J#1Ru%uGsLcLZ")a24EBLRo#>7aQL3>0,IM$;;A_a;"4=7*@l)T>a;]'1&__^/iJ1PX8FD2 +Z/poGfk.9^;k1F_aK;a+b$BRVqHXm,[%hlSL^d*$.FBS2_*Uf/7? +pKV<;8Y$LtdJt\m3+hf$Q&Veg^9:#MTpV6aK5Q:3PqsV#&[f58&^I[qr$;4O-X$;!NgqJ2`RYMJ0VIbm +R-VoI1Ps+gf$=>WBeZ2c4NK%Lt34X7ue>^:r&q806Co+hYIG(o;HhZq;?a^.C#/Z/S.qXN#*?Thd`J"u +5]=WFG\omuMrNEs3C3WQ\]65e+\n%Yt[]GN6'&d'*if?d9>.*Y%cTY"nDeE6S@p2q.nA>c@>$@MEm^^H +]afLOL#2q,HET>'Z++G8nY#qr\J[liRY?ueUl)4RNLHcJ:A6QDS*^rks5$e*UgpqLbTlp+th&.83hS&s +gqk;uTcTYPQO,kAI?EuR)2%/:pLRIHlIDq09hjSNpoFuOa+!cphu2I<56qe"6asc$dS!6!/.')eo[?UB5.&tD?G3qJV^Lf +*aKZ7>pn7+;"3DL-]$/<8Ie0&jbEl"#:M5(u\JN9GP+SYAR$\/?X092,8nuCCpmJkWpHF:8pt!k#e$?) +=HhN,a0>@3jecuh?E,p[hZfL#L_q.Le(UP6Q2JnoUeQ/)'l4M=liSiaY(W6U.14-:k;0[QhnDV:,)cs+ +C_7==?aX<_AZ.)2#gU`PR&3p'FcVi'CdF#4EoB,%f]RlY*VI`-R>K[R6q''jXa2NrQ94!5=d\TUqQJsL +ue6mTKH7Im52r7ZJFD:LZ:GLCp0\?!kp(hQQB5^^I;u$&H_kW+)b*=K@hf4>uJ&(TrcuX&bLun9a,,WDK('Yr=U#%h0! +(])Ag1bc93*4khgN`gT3kb4;>N%)3pnf9`6PmH;=hfE6QiWAR.Z)mWoril +,-(>Mk1GNZUbK&Pg`>QBjkf2/CcrmmOHFU),SK'/eb8CbMjOIDET>'[a9Z^.$ie*=ET4nCT)>rQ1Ga+Gm+F5QlN*qM@8c2o561`azzz!2Su]! +MNt&`;~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.72 36.36] CT +[1 0 0 1 0 0] CT +N +-954 -75.75 M +-954 310.5 L +237.25 310.5 L +237.25 -75.75 L +cp +clip +GS +0 0 translate +238 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 238 + /ImageType 1 + /DataSource Data + /ImageMatrix [238 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0TIqVjT+33of&KA?9N)T +XT+PFGjo*>;cE&\n])aOok!$+kFK+Xo(C_ECS-%KfntQ^`L8Z"SppP#rr2oenaYqsYJ:#+b>1:oe>!44 +`ES?g:B=;[3p<5>=`6l$%[e&\.:E9BVJ,I^G +f,(pUigtMkm`",AHgeYi5+oX=fBcWa@k`Rl>7l>VeAS4ia8+936VUie\>pR^f!An:9)AP+RQt&LB@u/5""Ho>s(kjQ;uN=gVki +U6R61(*5"ni]\6hjis8R8'e"jbfQ)SrShg#/se*V0728L'_gR2laM-/ioI7+fa"l"R4:5iGAYh'G-CCk +eOoNC%mYSPq/;%GH&LXbd\#qEqP,hs&VNPFT&k?F3O8X>Q3NK0H*1=bSm+J_C\EB2c +IYg.*XJH:8?HqO0=o(ub7+g)An@F_AXEC/GFj3:Z4(TYnb)^h7E +`*,3.csp2tho=&pJUoe%bEa`6LHFkjmZ'Kk/R.dK=m;V0(S>6[EQo=f92!IELCF\%CYpdNVigQ?INKo&]X):>!!l>M5]"(S>6[]tM)gPnZFk013k]ENp,t +*Zi%8j$q"_E>OU]@.b%Wn(bJFmk-f;-lL]'aiFZY7Q+'uZ@S1$%?uffpb.tmN1!9KoilJ8lKRP)HO(KW +aaTJU5fnJ>X?amVPcR-lI[$p!aaTJU5fnJ>X?amVPcR-lI[$p!aaTJU5fnJ>X?amVPcR-lI[$p!aaTJU +5fnJ>XADj;,9nErR'KsLQhD!U:HkChIN71m";UTK:DTU65>"tLA3W[eTO^8Z"tLA3W[e +TO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z +"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tL +A3W[eTO^8Z"tLA3W[eTO^8Z"tLA3W[eTO^8Z"tTWT92( +OWI2!/sf4X#k]R;cSn(KmV.91HFP=1:pM/h.o_-&NA'PgVISKeBk_:!DN6s=I.PTe[r/$*W;?5ZcTenH[1*TUQL< +PCJ,bE[^6niM\97,g1!.o4mC1u5J<(^42Ej>(_[etRG':#o]/K%BeZnSXs*i(Q-';tWaeqq,N]kK0J+L +oDEC+knGD_$sT&5"o/S=e,:HpH!M3l36'/+qdNiGk1]mKMFcTbd@@)3e#]MV`#MA7YQ]"3dVSiV!D::P +K7=ph%KW,q.I`K\B7V*!^+n<6X38S^lilfQH]mHs:JIedQ&;-J"AhrUZ*&Y]/RR-.^,GMp8fmG#*'?G-'@f3TtE)muF$[)>&lHp5;]fs5hZI&jo!!#9drW2ZJ&B"~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 574.8 -0.12] CT +[1 0 0 1 0 0] CT +N +0 0.25 M +0 386.5 L +1191.25 386.5 L +1191.25 0.25 L +cp +clip +GS +0 0 translate +954 76 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 954 + /ImageType 1 + /DataSource Data + /ImageMatrix [954 0 0 76 0 0] + /Height 76 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W?"apb)i&EA&IIG_BTODrP_+ek:kDS3aaluK8e_`B+;.9P,)D&"TRrbg,,J*u#XRhc@[B?<+BE:i +&Ku(,HOu(-[#.6`O"@!CQ69l\f-S,P_`M[Y2m_!*rkE_6\UKd+R@TD`2&,J5g"GV(zzzzzzzzzzzzz=9 +(rLk5/Ea!!"-J(GB*";PR&:XBN$3\$pR'FO]hc++F!Y*W5'ERpg;QD!]<0/$lphA*;[WDdQFA;tsp2Hg +S5iVba?h$31'VKd]XT76XYlWf_HnEnu>mGKn1epUQ]QX>3Hrt2IKg$ +a1Rgpt/5^3/bmfqSGKJb6ST!!$,W_R+-CC=W0$\\)S$*dd.bpYPL&=/f/fR[UN,gX"KS?*f/b:`F*?,s`mHk?5_]?L)cC@Ha/sT$V:7Q!8D@ +Wj.2)QC6E+"'@4PAZN_1O%V9GB4]K.aCnH0fA=$YQrO'8LHg/tT@CG(*GH]9eZi +.U>Y?h[9X#)dqB`$MQcWUbX!!%pEb-X8l3bg<"?bSWRrquB6iJufuY-)6=q!"]KY$&*N(2[Rk)]UkY`" +mRbgc7?(a,V1Kps(ti,R7$29X\"a4$2imp$:5ambnq;Wr&)$ShA]m]tN5+l/TgFT/q,[NAo^u2-[P-rC +7IVIJ7%%lh^;?s(F%c!!)*:Ct%sgHhTF8g12iW5(#>k$+DX=r:.hL2TZuS_o'C#\9-t5)dFm(Z=V4F6D +7eB$B(2@E:'25GjQ6]::+%A8Cp +U+?.@Y[$&?XZ7BP>IFZcRB:Am2Wkn)(mP]K/&5=a>dAp1ZP1l&o>0`F@pe@Ed$lClPJsYM.`KDJjpqkY +[Z5!<<,(W77B(f<&9/e^`4-hk/]/Cr9oIR$t5gCqX\(59E(:36^#lD/De(*ZZ:CY50hXp1ZP1l&o>Pm\ +%Z#,eEU1ceFcZqu6*&%iJ7G!!$ES8cP#^,39CP[_B;_3W+:$o]bl?ikPcG:HnS"feWH3SFL]D#NLclEg +Z5qY?kN1a)D"Y!2*60C&.VulaBr?GB_U6nAjBCCXuIf*$$%_J,]A+*BOU^K"X3)3#L4ZT9_:)DJmpDWD +j4s5^`Ir\$OuK$ig9X74@3'4?5E)Gmd"agbSE9#9SnaNS1"gceeoF@aSN<]6A0qbW[U;4C.>,FcTI> +;JJbfOK"!<<+M\h[%(B%sb2[cNi_I[R8dQcgBi*OaB+&fs!`f%*lKnDp!U[F90XpJ"^`\`p#m]1^;h`2 +Q\5QHXaDFi3)Q!!!#?4tYMOgt^[i-koFH3&*6HFSYLhp@e4LJE=#OqsV:ToYhMI)Gq0IAf!!'f^H2-l;7n8>o`0JgBd*RlV\TI(6\QljqpTA@pHqdLECB4D8J;# +-CSG`<\o7;ID2rB"(p[6jQs#r#QhnFO=rr'suf3a#E)a&TI3+Z:=IU6PQ<<]D6,H1`E'r\,t<<]D6Rrd +$8If"!Rk04\p>d,;"Vr^/Cr6lE7?G)YABlg\aq;R;cgtr33:fW(HmRX?C<<]BP:fW(HmcL%hB&MQrs*] +3*Cp':o^\dF\>9"\*Y2"9>L(,M+SofGm83p5ZWk'FS!`]u6W^'PfWk/*%8ZS`MDnc%e(2[Zdgnq3UmF\ +[R.ISEc:J];dn./[2q0i]36reFVn<>DI45ZN#.8`skmT#5i9NiEFcF[o=5W[^hFJZ9&VFm&b5jcP +Z;m_jn^<>DItNhd"]n<>DItc@&k+TBZ7nmCiZ8/?K&gq9;[!`e'=<.V%V<)oTk!`e&*VWI=7,fO5t>V?'Oj,Y3$*L4OKi]WqM<>DItNhd"]n<>DIt:,+>iZY.T2hgE+UhnJ`$^?2^N:fW(HmRX?C<<]BP:fW(HmW;$-]QeGZcCOSf=oBBWZ_!b[q0i +]36reFVn<>DI45ZN"_,\:.[R+q34OcHI08?+pT"fa%Kn<>DIt/r$/qb6ft/g&.PD4-D'iDI45ZIuf\r.^\e$;:K=#PQ,lAhYI<)oTk!`]u6l7Rge9Zm +bt>hT%KW]Q@n:fW(H$<1#uWk0+21+b#pE+SWrWpWEX5ZIuf.Ss,rWpX=eb-\5]_j/IgWk'FS!`]u6W^' +PfWk+*3.FYRh$s,?:W9TD=$<1#uWWH08W9Q!iUeJr=0jNE0TrL2;.Ss,rWX;^*TW5U@OtnU>`,K;ZLT9 +c5W^'PfW[^hFJZA-I8BS<@&AsdX+0)msW<-'7W2`*_"ff.l+XtmG64?pRJ#MXmU^C($TW/&n'r^CML87 +Md$8Gf=r1U,POt':.JZ9&VDI45ZIuf\r.`rnX!USqn:-1_9Volh/J[OO$Pg]/"8b&F[o=5W[^hFJZ9&VFVdEnJ+ZJD)ar^.ZY( +5QPuDc5=#PQ,lAhYI<)oTk!`]u6l7RgeoCE*qN+9;hT%KW]Q@n:fW(H$<1#uWk0+27fC +!]oV#GC908-[0jNE0TrL2;.Ss,rWX;^*TW5U@P"%SdVb/c1.)"hIf?PL)r>'!mj2U;W^' +PfW[^hFJZA. + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.72 -0.12] CT +[1 0 0 1 0 0] CT +N +-954 0.25 M +-954 386.5 L +237.25 386.5 L +237.25 0.25 L +cp +clip +GS +0 0 translate +238 76 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 238 + /ImageType 1 + /DataSource Data + /ImageMatrix [238 0 0 76 0 0] + /Height 76 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0S0kpbe(kp@f>HkOE\0DBDl!;\Q`Zg[0&3&MOXK9Wr9cu``n\:hPk>]=>j*CG'_.FEFe.F3YK#]>XIX?JW +9!4iJ4=+'*Eh8FE-ok)F'pP5dj9eN);fW$fk:*rnP4OoC6-4CEY40.OJ'a]?4hPpZ;dY)g;DN)c12TiC +D, + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 574.8 187.2] CT +N +0 0 M +0 310 L +953 310 L +953 0 L +cp +clip +1 GC +N +0 0 953 310 re +f +GR +GS +[0.48 0 0 0.48 574.8 224.64] CT +[1 0 0 1 0 0] CT +N +0 -78 M +0 309.5 L +1191.25 309.5 L +1191.25 -78 L +cp +clip +GS +0 0 translate +953 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 953 + /ImageType 1 + /DataSource Data + /ImageMatrix [953 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlHSTE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!! +!"LhZ+\G + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.24 224.64] CT +[1 0 0 1 0 0] CT +N +-953 -78 M +-953 309.5 L +238.25 309.5 L +238.25 -78 L +cp +clip +GS +0 0 translate +239 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlV]TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzp_ +u9P$uu~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 574.8 187.2] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 387.5 L +1191.25 387.5 L +1191.25 0 L +cp +clip +GS +0 0 translate +953 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 953 + /ImageType 1 + /DataSource Data + /ImageMatrix [953 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlQ&TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!+ +#lt)s+F~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.24 187.2] CT +[1 0 0 1 0 0] CT +N +-953 0 M +-953 387.5 L +238.25 387.5 L +238.25 0 L +cp +clip +GS +0 0 translate +239 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;!=]#/!5bE.WG`:P'EA+5zzzzzzzzzzzz!!'fe!EZk263~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 574.8 187.2] CT +N +0 0 M +0 310 L +953 310 L +953 0 L +cp +clip +0.941 GC +N +0 0 954 311 re +f +GR +GS +[0.6 0 0 0.6 574.8 187.2] CT +N +0 0 M +0 310 L +953 310 L +953 0 L +cp +clip +1 GC +N +0 0 954 311 re +f +GR +GS +[0.6 0 0 0.6 574.8 187.2] CT +N +0 0 M +0 310 L +953 310 L +953 0 L +cp +clip +1 GC +N +0 0 954 311 re +f +GR +GS +[0.48 0 0 0.48 574.8 224.16] CT +[1 0 0 1 0 0] CT +N +0 -77 M +0 310.5 L +1191.25 310.5 L +1191.25 -77 L +cp +clip +GS +0 0 translate +954 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 954 + /ImageType 1 + /DataSource Data + /ImageMatrix [954 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0Ws(bSHg;iaHJ++P)IM*j@&0`Z.,e0*;5qF;cWK[iI'Tkq*Td]k.A1>-Hm"0%Ccf +)H$W>g]f3*qC'0(,`!b"<$Gb.CX[=pC+,l1Si% +WnI)%0Xo*?b/T4:k")L;<<]BP:fW(Hm:uF7EhcL\6dj!'1,$c5NJEdY%opl:<.V%V<)oTk!`e"'p8FMV +8D8;l:)K@FiaHEY6WJ1Qn<>DI45h2EJ1+FY8OuAks'*dUeH!&bp:t6OW'r\,t<<]BP:j!#fB$4)O +g#):\[+2OR0JI^m_ofuSWocjP5ZIuf.Ss,rWi^A#< +?p,dkH!3s(IYA(jXDE9_C6#(s<)oTk!`]u6g/nlhn<>DID?-gj) +H:ZcC[*0gOg>cUQj$Qgf,ZUM;!`]u6W^,+d=M-3$6dj8Wjb/2q/KRqN5#aC0;?a'M:fW(H$<6t`d7)SJ +,jfh[:)K@Bk$_i]6WJ1Qn<>DI45h05oc&*R*Fu(]t>_IA-*`b0_<&I0D"fa%K0#/s4P=LR!i0n+6$=9h"+_aa$ +El,WIR"prd%opl:<.V%V<)oTk6IDh@/iDu=Bs6!+_F9h2n?B4gTrL,9.Ss,rWX;^*.!/J61T2@X)GJ=Z +,3'+R<]3-@2+LOJW[^hFJZ9'rXX\\&JVU$*drN=@]UahCN=T%6O]6n3$<1#uW^9"P.USP.+fW_nS1lY3 +`l#s=LT9K-W^'PfW[^hFTnl^foDYcU1URBfY-X97Gi. +A`H-XF!ObYWWH08W2`[qWi8*Tl63.s[%))0\X,F[Cj`0rNhd"]n<>>j?26BpoGXuR6/LLtN/ljko +<&I0D"fa%K,bI:()BqPj=Z%cYKmqX:U)Y`KU8g5:.Ss,rWX;^*-q)U-T+Pp)l.99l +*QHgtl05=PXTo@Xm,,R-@Uc(j4eFla(@HJ9gprns_Muh>A`:"$i(ISpia>=uDt*4PiOe/LSLXR,^D5Z9VE2,ZUM;!`]u6WXsI+/7)uD +B%"rKbqRUQ^OEk>9=\:;[p6OgmEI^g:/29.%CNq[g1`PrkeJ26_3,D1RNnd5<>DI45ZNN*X(HI=i&6dJ +o?B4&VS.'6:Ap0!p[11ck00<=5'c_[kih6XUIY2f)&jN1p_7h0Hs0:&GS0)*&!lQYeJq-qW2`*_"fa%? +>;a#?5MJjm4$+Bng+Y>7h`*STpsDSR]m8e'Oc_=g?bCVn<>D70g2MpBD33M['UPH +meQJ;>>35b^1=<9f9ElelU/ih;:QXX'r\,t<<]BP6s)>4Qddoe3^#W3s4$JV`l>m46Mmt)Ggi?RLW7BT +Rsd0-BfRWh^`;=LiW*!'O]6n3$<1#uW^;A=Zb@Q$_kbK!b;48jPtL^nqJP)u;7fWsQMTM9X'/_3S"#o% +G!V'@/N:;s>a"ZbS7LM>*>'>D%T([q8JS2(5ZIuf.`>B<_=O2a-QO3J4`TILmF,ZKOK6+`@WHLpG"\49 +qtg/dVP]g\b@sbq=[/ONNn=gW+/iq7U^C($TW/&n'r%_n?rTl^Is7(%5Q?+HC[8V$^>$DPIr0G%T:_aC +R&I]!H$k!eE,`&XZAhA8*ec=qA7VLT)ul`t/LQ5hR%l]Egn:Z-Ot':.JZ9&VpRaQndc-nD?s6=b+.2(K=?N:W3Kbjgh.r7SG5XDrNr(_3&#\]g%\Co*(^mDW^'PfW[d);Wn1*.nWD>?[%,).XsG,i +2Ek(t7oBMj.Ss,rWX=$Rl?Ea/OOVI\m.`k@(Hed8I&Le?U^C($TW/&n'lmI-pQ,)4,kWoE-W(>s*`b0_ +<&I0D"fa%K[2(^2<%*Tbn2Ek(t +7oBMj.Ss,rWX=$ORZk4on;u/U[%-2hq-*9lZLOWg3Kbjg[4"Vg?*gf*lCj`0r +Nhd"]n<>E/Jc.f1epdj[r>#&=00"V8t.iTS1RNnd5<>DI45ZJ!V0c)ZTZ"mn`eUSV +8?+pT"fa%KUcFg.PPbm6*`b0_<&I0D +"fa%Kf8[A)SSFDPlM@:Wr-6A`H-XF!ObY +WWH08W2edYC9F.ECreul4AF!jDLRi.@:eIdDI45ZItXn<>?KRc/GLa)tK(IQd^!7I37s_ZLOWg3Kbjg +PSCj`0rNhd"]n<>@9i)X42jpt/GRR$JD0YL;sFZLOWg3Kbjg +o'W>c,U^C($TW/&n'f)^+or+C:P:eLA%h/umbfmH. +>DI45ZNNF +X1ESJ:e-YT>V8fI:A4GV+$P(^LsrYjT1a[F2+LOJW[^hFJZ9&+<6]U'8`-tPS^,.7Ed=c^ +f@+cn<>C0e$TT[aDI0fRgBK,6?cBu\&N>-)Eu8SQ6:Yd3emW]?4l:fW(H$<1$0XBO,3?u4k37kNN'S>>[B%1as.+0(bS +W<-'7W2`*_"gb4fb.mMOUD@iQ:?2M`LR!i01?1q2rnC;/ag!9#kpe!.iTS1RNnd5<>DI45ZIu?WeJ1n`\nl]e3ZsSZCc5`=P*Vt\%3B]B6;Pd]C1-b>ZLOWg3Kbjgn<>DI4ct-30K$g*D*L1@7(T*8X4KN@HW+q?g$<1#u +WWH08\uEiNgldF5Q"g%QG-;_QYd3emW]?4l:fW(H$<1$h\149E:-@mA9ZsUo)ON[hCj`0rNhd"]n +<>@Qqb,F)"%m)8inobcNZ\Og*%opl:<.V%V<)oTk6>';D(Qp\$fMS'&)sDS9ortalTrL,9.Ss,rWX;^* +kb'EWMeeB/*L1?LJY%?HiaHEY6WJ1Qn<>DI4cu)iQjgKHe;43^hlYU&'Yd3emW]?4l:fW(H$<1#= +X"'lb-Tpao9Zp0'0JIiUg%\Co*(^mDW^'PfW[d@?Q$4D9OG=jABt9LH#A+Js)]u)K,H1`E'r\,te +UB_i`<2Sae[/Iee_ofuSWocjP5ZIuf.Ss-=5ugD9hK[[MR?fq]p5=qnXDE9_C6#(s<)oTk!`]uD=(PA! +R>2fJBt6%e%Eoko2Ek(t7oBMj.Ss,rWX>q0jjM9e4RB"?b8kA.@.ph1nY)sCOt':.JZ9&V@$TobKJ'r6UNXc1G/#N0JKa.Fn>4rc0if_ +glj&eo3G\<0>NG#*(^mDW^'PfW[dr(n1^WCX\O)i7un_YhnMD"[k4K=B@!0'4.JBVmUi#/A$c%g/bc9m +7@8S][>X(^*%,oN,ZUM;!`]u6C@ALT?4Hsob,3nM;9S(ljQ'j$J,eKFLKdQtiCi,>/]QgfR$N*r3IiFc@%59K6JePaVOt':.JZ9&V[D]!p +e'W6[04):^?B*43Vra*i:QCHq*^+hVn!1/_Z,_nI\+l;XQ>J<0lZ$Am7oBMj.Ss,rWb-Jl&u@4(e/bIK +Ocbd>HhUjZG]@nMG'\HqHoaAPr:K7&B(t2rP>\B%)/Zlf.Q)IYd]:tRhkd$#nB`iS9o'6M'r\,t<<]Ck +i4HbX[N2b:^6Zt([5sjRbMc))*^$BL]!SgYmXP66q%hG"ZS/"^4M"M'bgR]c8?+pT"fa%KeGqg:en_dj +:/=[;5IJ,.Yu1PO*N/!X*BJ:UI!iT[=.,A6n]E-q:@.+6W^'PfW[^iQ=,b;$BXjfi0/(Z$WDkA??YqqX +LTf"Jc#('87[TJh-750u"4OP=/g-q*M;9.C'r\,t<<]DrUrfRZ-"P8CZ(nb!QE.4$R%0\Jh%F>g^:q=\ +WMsM0+OdV_j1"Sf@."IOG&np/TW/&n'r]iM7dme$gQO(R9[!&!o+McN[FZmhO,je]%c4a;-[GB&-DR?2 +,9nH!lg)gp6)J4IQRghO3+?:`)CK]L,ZUM;!`]u6g:cXP\tl>dPA.T(3p52Lg$5JD0KsXE<*s!>ojuJ< +^]4:s-VmqDK1sGq03=I+1Ki`PA<*Gm<)oTk!`ck^ne[OhC/1,&>V7Z^/j=daI)gRh/M$b)Z4+sS3!G]p +W<-'7W2`,5DX@#-r)>Q:+XtmG_J^sJ^hh'/nIc_&.Ss,rWX;_m=<#kKV5cNd8XYkZ?<^Pb_JI0.nIc_& +.Ss,rWX;_m9RBQb;:V1lUeJr=gK0]!0O+$c]L7LM$<1#uWWH0^d.U7mWG9^G;Pn#Z"tRm4gbTJ]94USn +5ZIuf.T!NYS\1naWocDhAQi,J]Wqsi^%$'S7#%KE<)oTk!`e"'iU7jceXVDj9Zmc;=-eaupV=]:&fHi^ +:fW(H$<5hakJ+#m$2'lDL"3J,"#63 +W[^hFJZ:fqlLg??2XA4W^c<<]BP:plR+J(9@Tjuao#>#s:$AJpjskM^ZB$qV24 +W^'PfW]&DFDrjpJS;3n6b6i*g/FQcj+V2UdA4W^c<<]BP:pmRb^,o9DS;3n6b6mX>/D$-PKSSS61*fkm +WWH08W7jM_5P_.U3=e]LQ5)G_=U+#sGrT+S6W+)f.Ss,rWnU[h8U&30*fcT.OApe2$<1"R +>h)f&>[+_

UWUQ"6Bj&9$AJpk`*&!EtW<-'7W2`[e +W[V8@[6Ra@U.\ZBQPUDQQBmh[]I216Jn/F#5;7-c1.P4GW[^hFTdWpirU*gQW[Ugkh:U@>IJS%m?f19Q +%NIG>`o\'2B3J>4*A#0Yf+LhEk")L;<<]BP'V]@-:G`f\C:Y*N:RsO(1c0=Op?euaXd,0:;Q2K,0X/4& +h!G#6b6l+=V81]IitQ$aU^C($TW1?Ve/;LY:qOYBFgE' +%a:Gpb`PkUWX;^*Lqd[?QXGffC?j$,Y-5%d\ohe"AYjQCV,@Wkn:_5dRf*tuAHhnG;TOEW;ij#&.Ss,r +WX=kic0-*XjAiOJ6IMo9YD.>thS"9\m_H32ON,+7]QYsd'P*6"$<1#uW^=p0Z[5Frk,K:r-bY$0rnu_m +`5G'%qs<02WDpHf%ZQI-WW*WU9;S8)m99c)@/N<7MHL +Uc32HBR!g@1M=uiq^Y@`$J?CMYn,'KK$9?VJZ9&Vq,,sjaLSRVe$CS#tVk6GHo5M?1L85/UY(k[<<]BPjL)#J;)ua:!^kVI]oA/,Pbt +r:F_om],Ck8JS2(5ZNM#g"c.gcFo2.AH4="KADI4OFsPnI$,i^D#*$V?`<>DI4i4B2STkGE!COApe2$<1$p +=>f9:EoOE?c?Vt'[&i1oA_WKBJN6M+F+U1p<)oTkK"J%.^;alH];k\i+XtmGJqoWY5Y_OGS5mf9Ot':. +JZ9'kWoiA(d>iTSNIlq]9ZmbaGZ`XZ\UV"h0!"h< +;?a'M:fY>)eVH]V`)"p>GY#uaXPdMu]A\*Ec>-G#8?+pT"fa&^W#F9dS2dJd(Cs**Q5)N`CmQ+PbW2ik +'hK.=W^'Pf(KL'Mjo6@3W#?H_8XYkZ4KNAn]Ulb+?.C6a.Ss,rWX@E\Fo;/2fV+c4>p?6L<>DI4OE&ef8*)#fW\O(+P\F::_oh\kjhRD`WrA)ZJZ9&V1$kSL,qTb6n[c91qp4gVC1t&BVcJCg`:!W[B&lTW/&n'sah(loU+c +p/Q2$-f*G-3.:XF?XM^Y"WHAm/5kk@!iP$pZpTh&rZu<>DI4O?qFL8*,DP +-o\nMX(a$F(U&4pL0%W]ECC"fa%K +FjRp/QZ%PHM*mhn!rGAS%]]9;PMm;so>I(Z)grjQ,CKcqsVo^pL0SiPFM]@#hI+ejuf+<<]BP +Ufu=7Q2Jn3YF=cduPh,:dh[;R&L@p_+&/ST?T/XOXrFg_agp[6iV)B(YW +jN,?\7`4J7AnL2AK*Se[Q^3rS`-_L]q%hG"ZS/"fQXbGWRHY+*U90k)Ot':.JZ9&k<%ZP)b#Rf/jr,S; +St<*7C:AXA^3T<^It.L'A&k$tapf@PFD-&L]\-ASQ5*jm=L&m5A$274Ot':.JZ9(JWh-@ZRMtm"Et+QeOU +p/Q2$-bX]j=r;&@0-]"EolV(7PEWCY_X-+1f,#e$Aq,MK=X!cs-&WkJ`EtKF,ZUM;!`]tgDD5Bu\Uc.=O"/r$RTcd,B\9[mCL4?bqm3&l0GcCOIf7un^NNfK-YN3'4d2uCX`Q5'aHYbXH: +(SYZU8?+pT"fa&NWYThY:=rVOg.ZhQ`f5^SX#P1_9_.N8S7cJIX*5&+nYoHbC@@;`rql.KH1IcM5L-X; +:m?SYh1<*bGZ`X`V"cc5JZ>T$U^C($TW3Tle`'jfmS3&Kjuao#>#t-sZKXqr?nPL1<01(1WX;^* +0WrQsr!N>$3=e]LQ5)N`CoUANQjZtqO/*+^#t-sZ<>TabXJ]b5Z.cc.Ss-= +VlG]u9sCgio8o2N=i&"uAk[/pHE*DYW<-'7W2e3Ce\]'/8Mf_6juao#>#t-sZ5F<n<>E*Hb,F).ql2Xe8XYkZ4KPUlV?8VSc&ktVWX;^*kb]i) +nkRh=c$;k&[&i1oA\G;eR&Oh\U^C($TW1?!:sjWYQ/ +@:(nT+XtmGJqoWa`SuYs<&I0D"fa%K&FN66!I?T%!i.FYSSLR!"[kCK&$k")L;<<]BPZs/em\J+>1 +juao#>#t-sZ6&Zs0N6LA8JS2(5ZNNHW\Q&JLjdgDLSRVe$6E7!W?*[*:K;H]n<>F5fb.EaWLjdgD +LSRVe$6E7!W?.Y.TW1#8.Ss,rWX:ghja(p;@p_+V+XtmGJqoWaX4r2nWocjP5ZIuf.dTus;7S4DX6)Mi +AQi.(Yd1ES2)%tZ*(^mDW^'R<<`L$&]>_L!c9kL,AH`SplWrZU1KD\^<.V%V<)mU8Q$4D9!W/lW%nW1T +'g/R6%Wlq^VQ)Y>.Ss,rWX=`dEY48eYcjMeOHaH^_8FFX6RP+bUT->;.Ss,rWX9381+N$WIFXjCUeJr= +*`b0Xg)cW\C6#(s<)oTk_Y1!gaqiW2c$;k&[&i1oAb2_2JXNP#8?+pT"fa%gYKE],/[2FIo8o2N=i&"u +k"H_,Br[UHW<-'7W2f>jGZ`Ys +fkFikW]?4l:fW(H$GTPO<&V_0C/1+;I=6NmIQm\uNq['f8HC2`DQP]XYI2 +4?OSfjqd](>#sQhkt.(iq>Z2)Zei"?YbN[E?b=VrX.,[!XDB%)@anD:[FUh9sLHC:fW(H`jiC]%/[%b7FH4er;?B^g8AA"%eDL*Elp@D$oIk#P>[5J +X,iTCf4Vc+nU46!W]Bi^<0gqm0W.P^`oHabokW0^%1WmIo:Q&!EDNHC"V):ec(=mlK+C0@i5>8@;:u3W +J2po`\[f99DttD7FQgi1-S=bsHRGXfkqY:V2E$lo-:HkT'2"[B'B+QS559s1j6'^[mde'J#QoUfD+Qf0 +M9G#O>+PLCKhjWd2o#Clq]eUkm+]"Q`PB]=b6hOV-5tq/ANonIK+I2OcRPDn!&3#7dcp#\qBphQ2E'F) +cg'e1?XM^$F6B"j,>?8f*%[DmBq,tlC-G0\e\$9+:S0hQ^Y@]) +i.iD'f^'\g>#sQh;4spLOt':.JZ9&V<_lDP@f;uBj$">7=hK$8?+pT +"fa%Kn<>DI45ZMm:UmmjC$O[?CX/e>=PA+0NK11?kk")L;<<]BP +:fW(Hjuao#>#t-sZLOWg3Kbjgn<>DHIc9kL,AH`Sp)]u)K +,H1`E'r\,t<<]Cko8o2N=i&"u`eUSV8?+pT"fa%K>H%oj@CC6#(s<)oTk!`]u6 +Cn +<>DI45ZMkHP'Yc8peXtAWocjP5ZIuf.Ss,rWocDhornnqTrJ/oc&ktVWX;^*TW/&nc$;kF@2n2n<<]Ck +,H1`E'r\,t<<]Cko8qJ>2T1BN$A:1pjp.4>\lO1Gb6fL;[ +J0A)m4GdsZXmeU@,+JHOlHaa0`QD0'bS,i/F"P:'.HptIiLi^A9GJ(@g9M1L5/h6.3Io!7,Nh#$s6YD7 +mgX$-4GhuCq[hdjDhq0)5`^"nZqpu.!2Dd'lG5C^E@h>c;ThbAT4%S'.d:JXb^LBj(T*]s]5bJ_=eb1d +K.7un^FSfoc9ZADMX:5+HE[r:.6qs)>H8uYbsh`\m;OfZ#Ba$0Ltj3Va_DD(j4EH*^\dgQ>"ef,?bE*a +!/5LZopB@!0'R-2bc]3-N:R.YXagVUfo[H`Yd07Nc^Vf5NS*$"$pTtL^LDn[iOk6d?dqWb,_2_E(Q>e( +IcIei)liOaT,XPS9hX`rWI-fo2iqsCkR+$X*/1b<=mUrSN<%b"LN9du_dedI2G!ECR@B\8iP0X9qXrPo@3u;i>e>/2[;C`Jg=f +^i5X,/=e*mU>AJ=CqlaLcf$>5sa/Rd9>D1AQguH3B9 +)V1mIZ67-2CG(Z?))msk@!lOA^(#9S?_LHk\[bZ\.f8A:(EO,#N+mF*`&p`f(9p%*^f2rAurrel!ggo8 +&.n(t`jcC@IC)kn%$ODlC9?B*3'b9C#k?4Cc?h7K$HL?)YXmD=L<0\6aD%fZ'ls3,e1@PBYsouB;rEFW +L2C"^YNP:&jj7un]Sg@rbBfGHgTmEs_E>W@Ei/hVn7opp<`#t*OZF6E>C"[fW&ZTK9`&Veh!HhRC9msk +A`J,XNTeudoiiBRAdt>?Wpd\!/jEXI1AX +Vo>7l_]:p0LBP6VU>r9:,Odep:)/ZUTX/)7a\DeNhJaH7\d)]Mpu9+8[D]",BIrV,3)V#0lQ@pn`DM\e +&'HL%\/55D#N.FYSfE#*lki'HbZV5:#-C-@(GI"n,TGIMJG$4Ym>n%IG"4?l'FmDYLW>'R\"@c"V7,O5 +eq_1s+](qqq_PlB(Q?AYAnZu#0mc'r/>3-OX*UU%G\e^_CJ^@2[tlOCt@^]! +P<^\cjU_F\nDQ&e0/GgDsEi=A0Nn3bcr(7R[H%ZQO1B>#$?G5M"#l?*>nI!U$(+8B=$HV(keG9m$ZUWQ6!\@Ea@$u*'\ +nbiRLts5^F%9e[`6$D&:+rdk\,Gl:(CAq6.R4])K]tqt@:uReZjMJ'Y[T7.k!E[T>cmktce$3Uc1qk_T +s#C7fMM3Eo2#L;H=$&@;?[Ep1L5o('4^n?&dnKNi2p,!ZqoPtJU4eZ2dKqtn8QG`&I:e[]Rae0eHT9K3UAs%N[aBmDA>Bhk`Y`_SJDq3&iu]S%ST3=*X)%nr(>Ya6T%nN/s$TIC8p__L9:uO]1GX*&dJ@tp]qdB]-^T&Qjo<3ZBMb>rG$AX-(Fj&>0-9H\SpqX[1GY""gAmDunZ1c +Jpa,V2#r*tOsfuf9n2]D[GS(HfX%CRL$Ohd*PUsl9" +II'-?b;B+$NBi]cM\V<$t[pq?[-%zzz!$GkQ&*UGn)?~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.72 224.16] CT +[1 0 0 1 0 0] CT +N +-954 -77 M +-954 310.5 L +237.25 310.5 L +237.25 -77 L +cp +clip +GS +0 0 translate +238 311 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 238 + /ImageType 1 + /DataSource Data + /ImageMatrix [238 0 0 311 0 0] + /Height 311 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WH\bY_"30jQKSW#a&H*aa,.M%t6';d^_P2O3ZD*#.72?J$QAX&;DC,Q"78u\&Jr!!P,UFB.&V>gi +/@!PrP*3cqPGKdff[XP[kjU[(AoFh6cZ=9Lmg#!21M:=W3W#@khD8Zda9\6-^V@PO+gdr'NfL]+E7*q; +mB(^@D2=AH.+s07o-i:CP4L*TJWfUQfn#=X.+s07o-i:CP4L*TJWfUQfn#=X.+s07o-i:CP4L*TJWfUQ +fn#=X.+s07o-i:CP4L*TJWfUQfn#=X.+s07o-i:CP4L*TJWfUQfn#=X.+s07o-i:CP4L*TJWfUQfn#=X +.+s07o-i:CP4L*TJWfUQfn#=X.+s07o-i:CP4L*TJWfUQfjRK!p\4FV@bSE)]':>'lfmj*m[/t%@=g6, +1++#;EZQMg:i[U'4&Br=,s+qD*[NnTHHH1S)jsG-D,>9/6U5]hEns)JF&6o56U5]hEns)JF&6o56U5]h +Ens)JF&6o56U5]hEns*%3HF1OIt.Kc(C^ia*^4uHpYUImoG3H/5fNGlK"L@4a1//tHnEp3WtetPL;nak'k +?Z,\uLEAADQX4uPbl'!$EKbkEg=V:4[VTZf.pKK#eK*.UghDSn%CBenA:JC>I9cc4aXUaeO,*%BkMkapYUIe +=4_Ir>8kNWb:gWsg9mum;-20$)=Q73OrH?f;9?tR@9V,ZXLh_"j3&sV]V9>rY@%>"`ucfNc/B"-P@8HF +[H,T%2]!3C:L3qaX05j6?pRYk-HYhQmG#*WEQ&YrLDSPlf#\8!Nud'1SiqEqSN?GsB:iosZXIL-]&K*4GJ'+3>bHjAZ'*LU +J\K]?4i+o[D=L.WE8\O!5CL2-[Dl7\>'Q,+qlHLGRG`:bT<;%*[g=f2U#bIorKa*fYb(:U +7<`sb-IQr3`"A#/8hQQNG`UmN>AhgqTpXK5F0\gSjTIYOaeJ*EG'4Wml`>0?0bc";3l.?YDGkN26a/)l +[$m*>baDCXH+@&UC?8]9JTh%bdn`I\/:$^@ +]u\=c;>bqdRmjM\h^-J@Z>H>$c=*D7>c`24#WTd@madC%Wo? +5?_:2.s&ak*Fo)hHK2\A6cjH]TmiFOQ0#fs^nc<,g:k;8lFSG,F&AoaV7N>k:eeK!*US)"\aCVGUbm"* +Fn,%G0/r2$.Cnji>]_fLCYb^2c%6*,-N0:-kDtel*l>m0ldi2-2fB`W*,OVO;KCS\5]]bBh*XGK:ibAZ +5]]aWg;\5,B_@Nm&c]%0#>3::A-Y`PULXU%cD+nT9$X2B#>3::A-Y`PULXU%cD+nT54(:i:]%g*I+n#t +eB=@u5fn;9lp/[o`FV#WA@@(F0Yfmk>AMe +-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2 +""!,-Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2""!,- +Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9;6g9Mk>AMe-0J.2""!,-Z3)E9 +;6g9Mk>AMeTD"bpVu9p%54*Oul=E^KT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%X +T]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4- +G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY +8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G2UhuoD#]M$c&\=c9T(eaTe!"8')7*S4C[2 +Pak7c%[kB)eUDb!b(n])`@2JlX-E+)7,L]1VpIf7X?:u-b=D;TjCrCo6,FPsrclGgZ!u7`hoUAk#TYLd-2rIu8/\GuR*)bRj9c9'X\?!DciDr3bGE8nfm)FiDW%NRU+6`Ro%)t +bKOieoJ@25Jgrl`\&U=gDT8p?h^N>-c^tqKCJn?0.Ym?hpEp!cntN3Z1Bi9T(ciJ/L-[s[u0LH=e'ihiKEjPl"l4]hA\]Qg^,;! +Jm'gXTOem!'@ie]$XT0Bqt`JRBZ2Ussh(=l +aalfV#YBf3a"jo'_HRac[sr55T0PK5,7u9.o'=d>TeB4T+<-?ZpU9P%QNoXS*7Y:rqY_6Co,%MK1s/_!!'f6r + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 574.8 187.2] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 387.5 L +1191.25 387.5 L +1191.25 0 L +cp +clip +GS +0 0 translate +954 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 954 + /ImageType 1 + /DataSource Data + /ImageMatrix [954 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WIq4lB+33n+PFYD09JW.acH/[)O\YU?'$8G#reP**Rm^k/6rD&OM'SLq]6[Yzzzzzzzzzzzz!!!"Ld#F +!RmBZcm!'n&oWiGX\nerDbZL-#edUG;feC71`J3.r@Yl4%I^$Fnca*-k(AI#krqmO,if@SX2hEI,*g(A +MshHa5cQoAp)OsR#%TD!9%!!!!iR[Z6Sg;LL=h7@DaCtuPJG.<&%r\P-6SX-n"]8MQ(CtARMU\4BUQ^3 +rk!@oWL/mPn_?XLQil-_6]hZ3]W!.6jt7n4@J>\N/,:J]3=CE/tl*Q5p"TSNfZ5blF,\)@`*'\o-m[A6k_V;PO;-3p[4$$Bts<2jic&b[C*Cg,pd_"qk9seaEFe_fbW>_`d% +AhihC3-Veu=gJ,_\%0C%cbL`bDdD/H\u*]eCVMGaES!;ImXfW`33`X[C!<23d&Rl@Cp5pDm]P62fEMcG5KqAC'1\e82Sdbl^$qpMM_ +c<\ZDmVNf[#6h*R7SX&m>;qsE.I2W9m-VR\nk!!'_j,9nH)k09A[It"btf54['G]C*Oh,RER<*37iZ08[1^"o +P[)l2<7BNcGu*mW*XO+uL^B4klTP-t6d4(cKQAXdpK>?fo:auq>g3Bo^CQ^=&nY\dnGFT1ciNXjgS\@? +e3AK(UL!.\!P>AIa3X]r9ic*k'hN48esjiq@JAPN3ch?d:e=m:*fVIc681DY!j2Ak3BV+[/8NN\eTT/U +W@PiK]&:WkZHc^d.AiPL<:V@C5I_p4f7Vt2:JX3`.=,oK?P!!!"L%,Kaq2EFl?=1ark2fI!fc[Yc]A[I +)ZI^@_5AW,:pR[/#BIYDVl/hS4Br!@N#[Va3-@_&DuZ-OpigtE8Xn)H3R%f>teZTW0LNj%FB!!!#)=g` +"pS2kYFLPK_;[SaT0U*>'T_hSc#S8?5Te$Al-PA'lS\80:U,<,l4#Mg2Mj>c(^drIL?[Z*1$gWrNY/1E +/&c$b&\iO3FOVo4buhI1gY:K:g);PTLo+ +amb/19jLi_1H[r:0lDAJ8U32Xs@jB&C,Q%l!&,i@:#.ab9R;E$\$suO=EensUSFTt]tLGG.W>6cZ(p +WlnS-Y\fAbfLr;:q(>%_L2eZ2d'X]oH4g7tBP^3K1bNZ%4oUU;*o5!R)cjeknH\i%el0ZOtW];6`q=72 +,PIeDukqs/(-M@%S"4Zh\Q!!!!iZEC>_pTD2GT;Z?(o9Lt4U,Vi"N4O)ncM6\^N:6>Dq:8hFG37)@>@1 +a0Z0n?aVoN'YG&2D`?YYTsb@@0J7@R7+'gIMdNd]m'KcGT5qlm\KB(CZ0!!!!aA8#YTCL"mB,\Jne0OW +r/_hSaYk2_BrFE_q)Gr5DpK7edoLT7ASRf:>Y!rr<$hi>Fb^ARp,?+Y:)p5&bL!!!%6DJj?/D0=a5rV5 +pCffgDV!!",ad3@lNf+%i^!!!iTs3?k;[Pn$OCnCt5zzzi1lnE!!%P!K@u]ZBBAk7)c[WOl5`G2W2`*_ +"fa%Ke[lBUL2[XE6WF;iH:Lj:W[^hFJZ9&Voe(on+e1pj:fW(HhN:cgWX;^*TW/&n]aGut#j7:3<)oTk +G9g1]WWH08W2`*_mm9P`_DrkP<>DIt*D%!EW^'PfW[^iq]+C-[E"9uW<<]CK7oBMj.Ss,rWX;_u/qg#D +S3ujD#kpfR8"a&H:Lj:W[^hFJZ9&Voe(on9Zmbd4L<-sK^d24%op:fW<-'7W2`*_"fckb6;j[lKb0j/jIrSt,ZUM;!`]u6W^'SQ +6:f8+X:W6)Rlj[CNhd"]n<>DIt>mO#iQ5)`2As8"MhN:cgWX;^*TW/&n]aGu4e$;:GcYW/KeN8pD +:fW(H$<1#uWcoU3AQi+?iL*C]Wd705"fa%K:iLBiJtFA\V,EsuYSj&*(d9eI^Qnq#oo>gT<)oTk!`]u6l9VkZb+>KlF?9As.krg*JUuJZ?7ZF_oo>gT<)oTk +!`]u6l9QmWacL<@Wi(!_mQH!MY$K6'&qAeGVFf*hJU(jPYp`H!4-D'ik%\:E9,;!PccUnso!FEI0$5Q<8f096Xi'-1>^ +@.M8[%*UNCMa`2shN:cgWX;^*TW/&n^L!5,b("(g[-deeo?fL(n(ta)l>OD^[[NY9q.oM0;4sq5,ZUM; +!`]u6W^,+agPhs."-moh)Z~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.72 187.2] CT +[1 0 0 1 0 0] CT +N +-954 0 M +-954 387.5 L +237.25 387.5 L +237.25 0 L +cp +clip +GS +0 0 translate +238 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 238 + /ImageType 1 + /DataSource Data + /ImageMatrix [238 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0R!K"\o'L]d;3=g6G>EsAHH5aZ*0.!1J$OFo`KY61Bzzzzz!!!#7Vu)#UG4kb&F_nd"eXk#%XP1#V +JlR.U?a+:kjpP5WfmLW94e"*,[:f(7X7gAD@+Cjn(Tc=PNoTCp\030m?E_u2O)RHYmOWOr'A<;t>O'HV.U@E_jdb9("-R6-2890.=B/j96k(Pe<[MSrsnSgf*cqP']f-:i]@R$VKh9 +H/W#Ma5R#I^X)*)cZ$W\=7YD1gh34Ah*"NTmN1OQ\NeunMt?D3kMO71P']f-:i]@R$VKh9GsTP8,qg[W +VrWP1-3"!sjhMr~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 574.8 375.6] CT +N +0 0 M +0 309 L +953 309 L +953 0 L +cp +clip +1 GC +N +0 0 953 309 re +f +GR +GS +[0.48 0 0 0.48 574.8 413.04] CT +[1 0 0 1 0 0] CT +N +0 -78 M +0 308.25 L +1191.25 308.25 L +1191.25 -78 L +cp +clip +GS +0 0 translate +953 309 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 953 + /ImageType 1 + /DataSource Data + /ImageMatrix [953 0 0 309 0 0] + /Height 309 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;JH,ZM!5fq/l + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.24 413.04] CT +[1 0 0 1 0 0] CT +N +-953 -78 M +-953 308.25 L +238.25 308.25 L +238.25 -78 L +cp +clip +GS +0 0 translate +239 309 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 309 0 0] + /Height 309 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;!=]#/!5bE.WG`8*TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!!! +#WfDo,d64s~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 574.8 375.6] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 386.25 L +1191.25 386.25 L +1191.25 0 L +cp +clip +GS +0 0 translate +953 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 953 + /ImageType 1 + /DataSource Data + /ImageMatrix [953 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;0`_7S!5bE.WFlQ&TE"rlzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!+ +#lt)s+F~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.24 375.6] CT +[1 0 0 1 0 0] CT +N +-953 0 M +-953 386.25 L +238.25 386.25 L +238.25 0 L +cp +clip +GS +0 0 translate +239 78 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 239 + /ImageType 1 + /DataSource Data + /ImageMatrix [239 0 0 78 0 0] + /Height 78 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0;!=]#/!5bE.WG`:P'EA+5zzzzzzzzzzzz!!'fe!EZk263~> + +%AXGEndBitmap +GR +GR +GS +[0.6 0 0 0.6 574.8 375.6] CT +N +0 0 M +0 309 L +953 309 L +953 0 L +cp +clip +0.941 GC +N +0 0 954 310 re +f +GR +GS +[0.6 0 0 0.6 574.8 375.6] CT +N +0 0 M +0 309 L +953 309 L +953 0 L +cp +clip +1 GC +N +0 0 954 310 re +f +GR +GS +[0.6 0 0 0.6 574.8 375.6] CT +N +0 0 M +0 309 L +953 309 L +953 0 L +cp +clip +1 GC +N +0 0 954 310 re +f +GR +GS +[0.48 0 0 0.48 574.8 412.56] CT +[1 0 0 1 0 0] CT +N +0 -77 M +0 309.25 L +1191.25 309.25 L +1191.25 -77 L +cp +clip +GS +0 0 translate +954 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 954 + /ImageType 1 + /DataSource Data + /ImageMatrix [954 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0W@;+3SrV,j^j$3QVHglNr4p`Kj&gE9n&1hB'MRK<'/[?KbJjb3Cip_eh[;:Y8:c?b;Hs4Y>< +rUncB5D9/A24R5E<[Cj*dbVN2rp.=JHmk4l2:AX559Ag"D= +C*_DOdC`TfJlN+XNT=7iGuc0#QAanM6Ek2aP7jrhY\GH81agGA1D\dq)_jQ`scU?$!&?S23MaCjB7K'=tZ7&$c/k%9M2N=_5`W& +b9YDK,hI(a]NGM@%;/Rj<=)]cjB,]WtMKPagFh5X@@4^(X8G'@+B3C-!6F?eOZiSP%QY8CUn-8/-N#.1F^/?"KP"4 +/X&;tO^-'uo4B.J24R5E<[Cj*P&$KH=cKd;C\d\kgN.FWCVPjERGc=VY\H.]'!+^0C*_Cdag2nB?d#d^ +ZX$jBj5]08^R8l*i6=TmC,hCiX@f^3,hFfnZPj-Vb;1q14ZtrB$+lKRiQX]nC,hCiX@f^3,hFfnZPj-S +bI(MjA7T6*[V^BcmWC*_`LDQheSli]=I!:E8?QNeB/L=1QKRGoGOF73k007c&$G#oGd@s_)U>,]WtMKP +agFh5X@=Bu\%K5\puL[UFtHe7RtY8#E$cl5Q!i-5Y%P"M:p$XnEeQS"3pZdSW=u*/tcD1TD'!*"ncRk>CX-%H!gbSDc7WRH.M1jB=1)2[dI%iClkJ.`&-I>g"D=C*_DOdC`TfJlN+XNT=7iGuc0# +QAanM6Ek2aP7jrhY\GH81cU?$!&?S23MaCjB7K' +=tZ7&$c/k%9M2N=_5`W&b9YDK,hI(a]NGM@%;/Rj<=)]cjBVp@7JlT^VP[ILCn.>g#S-Ag(86S4O#f-!3$fCRO6dNT=7qRPtVjWo6WDYU[Jh +O^-'TcYB9,[J]!='g<5]1;TH6@+B3C-!6F?eOZiSP%QZ#/$7oZ]fn;22)dH1pdAB1"E,'-jZ`Y/JGd"#Z_CI;M%mT-4)sF7.hoBA[rEA>H9,lfFapCRO,*6Ek2aP7jrhY\GH81[qtIQae_f9ageU3p\X!QY/-N#.1F^/?"KP"4/X&;tOk-k)-RL1gVG3Pjop))=SO\_^IJ/YI +H[F5'*ZZ7qs0&]4Z1MLLGP?^p%p&SPgKnDu@,SEC.=I:Uoi!9#D2n7EhG24R5E<[Cj*P&$KH=cN&G5Fpm%fsAlFbI;BFlKRPINp"O+al24;k1;'n%1RH'FuI'N +@qK="c0Xc7WRH.M1jB=1)2[dI%iClkJ.`&-I>g"D=C*_DOdC`TfJlN+XNT=7i +Guc0#QAanM6Ek2aP7jrhY\GH81cU?$!&?S23MaC +jB7K'=tZ7&$c/k%9M2N=_5`W&b9YDK,hI(a]NGM@%;/Rj<=)]cjB,]WtMKPagFh5X@@4^(X8G'@+B3C-!6F?eOZiS +P%QY8CUn-8/-N#.1F^/?"KP"4/X&;tO^-'uo4B.J24R5E<[Cj*P&$KH=cEQfja+U(2)R6M2tt1uF=e0= +=U8%;B2X=]$!*&H=seMq,EZd;e>Z@e*?Aum*B8#rdA'3I]Wl7?i6=TmC,hCiX@f^3,hFfnZPj-SbEjhe +8csODIM=seL#`;AVZIs-&9lc/H, +pFrRH\&uUgOcS)>f=@3B3<164[&iA2DArh*_ld:WQIbMi.p#n]\UFAKA&jV3p4%'6'>.J"IIbL@pPTP( +5(*-]HFT993<182Y3"eU_^6]WdeZ\-\b25YSXi93*aS151@hcDI/:At.iZgNbf"^sgP%trnbn@!u/P"SCC@\#1X@f]H_5[X=L*MlTp%9=dFK##p$bq=aCRO,*'$7P? +nXpMr9!Be]WW]AW,Eas63m$']B:"sODr&"Te'lc,StEp.%.IkcjBBQJ:^L/K=I!8oJlT]S8Ol9@Rm6^p +E,]d`T7$$0-?ujbE\1c`W!'.6X@f]H_5cJ(V"K'4k09AKfZ=T*AUl^4,hFfkK1`?t_5[X='!*!(MkmLe +5SdG0(p7G/#F/[sJlN+X-!3$0(.hfTJ1Rj>0jMm=%k>Dq"KN0:9!E'?/<[T1!Ei[\@^q^Y*`R_k$!&?S +Q!i-]=X8,A"1#GB`GdD=4K&C`'!+^0.`&.DZ:O7a#%V^bMr@jZGYe]I-!6F?7B+=`e-mf.`&+fWtMKP/rQO. +AO%#4[MZ/YN!GVUS]n:bCD.HCb]2<(_8+4WW]@8X@f^3\1EfSQ2t,nfj_IW0cj;G<=)\P +=I!:EE*4H//H`;gZGgc7@m$[mWtML+YU[JhiNc#>=pAMWAW,MN`ct;d<[Ck5?r_eY_e"t[[1(.9aqhn& +N:EPRX@f]H_5[X=L_5[X='!+^0C:GgX\shNSrQ8'F[JlN+X +-!6F?eSnY:FY*^$\?a(m/RpoA"KN0:9!Be]Wo:6Sl<4C&EFk'e>/ke`$!&?SQ!dUDaXK'!+^0.`&+fX+neAWGa>!8R9Mi/?WeD>g!rp.`&+fWtML+Y\L]@-EYnc;c?V9F*!I>_R0gA +Ar5SP`ct;d<[Ck5?r_eY_lYHRWDf]U3]bud3cO#mUIUAGHcmEkG-q4tjQ,C%^TVDji6;nV[>aXK'!+^0 +.`&+fX+kusUmE]>8u$THCi"BBH$XeJVl+0PGkegfq=*@\oTmOY+9/'(HKD0A[;-Z4ZtBf.t.2-'E$]j$=tnQ!dUD<=)\8pQA+7QBor! +5(EQZo/3pZ/\U<%\T?*2n]8dB='+!7SqLksi6;nV[>aXK'!+^0.`&+fX+kuCUt;bT9AB6aPq/u?gqJ46 +Z"(hGIH/5#&,5bimGG[nrMfeqro\_;qF@]"J2^+Hf.t?u9!Be]WW]@8X;^LQP>4eFUpZ^NF6b><4$/KE +4o4Lap\+_5[X= +'!+^0C:NFR*?,r5rRC8P_SXgPeB<=f`j9jWhE<\P[-BDq"KN0:9!E'?k9cajSK2WG +YLa_CVshKV$dH.p]VbAjrADgFcX4A3Fi?;X88ELYD`@DDY%<,B9-i+E`e-mf.`&+fWtMKPmcBZ+p%9=d +FK##pC>V=[nT>Wm,9tIbV![!phL>%Va]3h+f.t.2-'E$]j$=tnQ!dUD<=)\8p[Ulp5Fpm%fsAlFbI;BF +lKRPINp"O+al24;k1;'n%1RH'FuI'N@qK="c0X7B+=`e-mf.`&+fWtMKP/rQO.AO%#4[MZ/YN!GVU +S]n:bCD.HCb]2<(_8+4WW]@8X@f^3\1EfSQ2t,nfj_IW0cj;G<=)\P=I!:EE*4H//H`;g +ZGgc7@m$[mWtML+YU[JhiNc#>=pAMWAW,MN`ct;d<[Ck5?r_eY_e"t[[1(.9aqhn&N:EPRX@f]H_5[X= +L_5[X='!+^0C:GgX\shNSrQ8'F[JlN+X-!6F?eSnY:FY*^$ +\?a(m/RpoA"KN0:9!Be]Wo:6Sl<4C&EFk'e>/ke`$!&?SQ!dUDaXK'!+^0.`&+f +X+neAWGa>!8R9Mi/?WeD>g!rp.`&+fWtML+Y\L[!P.Hq[=^9Wh\<\ij<.\*V<[Ck5@+B4!,]t[@ +ZFI3ZE%PQ^W!'.6X@f]H_5cJ"8Es@_A8u1=iEF6G:^L/K=I!8oJlT^"OjpcIaPt>X_mPHnTG"=uYU[Jh +"KR9PPMX$%HZ!`h"`NI*#'dIBE\1c`W!'.6X@f]H_5cJ@UpID6?!Uarj5Ve;44c7'RjF55)<4"/=I!8o +JlN+X%EBccR@3=(l"OpKF"LJ1RjF55)<4"/=I!8oJlN+X%7_\GeS6SV1C8?QN_#F/[sJlN+X-!3#EoXX)&`Pj3#Bg!rp +.`&+fWtML+Y\H-Sl+5I:l`\"Q[YfWeJ^[f#rr2nsT0bQPId=,u>5i/YGL]Ke-lu?><=)\P=I!:EE%@*S +G7W;?[J:RW=j)833S#]c9s)Ra/RpoA"KN0:9!Be]Wo5k'FDPcpoom0!$Oa)Ol<:?P2bPFJ1<+r=YU[Jh +"KN0:)K(1!B4kkt>L`!3emEPO=3-jW;Hh/EAVLGT?r_eY$!&?S2&r(7[9OknQ:S/iN!GVU_5[X='!+^0C,jgcWOu@X\X"rk<.\*V<[Ck5@+B4!,]t[@ZFI3ZE%PQ^W!'.6X@f]H +_5cJ"8Es@_A8u1=iEF6G:^L/K=I!8oJlT^"OjpcIaPt>X_mPHnTG"=uYU[Jh"KR:#,,o>qON!J:L2.[e +5p]Wu?r_eY$!%M%78h_m+H%aS%+d8SK1`?t_5[X='!*!(MkmLe5SdG0(p7G/#F/[sJlN+X-!3$0(.hfT +J1Rj>0jMm=%k>Dq"KN0:9!E'?/<[T1!Ei[\@^q^Y*`R_k$!&?SQ!i-]=X8,A"1#GB`GdD=4K&C`'!+^0 +.`&.DZ:O7a#%V^bMr@jZGYe]I-!6F?7B+=`e-mf.`&+fWtMKP/rQO.AO%#4[MZ/YN!GVUS]n: +bCD.HCb]2<(_8+4WW]@8X@f^3\1EfSQ2t,nfj_IW0cj;G<=)\P=I!:EE*4H//H`;gZGgc7@m$[mWtML+ +YU[JhiNc#>=pAMWAW,MN`ct;d<[Ck5?r_eY_e"t[[1(.9aqhn&N:EPRX@f]H_5[X=L7B+= +`e-mf.`&+fWtMKP/rQO.AO%#4[MZ/YN!GVUT`DsCL1GdBeY/RpoA"KN0:9!Be]Wo78JBrtDT)&_,9D;3p\He2TIl'Kg;X>Q&? +BT/#`5CE%D-U+7G?_pV$olDPEZ"o\A)<-!cJlN+X-!6F?eSm16FFJ[fkK]W2cC@K+YkD6qUUmM,CE63F +\O1c>aT(4&%cr%[JlN+X-!6F?eSsjNh"'Iaifg:HH0r"eq!d7V^:sRCpUTpCj5hAc +j\WZm-WlXA3]\uM0fCqU?L7+JIdl:3WkW'*qo=JQQ[=,W^]/XaU!cH[ +]0Aa6\-b]J8'.6h-qg\Pl&='rJ'T='%?m\->JeUBCSDD2#okp:V]&?HeqU]d_2 +X@f]H_5cIcRrigsG.^8W?>oem[F]HqeZ2cTY5!_gBe9i+&$P[H>g!7g(4oQ(R?V)ZJlN+X-!3%[.jSDI +/N:E3@Us#Fq3n\(gY:IYSijR;Ds1@OGd@s_)U>,]WtML+YU[JhiBl,djQ,Ckn8Ru7Q%t*K3]fF,>e0Fd +kta,"lg*m(m="Wjd;$FiCRO,*6Ek2aP7jrhYU[Jh"KR7b=j)f.F.SbdBY9^enW>%dg=t@$?hV-NhnFI3 +m]H!u\sNF-!6F?hS[M24R5E<[Ck5?r_eY_r[!0[7n[#b*L)JiClkJ.`&+fWtMKP/r-7*AO6l.[QIS" +B2X=]$!&?SQ!i-]3@&`!'6:UgjfrP[P7jrhYU[Jh"KR:#+fT7F0JG1Woi!9#JlN+X-!6F?eSmMoc/C[Ia/T9CL,19pWW]@8X@f^3 +\/^[on/TS\MaU:`cYMX;]B[ +"KN0:9!Be]Wo7thR_s1pO&ZKe%;/Rj<=)\P=I!:EE&f1\P9`__[XtN7ZQr,*,Y=.E8Wk>jOsEV^,Y=.E +8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Ws'?ea)n-WN!G=\Z28mJSsAZD:h+]=tQ;q4*U*lrC"fukg?0I +aiX1iri5(>"q=(V`Hk"g7C?Ei.-;j#+D*R +mHWsT+!7]_M3MCV@H"]1rG?n.HfM3qg9iHe3)r2BX_hcd.4&j.\&3t3Hi(iEFQh)[D;2(>(5:DG@n59L +V2t2IISsD,V+[/d]=T"FH[:"@?+X/11YUQc?bbIk4R^&bIaq52CHHOq7.X4SGM[UXo/Ca%;prF,ChtYN +:cU[%SSjbPiSdL[%],V^R7TP??Is4R,=dci%m@Z)o#l,Un)(lE0JJUfoLp?f<;jJ-0\< +16VX$gq.d6CH4dTRQdl:Fm;rt2B9&2.'$!K;A5GS,!5U91qqB +hg#.uq;uJ@-:tC$GOF7`4bk6+cd/L.jNO#JkoX0R+W`dmFSYLhoU_uVRPa_9gX`X-oh+uUj2[4M0;b+N +5J*4?/iA%Nh02*Yi.L[9e?p9*A7VL:X/kMu;3]&=`UkeTmsk@aePDuO#7__urUndkeuJS'Ci)O,6g6aK +o>T$>q^`cDeic:1G3rK02dX%8V=g,.[r4KSm+A0uLE`Q,P26d6GrhF#bE,LarA:pqIf4TC:LqXAdk6&& +P'1,DBBNY$ns?AK:'j[e4Sc$brG#bL_6k9EnDV95I=;"emr*]U^VA\+#.d*%Y?jrl6>tYhOcJb^Dnhi4 +h=9OGrL41G-71);GU,ZqWuW3$J,J=Ks7)/">eYTr]=[r!^\Q,d5(U_M>k(hmktfNu7uqQMdjUT!)05?2 +c#ce\)Z()*5;\S[]6E/EGH^;0ZfU#SFfR5`dQb@0U\+9RDVr0VFEBAWSg9 +K7eOFmQH(!r5Tn/SDL6Zo?KGsf;0mD:K@?E$B[N:C]9U@2rB#SLECutb^[6?RH@[">eba$X/kR"pYK+V +=K)),PTZOFV5:#,^\p&:h77U)K6\[W(DktRj,H;'q9P$:?-?*nHdV*Bl/mnJmbCD&>#1YJ;;kI-l7QGX +S/T-=m^p_h4?5F4ieoVcb0n>R.HZa/l(()A;mWq(XigkYr$N2`K@FhOZoaYPO)l@=S'i++EarPn(p2a1fX.S +fYYB#o#o&I)/+GpT.Z&m]ANR)a,V25HG/SkZK7p.]]/D/4b!;g]jL`eTDl33c_9lQqH?,WBtVFr/YfhA +A">`-1A1S'K+Fu2+sfd81S)`)s"->2=m>Yah2$[g%mKa"Xh$bU,+[8b=0Je%L)=VVqmBNFdJ3Y;V+[.Y +?b(7@:=![2?iKV_^+TrCponYT=$cc\f>$2']R>3VLCP>>^?Bd&Mre+PPWlHJ0jDDLS2Oi"G':j6i4FL= +IX4`[WsJ>)lLfJ*`M]>W\o$31oFHD.Bd4P.DId:Rq3lDOgY:K3LX-]!WrhYA[B&sFNqBT_h\7TZpYUJh +pK3I(\G\Yhm'Ghi()Ie9qpeF?c_-8i:3-'pg#_>W>ISMCo^oMNB5N`G3D(31l0`ffbr8^Oj7gRNV:)D9 +2g]1MkJY/.,6?HqC,p,B`t;kS702nj1?n[$=I*^$rBa,]d8aX063dF$A(:VIki*;'2*+P1Xu$PtEXpHNWo2>.uo +YcuLL3E+0E-b>`'n,5J%CH5#`K.3AAk)[5N]?\0O)Bp3LY-+ose[OFb4+/APoeS)6q=EcJch&q_ePDtP +l&Vj>7uoQHs#8h_o:'[4WT9Jslh-`+Dn;./s$04M92ebfrjZ1o0?oOs2fAE4cWJB6bI;EIhTVVb+MEBH +;Y@eR8s1GkOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E +8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>jOsEV^,Y=.E8Wk>j +OsEV^,Y=.E8PT3qZZ9$e~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.72 412.56] CT +[1 0 0 1 0 0] CT +N +-954 -77 M +-954 309.25 L +237.25 309.25 L +237.25 -77 L +cp +clip +GS +0 0 translate +238 310 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 238 + /ImageType 1 + /DataSource Data + /ImageMatrix [238 0 0 310 0 0] + /Height 310 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WEG1U0"35CI66[V,k`:Q!#W*<"a<^W(#a6B_ZD)r,+btjr_]k$pZPiuZ79#)n+O%bjP*-H9&53LC +`1X2&ClC\QU9#2@ANBJ]j?jI@D0L&=T?lnbpPU8>ch#rZ\aR6#H+nJMD05Ddp%J*NosVZNVV',jq=CM" +:TD[.29U!KBOatWL6=8Y:i^Ut4*GH?rr#(]r3Q8:*B8"GE,a2#/,K/?%j*ttm<"0@nLX5J,]9, +U5LY8q<"0m47><+@G+M`\lHH*eOL[+AF%gKds1oeh05L2kPb/4*uPqVEob;92r/Sl"W)RdrV,>i9Z^#9 +A]kiPdXqc8lEKmHZd2g)=.nX=V+HAqjdBbQ.@98;Y2O^+q<,fS^1Bq9jHIt`V,]0e]J8il>e5%Ap"*QS ++)\jlp+*;UU1WfYYJ7eZ_#cch3Y8DrZUUkH$!1uu[]NY\rqY`\?G0O+nF@n`-.goBc9(LE0i1A)XjV5A +q=Dos2f.->h:!"-41)oE.DE:#pX2\fSMOo=2Vto]?ZFicfAi`&S$'Epb$l`"X]e_rFL9^k++9s^S3(qt +eol4[lCGfE_>3K-hmRI\Y![3>7&M_mZ$uZJEq'km%Q%6mf[d;e[2klFm.0qh +d3S`c`R?bHnWg0#f/8bO;S?)!,\X^unoN)uf3NS8)jDX-hGTcl]J1BC(skrAgZqm/:(rqpl%HLm"M=MZ +F?%h0I<<\gFVC(BHdhE=Em$$D1*1;+P1tn,[[16>aj)\JHgS4%Zg4u&*&aji>b1;@\S/ZV%H9%*,ra\d +VI\MhDV3Li[;4AH]`kIiJU/UY0:&&F8T)&sL1:!9WKK+^d998ulTMB`-0H:1G"d\$2/Cdm/DmsWUE6r- +NLKKSVG[:f@Z&:gB:jTQDq0UbI]2?I2Jae.lNj@ti7NBeQ\jPQP*;[3_Co( +9hKR$i5(&bXf=UtBWu&BaTi:qd&uXmNh!OpgGVb[ZpaB[MET>sl_RQSn#I,P8OpNm#A;jb@;AUnGOF7k ++*!7j03S%K8OZ->a4<%\Qc:%G9$UYF@)9L$o>Y(4Vc]".E,\X5WKZn2g8.H/QCl4K>G^El558M?Te4"5 +Xj3]Qj9AhKSL:UJ^OGm+3]fFr!`WRH8O;l^Heh&/KWe-MPN:`r;Z)E+$Q#+I!7.t3ibPaRA$=J$1IfY[les,UlL,I4F,_iokrDis/-k`UHMF/g&iXr4]!of"!s^*:bpbBN%Gp7 +XJmIX.N&\[?U/*>;0!^(4&Br=,s+f#jFMl55"o+'GB^B;U%MA7Q2)*`]hC,_5*!_hPak7c%[\<,PP.@s?Kp'!Ra+\eZrk^eNET+6^4?gArT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg +4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'di +j?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%XT]A4-G+dnY8p6lg4A'dij?-%X +T]A4-G+dnY8p6lg4A'dij?-%XQrM00!s3:: +A-Y`PULXU%cD+nT9$X2B#>3::A-Y`PULXU%cD+nT9$X2B#>3::A-Y`PULXU%cD+nT9$X2B#>3::A-Y`P +ULXU%cD+nT9$X2B#>3::A-Y`PULXU%cD+nT9$X2B#>3::A-Y`PULXU%cD+nT9$X2B#>3::A-Y`PULXU% +cD+nT9$X2B#>3::A-Y`PULXU%cD+nT9$X2B#>3::A-Y`PULXU%cD+nTreai"J&Uaon5mhGTK\CX#*bWJ +aUXQ*8')7*S4C[2Pak7c%[[1j^%m&1-Kojpm>b*b4WRW9,hs%ZG-DA+%.2jtZ!.N+sDD +m_Gd`1$/t)C*$mQ^o6BMjpm>b*b4UlWDT5UIk_0bOmci_zz!!!"fm_jqqrVc]8Nur<6NjOX]^]!lX?[o +1\GjG'J8AffRanY.EnuiZAl:=L! +HDSotW/r3-7ZZ"(hqaN2`Td'@6+Q7lV%m%%m"K7ee^?!YU\]&[//G%jN&aBsGq[5R75l"T-P[\$oUq*c +P+dRu6Y96=@e&tK/p\5t9In%\nZ\8gQZ='pCMh07J8I-[oDY$/65]_]GL+)$;q^AG&>HeG:imG"N^eUO +V9l6Q/[F3c^ZQWi#0^]2#]dA"mT2sZ7aOu7aO+tHhWh!FS[M"b>$ +cm(8X4ppu@F_dn*W-r9:,OedSihF3QS'4n_tkp9ID,n9$Y%s8Mo=-1APASq`e=gf2df>b(T=F7hF!(G= +3!P\08fgX=bOj4rJHrT9Sf55t?;M\e%\j,Q4$gW!c$EjOu#Fer!Hn\uLiIItp^7uo9,[VXd2Fj8dB(04 +A>hEEa-a?+P0Pig*#)4?ANP)K'YR^SJ#'MGI`"(LP]/6T8ibEh_aNZC2I8(kP>=Bqm%%m.@nSGg):9%+A7XcE0>Hl=lDb=5dpdQHGid)5`ODe7=$6 +HRe!aZ$H`('%fTP?]='d>jXk&$oD7,3)p5:T[Y6LDL!<<*"zzzzzzz5\p0[Q'>a+~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 574.8 375.6] CT +[1 0 0 1 0 0] CT +N +0 0 M +0 386.25 L +1191.25 386.25 L +1191.25 0 L +cp +clip +GS +0 0 translate +954 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 954 + /ImageType 1 + /DataSource Data + /ImageMatrix [954 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0WIq2jZ+33njeO7B0ar_?SUPmccMG.hS7?sm/Ff]Bm'fFO!R*$MGVIH8dK^877f#\ +M2>5L;cuN!g5>)L&Qp\?q7b\2f5YtOn&`-=IU9R_a7J9hOaHNZhXe:]HZ-Rboss7'D#jV:zzzzzzzzzz +zz!'nP@(0#Sq!!!!I_kg"mWiE(>\_P)X%kBDdO$$;gd*tQX<5CVC2IIlSqohIaMo[E +*jlDfHf?@2)!`J`/-!WW59QabgtX1#If>25HOIT_d>lr[K+HhVDP9oTn>@*9%ejlPUc]mKM-?J=/g042 +HKf@SX$csUK^S2d4s.0'>J]U51XZf^uWkF^AVe^`:'s7=Q$4$SK@CWkUbs1ReJgm0lX2uimSDs=t'l04 +&;'3SODZZdh+EHZM+[pK+8!;ul.]=Y2R9i"Qs;E$4X<%\6`"eJW& +9'bg"/r3Z,;Be)%]\mn5=I'H&ei3bgJ8lg'JD/cYkO.VNmEF8,+Y

fC/mC/C3O3!,^f8U>MaP92>fUap+.aG]_^oo2P@du5+g/>'g=!`Df=O@CR;Z-rUI +]/*s@M,m92A)27D>PX'OQ+d+q[*l_UGS'.6Q-J7-tr6USi)?RZ[uAn\$g[Ti@1Nd($Q7CNU_BRoUTXSTM.#9=g*=0G +r>?s1/N.K]C9">84*^7fm-J?pWX.XXT_83sRQ52F'?-DAB?.u_D2E"b'FfQY%l]r +VWIdCZaGJoZTE8cE!IQM=t,J/F!h03_62PpEPIM\GIUTEMe*BI`_\oHb"=k<]T!5M@8Nb=o"IX^.E*>f +WLoMX/mgM7bSSN@X(ali9-G3sV;Puj+W3-44Uj?qFp:S0O(2qu>ZIegs"$+K"Aj'?6P^OO"&L>e07pAX +sX(GFrjhks[^>&n86=(6j;ZCAl0oB1a!K*B9De'jFbgT5u]G#)sbbf@m=DAe2b8Aals!!(YsMi3MEfih +Q_Jh>P'F1L(t4aY&(D!UBOWiN0FMg.`]7`NHfgp!.HemiK1C@'BiWnm48ESIl9@s9hEb;LonGZS%'LPN +cWVP^5n?G.8fO7&n_9K=:i3aiR*n@(-djJ*"h_@:@!SHl2[grr.AUf"qk(g2b(#Y-M4o0&EARm+L&pCY'@T>W? +onfZPq_*:C'dD:sSHEPPbbD/X*$#`k%JTLaMK3kI%'db^&_o[?Uj%L)n55R#]if2n6ag=ki3jR[Z+Njm +Mt`"-f%[$2e*NjrL`>>@0WRI,*-A``!pPJL>uEk,FJ98iZc@$7rIMXqFECIsf"E>9h@'AaJ4_hFlr[9O\/&N@,LE$R*d> +'(+L7*]$>2^nKDXA.b^b'7m^Y3dO\>?etQm$JkA0StdmL7Fs8dcN=.I+B:%"\n(ca>&L;?9he>?i:?!Qp@lI[&$>\3]0:T8m,[6'Pt?1QiLh74&'3 +r*G,b@,G@G#jI!j]ilHRG?N-baGG^F6CiZ+'u432Cg`Qrr)_3&[E-`=/!Mj2*X +OCg];bKJ?[_TIR1C;&E[80%prDhI^ZDrlph.%aHhZs=UFSE,SuoK6j1g;/+RM?!2dK]p`A_@Ng3o@T=PrQ4lO, +?R.CUn@A)EQWm;HqG!!(qGZ+@M]L%+Nd]?`Fol-lQZ"d(#cQW!6rB7>ArPR(g=f_@Blo`S,BI +pI0k11`zzzzzzzn=k.J!!!"D&QGb^=(K)!7]Hfkl5c!:OK&A].ZgY3WHsnc8CK8Jllf@4;Y?&@'hD>U< +#%r(JQ0n\Cn'Vb])Wm,EU +oH\[h@Z^gW@F"0!i[Z2Wcr051_"NI*`>u\C<Qe*K_h5WWK.LOK&A]HC'@WdkHRD+]S\Do +o;QX86(4?'hD>U<4tplUhQ2`p2kG"j3\J&?0tGU^Eot5 +cP0eW^*T@V'/ajZ9fHNg$Bk^mtaqo<#%r(JQ*@Te[mY+8m>=e_]tX6V!8t_.ZgY3W@F"0!i^L4L.H%%$ +>F`qZ\MMn,>n\Cn'VarD-8"kO)4NOWcpk25cP0eW^*QkV!;5(,,HkAUu@H\*`DZk8/4ZqJ +Q*@T"]G"G%?]))( +"2TmAe1ke8#-KfW^*QkUoH\[hPDAng9O$`*nfm'9*N7ksqs6K].k +K]VrK5&L9]N>k=eN4sT+B8SC$G45ho<%bjlL%mE;oe2s+K%E;WDK7$G9IT8C]dG< +?P9F;H4i>"]G"GH))Au)hWcqDnc&0LECs^hj3XR)\>r/,A>7-]N>k=eN4sT+B8SCk +<&.F1HIR3J&Jg-/(Mnhc,o-6>?9\$U-Pa8&ZGi2.ZgZ[[r+@;!moNe6 +>_a$b<8!O7un]SIf+G#6"ainK*MorDOIr7^cuuG;Y?&@'hD>U<'@j9CTFIm';lZ/eCRdQ'7AQCI,Mek\ +)2Z3l`\(;IJ`a2ldtOjHgeYW['WeIr9N,%lKT9WRI!X?V5KD@V!8t_.ZgY3WVTWjo#$;_RA\0`hZ#`cI +C*Rtkgs0k%F-RY0&$)OJUs2sS7KA_9>X14AKb(BLfAq)'hHlQ#7i<@HoQ*qqt/,lae.VJ[cLucmHpfHS +LSb2D-;F>ljBQR*K_h5WWK0r'PY`k69+d&hYCCGf=aS-V[fglRiAH#D4Uh*^+7SC`F?3.WpBNAHcBk$Dcr-OXcHm!ic#a>?9aolBAukI +-8eGmdBLNXn&GiVja'CVPg?$$GU1no]X[)#7dikQ3G>J>Rb\ +o#!+qmJS%`ONCd\$<3c*RWm,O$CqQo.`0p>?9\$U-Pa8&ZGi2.ZgZ+Zt1PTX$/[b>>AjAA)laEIpQ>`g +NFiZ=b.BI!i^K9U^Eot5cR#qQBc]tqBTt%ajHChGd/ZYWcpk25cP0el4\#;M?9aolI3)nnp^[4c +)&h~> + +%AXGEndBitmap +GR +GR +GS +[0.48 0 0 0.48 1032.72 375.6] CT +[1 0 0 1 0 0] CT +N +-954 0 M +-954 386.25 L +237.25 386.25 L +237.25 0 L +cp +clip +GS +0 0 translate +238 77 scale +%AXGBeginBitmap: java.awt.image.BufferedImage +{{ +/RawData currentfile /ASCII85Decode filter def +/Data RawData /FlateDecode filter def +/DeviceRGB setcolorspace +<< + /Decode [0 1 0 1 0 1] + /BitsPerComponent 8 + /Width 238 + /ImageType 1 + /DataSource Data + /ImageMatrix [238 0 0 77 0 0] + /Height 77 +>> image +} stopped {handleerror} if + RawData flushfile +} exec +Gb"0V_$u#N'L]c:PH$UP/QYS[5fp0cR4/,aXWAHB:B1@pzzzzz!9!;%E+>s+O2nMNJlV\aSm^)4RjV=G +cfPO7mB_(i)UZ[%.&qeVj3e\3cXl4Z(OjM!Y;l4Z(Oj?=k;.(N-q#tjVaEe9JO +PanE-4s_R)\PL\K`D8nJT+Ae3.L +5LSu3,Q~> + +%AXGEndBitmap +GR +GR +%%Trailer +%%Pages: 1 +%%EOF diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp.png b/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp.png new file mode 100644 index 0000000000..b57a928286 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp.svg b/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp.svg new file mode 100644 index 0000000000..17a053a9be --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp.svg @@ -0,0 +1,860 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + t + v + + + 0 + joint 1 (lead axis) + joint 2 + joint 3 + acceleration + constant + deceleration + + max v + max acc + max dec + + + + + + t + v + + + 0 + joint 1 (lead axis) + joint 2 + joint 3 + acceleration + constant + deceleration + + max v + max acc + max dec + + + + + + + lead axis + + brake + + + t + v + + + 0 + acc + constant + deceleration + + max v + max acc + max dec + + (1) + (2) + (3) + + diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp_movement_1.png b/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp_movement_1.png new file mode 100644 index 0000000000..7226ec5e6f Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp_movement_1.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp_movement_2.png b/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp_movement_2.png new file mode 100644 index 0000000000..5158e32959 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/doc/figure/ptp_movement_2.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/figure/rviz_planner.png b/moveit_planners/pilz_industrial_motion_planner/doc/figure/rviz_planner.png new file mode 100644 index 0000000000..759be7e3a4 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/doc/figure/rviz_planner.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/sequence_processing_overview.png b/moveit_planners/pilz_industrial_motion_planner/doc/sequence_processing_overview.png new file mode 100644 index 0000000000..2b82cc09bd Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/doc/sequence_processing_overview.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/sequence_processing_overview.uxf b/moveit_planners/pilz_industrial_motion_planner/doc/sequence_processing_overview.uxf new file mode 100644 index 0000000000..fa169e844c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/doc/sequence_processing_overview.uxf @@ -0,0 +1,1130 @@ + + 5 + + UMLDeployment + + 520 + 155 + 100 + 30 + + <<MotionPlanResponse>> +Item_1 (Group_1) +bg=cyan +transparency=0 +group=8 + + + + UMLDeployment + + 520 + 190 + 100 + 30 + + <<MotionPlanResponse>> +Item_2 (Group_1) +bg=cyan +transparency=0 + +group=8 + + + + UMLDeployment + + 520 + 230 + 100 + 30 + + bg=yellow +<<MotionPlanResponse>> +Item_3 (Group_2) +transparency=0 +group=8 + + + + UMLDeployment + + 520 + 270 + 100 + 30 + + <<MotionPlanResponse>> +Item_4 (Group_2) +bg=yellow +transparency=0 +group=8 + + + + UMLDeployment + + 520 + 310 + 100 + 30 + + <<MotionPlanResponse>> +Item_5 (Group_1) +bg=cyan +transparency=0 +group=8 + + + + UMLDeployment + + 520 + 365 + 100 + 30 + + <<MotionPlanResponse>> +Item_N (Group_2) +bg=yellow +transparency=0 +group=8 + + + + UMLClass + + 510 + 125 + 125 + 275 + + << Vector<MotionPlanResponse> >> +MySequence +layer=-50 +bg=light_gray +transparency=0 + +group=8 + + + + UMLDeployment + + 855 + 155 + 110 + 65 + + <<RobotTrajectory>> +Item_1 + Item_2 (Group_1) +bg=cyan +transparency=0 +group=2 + + + + UMLDeployment + + 855 + 230 + 110 + 70 + + bg=yellow +transparency=0 +<<RobotTrajectory>> +Item_3 + Item_4 (Group_2) +group=2 + + + + UMLDeployment + + 855 + 310 + 110 + 30 + + <<RobotTrajectory>> +Item_5 (Group_1) +bg=cyan +transparency=0 +group=2 + + + + UMLDeployment + + 855 + 365 + 110 + 30 + + <<RobotTrajectory>> +Item_N (Group_2) +bg=yellow +transparency=0 +group=2 + + + + UMLClass + + 850 + 125 + 125 + 275 + + << Vector<RobotTrajectoryPtr> >> +MySequence +layer=-50 +bg=light_gray +transparency=0 +group=2 + + + + UMLDeployment + + 1240 + 205 + 110 + 65 + + <<RobotTrajectory>> +Item_1 + Item_2 (Group_1) +bg=cyan +transparency=0 +group=3 + + + + UMLDeployment + + 1240 + 280 + 110 + 70 + + bg=yellow +<<RobotTrajectory>> +Item_3 + Item_4 (Group_2) +group=3 +transparency=0 + + + + UMLDeployment + + 1240 + 360 + 110 + 30 + + <<RobotTrajectory>> +Item_5 (Group_1) +bg=cyan +transparency=0 +group=3 + + + + UMLDeployment + + 1240 + 415 + 110 + 30 + + <<RobotTrajectory>> +Item_N (Group_2) +bg=yellow +group=3 +transparency=0 + + + + UMLClass + + 1230 + 90 + 130 + 365 + + <<ExecutableMotionPlan>> +MySequence +layer=-100 +group=3 + + + + UMLClass + + 1235 + 180 + 120 + 270 + + << Vector<<ExecutableTrajectory>> >> +plan_components_ +bg=light_gray +transparency=0 +layer=-50 +group=3 + + + + UMLClass + + 1235 + 135 + 120 + 15 + + planning_scene_monoitor_ + +group=3 + + + + UMLClass + + 1235 + 155 + 120 + 15 + + planning_scene_ + +group=3 + + + + UMLClass + + 1235 + 115 + 120 + 15 + + error_code_ + +group=3 + + + + UMLDeployment + + 1595 + 215 + 110 + 65 + + <<RobotTrajectory>> +Item_1 + Item_2 (Group_1) +bg=cyan +transparency=0 +group=9 + + + + UMLDeployment + + 1595 + 290 + 110 + 70 + + bg=yellow +<<RobotTrajectory>> +Item_3 + Item_4 (Group_2) +transparency=0 +group=9 + + + + UMLDeployment + + 1595 + 370 + 110 + 30 + + <<RobotTrajectory>> +Item_5 (Group_1) +bg=cyan +transparency=0 +group=9 + + + + UMLDeployment + + 1595 + 425 + 110 + 30 + + <<RobotTrajectory>> +Item_N (Group_2) +bg=yellow +transparency=0 +group=9 + + + + UMLClass + + 1590 + 190 + 120 + 270 + + << Vector<<:RobotTrajectory>> >> +planning_trajectory +bg=light_gray +transparency=0 +layer=-50 +group=9 + + + + UMLClass + + 1590 + 135 + 120 + 15 + + planning_time + +group=9 + + + + UMLClass + + 1590 + 155 + 120 + 25 + + << Vector<RobotState> >> +trajectory_start + +group=9 + + + + UMLClass + + 1590 + 115 + 120 + 15 + + error_code + +group=9 + + + + UMLClass + + 1585 + 80 + 130 + 385 + + <<MoveGroupSequenceResult>> +MySequence +layer=-100 +transparency=0 +group=9 + + + + UMLState + + 1125 + 230 + 85 + 25 + + halign=left +valign=top +*Build* +ExecutionMotionPlan + + + + UMLSpecialState + + 0 + 235 + 20 + 20 + + type=initial + + + + Relation + + 15 + 240 + 70 + 15 + + lt=<- + 120.0;10.0;10.0;10.0 + + + UMLState + + 245 + 225 + 130 + 40 + + halign=left +valign=top +*Validate:* +- blend radii all positive +- last blend radius zero +- only first request has start state + + + + + Relation + + 195 + 240 + 60 + 15 + + lt=<- + 100.0;10.0;10.0;10.0 + + + Relation + + 370 + 240 + 30 + 15 + + lt=<- + 40.0;10.0;10.0;10.0 + + + UMLState + + 390 + 230 + 105 + 30 + + halign=left +valign=top +*Calculate* +individual sequence items + + + + Relation + + 490 + 240 + 30 + 15 + + lt=<- + 40.0;10.0;10.0;10.0 + + + UMLState + + 770 + 230 + 60 + 25 + + halign=left +valign=top +*Blend* +*Trajectories* + + + + Relation + + 630 + 240 + 25 + 15 + + lt=<- + 30.0;10.0;10.0;10.0 + + + Relation + + 970 + 240 + 55 + 15 + + lt=<- + 90.0;10.0;10.0;10.0 + + + Relation + + 1205 + 240 + 35 + 15 + + lt=<- + 50.0;10.0;10.0;10.0 + + + UMLState + + 1385 + 235 + 60 + 25 + + halign=left +valign=top +*Execute* +*Trajectories* + + + + Relation + + 1355 + 240 + 40 + 15 + + lt=<- + 60.0;10.0;10.0;10.0 + + + Relation + + 1440 + 245 + 50 + 15 + + lt=<- + 80.0;10.0;10.0;10.0 + + + UMLState + + 1480 + 235 + 95 + 25 + + halign=left +valign=top +*Generate:* +MoveGroupSequenceResult + + + + Relation + + 1570 + 245 + 25 + 15 + + lt=<- + 30.0;10.0;10.0;10.0 + + + UMLState + + 645 + 230 + 110 + 25 + + halign=left +valign=top +*Validate* +Blend radii do not overlap + + + + Relation + + 825 + 240 + 35 + 15 + + lt=<- + 50.0;10.0;10.0;10.0 + + + Relation + + 750 + 240 + 30 + 15 + + lt=<- + 40.0;10.0;10.0;10.0 + + + UMLSpecialState + + 1015 + 225 + 50 + 40 + + type=decision + + + + Text + + 1060 + 230 + 35 + 15 + + Action + + + + Text + + 1045 + 265 + 40 + 15 + + Service + + + + Relation + + 1060 + 240 + 75 + 15 + + lt=<- + 130.0;10.0;10.0;10.0 + + + UMLState + + 1100 + 565 + 120 + 25 + + halign=left +valign=top +*Build* +GetMotionSequence::Response + + + + Relation + + 1035 + 260 + 75 + 325 + + lt=<- + 130.0;630.0;10.0;630.0;10.0;10.0 + + + Relation + + 1215 + 570 + 380 + 15 + + lt=<- + 740.0;10.0;10.0;10.0 + + + UMLSpecialState + + 1785 + 310 + 20 + 20 + + type=final + + + + Relation + + 1710 + 245 + 95 + 75 + + lt=<- + 170.0;130.0;170.0;10.0;10.0;10.0 + + + Relation + + 1710 + 325 + 95 + 260 + + lt=<- + 170.0;10.0;170.0;500.0;10.0;500.0 + + + UMLClass + + 65 + 80 + 145 + 310 + + << MotionSequenceRequest >> +MySequence +layer=-500 + +group=7 + + + + UMLDeployment + + 90 + 140 + 100 + 30 + + <<MotionSequenceItem>> +Item_1 (Group_1) +bg=cyan +transparency=0 +group=7 + + + + UMLDeployment + + 90 + 175 + 100 + 30 + + <<MotionSequenceItem>> +Item_2 (Group_1) +bg=cyan +transparency=0 +group=7 + + + + UMLDeployment + + 90 + 215 + 100 + 30 + + bg=yellow +<<MotionSequenceItem>> +Item_3 (Group_2) +bg=yellow +transparency=0 +group=7 + + + + UMLDeployment + + 90 + 255 + 100 + 30 + + <<MotionSequenceItem>> +Item_4 (Group_2) +bg=yellow +transparency=0 +group=7 + + + + UMLDeployment + + 90 + 295 + 100 + 30 + + <<MotionSequenceItem>> +Item_5 (Group_1) +bg=cyan +transparency=0 +group=7 + + + + UMLDeployment + + 90 + 350 + 100 + 30 + + <<MotionSequenceItem>> +Item_N (Group_2) +bg=yellow +transparency=0 +group=7 + + + + UMLClass + + 75 + 110 + 125 + 275 + + << Vector<MotionSequenceItem> >> +MySequence +layer=-100 +bg=light_gray +transparency=0 + +group=7 + + + + UMLGeneric + + 240 + 65 + 765 + 460 + + symbol=component +halign=left +valign=top +layer=-1000 +CommandListManager + + + + UMLGeneric + + 760 + 100 + 230 + 335 + + symbol=component +symbol=component +halign=left +valign=top +layer=-100 +PlanComponentsBuilder + + + + UMLGeneric + + 45 + 45 + 1710 + 830 + + symbol=component +halign=left +valign=top +layer=-1000000 +MoveGroupSequenceAction/Service + + + + UMLGeneric + + 1365 + 180 + 100 + 125 + + symbol=component +layer=-100 +MoveGroupContext + + + + UMLGeneric + + 1375 + 210 + 80 + 75 + + symbol=component +layer=-50 +PlanExecution + + + + UMLDeployment + + 1595 + 615 + 110 + 65 + + <<RobotTrajectory>> +Item_1 + Item_2 (Group_1) +bg=cyan +transparency=0 +group=10 + + + + UMLDeployment + + 1595 + 690 + 110 + 70 + + bg=yellow +<<RobotTrajectory>> +Item_3 + Item_4 (Group_2) +transparency=0 +group=10 + + + + UMLDeployment + + 1595 + 770 + 110 + 30 + + <<RobotTrajectory>> +Item_5 (Group_1) +bg=cyan +transparency=0 +group=10 + + + + UMLDeployment + + 1595 + 825 + 110 + 30 + + <<RobotTrajectory>> +Item_N (Group_2) +bg=yellow +transparency=0 +group=10 + + + + UMLClass + + 1590 + 590 + 120 + 270 + + << Vector<<:RobotTrajectory>> >> +planning_trajectory +bg=light_gray +transparency=0 +layer=-50 +group=10 + + + + UMLClass + + 1590 + 535 + 120 + 15 + + planning_time + +group=10 + + + + UMLClass + + 1590 + 555 + 120 + 25 + + << Vector<RobotState> >> +trajectory_start + +group=10 + + + + UMLClass + + 1590 + 515 + 120 + 15 + + error_code + +group=10 + + + + UMLClass + + 1585 + 480 + 130 + 385 + + <<GetMotionSequenceResponse>> +MySequence +layer=-100 +transparency=0 +group=10 + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.h new file mode 100644 index 0000000000..61737ba275 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.h @@ -0,0 +1,42 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +namespace pilz_industrial_motion_planner +{ +static const std::string SEQUENCE_SERVICE_NAME = "plan_sequence_path"; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limit.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limit.h new file mode 100644 index 0000000000..199791f76c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limit.h @@ -0,0 +1,159 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Set of cartesian limits, has values for velocity, acceleration and + * deceleration of both the + * translational and rotational part. + */ +class CartesianLimit +{ +public: + CartesianLimit(); + + // Translational Velocity Limit + + /** + * @brief Check if translational velocity limit is set. + * @return True if limit was set, false otherwise + */ + bool hasMaxTranslationalVelocity() const; + + /** + * @brief Set the maximal translational velocity + * @param Maximum translational velocity [m/s] + */ + void setMaxTranslationalVelocity(double max_trans_vel); + + /** + * @brief Return the maximal translational velocity [m/s], 0 if nothing was + * set + * @return Maximum translational velocity, 0 if nothing was set + */ + double getMaxTranslationalVelocity() const; + + // Translational Acceleration Limit + + /** + * @brief Check if translational acceleration limit is set. + * @return True if limit was set false otherwise + */ + bool hasMaxTranslationalAcceleration() const; + + /** + * @brief Set the maximum translational acceleration + * @param Maximum translational acceleration [m/s^2] + */ + void setMaxTranslationalAcceleration(double max_trans_acc); + + /** + * @brief Return the maximal translational acceleration [m/s^2], 0 if nothing + * was set + * @return maximal translational acceleration, 0 if nothing was set + */ + double getMaxTranslationalAcceleration() const; + + // Translational Deceleration Limit + + /** + * @brief Check if translational deceleration limit is set. + * @return True if limit was set false otherwise + */ + bool hasMaxTranslationalDeceleration() const; + + /** + * @brief Set the maximum translational deceleration + * @param Maximum translational deceleration, always <=0 [m/s^2] + */ + void setMaxTranslationalDeceleration(double max_trans_dec); + + /** + * @brief Return the maximal translational deceleration [m/s^2], 0 if nothing + * was set + * @return maximal translational deceleration, 0 if nothing was set, always + * <=0 [m/s^2] + */ + double getMaxTranslationalDeceleration() const; + + // Rotational Velocity Limit + + /** + * @brief Check if rotational velocity limit is set. + * @return True if limit was set false otherwise + */ + bool hasMaxRotationalVelocity() const; + + /** + * @brief Set the maximum rotational velocity + * @param Maximum rotational velocity [rad/s] + */ + void setMaxRotationalVelocity(double max_rot_vel); + + /** + * @brief Return the maximal rotational velocity [rad/s], 0 if nothing was set + * @return maximal rotational velocity, 0 if nothing was set + */ + double getMaxRotationalVelocity() const; + +private: + /// Flag if a maximum translational velocity was set + bool has_max_trans_vel_; + + /// Maximum translational velocity [m/s] + double max_trans_vel_; + + /// Flag if a maximum translational acceleration was set + bool has_max_trans_acc_; + + /// Maximum translational acceleration [m/s^2] + double max_trans_acc_; + + /// Flag if a maximum translational deceleration was set + bool has_max_trans_dec_; + + /// Maximum translational deceleration, always <=0 [m/s^2] + double max_trans_dec_; + + /// Flag if a maximum rotational velocity was set + bool has_max_rot_vel_; + + /// Maximum rotational velocity [rad/s] + double max_rot_vel_; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h new file mode 100644 index 0000000000..796dc01c4c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h @@ -0,0 +1,65 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/cartesian_limit.h" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Obtains cartesian limits from the parameter server + */ +class CartesianLimitsAggregator +{ +public: + /** + * @brief Loads cartesian limits from the parameter server + * + * The parameters are expected to be under "~/cartesian_limits" of the given + * node handle. + * The following limits can be specified: + * - "max_trans_vel", the maximum translational velocity [m/s] + * - "max_trans_acc, the maximum translational acceleration [m/s^2] + * - "max_trans_dec", the maximum translational deceleration (<= 0) [m/s^2] + * - "max_rot_vel", the maximum rotational velocity [rad/s] + * - "max_rot_acc", the maximum rotational acceleration [rad/s^2] + * - "max_rot_dec", the maximum rotational deceleration (<= 0)[rad/s^2] + * @param nh node handle to access the parameters + * @return the obtained cartesian limits + */ + static CartesianLimit getAggregatedLimits(const ros::NodeHandle& nh); +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.h new file mode 100644 index 0000000000..9d7b70431c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.h @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include + +#include "cartesian_trajectory_point.h" + +namespace pilz_industrial_motion_planner +{ +struct CartesianTrajectory +{ + std::string group_name; + std::string link_name; + std::vector points; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h new file mode 100644 index 0000000000..4a5a57e55e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h @@ -0,0 +1,53 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +struct CartesianTrajectoryPoint +{ + geometry_msgs::Pose pose; + geometry_msgs::Twist velocity; + geometry_msgs::Twist acceleartion; + ros::Duration time_from_start; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h new file mode 100644 index 0000000000..ae2ea66387 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h @@ -0,0 +1,225 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +#include + +#include +#include +#include + +#include "moveit_msgs/MotionSequenceRequest.h" +#include "pilz_industrial_motion_planner/plan_components_builder.h" +#include "pilz_industrial_motion_planner/trajectory_blender.h" +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" + +namespace pilz_industrial_motion_planner +{ +using RobotTrajCont = std::vector; + +// List of exceptions which can be thrown by the CommandListManager class. +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NegativeBlendRadiusException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LastBlendRadiusNotZeroException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateSetException, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OverlappingBlendRadiiException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PlanningPipelineException, moveit_msgs::MoveItErrorCodes::FAILURE); + +/** + * @brief This class orchestrates the planning of single commands and + * command lists. + */ +class CommandListManager +{ +public: + CommandListManager(const ros::NodeHandle& nh, const robot_model::RobotModelConstPtr& model); + + /** + * @brief Generates trajectories for the specified list of motion commands. + * + * The following rules apply: + * - If two consecutive trajectories are from the same group, they are + * simply attached to each other, given that the blend_radius is zero. + * - If two consecutive trajectories are from the same group, they are + * blended together, given that the blend_radius is GREATER than zero. + * - If two consecutive trajectories are from different groups, then + * the second trajectory is added as new element to the result container. + * All following trajectories are then attached to the new trajectory + * element (until all requests are processed or until the next group change). + * + * @param planning_scene The planning scene to be used for trajectory + * generation. + * @param req_list List of motion requests containing: PTP, LIN, CIRC + * and/or gripper commands. + * Please note: A request is only valid if: + * - All blending radii are non negative. + * - The blending radius of the last request is 0. + * - Only the first request of each group has a start state. + * - None of the blending radii overlap with each other. + * + * Please note: + * Starts states do not need to state the joints of all groups. + * It is sufficient if a start state states only the joints of the group + * which it belongs to. Starts states can even be incomplete. In this case + * default values are set for the unset joints. + * + * @return Contains the calculated/generated trajectories. + */ + RobotTrajCont solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_pipeline::PlanningPipelinePtr& planning_pipeline, + const moveit_msgs::MotionSequenceRequest& req_list); + +private: + using MotionResponseCont = std::vector; + using RobotState_OptRef = boost::optional; + using RadiiCont = std::vector; + using GroupNamesCont = std::vector; + +private: + /** + * @brief Validates that two consecutive blending radii do not overlap. + * + * @param motion_plan_responses Container of calculated/generated + * trajectories. + * @param radii Container stating the blend radii. + */ + void checkForOverlappingRadii(const MotionResponseCont& resp_cont, const RadiiCont& radii) const; + + /** + * @brief Solve each sequence item individually. + * + * @param planning_scene The planning_scene to be used for trajectory + * generation. + * @param req_list Container of requests for calculation/generation. + * + * @return Container of generated trajectories. + */ + MotionResponseCont solveSequenceItems(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_pipeline::PlanningPipelinePtr& planning_pipeline, + const moveit_msgs::MotionSequenceRequest& req_list) const; + + /** + * @return TRUE if the blending radii of specified trajectories overlap, + * otherwise FALSE. The functions returns FALSE if both trajectories are from + * different groups or if both trajectories are end-effector groups. + */ + bool checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A, + const robot_trajectory::RobotTrajectory& traj_B, const double radii_B) const; + +private: + /** + * @return The last RobotState of the specified group which can + * be found in the specified vector. + */ + static RobotState_OptRef getPreviousEndState(const MotionResponseCont& motion_plan_responses, + const std::string& group_name); + + /** + * @brief Set start state to end state of previous calculated trajectory + * from group. + */ + static void setStartState(const MotionResponseCont& motion_plan_responses, const std::string& group_name, + moveit_msgs::RobotState& start_state); + + /** + * @return Container of radii extracted from the specified request list. + * + * Please note: + * This functions sets invalid blend radii to zero. Invalid blend radii are: + * - blend radii between end-effectors and + * - blend raddi between different groups. + */ + static RadiiCont extractBlendRadii(const moveit::core::RobotModel& model, + const moveit_msgs::MotionSequenceRequest& req_list); + + /** + * @return True in case of an invalid blend radii between specified + * command A and B, otherwise False. Invalid blend radii are: + * - blend radii between end-effectors and + * - blend raddi between different groups. + */ + static bool isInvalidBlendRadii(const moveit::core::RobotModel& model, const moveit_msgs::MotionSequenceItem& item_A, + const moveit_msgs::MotionSequenceItem& item_B); + + /** + * @brief Checks that all blend radii are greater or equal to zero. + */ + static void checkForNegativeRadii(const moveit_msgs::MotionSequenceRequest& req_list); + + /** + * @brief Checks that last blend radius is zero. + */ + static void checkLastBlendRadiusZero(const moveit_msgs::MotionSequenceRequest& req_list); + + /** + * @brief Checks that only the first request of the specified group has + * a start state in the specified request list. + */ + static void checkStartStatesOfGroup(const moveit_msgs::MotionSequenceRequest& req_list, const std::string& group_name); + + /** + * @brief Checks that each group in the specified request list has only + * one start state. + */ + static void checkStartStates(const moveit_msgs::MotionSequenceRequest& req_list); + + /** + * @return Returns all group names which are present in the specified + * request. + */ + static GroupNamesCont getGroupNames(const moveit_msgs::MotionSequenceRequest& req_list); + +private: + //! Node handle + ros::NodeHandle nh_; + + //! Robot model + moveit::core::RobotModelConstPtr model_; + + //! @brief Builder to construct the container containing the final + //! trajectories. + PlanComponentsBuilder plan_comp_builder_; +}; + +inline void CommandListManager::checkLastBlendRadiusZero(const moveit_msgs::MotionSequenceRequest& req_list) +{ + if (req_list.items.back().blend_radius != 0.0) + { + throw LastBlendRadiusNotZeroException("The last blending radius must be zero"); + } +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h new file mode 100644 index 0000000000..0f60b353e5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h @@ -0,0 +1,150 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/joint_limits_container.h" +#include "pilz_industrial_motion_planner/joint_limits_extension.h" + +#include + +#include +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Unifies the joint limits from the given joint models with joint + * limits from the parameter server. + * + * Does not support MultiDOF joints. + */ +class JointLimitsAggregator +{ +public: + /** + * @brief Aggregates(combines) the joint limits from joint model and + * parameter server. + * The rules for the combination are: + * 1. Position and velocity limits on the parameter server must be stricter + * or equal if they are defined. + * 2. Limits on the parameter server where the corresponding + * has__limits are β€žfalseβ€œ + * are considered undefined(see point 1). + * 3. Not all joints have to be limited by the parameter server. Selective + * limitation is possible. + * 4. If max_deceleration is unset, it will be set to: max_deceleration = - + * max_acceleration. + * @note The acceleration/deceleration can only be set via the parameter + * server since they are not supported + * in the urdf so far. + * @param nh Node handle in whose namespace the joint limit parameters are + * expected. + * @param joint_models The joint models + * @return Container containing the limits + */ + static JointLimitsContainer getAggregatedLimits(const ros::NodeHandle& nh, + const std::vector& joint_models); + +protected: + /** + * @brief Update the position limits with the ones from the joint_model. + * + * If the joint model has no position limit, the value is unchanged. + * + * @param joint_model The joint model + * @param joint_limit The joint_limit to be filled with new values. + */ + static void updatePositionLimitFromJointModel(const moveit::core::JointModel* joint_model, JointLimit& joint_limit); + + /** + * @brief Update the velocity limit with the one from the joint_model. + * + * If the joint model has no velocity limit, the value is unchanged. + * + * @param joint_model The joint model + * @param joint_limit The joint_limit to be filled with new values. + */ + static void updateVelocityLimitFromJointModel(const moveit::core::JointModel* joint_model, JointLimit& joint_limit); + + /** + * @brief Checks if the position limits from the given joint_limit are + * stricter than the limits of the joint_model. + * Throws AggregationBoundsViolationException on violation + * @param joint_model The joint_model + * @param joint_limit The joint_limit + */ + static void checkPositionBoundsThrowing(const moveit::core::JointModel* joint_model, const JointLimit& joint_limit); + + /** + * @brief Checks if the velocity limit from the given joint_limit are stricter + * than the limit of the joint_model. + * Throws AggregationBoundsViolationException on violation + * @param joint_model The joint_model + * @param joint_limit The joint_limit + */ + static void checkVelocityBoundsThrowing(const moveit::core::JointModel* joint_model, const JointLimit& joint_limit); +}; + +/** + * @class AggregationException + * @brief A base class for all aggregation exceptions inheriting from + * std::runtime_exception + */ +class AggregationException : public std::runtime_error +{ +public: + AggregationException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; + +/** + * @class AggregationJointMissingException + * @brief Thrown the limits from the parameter server are weaker(forbidden) than + * the ones defined in the urdf + * + */ +class AggregationBoundsViolationException : public AggregationException +{ +public: + AggregationBoundsViolationException(const std::string& error_desc) : AggregationException(error_desc) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h new file mode 100644 index 0000000000..7a91ffe545 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h @@ -0,0 +1,164 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/joint_limits_extension.h" + +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Container for JointLimits, essentially a map with convenience + * functions. + * Adds the ability to as for limits and get a common limit that unifies all + * given limits + */ +class JointLimitsContainer +{ +public: + /** + * @brief Add a limit + * @param joint_name Name if the joint this limit belongs to + * @param joint_limit Limit of the joint + * @return true if the limit was added, false + * if joint_limit.has_deceleration_limit && + * joint_limit.max_deceleration >= 0 + */ + bool addLimit(const std::string& joint_name, JointLimit joint_limit); + + /** + * @brief Check if there is a limit for a joint with the given name in this + * container + * @param joint_name Name of the joint + */ + bool hasLimit(const std::string& joint_name) const; + + /** + * @brief Get Number of limits in the container + * @return Number of limits in the container + */ + size_t getCount() const; + + /** + * @brief Returns wether the container is empty + * @return true if empty, false otherwise + */ + bool empty() const; + + /** + * @brief Returns joint limit fusion of all(position, velocity, acceleration, + * deceleration) limits for all joint. + * There are cases where the most strict limit of all limits is needed. + * If there are no matching limits, the flag + * has_[position|velocity|...]_limits is set to false. + * + * @return joint limit + */ + JointLimit getCommonLimit() const; + + /** + * @brief Returns joint limit fusion of all(position, velocity, acceleration, + * deceleration) limits for given joints. + * There are cases where the most strict limit of all limits is needed. + * If there are no matching limits, the flag + * has_[position|velocity|...]_limits is set to false. + * + * @param joint_names + * @return joint limit + * @throws std::out_of_range if a joint limit with this name does not exist + */ + JointLimit getCommonLimit(const std::vector& joint_names) const; + + /** + * @brief getLimit get the limit for the given joint name + * @param joint_name + * @return joint limit + * @throws std::out_of_range if a joint limit with this name does not exist + */ + JointLimit getLimit(const std::string& joint_name) const; + + /** + * @brief ConstIterator to the underlying data structure + * @return + */ + std::map::const_iterator begin() const; + + /** + * @brief ConstIterator to the underlying data structure + * @return + */ + std::map::const_iterator end() const; + + /** + * @brief verify position limit of single joint + * @param joint_name + * @param joint_position + * @return + */ + bool verifyVelocityLimit(const std::string& joint_name, const double& joint_velocity) const; + + /** + * @brief verify position limit of single joint + * @param joint_name + * @param joint_position + * @return + */ + bool verifyPositionLimit(const std::string& joint_name, const double& joint_position) const; + + /** + * @brief verify position limits of multiple joints + * @param joint_names + * @param joint_positions + * @return + */ + bool verifyPositionLimits(const std::vector& joint_names, + const std::vector& joint_positions) const; + +private: + /** + * @brief update the most strict limit with given joint limit + * @param joint_limit + * @param common_limit the current most strict limit + */ + static void updateCommonLimit(const JointLimit& joint_limit, JointLimit& common_limit); + +protected: + /// Actual container object containing the data + std::map container_; +}; +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h new file mode 100644 index 0000000000..789cafa88e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef JOINT_LIMITS_EXTENSION_H +#define JOINT_LIMITS_EXTENSION_H + +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +namespace joint_limits_interface +{ +/** + * @brief Extends joint_limits_interface::JointLimits with a deceleration + * parameter + */ +struct JointLimits : ::joint_limits_interface::JointLimits +{ + JointLimits() : max_deceleration(0.0), has_deceleration_limits(false) + { + } + + /// maximum deceleration MUST(!) be negative + double max_deceleration; + + bool has_deceleration_limits; +}; +} // namespace joint_limits_interface + +typedef joint_limits_interface::JointLimits JointLimit; +typedef std::map JointLimitsMap; +} // namespace pilz_industrial_motion_planner + +#endif // JOINT_LIMITS_EXTENSION_H diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h new file mode 100644 index 0000000000..4791267e92 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h @@ -0,0 +1,101 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef JOINT_LIMITS_INTERFACE_EXTENSION_H +#define JOINT_LIMITS_INTERFACE_EXTENSION_H + +#include "pilz_industrial_motion_planner/joint_limits_extension.h" +#include + +namespace pilz_industrial_motion_planner +{ +namespace joint_limits_interface +{ +/** + * @see joint_limits_inteface::getJointLimits(...) + */ +inline bool getJointLimits(const std::string& joint_name, const ros::NodeHandle& nh, + joint_limits_interface::JointLimits& limits) +{ + // Node handle scoped where the joint limits are + // defined (copied from ::joint_limits_interface::getJointLimits(joint_name, + // nh, limits) + ros::NodeHandle limits_nh; + try + { + const std::string limits_namespace = "joint_limits/" + joint_name; + if (!nh.hasParam(limits_namespace)) + { + ROS_DEBUG_STREAM("No joint limits specification found for joint '" + << joint_name << "' in the parameter server (namespace " + << nh.getNamespace() + "/" + limits_namespace << ")."); + return false; + } + limits_nh = ros::NodeHandle(nh, limits_namespace); + } + catch (const ros::InvalidNameException& ex) + { + ROS_ERROR_STREAM(ex.what()); + return false; + } + + // Set the existing limits + if (!::joint_limits_interface::getJointLimits(joint_name, nh, limits)) + { + return false; // LCOV_EXCL_LINE // The case where getJointLimits returns + // false is covered above. + } + + // Deceleration limits + bool has_deceleration_limits = false; + if (limits_nh.getParam("has_deceleration_limits", has_deceleration_limits)) + { + if (!has_deceleration_limits) + { + limits.has_deceleration_limits = false; + } + double max_dec; + if (has_deceleration_limits && limits_nh.getParam("max_deceleration", max_dec)) + { + limits.has_deceleration_limits = true; + limits.max_deceleration = max_dec; + } + } + + return true; +} +} // namespace joint_limits_interface +} // namespace pilz_industrial_motion_planner + +#endif // JOINT_LIMITS_INTERFACE_EXTENSION_H diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h new file mode 100644 index 0000000000..8b95269266 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h @@ -0,0 +1,153 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/joint_limits_container.h" +#include "pilz_industrial_motion_planner/joint_limits_extension.h" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Validates the equality of all limits inside a container + */ +class JointLimitsValidator +{ +public: + /** + * @brief Validates that the position limits of all limits are equal + * @param joint_limits the joint limits + * @return true if all are equal + * @note always returns true if has_position_limits=false for all limits, or + * if the size of joint_limits is 0 or 1 + */ + static bool validateAllPositionLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + + /** + * @brief Validates that the velocity of all limits is equal + * @param joint_limits the joint limits + * @return true if all are equal + * @note always returns true if has_velocity_limits=false for all limits, or + * if the size of joint_limits is 0 or 1 + */ + static bool validateAllVelocityLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + + /** + * @brief Validates that the acceleration of all limits is equal + * @param joint_limits the joint limits + * @return true if all are equal + * @note always returns true if has_acceleration_limits=false for all limits, + * or if size of joint_limits is 0 or 1 + */ + static bool + validateAllAccelerationLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + + /** + * @brief Validates that the deceleration of all limits is equal + * @param joint_limits the joint limits + * @return true if all are equal + * @note always returns true if has_acceleration_limits=false for all limits, + * or if size of joint_limits is 0 or 1 + */ + static bool + validateAllDecelerationLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + +private: + static bool validateWithEqualFunc(bool (*eq_func)(const JointLimit&, const JointLimit&), + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + + static bool positionEqual(const JointLimit& lhs, const JointLimit& rhs); + + static bool velocityEqual(const JointLimit& lhs, const JointLimit& rhs); + + static bool accelerationEqual(const JointLimit& lhs, const JointLimit& rhs); + + static bool decelerationEqual(const JointLimit& lhs, const JointLimit& rhs); +}; + +/** + * @class ValidationException + * @brief A base class for all validations exceptions inheriting from + * std::runtime_exception + */ +class ValidationException : public std::runtime_error +{ +public: + ValidationException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; + +/** + * @class ValidationJointMissingException + * @brief Thrown the limits for a joint are defined in the urdf but not on the + * parameter server (loaded from yaml) + * + */ +class ValidationJointMissingException : public ValidationException +{ +public: + ValidationJointMissingException(const std::string& error_desc) : ValidationException(error_desc) + { + } +}; + +/** + * @class ValidationDifferentLimitsException + * @brief Thrown when the limits differ + * + */ +class ValidationDifferentLimitsException : public ValidationException +{ +public: + ValidationDifferentLimitsException(const std::string& error_desc) : ValidationException(error_desc) + { + } +}; + +/** + * @class ValidationBoundsViolationException + * @brief Thrown when the limits from the param server are weaker than the ones + * obtained from the urdf + * + */ +class ValidationBoundsViolationException : public ValidationException +{ +public: + ValidationBoundsViolationException(const std::string& error_desc) : ValidationException(error_desc) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h new file mode 100644 index 0000000000..79e9d3ba00 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h @@ -0,0 +1,104 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/cartesian_limit.h" +#include "pilz_industrial_motion_planner/joint_limits_container.h" +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief This class combines CartesianLimit and JointLimits into on single + * class. + */ +class LimitsContainer +{ +public: + LimitsContainer(); + + /** + * @brief Return if this LimitsContainer has defined joint limits + * @return True if container contains joint limits + */ + bool hasJointLimits() const; + + /** + * @brief Set joint limits + * @param joint_limits + */ + void setJointLimits(JointLimitsContainer& joint_limits); + + /** + * @brief Obtain the Joint Limits from the container + * @return the joint limits + */ + const JointLimitsContainer& getJointLimitContainer() const; + + /** + * @brief Return if this LimitsContainer has defined cartesian limits + * + * @return True if container contains cartesian limits including maximum + * velocity/acceleration/deceleration + */ + bool hasFullCartesianLimits() const; + + /** + * @brief Set cartesian limits + * @param cartesian_limit + */ + void setCartesianLimits(CartesianLimit& cartesian_limit); + + /** + * @brief Return the cartesian limits + * @return the cartesian limits + */ + const CartesianLimit& getCartesianLimits() const; + +private: + /// Flag if joint limits where set + bool has_joint_limits_; + + /// The joint limits + JointLimitsContainer joint_limits_; + + /// Flag if cartesian limits have been set + bool has_cartesian_limits_; + + /// The cartesian limits + CartesianLimit cartesian_limit_; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h new file mode 100644 index 0000000000..543c701579 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +class CommandListManager; + +/** + * @brief Provide action to handle multiple trajectories and execute the result + * in the form of a MoveGroup capability (plugin). + */ +class MoveGroupSequenceAction : public move_group::MoveGroupCapability +{ +public: + MoveGroupSequenceAction(); + + void initialize() override; + +private: + using ExecutableTrajs = std::vector; + + using StartStateMsg = moveit_msgs::MotionSequenceResponse::_sequence_start_type; + using StartStatesMsg = std::vector; + using PlannedTrajMsgs = moveit_msgs::MotionSequenceResponse::_planned_trajectories_type; + +private: + void executeSequenceCallback(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal); + void executeSequenceCallbackPlanAndExecute(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal, + moveit_msgs::MoveGroupSequenceResult& action_res); + void executeMoveCallbackPlanOnly(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal, + moveit_msgs::MoveGroupSequenceResult& res); + void startMoveExecutionCallback(); + void startMoveLookCallback(); + void preemptMoveCallback(); + void setMoveState(move_group::MoveGroupState state); + bool planUsingSequenceManager(const moveit_msgs::MotionSequenceRequest& req, + plan_execution::ExecutableMotionPlan& plan); + +private: + static void convertToMsg(const ExecutableTrajs& trajs, StartStatesMsg& startStatesMsg, + PlannedTrajMsgs& plannedTrajsMsgs); + +private: + std::unique_ptr> move_action_server_; + moveit_msgs::MoveGroupSequenceFeedback move_feedback_; + + move_group::MoveGroupState move_state_{ move_group::IDLE }; + std::unique_ptr command_list_manager_; +}; +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h new file mode 100644 index 0000000000..54c0af0f61 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h @@ -0,0 +1,66 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +// Forward declarations +class CommandListManager; + +/** + * @brief Provide service to blend multiple trajectories in the form of a + * MoveGroup capability (plugin). + */ +class MoveGroupSequenceService : public move_group::MoveGroupCapability +{ +public: + MoveGroupSequenceService(); + ~MoveGroupSequenceService() override; + + void initialize() override; + +private: + bool plan(moveit_msgs::GetMotionSequence::Request& req, moveit_msgs::GetMotionSequence::Response& res); + +private: + ros::ServiceServer sequence_service_; + std::unique_ptr command_list_manager_; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.h new file mode 100644 index 0000000000..b2f13f7a84 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.h @@ -0,0 +1,101 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Generator class for KDL::Path_Circle from different circle + * representations + */ +class PathCircleGenerator +{ +public: + /** + * @brief set the path circle from start, goal and center point + * + * Note that a half circle should use interim point and cannot be defined + * by circle center since start/goal/center points are colinear. + * @throws KDL::Error_MotionPlanning in case start and goal have different + * radii to the center point. + */ + static std::unique_ptr circleFromCenter(const KDL::Frame& start_pose, const KDL::Frame& goal_pose, + const KDL::Vector& center_point, double eqradius); + + /** + * @brief set circle from start, goal and interim point + + * @throws KDL::Error_MotionPlanning if the given points are colinear. + */ + static std::unique_ptr circleFromInterim(const KDL::Frame& start_pose, const KDL::Frame& goal_pose, + const KDL::Vector& interim_point, double eqradius); + +private: + PathCircleGenerator(){}; // no instantiation of this helper class! + + /** + * @brief law of cosines: returns angle gamma in cΒ² = aΒ²+bΒ²-2ab cos(gamma) + * + * @return angle in radians + */ + static double cosines(const double a, const double b, const double c); + + static constexpr double MAX_RADIUS_DIFF{ 1e-2 }; + static constexpr double MAX_COLINEAR_NORM{ 1e-5 }; +}; + +} // namespace pilz_industrial_motion_planner + +class ErrorMotionPlanningCenterPointDifferentRadius : public KDL::Error_MotionPlanning +{ +public: + const char* Description() const override + { + return "Distances between start-center and goal-center are different." + " A circle cannot be created."; + } + int GetType() const override + { + return ERROR_CODE_CENTER_POINT_DIFFERENT_RADIUS; + } // LCOV_EXCL_LINE + +private: + static constexpr int ERROR_CODE_CENTER_POINT_DIFFERENT_RADIUS{ 3006 }; +}; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h new file mode 100644 index 0000000000..13ae41e67e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h @@ -0,0 +1,139 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +#include "pilz_industrial_motion_planner/joint_limits_extension.h" +#include "pilz_industrial_motion_planner/planning_context_loader.h" + +#include +#include + +#include + +// Boost includes +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Moveit Plugin for Planning with Standart Robot Commands + * This planner is dedicated to return a instance of PlanningContext that + * corresponds to the requested motion command + * set as planner_id in the MotionPlanRequest). + * It can be easily extended with additional commands by creating a class + * inherting from PlanningContextLoader. + */ +class CommandPlanner : public planning_interface::PlannerManager +{ +public: + ~CommandPlanner() override + { + } + + /** + * @brief Initializes the planner + * Upon initialization this planner will look for plugins implementing + * pilz_industrial_motion_planner::PlanningContextLoader. + * @param model The robot model + * @param ns The namespace + * @return true on success, false otherwise + */ + bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns) override; + + /// Description of the planner + std::string getDescription() const override; + + /** + * @brief Returns the available planning commands + * @param list with the planning algorithms + * @note behined each command is a + * pilz_industrial_motion_planner::PlanningContextLoader loaded as plugin + */ + void getPlanningAlgorithms(std::vector& algs) const override; + + /** + * @brief Returns a PlanningContext that can be used to solve(calculate) the + * trajectory that corresponds to command + * given in motion request as planner_id. + * @param planning_scene + * @param req + * @param error_code + * @return + */ + planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::MoveItErrorCodes& error_code) const override; + + /** + * @brief Checks if the request can be handled + * @param motion request containing the planning_id that corresponds to the + * motion command + * @return true if the request can be handled + */ + bool canServiceRequest(const planning_interface::MotionPlanRequest& req) const override; + + /** + * @brief Register a PlanningContextLoader to be used by the CommandPlanner + * @param planning_context_loader + * @throw ContextLoaderRegistrationException if a loader with the same + * algorithm name is already registered + */ + void registerContextLoader(const pilz_industrial_motion_planner::PlanningContextLoaderPtr& planning_context_loader); + +private: + /// Plugin loader + boost::scoped_ptr> planner_context_loader; + + /// Mapping from command to loader + std::map context_loader_map_; + + /// Robot model obtained at initialize + moveit::core::RobotModelConstPtr model_; + + /// Namespace where the parameters are stored, obtained at initialize + std::string namespace_; + + /// aggregated limits of the active joints + pilz_industrial_motion_planner::JointLimitsContainer aggregated_limit_active_joints_; + + /// cartesian limit + pilz_industrial_motion_planner::CartesianLimit cartesian_limit_; +}; + +MOVEIT_CLASS_FORWARD(CommandPlanner) + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h new file mode 100644 index 0000000000..a89b3fbbd3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h @@ -0,0 +1,158 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include + +#include +#include + +#include "pilz_industrial_motion_planner/trajectory_blend_request.h" +#include "pilz_industrial_motion_planner/trajectory_blender.h" +#include "pilz_industrial_motion_planner/trajectory_functions.h" +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" + +namespace pilz_industrial_motion_planner +{ +using TipFrameFunc_t = std::function; + +// List of exceptions which can be thrown by the PlanComponentsBuilder class. +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoBlenderSetException, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoTipFrameFunctionSetException, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoRobotModelSetException, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(BlendingFailedException, moveit_msgs::MoveItErrorCodes::FAILURE); + +/** + * @brief Helper class to encapsulate the merge and blend process of + * trajectories. + */ +class PlanComponentsBuilder +{ +public: + /** + * @brief Sets the blender used to blend two trajectories. + */ + void setBlender(std::unique_ptr blender); + + /** + * @brief Sets the robot model needed to create new trajectory elements. + */ + void setModel(const moveit::core::RobotModelConstPtr& model); + + /** + * @brief Appends the specified trajectory to the trajectory container + * under construction. + * + * The appending complies to the following rules: + * - A trajectory is simply added/attached to the previous trajectory: + * - if they are from the same group and + * - if the specified blend_radius is zero. + * - A trajectory is blended together with the previous trajectory: + * - if they are from the same group and + * - if the specified blend_radius is GREATER than zero. + * - A new trajectory element is created and the given trajectory is + * appended/attached to the newly created empty trajectory: + * - if the given and previous trajectory are from different groups. + * + * @param other Trajectories which has to be added to the trajectory container + * under construction. + * + * @param blend_radius The blending radius between the previous and the + * specified trajectory. + */ + void append(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius); + + /** + * @brief Clears the trajectory container under construction. + */ + void reset(); + + /** + * @return The final trajectory container which results from the append calls. + */ + std::vector build() const; + +private: + void blend(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius); + +private: + /** + * @brief Appends a trajectory to a result trajectory leaving out the + * first point, if it matches the last point of the result trajectory. + * + * @note Controllers, so far at least the + * ros_controllers::JointTrajectoryController require a timewise strictly + * increasing trajectory. If through appending the last point of the + * original trajectory gets repeated, it is removed here. + */ + static void appendWithStrictTimeIncrease(robot_trajectory::RobotTrajectory& result, + const robot_trajectory::RobotTrajectory& source); + +private: + //! Blender used to blend two trajectories. + std::unique_ptr blender_; + + //! Robot model needed to create new trajectory container elements. + moveit::core::RobotModelConstPtr model_; + + //! The previously added trajectory. + robot_trajectory::RobotTrajectoryPtr traj_tail_; + + //! The trajectory container under construction. + std::vector traj_cont_; + +private: + //! Constant to check for equality of variables of two RobotState instances. + static constexpr double ROBOT_STATE_EQUALITY_EPSILON = 1e-4; +}; + +inline void PlanComponentsBuilder::setBlender(std::unique_ptr blender) +{ + blender_ = std::move(blender); +} + +inline void PlanComponentsBuilder::setModel(const moveit::core::RobotModelConstPtr& model) +{ + model_ = model; +} + +inline void PlanComponentsBuilder::reset() +{ + traj_tail_ = nullptr; + traj_cont_.clear(); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h new file mode 100644 index 0000000000..14143a0ed8 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h @@ -0,0 +1,181 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/joint_limits_container.h" +#include "pilz_industrial_motion_planner/trajectory_generator.h" + +#include + +#include +#include +#include +#include + +#include +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief PlanningContext for obtaining trajectories + */ +template +class PlanningContextBase : public planning_interface::PlanningContext +{ +public: + PlanningContextBase(const std::string& name, const std::string& group, + const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : planning_interface::PlanningContext(name, group) + , terminated_(false) + , model_(model) + , limits_(limits) + , generator_(model, limits_) + { + } + + ~PlanningContextBase() override + { + } + + /** + * @brief Calculates a trajectory for the request this context is currently + * set for + * @param res The result containing the respective trajectory, or error_code + * on failure + * @return true on success, false otherwise + */ + bool solve(planning_interface::MotionPlanResponse& res) override; + + /** + * @brief Will return the same trajectory as + * solve(planning_interface::MotionPlanResponse& res) + * This function just delegates to the common response however here the same + * trajectory is stored with the + * descriptions "plan", "simplify", "interpolate" + * @param res The detailed response + * @return true on success, false otherwise + */ + bool solve(planning_interface::MotionPlanDetailedResponse& res) override; + + /** + * @brief Will terminate solve() + * @return + * @note Currently will not stop a runnning solve but not start future solves. + */ + bool terminate() override; + + /** + * @copydoc planning_interface::PlanningContext::clear() + */ + void clear() override; + + /// Flag if terminated + std::atomic_bool terminated_; + + /// The robot model + robot_model::RobotModelConstPtr model_; + + /// Joint limits to be used during planning + pilz_industrial_motion_planner::LimitsContainer limits_; + +protected: + GeneratorT generator_; +}; + +template +bool pilz_industrial_motion_planner::PlanningContextBase::solve(planning_interface::MotionPlanResponse& res) +{ + if (!terminated_) + { + // Use current state as start state if not set + if (request_.start_state.joint_state.name.empty()) + { + moveit_msgs::RobotState current_state; + moveit::core::robotStateToRobotStateMsg(getPlanningScene()->getCurrentState(), current_state); + request_.start_state = current_state; + } + bool result = generator_.generate(request_, res); + return result; + // res.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + // return false; // TODO + } + + ROS_ERROR("Using solve on a terminated planning context!"); + res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; + return false; +} + +template +bool pilz_industrial_motion_planner::PlanningContextBase::solve( + planning_interface::MotionPlanDetailedResponse& res) +{ + // delegate to regular response + planning_interface::MotionPlanResponse undetailed_response; + bool result = solve(undetailed_response); + + res.description_.push_back("plan"); + res.trajectory_.push_back(undetailed_response.trajectory_); + res.processing_time_.push_back(undetailed_response.planning_time_); + + res.description_.push_back("simplify"); + res.trajectory_.push_back(undetailed_response.trajectory_); + res.processing_time_.push_back(0); + + res.description_.push_back("interpolate"); + res.trajectory_.push_back(undetailed_response.trajectory_); + res.processing_time_.push_back(0); + + res.error_code_ = undetailed_response.error_code_; + return result; +} + +template +bool pilz_industrial_motion_planner::PlanningContextBase::terminate() +{ + ROS_DEBUG_STREAM("Terminate called"); + terminated_ = true; + return true; +} + +template +void pilz_industrial_motion_planner::PlanningContextBase::clear() +{ + // No structures that need cleaning + return; +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h new file mode 100644 index 0000000000..f9e69f4791 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/limits_container.h" + +#include + +#include +#include + +#include +#include + +#include "pilz_industrial_motion_planner/planning_context_base.h" +#include "pilz_industrial_motion_planner/trajectory_generator_circ.h" + +namespace pilz_industrial_motion_planner +{ +MOVEIT_CLASS_FORWARD(PlanningContext) + +/** + * @brief PlanningContext for obtaining CIRC trajectories + */ +class PlanningContextCIRC : public pilz_industrial_motion_planner::PlanningContextBase +{ +public: + PlanningContextCIRC(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h new file mode 100644 index 0000000000..d0990c3913 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h @@ -0,0 +1,65 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/limits_container.h" + +#include + +#include +#include + +#include +#include + +#include "pilz_industrial_motion_planner/planning_context_base.h" +#include "pilz_industrial_motion_planner/trajectory_generator_lin.h" + +namespace pilz_industrial_motion_planner +{ +MOVEIT_CLASS_FORWARD(PlanningContext) + +/** + * @brief PlanningContext for obtaining LIN trajectories + */ +class PlanningContextLIN : public pilz_industrial_motion_planner::PlanningContextBase +{ +public: + PlanningContextLIN(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h new file mode 100644 index 0000000000..bb0839ed1e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h @@ -0,0 +1,144 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/limits_container.h" + +#include +#include + +#include + +#include "pilz_industrial_motion_planner/limits_container.h" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Base class for all PlanningContextLoaders. + * Since planning_interface::PlanningContext has a non empty ctor classes + * derived from it can not be plugins. + * This class serves as base class for wrappers. + */ +class PlanningContextLoader +{ +public: + PlanningContextLoader(); + virtual ~PlanningContextLoader(); + + /// Return the algorithm the loader uses + virtual std::string getAlgorithm() const; + + /** + * @brief Sets the robot model that can be passed to the planning context + * @param model The robot model + * @return false if could not be set + */ + virtual bool setModel(const moveit::core::RobotModelConstPtr& model); + + /** + * @brief Sets limits the planner can pass to the contexts + * @param limits container of limits, no guarantee to contain the limits for + * all joints of the model + * @return true if limits could be set + */ + virtual bool setLimits(const pilz_industrial_motion_planner::LimitsContainer& limits); + + /** + * @brief Return the planning context + * @param planning_context + * @param name context name + * @param group name of the planning group + * @return true on success, false otherwise + */ + virtual bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const = 0; + +protected: + /** + * @brief Return the planning context of type T + * @param planning_context + * @param name context name + * @param group name of the planning group + * @return true on success, false otherwise + */ + template + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const; + +protected: + /// Name of the algorithm + std::string alg_; + + /// True if limits are set + bool limits_set_; + + /// Limits to be used during planning + pilz_industrial_motion_planner::LimitsContainer limits_; + + /// True if model is set + bool model_set_; + + /// The robot model + moveit::core::RobotModelConstPtr model_; +}; + +typedef boost::shared_ptr PlanningContextLoaderPtr; +typedef boost::shared_ptr PlanningContextLoaderConstPtr; + +template +bool PlanningContextLoader::loadContext(planning_interface::PlanningContextPtr& planning_context, + const std::string& name, const std::string& group) const +{ + if (limits_set_ && model_set_) + { + planning_context.reset(new T(name, group, model_, limits_)); + return true; + } + else + { + if (!limits_set_) + { + ROS_ERROR_STREAM("Limits are not defined. Cannot load planning context. " + "Call setLimits loadContext"); + } + if (!model_set_) + { + ROS_ERROR_STREAM("Robot model was not set"); + } + return false; + } +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.h new file mode 100644 index 0000000000..b1760aac2c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.h @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/planning_context_loader.h" + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Plugin that can generate instances of PlanningContextCIRC. + * Generates instances of PlanningContextLIN. + */ +class PlanningContextLoaderCIRC : public PlanningContextLoader +{ +public: + PlanningContextLoaderCIRC(); + ~PlanningContextLoaderCIRC() override; + + /** + * @brief return a instance of + * pilz_industrial_motion_planner::PlanningContextCIRC + * @param planning_context returned context + * @param name + * @param group + * @return true on success, false otherwise + */ + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const override; +}; + +typedef boost::shared_ptr PlanningContextLoaderCIRCPtr; +typedef boost::shared_ptr PlanningContextLoaderCIRCConstPtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.h new file mode 100644 index 0000000000..f3215fd869 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.h @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/planning_context_loader.h" + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Plugin that can generate instances of PlanningContextLIN. + * Generates instances of PlanningContextLIN. + */ +class PlanningContextLoaderLIN : public PlanningContextLoader +{ +public: + PlanningContextLoaderLIN(); + ~PlanningContextLoaderLIN() override; + + /** + * @brief return a instance of + * pilz_industrial_motion_planner::PlanningContextLIN + * @param planning_context returned context + * @param name + * @param group + * @return true on success, false otherwise + */ + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const override; +}; + +typedef boost::shared_ptr PlanningContextLoaderLINPtr; +typedef boost::shared_ptr PlanningContextLoaderLINConstPtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.h new file mode 100644 index 0000000000..52ec4f6af7 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.h @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/planning_context_loader.h" + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Plugin that can generate instances of PlanningContextPTP. + * Generates instances of PlanningContextPTP. + */ +class PlanningContextLoaderPTP : public PlanningContextLoader +{ +public: + PlanningContextLoaderPTP(); + ~PlanningContextLoaderPTP() override; + + /** + * @brief return a instance of + * pilz_industrial_motion_planner::PlanningContextPTP + * @param planning_context returned context + * @param name + * @param group + * @return true on success, false otherwise + */ + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const override; +}; + +typedef boost::shared_ptr PlanningContextLoaderPTPPtr; +typedef boost::shared_ptr PlanningContextLoaderPTPConstPtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h new file mode 100644 index 0000000000..842c350f19 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/limits_container.h" + +#include + +#include +#include + +#include +#include + +#include "pilz_industrial_motion_planner/planning_context_base.h" +#include "pilz_industrial_motion_planner/trajectory_generator_ptp.h" + +namespace pilz_industrial_motion_planner +{ +MOVEIT_CLASS_FORWARD(PlanningContext) + +/** + * @brief PlanningContext for obtaining PTP trajectories + */ +class PlanningContextPTP : public pilz_industrial_motion_planner::PlanningContextBase +{ +public: + PlanningContextPTP(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.h new file mode 100644 index 0000000000..5dfc44da29 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.h @@ -0,0 +1,71 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @class PlanningException + * @brief A base class for all pilz_industrial_motion_planner exceptions + * inheriting from std::runtime_exception + */ +class PlanningException : public std::runtime_error +{ +public: + PlanningException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; + +/** + * @class PlanningContextFactoryRegistrationException + * @brief An exception class thrown when the planner manager is unable to load a + * factory + * + * Loading a PlanningContextFactory can fail if a factory is loaded that + * would provide a command which was already provided by another factory loaded + * before. + */ +class ContextLoaderRegistrationException : public PlanningException +{ +public: + ContextLoaderRegistrationException(const std::string& error_desc) : PlanningException(error_desc) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h new file mode 100644 index 0000000000..e173bbde37 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" + +namespace pilz_industrial_motion_planner +{ +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoSolverException, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(MoreThanOneTipFrameException, moveit_msgs::MoveItErrorCodes::FAILURE); + +/** + * @returns true if the JointModelGroup has a solver, false otherwise. + * + * @tparam JointModelGroup aims at moveit::core::JointModelGroup + * @throws exception in case group is null. + */ +template +static bool hasSolver(const JointModelGroup* group) +{ + if (group == nullptr) + { + throw std::invalid_argument("Group must not be null"); + } + return group->getSolverInstance() != nullptr; +} + +/** + * @return The name of the tip frame (link) of the specified group + * returned by the solver. + * + * @tparam JointModelGroup aims at moveit::core::JointModelGroup + * @throws exception in case the group has no solver. + * @throws exception in case the solver for the group has more than one tip + * frame. + */ +template +static const std::string& getSolverTipFrame(const JointModelGroup* group) +{ + if (!hasSolver(group)) + { + throw NoSolverException("No solver for group " + group->getName()); + } + + const std::vector& tip_frames{ group->getSolverInstance()->getTipFrames() }; + if (tip_frames.size() > 1) + { + throw MoreThanOneTipFrameException("Solver for group \"" + group->getName() + "\" has more than one tip frame"); + } + return tip_frames.front(); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.h new file mode 100644 index 0000000000..450f89fea5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.h @@ -0,0 +1,59 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +struct TrajectoryBlendRequest +{ + // The name of the group of joints on which this blender is operating + std::string group_name; + + // The name of the target link on which this blender is operating + std::string link_name; + + // Robot trajectories to be blended + robot_trajectory::RobotTrajectoryPtr first_trajectory; + robot_trajectory::RobotTrajectoryPtr second_trajectory; + + // Blend radius in meter + double blend_radius; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h new file mode 100644 index 0000000000..880f2b777e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +struct TrajectoryBlendResponse +{ + // The name of the group of joints on which this blender is operating + std::string group_name; + + // Resulted robot trajectories after blending + robot_trajectory::RobotTrajectoryPtr first_trajectory; + robot_trajectory::RobotTrajectoryPtr blend_trajectory; + robot_trajectory::RobotTrajectoryPtr second_trajectory; + + // Error code + moveit_msgs::MoveItErrorCodes error_code; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h new file mode 100644 index 0000000000..ce647b5479 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/limits_container.h" +#include + +#include "pilz_industrial_motion_planner/trajectory_blend_request.h" +#include "pilz_industrial_motion_planner/trajectory_blend_response.h" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Base class of trajectory blenders + */ +class TrajectoryBlender +{ +public: + TrajectoryBlender(const pilz_industrial_motion_planner::LimitsContainer& planner_limits) : limits_(planner_limits) + { + } + + virtual ~TrajectoryBlender() + { + } + + /** + * @brief Blend two robot trajectories with the given blending radius + * @param req: trajectory blend request + * @param res: trajectroy blend response + * @return true if blend succeed + */ + virtual bool blend(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + pilz_industrial_motion_planner::TrajectoryBlendResponse& res) = 0; + +protected: + const pilz_industrial_motion_planner::LimitsContainer limits_; +}; + +typedef std::unique_ptr TrajectoryBlenderUniquePtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h new file mode 100644 index 0000000000..233bdd3e31 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h @@ -0,0 +1,178 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "pilz_industrial_motion_planner/cartesian_trajectory.h" +#include "pilz_industrial_motion_planner/cartesian_trajectory_point.h" +#include "pilz_industrial_motion_planner/trajectory_blend_request.h" +#include "pilz_industrial_motion_planner/trajectory_blender.h" +#include "pilz_industrial_motion_planner/trajectory_functions.h" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Trajectory blender implementing transition window algorithm + * + * See doc/MotionBlendAlgorithmDescription.pdf for a mathematical description of + * the algorithmn. + */ +class TrajectoryBlenderTransitionWindow : public TrajectoryBlender +{ +public: + TrajectoryBlenderTransitionWindow(const LimitsContainer& planner_limits) + : TrajectoryBlender::TrajectoryBlender(planner_limits) + { + } + + ~TrajectoryBlenderTransitionWindow() override + { + } + + /** + * @brief Blend two trajectories using transition window. The trajectories + * have to be equally and uniformly + * discretized. + * @param req: following fields need to be filled for a valid request: + * - group_name : name of the planning group + * - link_name : name of the target link + * - first_trajectory: Joint trajectory stops at end point. + * The last point must be the same as the first point + * of the second trajectory. + * - second trajectory: Joint trajectory stops at end point. + * The first point must be the same as the last point + * of the first trajectory. + * - blend_radius: The blend radius determines a sphere with the + * intersection point of the two trajectories + * as the center. Trajectory blending happens inside of + * this sphere. + * @param res: following fields are returned as response by the blend + * algorithm + * - group_name : name of the planning group + * - first_trajectory: Part of the first original trajectory which is + * outside of the blend sphere. + * - blend_trajectory: Joint trajectory connecting the first and second + * trajectories without stop. + * The first waypoint has non-zero time from start. + * - second trajectory: Part of the second original trajectory which is + * outside of the blend sphere. + * The first waypoint has non-zero time from start. + * error_code: information of failed blend + * @return true if succeed + */ + bool blend(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + pilz_industrial_motion_planner::TrajectoryBlendResponse& res) override; + +private: + /** + * @brief validate trajectory blend request + * @param req + * @param sampling_time: get the same sampling time of the two input + * trajectories + * @param error_code + * @return + */ + bool validateRequest(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time, + moveit_msgs::MoveItErrorCodes& error_code) const; + /** + * @brief searchBlendPoint + * @param req: trajectory blend request + * @param first_interse_index: index of the first point of the first + * trajectory that is inside the blend sphere + * @param second_interse_index: index of the last point of the second + * trajectory that is still inside the blend sphere + */ + bool searchIntersectionPoints(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + std::size_t& first_interse_index, std::size_t& second_interse_index) const; + + /** + * @brief Determine how the second trajectory should be aligned with the first + * trajectory for blend. + * Let tau_1 be the time of the first trajectory from the first_interse_index + * to the end and tau_2 the time of the + * second trajectory from the beginning to the second_interse_index: + * - if tau_1 > tau_2:
+ * align the end of the first trajectory with second_interse_index + * 

+   *    first traj:  |-------------|--------!--------------|
+   *    second traj:                        |--------------|-------------------|
+   *    blend phase:               |-----------------------|
+   *
+ * - else:
+ * align the first_interse_index with the beginning of the second + * trajectory + * 
+   *    first traj:  |-------------|-----------------------|
+   *    second traj: |-----------------------!----------|-------------------|
+   *    blend phase:               |----------------------------------|
+   *
+ * + * @param req: trajectory blend request + * @param first_interse_index: index of the intersection point between first + * trajectory and blend sphere + * @param second_interse_index: index of the intersection point between second + * trajectory and blend sphere + * @param blend_align_index: index on the first trajectory, to which the first + * point on the second trajectory should + * be aligned to for motion blend. It is now always same as + * first_interse_index + * @param blend_time: time of the motion blend period + */ + void determineTrajectoryAlignment(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + std::size_t first_interse_index, std::size_t second_interse_index, + std::size_t& blend_align_index) const; + + /** + * @brief blend two trajectories in Cartesian space, result in a + * MultiDOFJointTrajectory which consists + * of a list of transforms for the blend phase. + * @param req + * @param first_interse_index + * @param second_interse_index + * @param blend_begin_index + * @param sampling_time + * @param trajectory: the resulting blend trajectory inside the blending + * sphere + */ + void blendTrajectoryCartesian(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + const std::size_t first_interse_index, const std::size_t second_interse_index, + const std::size_t blend_align_index, double sampling_time, + pilz_industrial_motion_planner::CartesianTrajectory& trajectory) const; + +private: // static members + // Constant to check for equality of values. + static constexpr double EPSILON = 1e-4; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h new file mode 100644 index 0000000000..6a23e3f41e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h @@ -0,0 +1,219 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/cartesian_trajectory.h" +#include "pilz_industrial_motion_planner/limits_container.h" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief compute the inverse kinematics of a given pose, also check robot self + * collision + * @param robot_model: kinematic model of the robot + * @param group_name: name of planning group + * @param link_name: name of target link + * @param pose: target pose in IK solver Frame + * @param frame_id: reference frame of the target pose + * @param seed: seed state of IK solver + * @param solution: solution of IK + * @param check_self_collision: true to enable self collision checking after IK + * computation + * @param timeout: timeout for IK, if not set the default solver timeout is used + * @return true if succeed + */ +bool computePoseIK(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, + const std::string& link_name, const Eigen::Isometry3d& pose, const std::string& frame_id, + const std::map& seed, std::map& solution, + bool check_self_collision = true, const double timeout = 0.0); + +bool computePoseIK(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, + const std::string& link_name, const geometry_msgs::Pose& pose, const std::string& frame_id, + const std::map& seed, std::map& solution, + bool check_self_collision = true, const double timeout = 0.0); + +/** + * @brief compute the pose of a link at give robot state + * @param robot_model: kinematic model of the robot + * @param link_name: target link name + * @param joint_state: joint positons of this group + * @param pose: pose of the link in base frame of robot model + * @return true if succeed + */ +bool computeLinkFK(const robot_model::RobotModelConstPtr& robot_model, const std::string& link_name, + const std::map& joint_state, Eigen::Isometry3d& pose); + +bool computeLinkFK(const robot_model::RobotModelConstPtr& robot_model, const std::string& link_name, + const std::vector& joint_names, const std::vector& joint_positions, + Eigen::Isometry3d& pose); + +/** + * @brief verify the velocity/acceleration limits of current sample (based on + * backward difference computation) + * v(k) = [x(k) - x(k-1)]/[t(k) - t(k-1)] + * a(k) = [v(k) - v(k-1)]/[t(k) - t(k-2)]*2 + * @param position_last: position of last sample + * @param velocity_last: velocity of last sample + * @param position_current: position of current sample + * @param duration_last: duration of last sample + * @param duration_current: duration of current sample + * @param joint_limits: joint limits + * @return + */ +bool verifySampleJointLimits(const std::map& position_last, + const std::map& velocity_last, + const std::map& position_current, double duration_last, + double duration_current, const JointLimitsContainer& joint_limits); + +/** + * @brief Generate joint trajectory from a KDL Cartesian trajectory + * @param robot_model: robot kinematics model + * @param joint_limits: joint limits + * @param trajectory: KDL Cartesian trajectory + * @param group_name: name of the planning group + * @param link_name: name of the target robot link + * @param initial_joint_position: initial joint positions, needed for selecting + * the ik solution + * @param sampling_time: sampling time of the generated trajectory + * @param joint_trajectory: output as robot joint trajectory, first and last + * point will have zero velocity + * and acceleration + * @param error_code: detailed error information + * @param check_self_collision: check for self collision during creation + * @return true if succeed + */ +bool generateJointTrajectory(const robot_model::RobotModelConstPtr& robot_model, + const JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, + const std::string& group_name, const std::string& link_name, + const std::map& initial_joint_position, const double& sampling_time, + trajectory_msgs::JointTrajectory& joint_trajectory, + moveit_msgs::MoveItErrorCodes& error_code, bool check_self_collision = false); + +/** + * @brief Generate joint trajectory from a MultiDOFJointTrajectory + * @param trajectory: Cartesian trajectory + * @param info: motion plan information + * @param sampling_time + * @param joint_trajectory + * @param error_code + * @return true if succeed + */ +bool generateJointTrajectory(const robot_model::RobotModelConstPtr& robot_model, + const JointLimitsContainer& joint_limits, + const pilz_industrial_motion_planner::CartesianTrajectory& trajectory, + const std::string& group_name, const std::string& link_name, + const std::map& initial_joint_position, + const std::map& initial_joint_velocity, + trajectory_msgs::JointTrajectory& joint_trajectory, + moveit_msgs::MoveItErrorCodes& error_code, bool check_self_collision = false); + +/** + * @brief Determines the sampling time and checks that both trajectroies use the + * same sampling time. + * @return TRUE if the sampling time is equal between all given points (except + * the last two points + * of each trajectory), otherwise FALSE. + */ +bool determineAndCheckSamplingTime(const robot_trajectory::RobotTrajectoryPtr& first_trajectory, + const robot_trajectory::RobotTrajectoryPtr& second_trajectory, double EPSILON, + double& sampling_time); + +/** + * @brief Check if the two robot states have the same joint + * position/velocity/acceleration. + * + * @param joint_group_name The name of the joint group. + * @param epsilon Constants defining how close the joint + * position/velocity/acceleration have to be to be + * recognized as equal. + * + * @return True if joint positions, joint velocities and joint accelerations are + * equal, otherwise false. + */ +bool isRobotStateEqual(const robot_state::RobotState& state1, const robot_state::RobotState& state2, + const std::string& joint_group_name, double epsilon); + +/** + * @brief check if the robot state have zero velocity/acceleartion + * @param state + * @param group + * @param EPSILON + * @return + */ +bool isRobotStateStationary(const robot_state::RobotState& state, const std::string& group, double EPSILON); + +/** + * @brief Performs a linear search for the intersection point of the trajectory + * with the blending radius. + * @param center_position Center of blending sphere. + * @param r Radius of blending sphere. + * @param traj The trajectory. + * @param inverseOrder TRUE: Farthest element from blending sphere center is + * located at the + * smallest index of trajectroy. + * @param index The intersection index which has to be determined. + */ +bool linearSearchIntersectionPoint(const std::string& link_name, const Eigen::Vector3d& center_position, + const double& r, const robot_trajectory::RobotTrajectoryPtr& traj, bool inverseOrder, + std::size_t& index); + +bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p_current, const Eigen::Vector3d& p_next, + const double& r); + +/** + * @brief Checks if current robot state is in self collision. + * @param test_for_self_collision Flag to deactivate this check during IK. + * @param robot_model: robot kinematics model. + * @param state Robot state instance used for . + * @param group + * @param ik_solution + * @return + */ +bool isStateColliding(const bool test_for_self_collision, const moveit::core::RobotModelConstPtr& robot_model, + robot_state::RobotState* state, const robot_state::JointModelGroup* const group, + const double* const ik_solution); +} // namespace pilz_industrial_motion_planner + +void normalizeQuaternion(geometry_msgs::Quaternion& quat); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h new file mode 100644 index 0000000000..8348850687 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h @@ -0,0 +1,121 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Exception storing an moveit_msgs::MoveItErrorCodes value. + */ +class MoveItErrorCodeException : public std::runtime_error +{ +public: + MoveItErrorCodeException(const std::string& msg); + +protected: + MoveItErrorCodeException(const MoveItErrorCodeException&) = default; + MoveItErrorCodeException(MoveItErrorCodeException&&) = default; + ~MoveItErrorCodeException() override = default; + + MoveItErrorCodeException& operator=(const MoveItErrorCodeException&) = default; + MoveItErrorCodeException& operator=(MoveItErrorCodeException&&) = default; + +public: + virtual const moveit_msgs::MoveItErrorCodes::_val_type& getErrorCode() const = 0; +}; + +template +class TemplatedMoveItErrorCodeException : public MoveItErrorCodeException +{ +public: + TemplatedMoveItErrorCodeException(const std::string& msg); + TemplatedMoveItErrorCodeException(const std::string& msg, const moveit_msgs::MoveItErrorCodes::_val_type& error_code); + +public: + const moveit_msgs::MoveItErrorCodes::_val_type& getErrorCode() const override; + +private: + const moveit_msgs::MoveItErrorCodes::_val_type error_code_{ ERROR_CODE }; +}; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +inline MoveItErrorCodeException::MoveItErrorCodeException(const std::string& msg) : std::runtime_error(msg) +{ +} + +template +inline TemplatedMoveItErrorCodeException::TemplatedMoveItErrorCodeException(const std::string& msg) + : MoveItErrorCodeException(msg) +{ +} + +template +inline TemplatedMoveItErrorCodeException::TemplatedMoveItErrorCodeException( + const std::string& msg, const moveit_msgs::MoveItErrorCodes::_val_type& error_code) + : MoveItErrorCodeException(msg), error_code_(error_code) +{ +} + +template +inline const moveit_msgs::MoveItErrorCodes::_val_type& +TemplatedMoveItErrorCodeException::getErrorCode() const +{ + return error_code_; +} + +/* + * @brief Macro to automatically generate a derived class of + * the MoveItErrorCodeException class. + */ +#define CREATE_MOVEIT_ERROR_CODE_EXCEPTION(EXCEPTION_CLASS_NAME, ERROR_CODE) \ + class EXCEPTION_CLASS_NAME : public TemplatedMoveItErrorCodeException \ + { \ + public: \ + EXCEPTION_CLASS_NAME(const std::string& msg) : TemplatedMoveItErrorCodeException(msg) \ + { \ + } \ + \ + EXCEPTION_CLASS_NAME(const std::string& msg, const moveit_msgs::MoveItErrorCodes::_val_type& error_code) \ + : TemplatedMoveItErrorCodeException(msg, error_code) \ + { \ + } \ + } + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h new file mode 100644 index 0000000000..09055fcb81 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -0,0 +1,291 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/joint_limits_extension.h" +#include "pilz_industrial_motion_planner/limits_container.h" +#include "pilz_industrial_motion_planner/trajectory_functions.h" +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" + +using namespace pilz_industrial_motion_planner; + +namespace pilz_industrial_motion_planner +{ +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(TrajectoryGeneratorInvalidLimitsException, moveit_msgs::MoveItErrorCodes::FAILURE); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(VelocityScalingIncorrect, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(AccelerationScalingIncorrect, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownPlanningGroup, moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoJointNamesInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(SizeMismatchInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointsOfStartStateOutOfRange, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NonZeroVelocityInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NotExactlyOneGoalConstraintGiven, + moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OnlyOneGoalTypeAllowed, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateGoalStateMismatch, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointConstraintDoesNotBelongToGroup, + moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointsOfGoalOutOfRange, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PositionConstraintNameMissing, + moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OrientationConstraintNameMissing, + moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PositionOrientationConstraintNameMismatch, + moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoIKSolverAvailable, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePoseGiven, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + +/** + * @brief Base class of trajectory generators + * + * Note: All derived classes cannot have a start velocity + */ +class TrajectoryGenerator +{ +public: + TrajectoryGenerator(const robot_model::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits) + : robot_model_(robot_model), planner_limits_(planner_limits) + { + } + + virtual ~TrajectoryGenerator() = default; + + /** + * @brief generate robot trajectory with given sampling time + * @param req: motion plan request + * @param res: motion plan response + * @param sampling_time: sampling time of the generate trajectory + * @return motion plan succeed/fail, detailed information in motion plan + * responce + */ + bool generate(const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, + double sampling_time = 0.1); + +protected: + /** + * @brief This class is used to extract needed information from motion plan + * request. + */ + class MotionPlanInfo + { + public: + std::string group_name; + std::string link_name; + Eigen::Isometry3d start_pose; + Eigen::Isometry3d goal_pose; + std::map start_joint_position; + std::map goal_joint_position; + std::pair circ_path_point; + }; + + /** + * @brief build cartesian velocity profile for the path + * + * Uses the path to get the cartesian length and the angular distance from + * start to goal. + * The trap profile returns uses the longer distance of translational and + * rotational motion. + */ + std::unique_ptr cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor, + const double& max_acceleration_scaling_factor, + const std::unique_ptr& path) const; + +private: + virtual void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const; + + /** + * @brief Extract needed information from a motion plan request in order to + * simplify + * further usages. + * @param req: motion plan request + * @param info: information extracted from motion plan request which is + * necessary for the planning + */ + virtual void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const = 0; + + virtual void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) = 0; + +private: + /** + * @brief Validate the motion plan request based on the common requirements of + * trajectroy generator + * Checks that: + * - req.max_velocity_scaling_factor [0.0001, 1], + * moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN on failure + * - req.max_acceleration_scaling_factor [0.0001, 1] , + * moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN on + * failure + * - req.group_name is a JointModelGroup of the Robotmodel, + * moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME on + * failure + * - req.start_state.joint_state is not empty, + * moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE on failure + * - req.start_state.joint_state is within the limits, + * moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE on + * failure + * - req.start_state.joint_state is all zero, + * moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE on failure + * - req.goal_constraints must have exactly 1 defined cartesian oder joint + * constraint + * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * A joint goal is checked for: + * - StartState joint-names matching goal joint-names, + * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on + * failure + * - Beeing defined in the req.group_name JointModelGroup + * - Beeing with the defined limits + * A cartesian goal is checked for: + * - A defined link_name for the constraint, + * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * - Matching link_name for position and orientation constraints, + * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * - A IK solver exists for the given req.group_name and constraint + * link_name, + * moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION on failure + * - A goal pose define in + * position_constraints[0].constraint_region.primitive_poses, + * moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * @param req: motion plan request + */ + void validateRequest(const planning_interface::MotionPlanRequest& req) const; + + /** + * @brief set MotionPlanResponse from joint trajectory + */ + void setSuccessResponse(const std::string& group_name, const moveit_msgs::RobotState& start_state, + const trajectory_msgs::JointTrajectory& joint_trajectory, const ros::Time& planning_start, + planning_interface::MotionPlanResponse& res) const; + + void setFailureResponse(const ros::Time& planning_start, planning_interface::MotionPlanResponse& res) const; + + void checkForValidGroupName(const std::string& group_name) const; + + /** + * @brief Validate that the start state of the request matches the + * requirements of the trajectory generator. + * + * These requirements are: + * - Names of the joints and given joint position match in size and are + * non-zero + * - The start state is withing the position limits + * - The start state velocity is below + * TrajectoryGenerator::VELOCITY_TOLERANCE + */ + void checkStartState(const moveit_msgs::RobotState& start_state) const; + + void checkGoalConstraints(const moveit_msgs::MotionPlanRequest::_goal_constraints_type& goal_constraints, + const std::vector& expected_joint_names, const std::string& group_name) const; + + void checkJointGoalConstraint(const moveit_msgs::Constraints& constraint, + const std::vector& expected_joint_names, + const std::string& group_name) const; + + void checkCartesianGoalConstraint(const moveit_msgs::Constraints& constraint, const std::string& group_name) const; + + void convertToRobotTrajectory(const trajectory_msgs::JointTrajectory& joint_trajectory, + const moveit_msgs::RobotState& start_state, + robot_trajectory::RobotTrajectory& robot_trajectory) const; + +private: + /** + * @return True if scaling factor is valid, otherwise false. + */ + static bool isScalingFactorValid(const double& scaling_factor); + static void checkVelocityScaling(const double& scaling_factor); + static void checkAccelerationScaling(const double& scaling_factor); + + /** + * @return True if ONE position + ONE orientation constraint given, + * otherwise false. + */ + static bool isCartesianGoalGiven(const moveit_msgs::Constraints& constraint); + + /** + * @return True if joint constraint given, otherwise false. + */ + static bool isJointGoalGiven(const moveit_msgs::Constraints& constraint); + + /** + * @return True if ONLY joint constraint or + * ONLY cartesian constraint (position+orientation) given, otherwise false. + */ + static bool isOnlyOneGoalTypeGiven(const moveit_msgs::Constraints& constraint); + +protected: + const robot_model::RobotModelConstPtr robot_model_; + const pilz_industrial_motion_planner::LimitsContainer planner_limits_; + static constexpr double MIN_SCALING_FACTOR{ 0.0001 }; + static constexpr double MAX_SCALING_FACTOR{ 1. }; + static constexpr double VELOCITY_TOLERANCE{ 1e-8 }; +}; + +inline bool TrajectoryGenerator::isScalingFactorValid(const double& scaling_factor) +{ + return (scaling_factor > MIN_SCALING_FACTOR && scaling_factor <= MAX_SCALING_FACTOR); +} + +inline bool TrajectoryGenerator::isCartesianGoalGiven(const moveit_msgs::Constraints& constraint) +{ + return constraint.position_constraints.size() == 1 && constraint.orientation_constraints.size() == 1; +} + +inline bool TrajectoryGenerator::isJointGoalGiven(const moveit_msgs::Constraints& constraint) +{ + return !constraint.joint_constraints.empty(); +} + +inline bool TrajectoryGenerator::isOnlyOneGoalTypeGiven(const moveit_msgs::Constraints& constraint) +{ + return (isJointGoalGiven(constraint) && !isCartesianGoalGiven(constraint)) || + (!isJointGoalGiven(constraint) && isCartesianGoalGiven(constraint)); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h new file mode 100644 index 0000000000..080239a581 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h @@ -0,0 +1,107 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include +#include + +#include "pilz_industrial_motion_planner/trajectory_generator.h" + +using namespace pilz_industrial_motion_planner; + +namespace pilz_industrial_motion_planner +{ +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleNoPlane, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleToSmall, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CenterPointDifferentRadius, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircTrajectoryConversionFailure, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownPathConstraintName, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPositionConstraints, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePose, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownLinkNameOfAuxiliaryPoint, moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NumberOfConstraintsMismatch, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircJointMissingInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircInverseForGoalIncalculable, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + +/** + * @brief This class implements a trajectory generator of arcs in Cartesian + * space. + * The arc is specified by a start pose, a goal pose and a interim point on the + * arc, + * or a point as the center of the circle which forms the arc. Complete circle + * is not + * covered by this generator. + */ +class TrajectoryGeneratorCIRC : public TrajectoryGenerator +{ +public: + /** + * @brief Constructor of CIRC Trajectory Generator. + * + * @param planner_limits Limits in joint and Cartesian spaces + * + * @throw TrajectoryGeneratorInvalidLimitsException + * + */ + TrajectoryGeneratorCIRC(const robot_model::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + +private: + void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override; + + void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; + + void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) override; + + /** + * @brief Construct a KDL::Path object for a Cartesian path of an arc. + * + * @return A unique pointer of the path object, null_ptr in case of an error. + * + * @throws CircleNoPlane if the plane in which the circle resides, + * could not be determined. + * + * @throws CircleToSmall if the specified circ radius is to small. + * + * @throws CenterPointDifferentRadius if the distances between start-center + * and goal-center are different. + */ + std::unique_ptr setPathCIRC(const MotionPlanInfo& info) const; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h new file mode 100644 index 0000000000..2d17768cad --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h @@ -0,0 +1,85 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include + +#include "pilz_industrial_motion_planner/trajectory_generator.h" +#include "pilz_industrial_motion_planner/velocity_profile_atrap.h" + +using namespace pilz_industrial_motion_planner; + +namespace pilz_industrial_motion_planner +{ +// TODO date type of units + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::MoveItErrorCodes::FAILURE); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinJointMissingInStartState, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + +/** + * @brief This class implements a linear trajectory generator in Cartesian + * space. + * The Cartesian trajetory are based on trapezoid velocity profile. + */ +class TrajectoryGeneratorLIN : public TrajectoryGenerator +{ +public: + /** + * @brief Constructor of LIN Trajectory Generator + * @throw TrajectoryGeneratorInvalidLimitsException + * @param model: robot model + * @param planner_limits: limits in joint and Cartesian spaces + */ + TrajectoryGeneratorLIN(const robot_model::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + +private: + void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; + + void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) override; + + /** + * @brief construct a KDL::Path object for a Cartesian straight line + * @return a unique pointer of the path object. null_ptr in case of an error. + */ + std::unique_ptr setPathLIN(const Eigen::Affine3d& start_pose, const Eigen::Affine3d& goal_pose) const; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h new file mode 100644 index 0000000000..09048557ba --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h @@ -0,0 +1,94 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "eigen3/Eigen/Eigen" +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" +#include "pilz_industrial_motion_planner/trajectory_generator.h" +#include "pilz_industrial_motion_planner/velocity_profile_atrap.h" + +using namespace pilz_industrial_motion_planner; + +namespace pilz_industrial_motion_planner +{ +// TODO date type of units + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PtpVelocityProfileSyncFailed, moveit_msgs::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PtpNoIkSolutionForGoalPose, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + +/** + * @brief This class implements a point-to-point trajectory generator based on + * VelocityProfileATrap. + */ +class TrajectoryGeneratorPTP : public TrajectoryGenerator +{ +public: + /** + * @brief Constructor of PTP Trajectory Generator + * @throw TrajectoryGeneratorInvalidLimitsException + * @param model: a map of joint limits information + */ + TrajectoryGeneratorPTP(const robot_model::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + +private: + void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const override; + + /** + * @brief plan ptp joint trajectory with zero start velocity + * @param start_pos + * @param goal_pos + * @param joint_trajectory + * @param group_name + * @param velocity_scaling_factor + * @param acceleration_scaling_factor + * @param sampling_time + */ + void planPTP(const std::map& start_pos, const std::map& goal_pos, + trajectory_msgs::JointTrajectory& joint_trajectory, const std::string& group_name, + const double& velocity_scaling_factor, const double& acceleration_scaling_factor, + const double& sampling_time); + + void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) override; + +private: + const double MIN_MOVEMENT = 0.001; + pilz_industrial_motion_planner::JointLimitsContainer joint_limits_; + // most strict joint limits for each group + std::map most_strict_limits_; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.h new file mode 100644 index 0000000000..2d71ccbb4b --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.h @@ -0,0 +1,215 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "kdl/velocityprofile.hpp" +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief A PTP Trajectory Generator of Asymmetric Trapezoidal Velocity Profile. + * Differences to VelocityProfile_Trap: + * - Maximal acceleration and deceleration can be different, resulting + * an asymmetric trapezoid shaped velocity profile. + * - Function to generate full synchronized PTP trajectory is provided. + * - Function to generate trapezoid shaped velocity profile with start + * velocity. + */ +class VelocityProfileATrap : public KDL::VelocityProfile +{ +public: + /** + * @brief Constructor + * @param max_vel: maximal velocity (absolute value, always positive) + * @param max_acc: maximal acceleration (absolute value, always positive) + * @param max_dec: maximal deceleration (absolute value, always positive) + */ + VelocityProfileATrap(double max_vel = 0, double max_acc = 0, double max_dec = 0); + + /** + * @brief compute the fastest profile + * Algorithm: + * - compute the minimal distance which is needed to reach maximal velocity + * - if maximal velocity can be reached + * - compute the coefficients of the trajectory + * - if maximal velocity can not be reached + * - compute the new velocity can be reached + * - compute the coefficients based on this new velocity + * + * @param pos1: start position + * @param pos2: goal position + */ + void SetProfile(double pos1, double pos2) override; + + /** + * @brief Profile scaled by the total duration + * @param pos1: start position + * @param pos2: goal position + * @param duration: trajectory duration (must be longer than fastest case, + * otherwise will be ignored) + */ + void SetProfileDuration(double pos1, double pos2, double duration) override; + + /** + * @brief Profile with given acceleration/constant/deceleration durations. + * Each duration must obey the maximal velocity/acceleration/deceleration + * constraints. + * Otherwise the operation will be ignored. + * Algorithm: + * - compute the maximal velocity of given durations + * - compute the acceleration and deceleration of given duraitons + * - if limits are fulfilled + * - compute the coefficients + * @param pos1: start position + * @param pos2: goal position + * @param acc_duration: time of acceleration phase + * @param const_duration: time of constant phase + * @param dec_duration: time of deceleration phase + * @return ture if the combination of three durations is valid + */ + bool setProfileAllDurations(double pos1, double pos2, double duration1, double duration2, double duration3); + + /** + * @brief Profile with start velocity + * Note: This function is not general and is currently only used for live + * control (vel1*(pos2-pos1)>0). + * @param pos1: start position + * @param pos2: goal position + * @param vel1: start velocity + * @return + */ + bool setProfileStartVelocity(double pos1, double pos2, double vel1); + + /** + * @brief get the time of first phase + * @return + */ + double firstPhaseDuration() const + { + return t_a_; + } + /** + * @brief get the time of second phase + * @return + */ + double secondPhaseDuration() const + { + return t_b_; + } + /** + * @brief get the time of third phase + * @return + */ + double thirdPhaseDuration() const + { + return t_c_; + } + + /** + * @brief Compares two Asymmetric Trapezoidal Velocity Profiles. + * + * @return True if equal, false otherwise. + */ + bool operator==(const VelocityProfileATrap& other) const; + + /** + * @brief Duration + * @return total duration of the trajectory + */ + double Duration() const override; + /** + * @brief Get position at given time + * @param time + * @return + */ + double Pos(double time) const override; + /** + * @brief Get velocity at given time + * @param time + * @return + */ + double Vel(double time) const override; + /** + * @brief Get given acceleration/deceleration at given time + * @param time + * @return + */ + double Acc(double time) const override; + /** + * @brief Write basic information + * @param os + */ + void Write(std::ostream& os) const override; + /** + * @brief returns copy of current VelocityProfile object + * @return + */ + KDL::VelocityProfile* Clone() const override; + + friend std::ostream& operator<<(std::ostream& os, const VelocityProfileATrap& p); // LCOV_EXCL_LINE + + ~VelocityProfileATrap() override; + +private: + /// helper functions + void setEmptyProfile(); + +private: + /// specification of the motion profile : + const double max_vel_; + const double max_acc_; + const double max_dec_; + double start_pos_; + double end_pos_; + + /// for initial velocity + double start_vel_; + + /// three phases of trapezoid + double a1_, a2_, a3_; /// coef. from ^0 -> ^2 of first phase + double b1_, b2_, b3_; /// of second phase + double c1_, c2_, c3_; /// of third phase + + /// time of three phases + double t_a_; /// the duration of first phase + double t_b_; /// the duration of second phase + double t_c_; /// the duration of third phase +}; + +std::ostream& operator<<(std::ostream& os, + const VelocityProfileATrap& p); // LCOV_EXCL_LINE + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml new file mode 100644 index 0000000000..5c4635686f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -0,0 +1,56 @@ + + + pilz_industrial_motion_planner + 1.1.1 + MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof. + + Alexander Gutenkunst + Christian Henkel + Immanuel Martini + Joachim Schleicher + Hagen Slusarek + + BSD + + http://moveit.ros.org + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + eigen_conversions + joint_limits_interface + kdl_conversions + moveit_ros_planning_interface + moveit_msgs + moveit_core + moveit_ros_planning + moveit_ros_move_group + orocos_kdl + liborocos-kdl-dev + pluginlib + roscpp + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros + + + rostest + rosunit + cmake_modules + moveit_resources_panda_moveit_config + pilz_industrial_motion_planner_testutils + moveit_resources_prbt_moveit_config + moveit_resources_prbt_support + moveit_resources_prbt_pg70_support + code_coverage + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/plugins/pilz_industrial_motion_planner_plugin_description.xml b/moveit_planners/pilz_industrial_motion_planner/plugins/pilz_industrial_motion_planner_plugin_description.xml new file mode 100644 index 0000000000..3a6f16cf50 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/plugins/pilz_industrial_motion_planner_plugin_description.xml @@ -0,0 +1,5 @@ + + + Pilz Industrial Motion Planner + + diff --git a/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml b/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml new file mode 100644 index 0000000000..0ff208f2a2 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml @@ -0,0 +1,18 @@ + + + Loader for PTP Context + + + + + Loader for LIN Context + + + + + Loader for CIRC Context + + diff --git a/moveit_planners/pilz_industrial_motion_planner/plugins/sequence_capability_plugin_description.xml b/moveit_planners/pilz_industrial_motion_planner/plugins/sequence_capability_plugin_description.xml new file mode 100644 index 0000000000..9daf67146c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/plugins/sequence_capability_plugin_description.xml @@ -0,0 +1,13 @@ + + + Motion Planner a sequence of trajectories + + + + Plan a sequence of trajectory via ROS serivce + + diff --git a/moveit_planners/pilz_industrial_motion_planner/rosdoc.yaml b/moveit_planners/pilz_industrial_motion_planner/rosdoc.yaml new file mode 100644 index 0000000000..119e416808 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/rosdoc.yaml @@ -0,0 +1,6 @@ +- builder: doxygen + name: Documentation of pilz_industrial_motion_planner package + output_dir: pilz_industrial_motion_planner + file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox *.md' + exclude_patterns: 'test' + use_mdfile_as_mainpage: 'README.md' diff --git a/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limit.cpp b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limit.cpp new file mode 100644 index 0000000000..3d67a9be13 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limit.cpp @@ -0,0 +1,119 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/cartesian_limit.h" + +pilz_industrial_motion_planner::CartesianLimit::CartesianLimit() + : has_max_trans_vel_(false) + , max_trans_vel_(0.0) + , has_max_trans_acc_(false) + , max_trans_acc_(0.0) + , has_max_trans_dec_(false) + , max_trans_dec_(0.0) + , has_max_rot_vel_(false) + , max_rot_vel_(0.0) +{ +} + +// Translational Velocity Limit + +bool pilz_industrial_motion_planner::CartesianLimit::hasMaxTranslationalVelocity() const +{ + return has_max_trans_vel_; +} + +void pilz_industrial_motion_planner::CartesianLimit::setMaxTranslationalVelocity(double max_trans_vel) +{ + has_max_trans_vel_ = true; + max_trans_vel_ = max_trans_vel; +} + +double pilz_industrial_motion_planner::CartesianLimit::getMaxTranslationalVelocity() const +{ + return max_trans_vel_; +} + +// Translational Acceleration Limit + +bool pilz_industrial_motion_planner::CartesianLimit::hasMaxTranslationalAcceleration() const +{ + return has_max_trans_acc_; +} + +void pilz_industrial_motion_planner::CartesianLimit::setMaxTranslationalAcceleration(double max_trans_acc) +{ + has_max_trans_acc_ = true; + max_trans_acc_ = max_trans_acc; +} + +double pilz_industrial_motion_planner::CartesianLimit::getMaxTranslationalAcceleration() const +{ + return max_trans_acc_; +} + +// Translational Deceleration Limit + +bool pilz_industrial_motion_planner::CartesianLimit::hasMaxTranslationalDeceleration() const +{ + return has_max_trans_dec_; +} + +void pilz_industrial_motion_planner::CartesianLimit::setMaxTranslationalDeceleration(double max_trans_dec) +{ + has_max_trans_dec_ = true; + max_trans_dec_ = max_trans_dec; +} + +double pilz_industrial_motion_planner::CartesianLimit::getMaxTranslationalDeceleration() const +{ + return max_trans_dec_; +} + +// Rotational Velocity Limit + +bool pilz_industrial_motion_planner::CartesianLimit::hasMaxRotationalVelocity() const +{ + return has_max_rot_vel_; +} + +void pilz_industrial_motion_planner::CartesianLimit::setMaxRotationalVelocity(double max_rot_vel) +{ + has_max_rot_vel_ = true; + max_rot_vel_ = max_rot_vel; +} + +double pilz_industrial_motion_planner::CartesianLimit::getMaxRotationalVelocity() const +{ + return max_rot_vel_; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp new file mode 100644 index 0000000000..30dc95ad41 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_aggregator.cpp @@ -0,0 +1,96 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "ros/ros.h" + +#include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" + +static const std::string PARAM_CARTESIAN_LIMITS_NS = "cartesian_limits"; + +static const std::string PARAM_MAX_TRANS_VEL = "max_trans_vel"; +static const std::string PARAM_MAX_TRANS_ACC = "max_trans_acc"; +static const std::string PARAM_MAX_TRANS_DEC = "max_trans_dec"; +static const std::string PARAM_MAX_ROT_VEL = "max_rot_vel"; +static const std::string PARAM_MAX_ROT_ACC = "max_rot_acc"; +static const std::string PARAM_MAX_ROT_DEC = "max_rot_dec"; + +pilz_industrial_motion_planner::CartesianLimit +pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(const ros::NodeHandle& nh) +{ + std::string param_prefix = PARAM_CARTESIAN_LIMITS_NS + "/"; + + pilz_industrial_motion_planner::CartesianLimit cartesian_limit; + + // translational velocity + double max_trans_vel; + if (nh.getParam(param_prefix + PARAM_MAX_TRANS_VEL, max_trans_vel)) + { + cartesian_limit.setMaxTranslationalVelocity(max_trans_vel); + } + + // translational acceleration + double max_trans_acc; + if (nh.getParam(param_prefix + PARAM_MAX_TRANS_ACC, max_trans_acc)) + { + cartesian_limit.setMaxTranslationalAcceleration(max_trans_acc); + } + + // translational deceleration + double max_trans_dec; + if (nh.getParam(param_prefix + PARAM_MAX_TRANS_DEC, max_trans_dec)) + { + cartesian_limit.setMaxTranslationalDeceleration(max_trans_dec); + } + + // rotational velocity + double max_rot_vel; + if (nh.getParam(param_prefix + PARAM_MAX_ROT_VEL, max_rot_vel)) + { + cartesian_limit.setMaxRotationalVelocity(max_rot_vel); + } + + // rotational acceleration + deceleration deprecated + // LCOV_EXCL_START + if (nh.hasParam(param_prefix + PARAM_MAX_ROT_ACC) || nh.hasParam(param_prefix + PARAM_MAX_ROT_DEC)) + { + ROS_WARN_STREAM("Ignoring cartesian limits parameters for rotational " + "acceleration / deceleration;" + << "these parameters are deprecated and are automatically " + "calculated from" + << "translational to rotational ratio."); + } + // LCOV_EXCL_STOP + + return cartesian_limit; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp new file mode 100644 index 0000000000..52941969d2 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -0,0 +1,315 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/command_list_manager.h" + +#include +#include +#include + +#include +#include +#include + +#include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/tip_frame_getter.h" +#include "pilz_industrial_motion_planner/trajectory_blend_request.h" +#include "pilz_industrial_motion_planner/trajectory_blender_transition_window.h" + +namespace pilz_industrial_motion_planner +{ +static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; + +CommandListManager::CommandListManager(const ros::NodeHandle& nh, const moveit::core::RobotModelConstPtr& model) + : nh_(nh), model_(model) +{ + // Obtain the aggregated joint limits + pilz_industrial_motion_planner::JointLimitsContainer aggregated_limit_active_joints; + + aggregated_limit_active_joints = pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + ros::NodeHandle(PARAM_NAMESPACE_LIMITS), model_->getActiveJointModels()); + + // Obtain cartesian limits + pilz_industrial_motion_planner::CartesianLimit cartesian_limit = + pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits( + ros::NodeHandle(PARAM_NAMESPACE_LIMITS)); + + pilz_industrial_motion_planner::LimitsContainer limits; + limits.setJointLimits(aggregated_limit_active_joints); + limits.setCartesianLimits(cartesian_limit); + + plan_comp_builder_.setModel(model); + plan_comp_builder_.setBlender(std::unique_ptr( + new pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow(limits))); +} + +RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_pipeline::PlanningPipelinePtr& planning_pipeline, + const moveit_msgs::MotionSequenceRequest& req_list) +{ + if (req_list.items.empty()) + { + return RobotTrajCont(); + } + + checkForNegativeRadii(req_list); + checkLastBlendRadiusZero(req_list); + checkStartStates(req_list); + + MotionResponseCont resp_cont{ solveSequenceItems(planning_scene, planning_pipeline, req_list) }; + + assert(model_); + RadiiCont radii{ extractBlendRadii(*model_, req_list) }; + checkForOverlappingRadii(resp_cont, radii); + + plan_comp_builder_.reset(); + for (MotionResponseCont::size_type i = 0; i < resp_cont.size(); ++i) + { + plan_comp_builder_.append(resp_cont.at(i).trajectory_, + // The blend radii has to be "attached" to + // the second part of a blend trajectory, + // therefore: "i-1". + (i > 0 ? radii.at(i - 1) : 0.)); + } + return plan_comp_builder_.build(); +} + +bool CommandListManager::checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A, + const robot_trajectory::RobotTrajectory& traj_B, + const double radii_B) const +{ + // No blending between trajectories from different groups + if (traj_A.getGroupName() != traj_B.getGroupName()) + { + return false; + } + + auto sum_radii{ radii_A + radii_B }; + if (sum_radii == 0.) + { + return false; + } + + const std::string& blend_frame{ getSolverTipFrame(model_->getJointModelGroup(traj_A.getGroupName())) }; + auto distance_endpoints = (traj_A.getLastWayPoint().getFrameTransform(blend_frame).translation() - + traj_B.getLastWayPoint().getFrameTransform(blend_frame).translation()) + .norm(); + return distance_endpoints <= sum_radii; +} + +void CommandListManager::checkForOverlappingRadii(const MotionResponseCont& resp_cont, const RadiiCont& radii) const +{ + if (resp_cont.empty()) + { + return; + } + if (resp_cont.size() < 3) + { + return; + } + + for (MotionResponseCont::size_type i = 0; i < resp_cont.size() - 2; ++i) + { + if (checkRadiiForOverlap(*(resp_cont.at(i).trajectory_), radii.at(i), *(resp_cont.at(i + 1).trajectory_), + radii.at(i + 1))) + { + std::ostringstream os; + os << "Overlapping blend radii between command [" << i << "] and [" << i + 1 << "]."; + throw OverlappingBlendRadiiException(os.str()); + } + } +} + +CommandListManager::RobotState_OptRef +CommandListManager::getPreviousEndState(const MotionResponseCont& motion_plan_responses, const std::string& group_name) +{ + for (MotionResponseCont::const_reverse_iterator it = motion_plan_responses.crbegin(); + it != motion_plan_responses.crend(); ++it) + { + if (it->trajectory_->getGroupName() == group_name) + { + return it->trajectory_->getLastWayPoint(); + } + } + return boost::none; +} + +void CommandListManager::setStartState(const MotionResponseCont& motion_plan_responses, const std::string& group_name, + moveit_msgs::RobotState& start_state) +{ + RobotState_OptRef rob_state_op{ getPreviousEndState(motion_plan_responses, group_name) }; + if (rob_state_op) + { + moveit::core::robotStateToRobotStateMsg(rob_state_op.value(), start_state); + } +} + +bool CommandListManager::isInvalidBlendRadii(const moveit::core::RobotModel& model, + const moveit_msgs::MotionSequenceItem& item_A, + const moveit_msgs::MotionSequenceItem& item_B) +{ + // Zero blend radius is always valid + if (item_A.blend_radius == 0.) + { + return false; + } + + // No blending between different groups + if (item_A.req.group_name != item_B.req.group_name) + { + ROS_WARN_STREAM("Blending between different groups (in this case: \"" + << item_A.req.group_name << "\" and \"" << item_B.req.group_name << "\") not allowed"); + return true; + } + + // No blending for groups without solver + if (!hasSolver(model.getJointModelGroup(item_A.req.group_name))) + { + ROS_WARN_STREAM("Blending for groups without solver not allowed"); + return true; + } + + return false; +} + +CommandListManager::RadiiCont CommandListManager::extractBlendRadii(const moveit::core::RobotModel& model, + const moveit_msgs::MotionSequenceRequest& req_list) +{ + RadiiCont radii(req_list.items.size(), 0.); + for (RadiiCont::size_type i = 0; i < (radii.size() - 1); ++i) + { + if (isInvalidBlendRadii(model, req_list.items.at(i), req_list.items.at(i + 1))) + { + ROS_WARN_STREAM("Invalid blend radii between commands: [" << i << "] and [" << i + 1 + << "] => Blend radii set to zero"); + continue; + } + radii.at(i) = req_list.items.at(i).blend_radius; + } + return radii; +} + +CommandListManager::MotionResponseCont +CommandListManager::solveSequenceItems(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_pipeline::PlanningPipelinePtr& planning_pipeline, + const moveit_msgs::MotionSequenceRequest& req_list) const +{ + MotionResponseCont motion_plan_responses; + size_t curr_req_index{ 0 }; + const size_t num_req{ req_list.items.size() }; + for (const auto& seq_item : req_list.items) + { + planning_interface::MotionPlanRequest req{ seq_item.req }; + setStartState(motion_plan_responses, req.group_name, req.start_state); + + planning_interface::MotionPlanResponse res; + planning_pipeline->generatePlan(planning_scene, req, res); + if (res.error_code_.val != res.error_code_.SUCCESS) + { + std::ostringstream os; + os << "Could not solve request\n---\n" << req << "\n---\n"; + throw PlanningPipelineException(os.str(), res.error_code_.val); + } + motion_plan_responses.emplace_back(res); + ROS_DEBUG_STREAM("Solved [" << ++curr_req_index << "/" << num_req << "]"); + } + return motion_plan_responses; +} + +void CommandListManager::checkForNegativeRadii(const moveit_msgs::MotionSequenceRequest& req_list) +{ + if (!std::all_of(req_list.items.begin(), req_list.items.end(), + [](const moveit_msgs::MotionSequenceItem& req) { return (req.blend_radius >= 0.); })) + { + throw NegativeBlendRadiusException("All blending radii MUST be non negative"); + } +} + +void CommandListManager::checkStartStatesOfGroup(const moveit_msgs::MotionSequenceRequest& req_list, + const std::string& group_name) +{ + bool first_elem{ true }; + for (const moveit_msgs::MotionSequenceItem& item : req_list.items) + { + if (item.req.group_name != group_name) + { + continue; + } + + if (first_elem) + { + first_elem = false; + continue; + } + + if (!(item.req.start_state.joint_state.position.empty() && item.req.start_state.joint_state.velocity.empty() && + item.req.start_state.joint_state.effort.empty() && item.req.start_state.joint_state.name.empty())) + { + std::ostringstream os; + os << "Only the first request is allowed to have a start state, but" + << " the requests for group: \"" << group_name << "\" violate the rule"; + throw StartStateSetException(os.str()); + } + } +} + +void CommandListManager::checkStartStates(const moveit_msgs::MotionSequenceRequest& req_list) +{ + if (req_list.items.size() <= 1) + { + return; + } + + GroupNamesCont group_names{ getGroupNames(req_list) }; + for (const auto& curr_group_name : group_names) + { + checkStartStatesOfGroup(req_list, curr_group_name); + } +} + +CommandListManager::GroupNamesCont CommandListManager::getGroupNames(const moveit_msgs::MotionSequenceRequest& req_list) +{ + GroupNamesCont group_names; + std::for_each(req_list.items.cbegin(), req_list.items.cend(), + [&group_names](const moveit_msgs::MotionSequenceItem& item) { + if (std::find(group_names.cbegin(), group_names.cend(), item.req.group_name) == group_names.cend()) + { + group_names.emplace_back(item.req.group_name); + } + }); + return group_names; +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp new file mode 100644 index 0000000000..5c800573b5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp @@ -0,0 +1,181 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/joint_limits_interface_extension.h" + +#include +#include + +#include + +pilz_industrial_motion_planner::JointLimitsContainer +pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + const ros::NodeHandle& nh, const std::vector& joint_models) +{ + JointLimitsContainer container; + + ROS_INFO_STREAM("Reading limits from namespace " << nh.getNamespace()); + + // Iterate over all joint models and generate the map + for (auto joint_model : joint_models) + { + JointLimit joint_limit; + + // If there is something defined for the joint on the parameter server + if (joint_limits_interface::getJointLimits(joint_model->getName(), nh, joint_limit)) + { + if (joint_limit.has_position_limits) + { + checkPositionBoundsThrowing(joint_model, joint_limit); + } + else + { + updatePositionLimitFromJointModel(joint_model, joint_limit); + } + + if (joint_limit.has_velocity_limits) + { + checkVelocityBoundsThrowing(joint_model, joint_limit); + } + else + { + updateVelocityLimitFromJointModel(joint_model, joint_limit); + } + } + else + { + // If there is nothing defined for this joint on the parameter server just + // update the values by the values of + // the urdf + + updatePositionLimitFromJointModel(joint_model, joint_limit); + updateVelocityLimitFromJointModel(joint_model, joint_limit); + } + + // Update max_deceleration if no max_acceleration has been set + if (joint_limit.has_acceleration_limits && !joint_limit.has_deceleration_limits) + { + joint_limit.max_deceleration = -joint_limit.max_acceleration; + joint_limit.has_deceleration_limits = true; + } + + // Insert the joint limit into the map + container.addLimit(joint_model->getName(), joint_limit); + } + + return container; +} + +void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitFromJointModel( + const moveit::core::JointModel* joint_model, JointLimit& joint_limit) +{ + switch (joint_model->getVariableBounds().size()) + { + // LCOV_EXCL_START + case 0: + ROS_ERROR_STREAM("no bounds set for joint " << joint_model->getName()); + break; + // LCOV_EXCL_STOP + case 1: + joint_limit.has_position_limits = joint_model->getVariableBounds()[0].position_bounded_; + joint_limit.min_position = joint_model->getVariableBounds()[0].min_position_; + joint_limit.max_position = joint_model->getVariableBounds()[0].max_position_; + break; + // LCOV_EXCL_START + default: + ROS_ERROR_STREAM("Multi-DOF-Joints not supported. The robot won't move. " << joint_model->getName()); + joint_limit.has_position_limits = true; + joint_limit.min_position = 0; + joint_limit.max_position = 0; + break; + // LCOV_EXCL_STOP + } + + ROS_DEBUG_STREAM("Limit(" << joint_model->getName() << " min:" << joint_limit.min_position + << " max:" << joint_limit.max_position); +} + +void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitFromJointModel( + const moveit::core::JointModel* joint_model, JointLimit& joint_limit) +{ + switch (joint_model->getVariableBounds().size()) + { + // LCOV_EXCL_START + case 0: + ROS_ERROR_STREAM("no bounds set for joint " << joint_model->getName()); + break; + // LCOV_EXCL_STOP + case 1: + joint_limit.has_velocity_limits = joint_model->getVariableBounds()[0].velocity_bounded_; + joint_limit.max_velocity = joint_model->getVariableBounds()[0].max_velocity_; + break; + // LCOV_EXCL_START + default: + ROS_ERROR_STREAM("Multi-DOF-Joints not supported. The robot won't move."); + joint_limit.has_velocity_limits = true; + joint_limit.max_velocity = 0; + break; + // LCOV_EXCL_STOP + } +} + +void pilz_industrial_motion_planner::JointLimitsAggregator::checkPositionBoundsThrowing( + const moveit::core::JointModel* joint_model, const JointLimit& joint_limit) +{ + // Check min position + if (!joint_model->satisfiesPositionBounds(&joint_limit.min_position)) + { + throw AggregationBoundsViolationException("min_position of " + joint_model->getName() + + " violates min limit from URDF"); + } + + // Check max position + if (!joint_model->satisfiesPositionBounds(&joint_limit.max_position)) + { + throw AggregationBoundsViolationException("max_position of " + joint_model->getName() + + " violates max limit from URDF"); + } +} + +void pilz_industrial_motion_planner::JointLimitsAggregator::checkVelocityBoundsThrowing( + const moveit::core::JointModel* joint_model, const JointLimit& joint_limit) +{ + // Check min position + if (!joint_model->satisfiesVelocityBounds(&joint_limit.max_velocity)) + { + throw AggregationBoundsViolationException("max_velocity of " + joint_model->getName() + + " violates velocity limit from URDF"); + } +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp new file mode 100644 index 0000000000..7bdb1d983f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp @@ -0,0 +1,182 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/joint_limits_container.h" + +#include "ros/ros.h" +#include + +namespace pilz_industrial_motion_planner +{ +bool JointLimitsContainer::addLimit(const std::string& joint_name, JointLimit joint_limit) +{ + if (joint_limit.has_deceleration_limits && joint_limit.max_deceleration >= 0) + { + ROS_ERROR_STREAM("joint_limit.max_deceleration MUST be negative!"); + return false; + } + const auto& insertion_result{ container_.insert(std::pair(joint_name, joint_limit)) }; + if (!insertion_result.second) + { + ROS_ERROR_STREAM("joint_limit for joint " << joint_name << " already contained."); + return false; + } + return true; +} + +bool JointLimitsContainer::hasLimit(const std::string& joint_name) const +{ + return container_.find(joint_name) != container_.end(); +} + +size_t JointLimitsContainer::getCount() const +{ + return container_.size(); +} + +bool JointLimitsContainer::empty() const +{ + return container_.empty(); +} + +JointLimit JointLimitsContainer::getCommonLimit() const +{ + JointLimit common_limit; + for (const auto& limit : container_) + { + updateCommonLimit(limit.second, common_limit); + } + return common_limit; +} + +JointLimit JointLimitsContainer::getCommonLimit(const std::vector& joint_names) const +{ + JointLimit common_limit; + for (const auto& joint_name : joint_names) + { + updateCommonLimit(container_.at(joint_name), common_limit); + } + return common_limit; +} + +JointLimit JointLimitsContainer::getLimit(const std::string& joint_name) const +{ + return container_.at(joint_name); +} + +std::map::const_iterator JointLimitsContainer::begin() const +{ + return container_.begin(); +} + +std::map::const_iterator JointLimitsContainer::end() const +{ + return container_.end(); +} + +bool JointLimitsContainer::verifyVelocityLimit(const std::string& joint_name, const double& joint_velocity) const +{ + return (!(hasLimit(joint_name) && getLimit(joint_name).has_velocity_limits && + fabs(joint_velocity) > getLimit(joint_name).max_velocity)); +} + +bool JointLimitsContainer::verifyPositionLimit(const std::string& joint_name, const double& joint_position) const +{ + return (!(hasLimit(joint_name) && getLimit(joint_name).has_position_limits && + (joint_position < getLimit(joint_name).min_position || joint_position > getLimit(joint_name).max_position))); +} + +bool JointLimitsContainer::verifyPositionLimits(const std::vector& joint_names, + const std::vector& joint_positions) const +{ + if (joint_names.size() != joint_positions.size()) + { + throw std::out_of_range("joint_names vector has a different size than joint_positions vector."); + } + + for (std::size_t i = 0; i < joint_names.size(); ++i) + { + if (!verifyPositionLimit(joint_names.at(i), joint_positions.at(i))) + { + return false; + } + } + + return true; +} + +void JointLimitsContainer::updateCommonLimit(const JointLimit& joint_limit, JointLimit& common_limit) +{ + // check position limits + if (joint_limit.has_position_limits) + { + double min_position = joint_limit.min_position; + double max_position = joint_limit.max_position; + + common_limit.min_position = + (!common_limit.has_position_limits) ? min_position : std::max(common_limit.min_position, min_position); + common_limit.max_position = + (!common_limit.has_position_limits) ? max_position : std::min(common_limit.max_position, max_position); + common_limit.has_position_limits = true; + } + + // check velocity limits + if (joint_limit.has_velocity_limits) + { + double max_velocity = joint_limit.max_velocity; + common_limit.max_velocity = + (!common_limit.has_velocity_limits) ? max_velocity : std::min(common_limit.max_velocity, max_velocity); + common_limit.has_velocity_limits = true; + } + + // check acceleration limits + if (joint_limit.has_acceleration_limits) + { + double max_acc = joint_limit.max_acceleration; + common_limit.max_acceleration = + (!common_limit.has_acceleration_limits) ? max_acc : std::min(common_limit.max_acceleration, max_acc); + common_limit.has_acceleration_limits = true; + } + + // check deceleration limits + if (joint_limit.has_deceleration_limits) + { + double max_dec = joint_limit.max_deceleration; + common_limit.max_deceleration = + (!common_limit.has_deceleration_limits) ? max_dec : std::max(common_limit.max_deceleration, max_dec); + common_limit.has_deceleration_limits = true; + } +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp new file mode 100644 index 0000000000..66c5b198a9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp @@ -0,0 +1,157 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include + +#include "pilz_industrial_motion_planner/joint_limits_extension.h" +#include "pilz_industrial_motion_planner/joint_limits_interface_extension.h" + +#include "pilz_industrial_motion_planner/joint_limits_validator.h" + +bool pilz_industrial_motion_planner::JointLimitsValidator::validateAllPositionLimitsEqual( + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + return validateWithEqualFunc(&pilz_industrial_motion_planner::JointLimitsValidator::positionEqual, joint_limits); +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::validateAllVelocityLimitsEqual( + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + return validateWithEqualFunc(&pilz_industrial_motion_planner::JointLimitsValidator::velocityEqual, joint_limits); +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::validateAllAccelerationLimitsEqual( + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + return validateWithEqualFunc(&pilz_industrial_motion_planner::JointLimitsValidator::accelerationEqual, joint_limits); +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::validateAllDecelerationLimitsEqual( + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + return validateWithEqualFunc(&pilz_industrial_motion_planner::JointLimitsValidator::decelerationEqual, joint_limits); +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::validateWithEqualFunc( + bool (*eq_func)(const JointLimit&, const JointLimit&), + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + // If there are no joint_limits it is considered equal + if (joint_limits.empty()) + { + return true; + } + + JointLimit reference = joint_limits.begin()->second; + + for (auto it = std::next(joint_limits.begin()); it != joint_limits.end(); ++it) + { + if (!eq_func(reference, it->second)) + { + return false; + } + } + + return true; +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::positionEqual(const JointLimit& lhs, const JointLimit& rhs) +{ + // Return false if .has_velocity_limits differs + if (lhs.has_position_limits != rhs.has_position_limits) + { + return false; + } + + // If velocity limits are the same make sure they are equal + if (lhs.has_position_limits && ((lhs.max_position != rhs.max_position) || (lhs.min_position != rhs.min_position))) + { + return false; + } + return true; +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::velocityEqual(const JointLimit& lhs, const JointLimit& rhs) +{ + // Return false if .has_velocity_limits differs + if (lhs.has_velocity_limits != rhs.has_velocity_limits) + { + return false; + } + + // If velocity limits are the same make sure they are equal + if (lhs.has_velocity_limits && (lhs.max_velocity != rhs.max_velocity)) + { + return false; + } + + return true; +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::accelerationEqual(const JointLimit& lhs, + const JointLimit& rhs) +{ + // Return false if .has_acceleration_limits differs + if (lhs.has_acceleration_limits != rhs.has_acceleration_limits) + { + return false; + } + + // If velocity limits are the same make sure they are equal + if (lhs.has_acceleration_limits && (lhs.max_acceleration != rhs.max_acceleration)) + { + return false; + } + + return true; +} + +bool pilz_industrial_motion_planner::JointLimitsValidator::decelerationEqual(const JointLimit& lhs, + const JointLimit& rhs) +{ + // Return false if .has_acceleration_limits differs + if (lhs.has_deceleration_limits != rhs.has_deceleration_limits) + { + return false; + } + + // If velocity limits are the same make sure they are equal + if (lhs.has_deceleration_limits && (lhs.max_deceleration != rhs.max_deceleration)) + { + return false; + } + + return true; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp new file mode 100644 index 0000000000..6ee51df0b6 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp @@ -0,0 +1,78 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/limits_container.h" + +pilz_industrial_motion_planner::LimitsContainer::LimitsContainer() + : has_joint_limits_(false), has_cartesian_limits_(false) +{ +} + +bool pilz_industrial_motion_planner::LimitsContainer::hasJointLimits() const +{ + return has_joint_limits_; +} + +void pilz_industrial_motion_planner::LimitsContainer::setJointLimits( + pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + has_joint_limits_ = true; + joint_limits_ = joint_limits; +} + +const pilz_industrial_motion_planner::JointLimitsContainer& +pilz_industrial_motion_planner::LimitsContainer::getJointLimitContainer() const +{ + return joint_limits_; +} + +bool pilz_industrial_motion_planner::LimitsContainer::hasFullCartesianLimits() const +{ + return (has_cartesian_limits_ && cartesian_limit_.hasMaxTranslationalVelocity() && + cartesian_limit_.hasMaxTranslationalAcceleration() && cartesian_limit_.hasMaxTranslationalDeceleration() && + cartesian_limit_.hasMaxRotationalVelocity()); +} + +void pilz_industrial_motion_planner::LimitsContainer::setCartesianLimits( + pilz_industrial_motion_planner::CartesianLimit& cartesian_limit) +{ + has_cartesian_limits_ = true; + cartesian_limit_ = cartesian_limit; +} + +const pilz_industrial_motion_planner::CartesianLimit& +pilz_industrial_motion_planner::LimitsContainer::getCartesianLimits() const +{ + return cartesian_limit_; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp new file mode 100644 index 0000000000..6a6b84cab0 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp @@ -0,0 +1,294 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Ioan Sucan */ + +// Modified by Pilz GmbH & Co. KG + +#include "pilz_industrial_motion_planner/move_group_sequence_action.h" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/command_list_manager.h" +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" + +namespace pilz_industrial_motion_planner +{ +MoveGroupSequenceAction::MoveGroupSequenceAction() : MoveGroupCapability("SequenceAction") +{ +} + +void MoveGroupSequenceAction::initialize() +{ + // start the move action server + ROS_INFO_STREAM("initialize move group sequence action"); + move_action_server_.reset(new actionlib::SimpleActionServer( + root_node_handle_, "sequence_move_group", + boost::bind(&MoveGroupSequenceAction::executeSequenceCallback, this, _1), false)); + move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupSequenceAction::preemptMoveCallback, this)); + move_action_server_->start(); + + command_list_manager_.reset(new pilz_industrial_motion_planner::CommandListManager( + ros::NodeHandle("~"), context_->planning_scene_monitor_->getRobotModel())); +} + +void MoveGroupSequenceAction::executeSequenceCallback(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal) +{ + setMoveState(move_group::PLANNING); + + // Handle empty requests + if (goal->request.items.empty()) + { + ROS_WARN("Received empty request. That's ok but maybe not what you intended."); + setMoveState(move_group::IDLE); + moveit_msgs::MoveGroupSequenceResult action_res; + action_res.response.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + move_action_server_->setSucceeded(action_res, "Received empty request."); + return; + } + + // before we start planning, ensure that we have the latest robot state + // received... + context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now()); + context_->planning_scene_monitor_->updateFrameTransforms(); + + moveit_msgs::MoveGroupSequenceResult action_res; + if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_) + { + if (!goal->planning_options.plan_only) + { + ROS_WARN("Only plan will be calculated, although plan_only == false."); // LCOV_EXCL_LINE + } + executeMoveCallbackPlanOnly(goal, action_res); + } + else + { + executeSequenceCallbackPlanAndExecute(goal, action_res); + } + + switch (action_res.response.error_code.val) + { + case moveit_msgs::MoveItErrorCodes::SUCCESS: + move_action_server_->setSucceeded(action_res, "Success"); + break; + case moveit_msgs::MoveItErrorCodes::PREEMPTED: + move_action_server_->setPreempted(action_res, "Preempted"); + break; + default: + move_action_server_->setAborted(action_res, "See error code for more information"); + break; + } + + setMoveState(move_group::IDLE); +} + +void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute( + const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal, moveit_msgs::MoveGroupSequenceResult& action_res) +{ + ROS_INFO("Combined planning and execution request received for " + "MoveGroupSequenceAction."); + + plan_execution::PlanExecution::Options opt; + + const moveit_msgs::PlanningScene& planning_scene_diff = + moveit::core::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ? + goal->planning_options.planning_scene_diff : + clearSceneRobotState(goal->planning_options.planning_scene_diff); + + opt.replan_ = goal->planning_options.replan; + opt.replan_attempts_ = goal->planning_options.replan_attempts; + opt.replan_delay_ = goal->planning_options.replan_delay; + opt.before_execution_callback_ = boost::bind(&MoveGroupSequenceAction::startMoveExecutionCallback, this); + + opt.plan_callback_ = + boost::bind(&MoveGroupSequenceAction::planUsingSequenceManager, this, boost::cref(goal->request), _1); + + if (goal->planning_options.look_around && context_->plan_with_sensing_) + { + ROS_WARN("Plan with sensing not yet implemented/tested. This option is " + "ignored."); // LCOV_EXCL_LINE + } + + plan_execution::ExecutableMotionPlan plan; + context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt); + + StartStatesMsg start_states_msg; + convertToMsg(plan.plan_components_, start_states_msg, action_res.response.planned_trajectories); + try + { + action_res.response.sequence_start = start_states_msg.at(0); + } + catch (std::out_of_range&) + { + ROS_WARN("Can not determine start state from empty sequence."); + } + action_res.response.error_code = plan.error_code_; +} + +void MoveGroupSequenceAction::convertToMsg(const ExecutableTrajs& trajs, StartStatesMsg& start_states_msg, + PlannedTrajMsgs& planned_trajs_msgs) +{ + start_states_msg.resize(trajs.size()); + planned_trajs_msgs.resize(trajs.size()); + for (size_t i = 0; i < trajs.size(); ++i) + { + robot_state::robotStateToRobotStateMsg(trajs.at(i).trajectory_->getFirstWayPoint(), start_states_msg.at(i)); + trajs.at(i).trajectory_->getRobotTrajectoryMsg(planned_trajs_msgs.at(i)); + } +} + +void MoveGroupSequenceAction::executeMoveCallbackPlanOnly(const moveit_msgs::MoveGroupSequenceGoalConstPtr& goal, + moveit_msgs::MoveGroupSequenceResult& res) +{ + ROS_INFO("Planning request received for MoveGroupSequenceAction action."); + + // lock the scene so that it does not modify the world representation while + // diff() is called + planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); + + const planning_scene::PlanningSceneConstPtr& the_scene = + (moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) ? + static_cast(lscene) : + lscene->diff(goal->planning_options.planning_scene_diff); + + ros::Time planning_start = ros::Time::now(); + RobotTrajCont traj_vec; + try + { + traj_vec = command_list_manager_->solve(the_scene, context_->planning_pipeline_, goal->request); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM("> Planning pipeline threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); + res.response.error_code.val = ex.getErrorCode(); + return; + } + // LCOV_EXCL_START // Keep moveit up even if lower parts throw + catch (const std::exception& ex) + { + ROS_ERROR("Planning pipeline threw an exception: %s", ex.what()); + res.response.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; + return; + } + // LCOV_EXCL_STOP + + StartStatesMsg start_states_msg; + start_states_msg.resize(traj_vec.size()); + res.response.planned_trajectories.resize(traj_vec.size()); + for (RobotTrajCont::size_type i = 0; i < traj_vec.size(); ++i) + { + move_group::MoveGroupCapability::convertToMsg(traj_vec.at(i), start_states_msg.at(i), + res.response.planned_trajectories.at(i)); + } + try + { + res.response.sequence_start = start_states_msg.at(0); + } + catch (std::out_of_range&) + { + ROS_WARN("Can not determine start state from empty sequence."); + } + + res.response.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + res.response.planning_time = ros::Time::now().toSec() - planning_start.toSec(); +} + +bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::MotionSequenceRequest& req, + plan_execution::ExecutableMotionPlan& plan) +{ + setMoveState(move_group::PLANNING); + + planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_); + RobotTrajCont traj_vec; + try + { + traj_vec = command_list_manager_->solve(plan.planning_scene_, context_->planning_pipeline_, req); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM("Planning pipeline threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); + plan.error_code_.val = ex.getErrorCode(); + return false; + } + // LCOV_EXCL_START // Keep moveit up even if lower parts throw + catch (const std::exception& ex) + { + ROS_ERROR_STREAM("Planning pipeline threw an exception: " << ex.what()); + plan.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE; + return false; + } + // LCOV_EXCL_STOP + + if (!traj_vec.empty()) + { + plan.plan_components_.resize(traj_vec.size()); + for (size_t i = 0; i < traj_vec.size(); ++i) + { + plan.plan_components_.at(i).trajectory_ = traj_vec.at(i); + plan.plan_components_.at(i).description_ = "plan"; + } + } + plan.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + return true; +} + +void MoveGroupSequenceAction::startMoveExecutionCallback() +{ + setMoveState(move_group::MONITOR); +} + +void MoveGroupSequenceAction::preemptMoveCallback() +{ + context_->plan_execution_->stop(); +} + +void MoveGroupSequenceAction::setMoveState(move_group::MoveGroupState state) +{ + move_state_ = state; + move_feedback_.state = stateToStr(state); + move_action_server_->publishFeedback(move_feedback_); +} + +} // namespace pilz_industrial_motion_planner + +#include +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::MoveGroupSequenceAction, move_group::MoveGroupCapability) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp new file mode 100644 index 0000000000..ce6f789eb4 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp @@ -0,0 +1,105 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Ioan Sucan */ + +// Modified by Pilz GmbH & Co. KG + +#include "pilz_industrial_motion_planner/move_group_sequence_service.h" + +#include "pilz_industrial_motion_planner/capability_names.h" +#include "pilz_industrial_motion_planner/command_list_manager.h" +#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h" + +namespace pilz_industrial_motion_planner +{ +MoveGroupSequenceService::MoveGroupSequenceService() : MoveGroupCapability("SequenceService") +{ +} + +MoveGroupSequenceService::~MoveGroupSequenceService() +{ +} + +void MoveGroupSequenceService::initialize() +{ + command_list_manager_.reset(new pilz_industrial_motion_planner::CommandListManager( + ros::NodeHandle("~"), context_->planning_scene_monitor_->getRobotModel())); + + sequence_service_ = root_node_handle_.advertiseService(SEQUENCE_SERVICE_NAME, &MoveGroupSequenceService::plan, this); +} + +bool MoveGroupSequenceService::plan(moveit_msgs::GetMotionSequence::Request& req, + moveit_msgs::GetMotionSequence::Response& res) +{ + // TODO: Do we lock on the correct scene? Does the lock belong to the scene + // used for planning? + planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_); + + ros::Time planning_start = ros::Time::now(); + RobotTrajCont traj_vec; + try + { + traj_vec = command_list_manager_->solve(ps, context_->planning_pipeline_, req.request); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM("Planner threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); + res.response.error_code.val = ex.getErrorCode(); + return true; + } + // LCOV_EXCL_START // Keep moveit up even if lower parts throw + catch (const std::exception& ex) + { + ROS_ERROR_STREAM("Planner threw an exception: " << ex.what()); + // If 'FALSE' then no response will be sent to the caller. + return false; + } + // LCOV_EXCL_STOP + + res.response.planned_trajectories.resize(traj_vec.size()); + for (RobotTrajCont::size_type i = 0; i < traj_vec.size(); ++i) + { + move_group::MoveGroupCapability::convertToMsg(traj_vec.at(i), res.response.sequence_start, + res.response.planned_trajectories.at(i)); + } + res.response.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + res.response.planning_time = (ros::Time::now() - planning_start).toSec(); + return true; +} + +} // namespace pilz_industrial_motion_planner + +#include +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::MoveGroupSequenceService, move_group::MoveGroupCapability) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp new file mode 100644 index 0000000000..503a1a3c89 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp @@ -0,0 +1,147 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/path_circle_generator.h" + +namespace pilz_industrial_motion_planner +{ +std::unique_ptr PathCircleGenerator::circleFromCenter(const KDL::Frame& start_pose, + const KDL::Frame& goal_pose, + const KDL::Vector& center_point, double eqradius) +{ + double a = (start_pose.p - center_point).Norm(); + double b = (goal_pose.p - center_point).Norm(); + double c = (start_pose.p - goal_pose.p).Norm(); + + if (fabs(a - b) > MAX_RADIUS_DIFF) + { + throw ErrorMotionPlanningCenterPointDifferentRadius(); + } + + // compute the rotation angle + double alpha = cosines(a, b, c); + + KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); + double old_kdl_epsilon = KDL::epsilon; + try + { + KDL::epsilon = MAX_COLINEAR_NORM; + auto circle = std::unique_ptr( + new KDL::Path_Circle(start_pose, center_point, goal_pose.p, goal_pose.M, alpha, rot_interpo, eqradius, + true /* take ownership of RotationalInterpolation */)); + KDL::epsilon = old_kdl_epsilon; + return circle; + } + catch (KDL::Error_MotionPlanning&) + { + delete rot_interpo; // in case we could not construct the Path object, avoid + // a memory leak + KDL::epsilon = old_kdl_epsilon; + throw; // and pass the exception on to the caller + } +} + +std::unique_ptr PathCircleGenerator::circleFromInterim(const KDL::Frame& start_pose, + const KDL::Frame& goal_pose, + const KDL::Vector& interim_point, double eqradius) +{ + // compute the center point from interim point + // triangle edges + const KDL::Vector t = interim_point - start_pose.p; + const KDL::Vector u = goal_pose.p - start_pose.p; + const KDL::Vector v = goal_pose.p - interim_point; + // triangle normal + const KDL::Vector w = t * u; // cross product + + // circle center + if (w.Norm() < MAX_COLINEAR_NORM) + { + throw KDL::Error_MotionPlanning_Circle_No_Plane(); + } + const KDL::Vector center_point = + start_pose.p + (u * dot(t, t) * dot(u, v) - t * dot(u, u) * dot(t, v)) * 0.5 / pow(w.Norm(), 2); + + // compute the rotation angle + // triangle edges + const KDL::Vector t_center = center_point - start_pose.p; + const KDL::Vector v_center = goal_pose.p - center_point; + double a = t_center.Norm(); + double b = v_center.Norm(); + double c = u.Norm(); + double alpha = cosines(a, b, c); + + KDL::Vector kdl_aux_point(interim_point); + + // if the angle at the interim is an acute angle (<90deg), rotation angle is + // an obtuse angle (>90deg) + // in this case using the interim as auxiliary point for KDL::Path_Circle can + // lead to a path in the wrong direction + double interim_angle = cosines(t.Norm(), v.Norm(), u.Norm()); + if (interim_angle < M_PI / 2) + { + alpha = 2 * M_PI - alpha; + + // exclude that the goal is not colinear with start and center, then use the + // opposite of the goal as auxiliary point + if ((t_center * v_center).Norm() > MAX_COLINEAR_NORM) + { + kdl_aux_point = 2 * center_point - goal_pose.p; + } + } + + KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); + try + { + return std::unique_ptr(new KDL::Path_Circle(start_pose, center_point, kdl_aux_point, goal_pose.M, alpha, + rot_interpo, eqradius, + true /* take ownership of RotationalInterpolation */)); + } + catch (KDL::Error_MotionPlanning&) // LCOV_EXCL_START // The cases where + // KDL would throw are already handled + // above, + // we keep these lines to be safe + { + delete rot_interpo; // in case we could not construct the Path object, avoid + // a memory leak + throw; // and pass the exception on to the caller + // LCOV_EXCL_STOP + } +} + +double PathCircleGenerator::cosines(const double a, const double b, const double c) +{ + return acos(std::max(std::min((pow(a, 2) + pow(b, 2) - pow(c, 2)) / (2.0 * a * b), 1.0), -1.0)); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp new file mode 100644 index 0000000000..a7e592b204 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp @@ -0,0 +1,173 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/pilz_industrial_motion_planner.h" + +#include "pilz_industrial_motion_planner/planning_context_loader.h" +#include "pilz_industrial_motion_planner/planning_context_loader_ptp.h" +#include "pilz_industrial_motion_planner/planning_exceptions.h" + +#include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" + +// Boost includes +#include + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +static const std::string PARAM_NAMESPACE_LIMTS = "robot_description_planning"; + +bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) +{ + // Call parent class initialize + planning_interface::PlannerManager::initialize(model, ns); + + // Store the model and the namespace + model_ = model; + namespace_ = ns; + + // Obtain the aggregated joint limits + aggregated_limit_active_joints_ = pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + ros::NodeHandle(PARAM_NAMESPACE_LIMTS), model->getActiveJointModels()); + + // Obtain cartesian limits + cartesian_limit_ = pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits( + ros::NodeHandle(PARAM_NAMESPACE_LIMTS)); + + // Load the planning context loader + planner_context_loader.reset(new pluginlib::ClassLoader( + "pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader")); + + // List available plugins + const std::vector& factories = planner_context_loader->getDeclaredClasses(); + std::stringstream ss; + for (const auto& factory : factories) + { + ss << factory << " "; + } + + ROS_INFO_STREAM("Available plugins: " << ss.str()); + + // Load each factory + for (const auto& factory : factories) + { + ROS_INFO_STREAM("About to load: " << factory); + PlanningContextLoaderPtr loader_pointer(planner_context_loader->createInstance(factory)); + + pilz_industrial_motion_planner::LimitsContainer limits; + limits.setJointLimits(aggregated_limit_active_joints_); + limits.setCartesianLimits(cartesian_limit_); + + loader_pointer->setLimits(limits); + loader_pointer->setModel(model_); + + registerContextLoader(loader_pointer); + } + + return true; +} + +std::string CommandPlanner::getDescription() const +{ + return "Pilz Industrial Motion Planner"; +} + +void CommandPlanner::getPlanningAlgorithms(std::vector& algs) const +{ + algs.clear(); + + for (const auto& context_loader : context_loader_map_) + { + algs.push_back(context_loader.first); + } +} + +planning_interface::PlanningContextPtr +CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const moveit_msgs::MotionPlanRequest& req, + moveit_msgs::MoveItErrorCodes& error_code) const +{ + ROS_DEBUG_STREAM("Loading PlanningContext for request\n\n" << req << "\n"); + + // Check that a loaded for this request exists + if (!canServiceRequest(req)) + { + ROS_ERROR_STREAM("No ContextLoader for planner_id " << req.planner_id.c_str() << " found. Planning not possible."); + return nullptr; + } + + planning_interface::PlanningContextPtr planning_context; + + if (context_loader_map_.at(req.planner_id)->loadContext(planning_context, req.planner_id, req.group_name)) + { + ROS_DEBUG_STREAM("Found planning context loader for " << req.planner_id << " group:" << req.group_name); + planning_context->setMotionPlanRequest(req); + planning_context->setPlanningScene(planning_scene); + return planning_context; + } + else + { + error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; + return nullptr; + } +} + +bool CommandPlanner::canServiceRequest(const moveit_msgs::MotionPlanRequest& req) const +{ + return context_loader_map_.find(req.planner_id) != context_loader_map_.end(); +} + +void CommandPlanner::registerContextLoader( + const pilz_industrial_motion_planner::PlanningContextLoaderPtr& planning_context_loader) +{ + // Only add if command is not already in list, throw exception if not + if (context_loader_map_.find(planning_context_loader->getAlgorithm()) == context_loader_map_.end()) + { + context_loader_map_[planning_context_loader->getAlgorithm()] = planning_context_loader; + ROS_INFO_STREAM("Registered Algorithm [" << planning_context_loader->getAlgorithm() << "]"); + } + else + { + throw ContextLoaderRegistrationException("The command [" + planning_context_loader->getAlgorithm() + + "] is already registered"); + } +} + +} // namespace pilz_industrial_motion_planner + +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::CommandPlanner, planning_interface::PlannerManager) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp new file mode 100644 index 0000000000..175a7deca9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp @@ -0,0 +1,137 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/plan_components_builder.h" + +#include + +#include "pilz_industrial_motion_planner/tip_frame_getter.h" + +namespace pilz_industrial_motion_planner +{ +std::vector PlanComponentsBuilder::build() const +{ + std::vector res_vec{ traj_cont_ }; + if (traj_tail_) + { + assert(!res_vec.empty()); + appendWithStrictTimeIncrease(*(res_vec.back()), *traj_tail_); + } + return res_vec; +} + +void PlanComponentsBuilder::appendWithStrictTimeIncrease(robot_trajectory::RobotTrajectory& result, + const robot_trajectory::RobotTrajectory& source) +{ + if (result.empty() || + !pilz_industrial_motion_planner::isRobotStateEqual(result.getLastWayPoint(), source.getFirstWayPoint(), + result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON)) + { + result.append(source, 0.0); + return; + } + + for (size_t i = 1; i < source.getWayPointCount(); ++i) + { + result.addSuffixWayPoint(source.getWayPoint(i), source.getWayPointDurationFromPrevious(i)); + } +} + +void PlanComponentsBuilder::blend(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius) +{ + if (!blender_) + { + throw NoBlenderSetException("No blender set"); + } + + assert(other->getGroupName() == traj_tail_->getGroupName()); + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_request; + + blend_request.first_trajectory = traj_tail_; + blend_request.second_trajectory = other; + blend_request.blend_radius = blend_radius; + blend_request.group_name = traj_tail_->getGroupName(); + blend_request.link_name = getSolverTipFrame(model_->getJointModelGroup(blend_request.group_name)); + + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_response; + if (!blender_->blend(blend_request, blend_response)) + { + throw BlendingFailedException("Blending failed"); + } + + // Append the new trajectory elements + appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.first_trajectory); + traj_cont_.back()->append(*blend_response.blend_trajectory, 0.0); + // Store the last new trajectory element for future processing + traj_tail_ = blend_response.second_trajectory; // first for next blending segment +} + +void PlanComponentsBuilder::append(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius) +{ + if (!model_) + { + throw NoRobotModelSetException("No robot model set"); + } + + if (!traj_tail_) + { + traj_tail_ = other; + // Reserve space in container for new trajectory + traj_cont_.emplace_back(new robot_trajectory::RobotTrajectory(model_, other->getGroupName())); + return; + } + + // Create new trajectory for every group change + if (other->getGroupName() != traj_tail_->getGroupName()) + { + appendWithStrictTimeIncrease(*(traj_cont_.back()), *traj_tail_); + traj_tail_ = other; + // Create new container element + traj_cont_.emplace_back(new robot_trajectory::RobotTrajectory(model_, other->getGroupName())); + return; + } + + // No blending + if (blend_radius <= 0.0) + { + appendWithStrictTimeIncrease(*(traj_cont_.back()), *traj_tail_); + traj_tail_ = other; + return; + } + + blend(other, blend_radius); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp new file mode 100644 index 0000000000..9a071bba58 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp @@ -0,0 +1,64 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/planning_context_loader.h" +#include + +pilz_industrial_motion_planner::PlanningContextLoader::PlanningContextLoader() : limits_set_(false), model_set_(false) +{ +} + +pilz_industrial_motion_planner::PlanningContextLoader::~PlanningContextLoader() +{ +} + +bool pilz_industrial_motion_planner::PlanningContextLoader::setModel(const moveit::core::RobotModelConstPtr& model) +{ + model_ = model; + model_set_ = true; + return true; +} + +bool pilz_industrial_motion_planner::PlanningContextLoader::setLimits( + const pilz_industrial_motion_planner::LimitsContainer& limits) +{ + limits_ = limits; + limits_set_ = true; + return true; +} + +std::string pilz_industrial_motion_planner::PlanningContextLoader::getAlgorithm() const +{ + return alg_; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp new file mode 100644 index 0000000000..0234b989dc --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp @@ -0,0 +1,75 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/planning_context_loader_circ.h" +#include "moveit/planning_scene/planning_scene.h" +#include "pilz_industrial_motion_planner/planning_context_base.h" +#include "pilz_industrial_motion_planner/planning_context_circ.h" + +#include + +pilz_industrial_motion_planner::PlanningContextLoaderCIRC::PlanningContextLoaderCIRC() +{ + alg_ = "CIRC"; +} + +pilz_industrial_motion_planner::PlanningContextLoaderCIRC::~PlanningContextLoaderCIRC() +{ +} + +bool pilz_industrial_motion_planner::PlanningContextLoaderCIRC::loadContext( + planning_interface::PlanningContextPtr& planning_context, const std::string& name, const std::string& group) const +{ + if (limits_set_ && model_set_) + { + planning_context.reset(new PlanningContextCIRC(name, group, model_, limits_)); + return true; + } + else + { + if (!limits_set_) + { + ROS_ERROR_STREAM("Limits are not defined. Cannot load planning context. " + "Call setLimits loadContext"); + } + if (!model_set_) + { + ROS_ERROR_STREAM("Robot model was not set"); + } + return false; + } +} + +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::PlanningContextLoaderCIRC, + pilz_industrial_motion_planner::PlanningContextLoader) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp new file mode 100644 index 0000000000..85cf3b6fe9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp @@ -0,0 +1,75 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/planning_context_loader_lin.h" +#include "moveit/planning_scene/planning_scene.h" +#include "pilz_industrial_motion_planner/planning_context_base.h" +#include "pilz_industrial_motion_planner/planning_context_lin.h" + +#include + +pilz_industrial_motion_planner::PlanningContextLoaderLIN::PlanningContextLoaderLIN() +{ + alg_ = "LIN"; +} + +pilz_industrial_motion_planner::PlanningContextLoaderLIN::~PlanningContextLoaderLIN() +{ +} + +bool pilz_industrial_motion_planner::PlanningContextLoaderLIN::loadContext( + planning_interface::PlanningContextPtr& planning_context, const std::string& name, const std::string& group) const +{ + if (limits_set_ && model_set_) + { + planning_context.reset(new PlanningContextLIN(name, group, model_, limits_)); + return true; + } + else + { + if (!limits_set_) + { + ROS_ERROR_STREAM("Limits are not defined. Cannot load planning context. " + "Call setLimits loadContext"); + } + if (!model_set_) + { + ROS_ERROR_STREAM("Robot model was not set"); + } + return false; + } +} + +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::PlanningContextLoaderLIN, + pilz_industrial_motion_planner::PlanningContextLoader) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp new file mode 100644 index 0000000000..5c72e69cec --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/planning_context_loader_ptp.h" +#include "moveit/planning_scene/planning_scene.h" +#include "pilz_industrial_motion_planner/planning_context_ptp.h" + +#include + +pilz_industrial_motion_planner::PlanningContextLoaderPTP::PlanningContextLoaderPTP() +{ + alg_ = "PTP"; +} + +pilz_industrial_motion_planner::PlanningContextLoaderPTP::~PlanningContextLoaderPTP() +{ +} + +bool pilz_industrial_motion_planner::PlanningContextLoaderPTP::loadContext( + planning_interface::PlanningContextPtr& planning_context, const std::string& name, const std::string& group) const +{ + if (limits_set_ && model_set_) + { + planning_context.reset(new PlanningContextPTP(name, group, model_, limits_)); + return true; + } + else + { + if (!limits_set_) + { + ROS_ERROR_STREAM("Joint Limits are not defined. Cannot load planning " + "context. Call setLimits loadContext"); + } + if (!model_set_) + { + ROS_ERROR_STREAM("Robot model was not set"); + } + return false; + } +} + +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::PlanningContextLoaderPTP, + pilz_industrial_motion_planner::PlanningContextLoader) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp new file mode 100644 index 0000000000..94c108ced8 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -0,0 +1,301 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/trajectory_blender_transition_window.h" + +#include +#include +#include +#include + +bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + pilz_industrial_motion_planner::TrajectoryBlendResponse& res) +{ + ROS_INFO("Start trajectory blending using transition window."); + + double sampling_time = 0.; + if (!validateRequest(req, sampling_time, res.error_code)) + { + ROS_ERROR("Trajectory blend request is not valid."); + return false; + } + + // search for intersection points of the two trajectories with the blending + // sphere + // intersection points belongs to blend trajectory after blending + std::size_t first_intersection_index; + std::size_t second_intersection_index; + if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index)) + { + ROS_ERROR("Blend radius to large."); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + return false; + } + + // Select blending period and adjust the start and end point of the blend + // phase + std::size_t blend_align_index; + determineTrajectoryAlignment(req, first_intersection_index, second_intersection_index, blend_align_index); + + // blend the trajectories in Cartesian space + pilz_industrial_motion_planner::CartesianTrajectory blend_trajectory_cartesian; + blendTrajectoryCartesian(req, first_intersection_index, second_intersection_index, blend_align_index, sampling_time, + blend_trajectory_cartesian); + + // generate the blending trajectory in joint space + std::map initial_joint_position, initial_joint_velocity; + for (const std::string& joint_name : + req.first_trajectory->getFirstWayPointPtr()->getJointModelGroup(req.group_name)->getActiveJointModelNames()) + { + initial_joint_position[joint_name] = + req.first_trajectory->getWayPoint(first_intersection_index - 1).getVariablePosition(joint_name); + initial_joint_velocity[joint_name] = + req.first_trajectory->getWayPoint(first_intersection_index - 1).getVariableVelocity(joint_name); + } + trajectory_msgs::JointTrajectory blend_joint_trajectory; + moveit_msgs::MoveItErrorCodes error_code; + if (!generateJointTrajectory(req.first_trajectory->getFirstWayPointPtr()->getRobotModel(), + limits_.getJointLimitContainer(), blend_trajectory_cartesian, req.group_name, + req.link_name, initial_joint_position, initial_joint_velocity, blend_joint_trajectory, + error_code, true)) + { + // LCOV_EXCL_START + ROS_INFO("Failed to generate joint trajectory for blending trajectory."); + res.error_code.val = error_code.val; + return false; + // LCOV_EXCL_STOP + } + + res.first_trajectory = std::shared_ptr( + new robot_trajectory::RobotTrajectory(req.first_trajectory->getRobotModel(), req.first_trajectory->getGroup())); + res.blend_trajectory = std::shared_ptr( + new robot_trajectory::RobotTrajectory(req.first_trajectory->getRobotModel(), req.first_trajectory->getGroup())); + res.second_trajectory = std::shared_ptr( + new robot_trajectory::RobotTrajectory(req.first_trajectory->getRobotModel(), req.first_trajectory->getGroup())); + + // set the three trajectories after blending in response + // erase the points [first_intersection_index, back()] from the first + // trajectory + for (size_t i = 0; i < first_intersection_index; ++i) + { + res.first_trajectory->insertWayPoint(i, req.first_trajectory->getWayPoint(i), + req.first_trajectory->getWayPointDurationFromPrevious(i)); + } + + // append the blend trajectory + res.blend_trajectory->setRobotTrajectoryMsg(req.first_trajectory->getFirstWayPoint(), blend_joint_trajectory); + // copy the points [second_intersection_index, len] from the second trajectory + for (size_t i = second_intersection_index + 1; i < req.second_trajectory->getWayPointCount(); ++i) + { + res.second_trajectory->insertWayPoint(i - (second_intersection_index + 1), req.second_trajectory->getWayPoint(i), + req.second_trajectory->getWayPointDurationFromPrevious(i)); + } + + // adjust the time from start + res.second_trajectory->setWayPointDurationFromPrevious(0, sampling_time); + + res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + return true; +} + +bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validateRequest( + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time, + moveit_msgs::MoveItErrorCodes& error_code) const +{ + ROS_DEBUG("Validate the trajectory blend request."); + + // check planning group + if (!req.first_trajectory->getRobotModel()->hasJointModelGroup(req.group_name)) + { + ROS_ERROR_STREAM("Unknown planning group: " << req.group_name); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; + return false; + } + + // check link exists + if (!req.first_trajectory->getRobotModel()->hasLinkModel(req.link_name) && + !req.first_trajectory->getLastWayPoint().hasAttachedBody(req.link_name)) + { + ROS_ERROR_STREAM("Unknown link name: " << req.link_name); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME; + return false; + } + + if (req.blend_radius <= 0) + { + ROS_ERROR("Blending radius must be positive"); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + return false; + } + + // end position of the first trajectory and start position of second + // trajectory must be the same + if (!pilz_industrial_motion_planner::isRobotStateEqual( + req.first_trajectory->getLastWayPoint(), req.second_trajectory->getFirstWayPoint(), req.group_name, EPSILON)) + { + ROS_ERROR_STREAM("During blending the last point of the preceding and the first point of the succeding trajectory"); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + return false; + } + + // same uniform sampling time + if (!pilz_industrial_motion_planner::determineAndCheckSamplingTime(req.first_trajectory, req.second_trajectory, + EPSILON, sampling_time)) + { + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + return false; + } + + // end position of the first trajectory and start position of second + // trajectory must have zero + // velocities/accelerations + if (!pilz_industrial_motion_planner::isRobotStateStationary(req.first_trajectory->getLastWayPoint(), req.group_name, + EPSILON) || + !pilz_industrial_motion_planner::isRobotStateStationary(req.second_trajectory->getFirstWayPoint(), req.group_name, + EPSILON)) + { + ROS_ERROR("Intersection point of the blending trajectories has non-zero " + "velocities/accelerations."); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + return false; + } + + return true; +} + +void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTrajectoryCartesian( + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, const std::size_t first_interse_index, + const std::size_t second_interse_index, const std::size_t blend_align_index, double sampling_time, + pilz_industrial_motion_planner::CartesianTrajectory& trajectory) const +{ + // other fields of the trajectory + trajectory.group_name = req.group_name; + trajectory.link_name = req.link_name; + + // Pose on first trajectory + Eigen::Isometry3d blend_sample_pose1 = + req.first_trajectory->getWayPoint(first_interse_index).getFrameTransform(req.link_name); + + // Pose on second trajectory + Eigen::Isometry3d blend_sample_pose2 = + req.second_trajectory->getWayPoint(second_interse_index).getFrameTransform(req.link_name); + + // blend the trajectory + double blend_sample_num = second_interse_index + blend_align_index - first_interse_index + 1; + pilz_industrial_motion_planner::CartesianTrajectoryPoint waypoint; + blend_sample_pose2 = req.second_trajectory->getFirstWayPoint().getFrameTransform(req.link_name); + + // Pose on blending trajectory + Eigen::Isometry3d blend_sample_pose; + for (std::size_t i = 0; i < blend_sample_num; ++i) + { + // if the first trajectory does not reach the last sample, update + if ((first_interse_index + i) < req.first_trajectory->getWayPointCount()) + { + blend_sample_pose1 = req.first_trajectory->getWayPoint(first_interse_index + i).getFrameTransform(req.link_name); + } + + // if after the alignment, the second trajectory starts, update + if ((first_interse_index + i) > blend_align_index) + { + blend_sample_pose2 = req.second_trajectory->getWayPoint(first_interse_index + i - blend_align_index) + .getFrameTransform(req.link_name); + } + + double s = (i + 1) / blend_sample_num; + double alpha = 6 * std::pow(s, 5) - 15 * std::pow(s, 4) + 10 * std::pow(s, 3); + + // blend the translation + blend_sample_pose.translation() = blend_sample_pose1.translation() + + alpha * (blend_sample_pose2.translation() - blend_sample_pose1.translation()); + + // blend the orientation + Eigen::Quaterniond start_quat(blend_sample_pose1.rotation()); + Eigen::Quaterniond end_quat(blend_sample_pose2.rotation()); + blend_sample_pose.linear() = start_quat.slerp(alpha, end_quat).toRotationMatrix(); + + // push to the trajectory + waypoint.pose = tf2::toMsg(blend_sample_pose); + waypoint.time_from_start = ros::Duration((i + 1.0) * sampling_time); + trajectory.points.push_back(waypoint); + } +} + +bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::searchIntersectionPoints( + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, std::size_t& first_interse_index, + std::size_t& second_interse_index) const +{ + ROS_INFO("Search for start and end point of blending trajectory."); + + // compute the position of the center of the blend sphere + // (last point of the first trajectory, first point of the second trajectory) + Eigen::Isometry3d circ_pose = req.first_trajectory->getLastWayPoint().getFrameTransform(req.link_name); + + // Searh for intersection points according to distance + if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.first_trajectory, + true, first_interse_index)) + { + ROS_ERROR_STREAM("Intersection point of first trajectory not found."); + return false; + } + ROS_INFO_STREAM("Intersection point of first trajectory found, index: " << first_interse_index); + + if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.second_trajectory, + false, second_interse_index)) + { + ROS_ERROR_STREAM("Intersection point of second trajectory not found."); + return false; + } + + ROS_INFO_STREAM("Intersection point of second trajectory found, index: " << second_interse_index); + return true; +} + +void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::determineTrajectoryAlignment( + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, std::size_t first_interse_index, + std::size_t second_interse_index, std::size_t& blend_align_index) const +{ + size_t way_point_count_1 = req.first_trajectory->getWayPointCount() - first_interse_index; + size_t way_point_count_2 = second_interse_index + 1; + + if (way_point_count_1 > way_point_count_2) + { + blend_align_index = req.first_trajectory->getWayPointCount() - second_interse_index - 1; + } + else + { + blend_align_index = first_interse_index; + } +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp new file mode 100644 index 0000000000..5d37a4d4ea --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -0,0 +1,584 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/trajectory_functions.h" + +#include +#include +#include +#include +#include + +bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotModelConstPtr& robot_model, + const std::string& group_name, const std::string& link_name, + const Eigen::Isometry3d& pose, const std::string& frame_id, + const std::map& seed, + std::map& solution, bool check_self_collision, + const double timeout) +{ + if (!robot_model->hasJointModelGroup(group_name)) + { + ROS_ERROR_STREAM("Robot model has no planning group named as " << group_name); + return false; + } + + if (!robot_model->getJointModelGroup(group_name)->canSetStateFromIK(link_name)) + { + ROS_ERROR_STREAM("No valid IK solver exists for " << link_name << " in planning group " << group_name); + return false; + } + + if (frame_id != robot_model->getModelFrame()) + { + ROS_ERROR_STREAM("Given frame (" << frame_id << ") is unequal to model frame(" << robot_model->getModelFrame() + << ")"); + return false; + } + + robot_state::RobotState rstate(robot_model); + // By setting the robot state to default values, we basically allow + // the user of this function to supply an incomplete or even empty seed. + rstate.setToDefaultValues(); + rstate.setVariablePositions(seed); + + moveit::core::GroupStateValidityCallbackFn ik_constraint_function; + ik_constraint_function = + boost::bind(&pilz_industrial_motion_planner::isStateColliding, check_self_collision, robot_model, _1, _2, _3); + + // call ik + if (rstate.setFromIK(robot_model->getJointModelGroup(group_name), pose, link_name, timeout, ik_constraint_function)) + { + // copy the solution + for (const auto& joint_name : robot_model->getJointModelGroup(group_name)->getActiveJointModelNames()) + { + solution[joint_name] = rstate.getVariablePosition(joint_name); + } + return true; + } + else + { + ROS_ERROR_STREAM("Inverse kinematics for pose \n" << pose.translation() << " has no solution."); + return false; + } +} + +bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotModelConstPtr& robot_model, + const std::string& group_name, const std::string& link_name, + const geometry_msgs::Pose& pose, const std::string& frame_id, + const std::map& seed, + std::map& solution, bool check_self_collision, + const double timeout) +{ + Eigen::Isometry3d pose_eigen; + tf2::convert(pose, pose_eigen); + return computePoseIK(robot_model, group_name, link_name, pose_eigen, frame_id, seed, solution, check_self_collision, + timeout); +} + +bool pilz_industrial_motion_planner::computeLinkFK(const moveit::core::RobotModelConstPtr& robot_model, + const std::string& link_name, + const std::map& joint_state, + Eigen::Isometry3d& pose) +{ // create robot state + robot_state::RobotState rstate(robot_model); + + // check the reference frame of the target pose + if (!rstate.knowsFrameTransform(link_name)) + { + ROS_ERROR_STREAM("The target link " << link_name << " is not known by robot."); + return false; + } + + // set the joint positions + rstate.setToDefaultValues(); + rstate.setVariablePositions(joint_state); + + // update the frame + rstate.update(); + pose = rstate.getFrameTransform(link_name); + + return true; +} + +bool pilz_industrial_motion_planner::verifySampleJointLimits( + const std::map& position_last, const std::map& velocity_last, + const std::map& position_current, double duration_last, double duration_current, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + const double epsilon = 10e-6; + if (duration_current <= epsilon) + { + ROS_ERROR("Sample duration too small, cannot compute the velocity"); + return false; + } + + double velocity_current, acceleration_current; + + for (const auto& pos : position_current) + { + velocity_current = (pos.second - position_last.at(pos.first)) / duration_current; + + if (!joint_limits.verifyVelocityLimit(pos.first, velocity_current)) + { + ROS_ERROR_STREAM("Joint velocity limit of " << pos.first << " violated. Set the velocity scaling factor lower!" + << " Actual joint velocity is " << velocity_current + << ", while the limit is " + << joint_limits.getLimit(pos.first).max_velocity << ". "); + return false; + } + + acceleration_current = (velocity_current - velocity_last.at(pos.first)) / (duration_last + duration_current) * 2; + // acceleration case + if (fabs(velocity_last.at(pos.first)) <= fabs(velocity_current)) + { + if (joint_limits.getLimit(pos.first).has_acceleration_limits && + fabs(acceleration_current) > fabs(joint_limits.getLimit(pos.first).max_acceleration)) + { + ROS_ERROR_STREAM("Joint acceleration limit of " + << pos.first << " violated. Set the acceleration scaling factor lower!" + << " Actual joint acceleration is " << acceleration_current << ", while the limit is " + << joint_limits.getLimit(pos.first).max_acceleration << ". "); + return false; + } + } + // deceleration case + else + { + if (joint_limits.getLimit(pos.first).has_deceleration_limits && + fabs(acceleration_current) > fabs(joint_limits.getLimit(pos.first).max_deceleration)) + { + ROS_ERROR_STREAM("Joint deceleration limit of " + << pos.first << " violated. Set the acceleration scaling factor lower!" + << " Actual joint deceleration is " << acceleration_current << ", while the limit is " + << joint_limits.getLimit(pos.first).max_deceleration << ". "); + return false; + } + } + } + + return true; +} + +bool pilz_industrial_motion_planner::generateJointTrajectory( + const moveit::core::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, + const std::string& group_name, const std::string& link_name, + const std::map& initial_joint_position, const double& sampling_time, + trajectory_msgs::JointTrajectory& joint_trajectory, moveit_msgs::MoveItErrorCodes& error_code, + bool check_self_collision) +{ + ROS_DEBUG("Generate joint trajectory from a Cartesian trajectory."); + + ros::Time generation_begin = ros::Time::now(); + + // generate the time samples + const double epsilon = 10e-06; // avoid adding the last time sample twice + std::vector time_samples; + for (double t_sample = 0.0; t_sample < trajectory.Duration() - epsilon; t_sample += sampling_time) + { + time_samples.push_back(t_sample); + } + time_samples.push_back(trajectory.Duration()); + + // sample the trajectory and solve the inverse kinematics + Eigen::Isometry3d pose_sample; + std::map ik_solution_last, ik_solution, joint_velocity_last; + ik_solution_last = initial_joint_position; + for (const auto& item : ik_solution_last) + { + joint_velocity_last[item.first] = 0.0; + } + + for (std::vector::const_iterator time_iter = time_samples.begin(); time_iter != time_samples.end(); + ++time_iter) + { + tf::transformKDLToEigen(trajectory.Pos(*time_iter), pose_sample); + + if (!computePoseIK(robot_model, group_name, link_name, pose_sample, robot_model->getModelFrame(), ik_solution_last, + ik_solution, check_self_collision)) + { + ROS_ERROR("Failed to compute inverse kinematics solution for sampled " + "Cartesian pose."); + error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; + joint_trajectory.points.clear(); + return false; + } + + // check the joint limits + double duration_current_sample = sampling_time; + // last interval can be shorter than the sampling time + if (time_iter == (time_samples.end() - 1) && time_samples.size() > 1) + { + duration_current_sample = *time_iter - *(time_iter - 1); + } + if (time_samples.size() == 1) + { + duration_current_sample = *time_iter; + } + + // skip the first sample with zero time from start for limits checking + if (time_iter != time_samples.begin() && + !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, sampling_time, + duration_current_sample, joint_limits)) + { + ROS_ERROR_STREAM("Inverse kinematics solution at " + << *time_iter << "s violates the joint velocity/acceleration/deceleration limits."); + error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; + joint_trajectory.points.clear(); + return false; + } + + // fill the point with joint values + trajectory_msgs::JointTrajectoryPoint point; + + // set joint names + joint_trajectory.joint_names.clear(); + for (const auto& start_joint : initial_joint_position) + { + joint_trajectory.joint_names.push_back(start_joint.first); + } + + point.time_from_start = ros::Duration(*time_iter); + for (const auto& joint_name : joint_trajectory.joint_names) + { + point.positions.push_back(ik_solution.at(joint_name)); + + if (time_iter != time_samples.begin() && time_iter != time_samples.end() - 1) + { + double joint_velocity = + (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current_sample; + point.velocities.push_back(joint_velocity); + point.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) / + (duration_current_sample + sampling_time) * 2); + joint_velocity_last[joint_name] = joint_velocity; + } + else + { + point.velocities.push_back(0.); + point.accelerations.push_back(0.); + joint_velocity_last[joint_name] = 0.; + } + } + + // update joint trajectory + joint_trajectory.points.push_back(point); + ik_solution_last = ik_solution; + } + + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + double duration_ms = (ros::Time::now() - generation_begin).toSec() * 1000; + ROS_DEBUG_STREAM("Generate trajectory (N-Points: " << joint_trajectory.points.size() << ") took " << duration_ms + << " ms | " << duration_ms / joint_trajectory.points.size() + << " ms per Point"); + + return true; +} + +bool pilz_industrial_motion_planner::generateJointTrajectory( + const moveit::core::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, + const pilz_industrial_motion_planner::CartesianTrajectory& trajectory, const std::string& group_name, + const std::string& link_name, const std::map& initial_joint_position, + const std::map& initial_joint_velocity, trajectory_msgs::JointTrajectory& joint_trajectory, + moveit_msgs::MoveItErrorCodes& error_code, bool check_self_collision) +{ + ROS_DEBUG("Generate joint trajectory from a Cartesian trajectory."); + + ros::Time generation_begin = ros::Time::now(); + + std::map ik_solution_last = initial_joint_position; + std::map joint_velocity_last = initial_joint_velocity; + double duration_last = 0; + double duration_current = 0; + joint_trajectory.joint_names.clear(); + for (const auto& joint_position : ik_solution_last) + { + joint_trajectory.joint_names.push_back(joint_position.first); + } + std::map ik_solution; + for (size_t i = 0; i < trajectory.points.size(); ++i) + { + // compute inverse kinematics + if (!computePoseIK(robot_model, group_name, link_name, trajectory.points.at(i).pose, robot_model->getModelFrame(), + ik_solution_last, ik_solution, check_self_collision)) + { + ROS_ERROR("Failed to compute inverse kinematics solution for sampled " + "Cartesian pose."); + error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; + joint_trajectory.points.clear(); + return false; + } + + // verify the joint limits + if (i == 0) + { + duration_current = trajectory.points.front().time_from_start.toSec(); + duration_last = duration_current; + } + else + { + duration_current = + trajectory.points.at(i).time_from_start.toSec() - trajectory.points.at(i - 1).time_from_start.toSec(); + } + + if (!verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, duration_last, duration_current, + joint_limits)) + { + // LCOV_EXCL_START since the same code was captured in a test in the other + // overload generateJointTrajectory(..., + // KDL::Trajectory, ...) + // TODO: refactor to avoid code duplication. + ROS_ERROR_STREAM("Inverse kinematics solution of the " << i + << "th sample violates the joint " + "velocity/acceleration/deceleration limits."); + error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; + joint_trajectory.points.clear(); + return false; + // LCOV_EXCL_STOP + } + + // compute the waypoint + trajectory_msgs::JointTrajectoryPoint waypoint_joint; + waypoint_joint.time_from_start = ros::Duration(trajectory.points.at(i).time_from_start); + for (const auto& joint_name : joint_trajectory.joint_names) + { + waypoint_joint.positions.push_back(ik_solution.at(joint_name)); + double joint_velocity = (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current; + waypoint_joint.velocities.push_back(joint_velocity); + waypoint_joint.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) / + (duration_current + duration_last) * 2); + // update the joint velocity + joint_velocity_last[joint_name] = joint_velocity; + } + + // update joint trajectory + joint_trajectory.points.push_back(waypoint_joint); + ik_solution_last = ik_solution; + duration_last = duration_current; + } + + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + + double duration_ms = (ros::Time::now() - generation_begin).toSec() * 1000; + ROS_DEBUG_STREAM("Generate trajectory (N-Points: " << joint_trajectory.points.size() << ") took " << duration_ms + << " ms | " << duration_ms / joint_trajectory.points.size() + << " ms per Point"); + + return true; +} + +bool pilz_industrial_motion_planner::determineAndCheckSamplingTime( + const robot_trajectory::RobotTrajectoryPtr& first_trajectory, + const robot_trajectory::RobotTrajectoryPtr& second_trajectory, double epsilon, double& sampling_time) +{ + // The last sample is ignored because it is allowed to violate the sampling + // time. + std::size_t n1 = first_trajectory->getWayPointCount() - 1; + std::size_t n2 = second_trajectory->getWayPointCount() - 1; + if ((n1 < 2) && (n2 < 2)) + { + ROS_ERROR_STREAM("Both trajectories do not have enough points to determine " + "sampling time."); + return false; + } + + if (n1 >= 2) + { + sampling_time = first_trajectory->getWayPointDurationFromPrevious(1); + } + else + { + sampling_time = second_trajectory->getWayPointDurationFromPrevious(1); + } + + for (std::size_t i = 1; i < std::max(n1, n2); ++i) + { + if (i < n1) + { + if (fabs(sampling_time - first_trajectory->getWayPointDurationFromPrevious(i)) > epsilon) + { + ROS_ERROR_STREAM("First trajectory violates sampline time " << sampling_time << " between points " << (i - 1) + << "and " << i << " (indices)."); + return false; + } + } + + if (i < n2) + { + if (fabs(sampling_time - second_trajectory->getWayPointDurationFromPrevious(i)) > epsilon) + { + ROS_ERROR_STREAM("Second trajectory violates sampline time " << sampling_time << " between points " << (i - 1) + << "and " << i << " (indices)."); + return false; + } + } + } + + return true; +} + +bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, + const std::string& joint_group_name, double epsilon) +{ + Eigen::VectorXd joint_position_1, joint_position_2; + + state1.copyJointGroupPositions(joint_group_name, joint_position_1); + state2.copyJointGroupPositions(joint_group_name, joint_position_2); + + if ((joint_position_1 - joint_position_2).norm() > epsilon) + { + ROS_DEBUG_STREAM("Joint positions of the two states are different. state1: " << joint_position_1 + << " state2: " << joint_position_2); + return false; + } + + Eigen::VectorXd joint_velocity_1, joint_velocity_2; + + state1.copyJointGroupVelocities(joint_group_name, joint_velocity_1); + state2.copyJointGroupVelocities(joint_group_name, joint_velocity_2); + + if ((joint_velocity_1 - joint_velocity_2).norm() > epsilon) + { + ROS_DEBUG_STREAM("Joint velocities of the two states are different. state1: " << joint_velocity_1 + << " state2: " << joint_velocity_2); + return false; + } + + Eigen::VectorXd joint_acc_1, joint_acc_2; + + state1.copyJointGroupAccelerations(joint_group_name, joint_acc_1); + state2.copyJointGroupAccelerations(joint_group_name, joint_acc_2); + + if ((joint_acc_1 - joint_acc_2).norm() > epsilon) + { + ROS_DEBUG_STREAM("Joint accelerations of the two states are different. state1: " << joint_acc_1 + << " state2: " << joint_acc_2); + return false; + } + + return true; +} + +bool pilz_industrial_motion_planner::isRobotStateStationary(const moveit::core::RobotState& state, + const std::string& group, double EPSILON) +{ + Eigen::VectorXd joint_variable; + state.copyJointGroupVelocities(group, joint_variable); + if (joint_variable.norm() > EPSILON) + { + ROS_DEBUG("Joint velocities are not zero."); + return false; + } + state.copyJointGroupAccelerations(group, joint_variable); + if (joint_variable.norm() > EPSILON) + { + ROS_DEBUG("Joint accelerations are not zero."); + return false; + } + return true; +} + +bool pilz_industrial_motion_planner::linearSearchIntersectionPoint(const std::string& link_name, + const Eigen::Vector3d& center_position, + const double& r, + const robot_trajectory::RobotTrajectoryPtr& traj, + bool inverseOrder, std::size_t& index) +{ + ROS_DEBUG("Start linear search for intersection point."); + + const size_t waypoint_num = traj->getWayPointCount(); + + if (inverseOrder) + { + for (size_t i = waypoint_num - 1; i > 0; --i) + { + if (intersectionFound(center_position, traj->getWayPointPtr(i)->getFrameTransform(link_name).translation(), + traj->getWayPointPtr(i - 1)->getFrameTransform(link_name).translation(), r)) + { + index = i; + return true; + } + } + } + else + { + for (size_t i = 0; i < waypoint_num - 1; ++i) + { + if (intersectionFound(center_position, traj->getWayPointPtr(i)->getFrameTransform(link_name).translation(), + traj->getWayPointPtr(i + 1)->getFrameTransform(link_name).translation(), r)) + { + index = i; + return true; + } + } + } + + return false; +} + +bool pilz_industrial_motion_planner::intersectionFound(const Eigen::Vector3d& p_center, + const Eigen::Vector3d& p_current, const Eigen::Vector3d& p_next, + const double& r) +{ + return ((p_current - p_center).norm() <= r) && ((p_next - p_center).norm() >= r); +} + +bool pilz_industrial_motion_planner::isStateColliding(const bool test_for_self_collision, + const moveit::core::RobotModelConstPtr& robot_model, + robot_state::RobotState* rstate, + const robot_state::JointModelGroup* const group, + const double* const ik_solution) +{ + if (!test_for_self_collision) + { + return true; + } + + rstate->setJointGroupPositions(group, ik_solution); + rstate->update(); + collision_detection::CollisionRequest collision_req; + collision_req.group_name = group->getName(); + collision_detection::CollisionResult collision_res; + planning_scene::PlanningScene(robot_model).checkSelfCollision(collision_req, collision_res, *rstate); + + return !collision_res.collision; +} + +void normalizeQuaternion(geometry_msgs::Quaternion& quat) +{ + tf2::Quaternion q; + tf2::convert(quat, q); + quat = tf2::toMsg(q.normalize()); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp new file mode 100644 index 0000000000..8069004d2c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -0,0 +1,332 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/trajectory_generator.h" + +#include + +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/limits_container.h" + +namespace pilz_industrial_motion_planner +{ +void TrajectoryGenerator::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& /*req*/) const +{ + // Empty implementation, in case the derived class does not want + // to provide a command specific request validation. +} + +void TrajectoryGenerator::checkVelocityScaling(const double& scaling_factor) +{ + if (!isScalingFactorValid(scaling_factor)) + { + std::ostringstream os; + os << "Velocity scaling not in range [" << MIN_SCALING_FACTOR << ", " << MAX_SCALING_FACTOR << "], " + << "actual value is: " << scaling_factor; + throw VelocityScalingIncorrect(os.str()); + } +} + +void TrajectoryGenerator::checkAccelerationScaling(const double& scaling_factor) +{ + if (!isScalingFactorValid(scaling_factor)) + { + std::ostringstream os; + os << "Acceleration scaling not in range [" << MIN_SCALING_FACTOR << ", " << MAX_SCALING_FACTOR << "], " + << "actual value is: " << scaling_factor; + throw AccelerationScalingIncorrect(os.str()); + } +} + +void TrajectoryGenerator::checkForValidGroupName(const std::string& group_name) const +{ + if (!robot_model_->hasJointModelGroup(group_name)) + { + std::ostringstream os; + os << "Unknown planning group: " << group_name; + throw UnknownPlanningGroup(os.str()); + } +} + +void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_state) const +{ + if (start_state.joint_state.name.empty()) + { + throw NoJointNamesInStartState("No joint names for state state given"); + } + + if (start_state.joint_state.name.size() != start_state.joint_state.position.size()) + { + throw SizeMismatchInStartState("Joint state name and position do not match in start state"); + } + + if (!planner_limits_.getJointLimitContainer().verifyPositionLimits(start_state.joint_state.name, + start_state.joint_state.position)) + { + throw JointsOfStartStateOutOfRange("Joint state out of range in start state"); + } + + // does not allow start velocity + if (!std::all_of(start_state.joint_state.velocity.begin(), start_state.joint_state.velocity.end(), + [this](double v) { return std::fabs(v) < this->VELOCITY_TOLERANCE; })) + { + throw NonZeroVelocityInStartState("Trajectory Generator does not allow non-zero start velocity"); + } +} + +void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::Constraints& constraint, + const std::vector& expected_joint_names, + const std::string& group_name) const +{ + for (auto const& joint_constraint : constraint.joint_constraints) + { + const std::string& curr_joint_name{ joint_constraint.joint_name }; + if (std::find(expected_joint_names.cbegin(), expected_joint_names.cend(), curr_joint_name) == + expected_joint_names.cend()) + { + std::ostringstream os; + os << "Cannot find joint \"" << curr_joint_name << "\" from start state in goal constraint"; + throw StartStateGoalStateMismatch(os.str()); + } + + if (!robot_model_->getJointModelGroup(group_name)->hasJointModel(curr_joint_name)) + { + std::ostringstream os; + os << "Joint \"" << curr_joint_name << "\" does not belong to group \"" << group_name << "\""; + throw JointConstraintDoesNotBelongToGroup(os.str()); + } + + if (!planner_limits_.getJointLimitContainer().verifyPositionLimit(curr_joint_name, joint_constraint.position)) + { + std::ostringstream os; + os << "Joint \"" << curr_joint_name << "\" violates joint limits in goal constraints"; + throw JointsOfGoalOutOfRange(os.str()); + } + } +} + +void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::Constraints& constraint, + const std::string& group_name) const +{ + assert(constraint.position_constraints.size() == 1); + assert(constraint.orientation_constraints.size() == 1); + const moveit_msgs::PositionConstraint& pos_constraint{ constraint.position_constraints.front() }; + const moveit_msgs::OrientationConstraint& ori_constraint{ constraint.orientation_constraints.front() }; + + if (pos_constraint.link_name.empty()) + { + throw PositionConstraintNameMissing("Link name of position constraint missing"); + } + + if (ori_constraint.link_name.empty()) + { + throw OrientationConstraintNameMissing("Link name of orientation constraint missing"); + } + + if (pos_constraint.link_name != ori_constraint.link_name) + { + std::ostringstream os; + os << "Position and orientation constraint name do not match" + << "(Position constraint name: \"" << pos_constraint.link_name << "\" | Orientation constraint name: \"" + << ori_constraint.link_name << "\")"; + throw PositionOrientationConstraintNameMismatch(os.str()); + } + + if (!robot_model_->getJointModelGroup(group_name)->canSetStateFromIK(pos_constraint.link_name)) + { + std::ostringstream os; + os << "No IK solver available for link: \"" << pos_constraint.link_name << "\""; + throw NoIKSolverAvailable(os.str()); + } + + if (pos_constraint.constraint_region.primitive_poses.empty()) + { + throw NoPrimitivePoseGiven("Primitive pose in position constraints of goal missing"); + } +} + +void TrajectoryGenerator::checkGoalConstraints( + const moveit_msgs::MotionPlanRequest::_goal_constraints_type& goal_constraints, + const std::vector& expected_joint_names, const std::string& group_name) const +{ + if (goal_constraints.size() != 1) + { + std::ostringstream os; + os << "Exaclty one goal constraint required, but " << goal_constraints.size() << " goal constraints given"; + throw NotExactlyOneGoalConstraintGiven(os.str()); + } + + const moveit_msgs::Constraints& goal_con{ goal_constraints.front() }; + if (!isOnlyOneGoalTypeGiven(goal_con)) + { + throw OnlyOneGoalTypeAllowed("Only cartesian XOR joint goal allowed"); + } + + if (isJointGoalGiven(goal_con)) + { + checkJointGoalConstraint(goal_con, expected_joint_names, group_name); + } + else + { + checkCartesianGoalConstraint(goal_con, group_name); + } +} + +void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRequest& req) const +{ + checkVelocityScaling(req.max_velocity_scaling_factor); + checkAccelerationScaling(req.max_acceleration_scaling_factor); + checkForValidGroupName(req.group_name); + checkStartState(req.start_state); + checkGoalConstraints(req.goal_constraints, req.start_state.joint_state.name, req.group_name); +} + +void TrajectoryGenerator::convertToRobotTrajectory(const trajectory_msgs::JointTrajectory& joint_trajectory, + const moveit_msgs::RobotState& start_state, + robot_trajectory::RobotTrajectory& robot_trajectory) const +{ + moveit::core::RobotState start_rs(robot_model_); + start_rs.setToDefaultValues(); + moveit::core::robotStateMsgToRobotState(start_state, start_rs, false); + robot_trajectory.setRobotTrajectoryMsg(start_rs, joint_trajectory); +} + +void TrajectoryGenerator::setSuccessResponse(const std::string& group_name, const moveit_msgs::RobotState& start_state, + const trajectory_msgs::JointTrajectory& joint_trajectory, + const ros::Time& planning_start, + planning_interface::MotionPlanResponse& res) const +{ + robot_trajectory::RobotTrajectoryPtr rt(new robot_trajectory::RobotTrajectory(robot_model_, group_name)); + convertToRobotTrajectory(joint_trajectory, start_state, *rt); + + res.trajectory_ = rt; + res.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + res.planning_time_ = (ros::Time::now() - planning_start).toSec(); +} + +void TrajectoryGenerator::setFailureResponse(const ros::Time& planning_start, + planning_interface::MotionPlanResponse& res) const +{ + if (res.trajectory_) + { + res.trajectory_->clear(); + } + res.planning_time_ = (ros::Time::now() - planning_start).toSec(); +} + +std::unique_ptr +TrajectoryGenerator::cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor, + const double& max_acceleration_scaling_factor, + const std::unique_ptr& path) const +{ + std::unique_ptr vp_trans(new KDL::VelocityProfile_Trap( + max_velocity_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalVelocity(), + max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalAcceleration())); + + if (path->PathLength() > std::numeric_limits::epsilon()) // avoid division by zero + { + vp_trans->SetProfile(0, path->PathLength()); + } + else + { + vp_trans->SetProfile(0, std::numeric_limits::epsilon()); + } + return vp_trans; +} + +bool TrajectoryGenerator::generate(const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, double sampling_time) +{ + ROS_INFO_STREAM("Generating " << req.planner_id << " trajectory..."); + ros::Time planning_begin = ros::Time::now(); + + try + { + validateRequest(req); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM(ex.what()); + res.error_code_.val = ex.getErrorCode(); + setFailureResponse(planning_begin, res); + return false; + } + + try + { + cmdSpecificRequestValidation(req); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM(ex.what()); + res.error_code_.val = ex.getErrorCode(); + setFailureResponse(planning_begin, res); + return false; + } + + MotionPlanInfo plan_info; + try + { + extractMotionPlanInfo(req, plan_info); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM(ex.what()); + res.error_code_.val = ex.getErrorCode(); + setFailureResponse(planning_begin, res); + return false; + } + + trajectory_msgs::JointTrajectory joint_trajectory; + try + { + plan(req, plan_info, sampling_time, joint_trajectory); + } + catch (const MoveItErrorCodeException& ex) + { + ROS_ERROR_STREAM(ex.what()); + res.error_code_.val = ex.getErrorCode(); + setFailureResponse(planning_begin, res); + return false; + } + + setSuccessResponse(req.group_name, req.start_state, joint_trajectory, planning_begin, res); + return true; +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp new file mode 100644 index 0000000000..6e1e57f0d1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -0,0 +1,270 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/trajectory_generator_circ.h" +#include "pilz_industrial_motion_planner/path_circle_generator.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +TrajectoryGeneratorCIRC::TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model, + const LimitsContainer& planner_limits) + : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) +{ + if (!planner_limits_.hasFullCartesianLimits()) + { + throw TrajectoryGeneratorInvalidLimitsException( + "Cartesian limits are not fully set for CIRC trajectory generator."); + } +} + +void TrajectoryGeneratorCIRC::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const +{ + if (!(req.path_constraints.name == "interim" || req.path_constraints.name == "center")) + { + std::ostringstream os; + os << "No path constraint named \"interim\" or \"center\" found (found " + "unknown constraint: " + << "\"req.path_constraints.name\"" + << " instead)"; + throw UnknownPathConstraintName(os.str()); + } + + if (req.path_constraints.position_constraints.size() != 1) + { + throw NoPositionConstraints("CIRC trajectory generator needs valid a position constraint"); + } + + if (req.path_constraints.position_constraints.front().constraint_region.primitive_poses.size() != 1) + { + throw NoPrimitivePose("CIRC trajectory generator needs valid a primitive pose"); + } +} + +void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, + TrajectoryGenerator::MotionPlanInfo& info) const +{ + ROS_DEBUG("Extract necessary information from motion plan request."); + + info.group_name = req.group_name; + std::string frame_id{ robot_model_->getModelFrame() }; + + // goal given in joint space + if (!req.goal_constraints.front().joint_constraints.empty()) + { + // TODO: link name from goal constraint and path constraint + info.link_name = req.path_constraints.position_constraints.front().link_name; + if (!robot_model_->hasLinkModel(info.link_name)) + { + throw UnknownLinkNameOfAuxiliaryPoint("Unknown link name of CIRC auxiliary point"); + } + + if (req.goal_constraints.front().joint_constraints.size() != + robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size()) + { + std::ostringstream os; + os << "Number of joint constraint = " << req.goal_constraints.front().joint_constraints.size() + << " not equal to active joints of group = " + << robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size(); + throw NumberOfConstraintsMismatch(os.str()); + } + + // initializing all joints of the model + for (const auto& joint_name : robot_model_->getVariableNames()) + { + info.goal_joint_position[joint_name] = 0; + } + + for (const auto& joint_item : req.goal_constraints.front().joint_constraints) + { + info.goal_joint_position[joint_item.joint_name] = joint_item.position; + } + + computeLinkFK(robot_model_, info.link_name, info.goal_joint_position, info.goal_pose); + } + // goal given in Cartesian space + else + { + info.link_name = req.goal_constraints.front().position_constraints.front().link_name; + if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || + req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) + { + ROS_WARN("Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); + frame_id = robot_model_->getModelFrame(); + } + else + { + frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; + } + geometry_msgs::Pose goal_pose_msg; + goal_pose_msg.position = + req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position; + goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation; + normalizeQuaternion(goal_pose_msg.orientation); + tf2::convert(goal_pose_msg, info.goal_pose); + } + + assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); + for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames()) + { + auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) }; + if (it == req.start_state.joint_state.name.cend()) + { + std::ostringstream os; + os << "Could not find joint \"" << joint_name << "\" of group \"" << req.group_name + << "\" in start state of request"; + throw CircJointMissingInStartState(os.str()); + } + size_t index = it - req.start_state.joint_state.name.cbegin(); + info.start_joint_position[joint_name] = req.start_state.joint_state.position[index]; + } + + computeLinkFK(robot_model_, info.link_name, info.start_joint_position, info.start_pose); + + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(robot_model_, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) + { + // LCOV_EXCL_START + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw CircInverseForGoalIncalculable(os.str()); + // LCOV_EXCL_STOP // not able to trigger here since lots of checks before + // are in place + } + + Eigen::Vector3d circ_path_point; + tf2::convert(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, + circ_path_point); + + info.circ_path_point.first = req.path_constraints.name; + info.circ_path_point.second = circ_path_point; +} + +void TrajectoryGeneratorCIRC::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) +{ + std::unique_ptr cart_path(setPathCIRC(plan_info)); + std::unique_ptr vel_profile( + cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, cart_path)); + + // combine path and velocity profile into Cartesian trajectory + // with the third parameter set to false, KDL::Trajectory_Segment does not + // take + // the ownship of Path and Velocity Profile + KDL::Trajectory_Segment cart_trajectory(cart_path.get(), vel_profile.get(), false); + + moveit_msgs::MoveItErrorCodes error_code; + // sample the Cartesian trajectory and compute joint trajectory using inverse + // kinematics + if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_trajectory, + plan_info.group_name, plan_info.link_name, plan_info.start_joint_position, sampling_time, + joint_trajectory, error_code)) + { + throw CircTrajectoryConversionFailure("Failed to generate valid joint trajectory from the Cartesian path", + error_code.val); + } +} + +std::unique_ptr TrajectoryGeneratorCIRC::setPathCIRC(const MotionPlanInfo& info) const +{ + ROS_DEBUG("Set Cartesian path for CIRC command."); + + KDL::Frame start_pose, goal_pose; + tf::transformEigenToKDL(info.start_pose, start_pose); + tf::transformEigenToKDL(info.goal_pose, goal_pose); + + KDL::Vector path_point; + tf::vectorEigenToKDL(info.circ_path_point.second, path_point); + + // pass the ratio of translational by rotational velocity as equivalent radius + // to get a trajectory with rotational speed, if no (or very little) + // translational distance + // The KDL::Path implementation chooses the motion with the longer duration + // (translation vs. rotation) + // and uses eqradius as scaling factor between the distances. + double eqradius = planner_limits_.getCartesianLimits().getMaxTranslationalVelocity() / + planner_limits_.getCartesianLimits().getMaxRotationalVelocity(); + + try + { + if (info.circ_path_point.first == "center") + { + return PathCircleGenerator::circleFromCenter(start_pose, goal_pose, path_point, eqradius); + } + else // if (info.circ_path_point.first == "interim") + { + return PathCircleGenerator::circleFromInterim(start_pose, goal_pose, path_point, eqradius); + } + } + catch (KDL::Error_MotionPlanning_Circle_No_Plane& e) + { + std::ostringstream os; + os << "Failed to create path object for circle." << e.Description(); + throw CircleNoPlane(os.str()); + } + catch (KDL::Error_MotionPlanning_Circle_ToSmall& e) + { + std::ostringstream os; + os << "Failed to create path object for circle." << e.Description(); + throw CircleToSmall(os.str()); + } + catch (ErrorMotionPlanningCenterPointDifferentRadius& e) + { + std::ostringstream os; + os << "Failed to create path object for circle." << e.Description(); + throw CenterPointDifferentRadius(os.str()); + } + + return nullptr; +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp new file mode 100644 index 0000000000..a91c6e943a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -0,0 +1,203 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/trajectory_generator_lin.h" + +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model, + const LimitsContainer& planner_limits) + : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) +{ + if (!planner_limits_.hasFullCartesianLimits()) + { + ROS_ERROR("Cartesian limits not set for LIN trajectory generator."); + throw TrajectoryGeneratorInvalidLimitsException("Cartesian limits are not fully set for LIN trajectory generator."); + } +} + +void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, + TrajectoryGenerator::MotionPlanInfo& info) const +{ + ROS_DEBUG("Extract necessary information from motion plan request."); + + info.group_name = req.group_name; + std::string frame_id{ robot_model_->getModelFrame() }; + + // goal given in joint space + if (!req.goal_constraints.front().joint_constraints.empty()) + { + info.link_name = robot_model_->getJointModelGroup(req.group_name)->getSolverInstance()->getTipFrame(); + + if (req.goal_constraints.front().joint_constraints.size() != + robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size()) + { + std::ostringstream os; + os << "Number of joints in goal does not match number of joints of group " + "(Number joints goal: " + << req.goal_constraints.front().joint_constraints.size() << " | Number of joints of group: " + << robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size() << ")"; + throw JointNumberMismatch(os.str()); + } + // initializing all joints of the model + for (const auto& joint_name : robot_model_->getVariableNames()) + { + info.goal_joint_position[joint_name] = 0; + } + + for (const auto& joint_item : req.goal_constraints.front().joint_constraints) + { + info.goal_joint_position[joint_item.joint_name] = joint_item.position; + } + + // Ignored return value because at this point the function should always + // return 'true'. + computeLinkFK(robot_model_, info.link_name, info.goal_joint_position, info.goal_pose); + } + // goal given in Cartesian space + else + { + info.link_name = req.goal_constraints.front().position_constraints.front().link_name; + if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || + req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) + { + ROS_WARN("Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); + frame_id = robot_model_->getModelFrame(); + } + else + { + frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; + } + geometry_msgs::Pose goal_pose_msg; + goal_pose_msg.position = + req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position; + goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation; + normalizeQuaternion(goal_pose_msg.orientation); + tf2::convert(goal_pose_msg, info.goal_pose); + } + + assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); + for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames()) + { + auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) }; + if (it == req.start_state.joint_state.name.cend()) + { + std::ostringstream os; + os << "Could not find joint \"" << joint_name << "\" of group \"" << req.group_name + << "\" in start state of request"; + throw LinJointMissingInStartState(os.str()); + } + size_t index = it - req.start_state.joint_state.name.cbegin(); + info.start_joint_position[joint_name] = req.start_state.joint_state.position[index]; + } + + // Ignored return value because at this point the function should always + // return 'true'. + computeLinkFK(robot_model_, info.link_name, info.start_joint_position, info.start_pose); + + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(robot_model_, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) + { + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw LinInverseForGoalIncalculable(os.str()); + } +} + +void TrajectoryGeneratorLIN::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) +{ + // create Cartesian path for lin + std::unique_ptr path(setPathLIN(plan_info.start_pose, plan_info.goal_pose)); + + // create velocity profile + std::unique_ptr vp( + cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); + + // combine path and velocity profile into Cartesian trajectory + // with the third parameter set to false, KDL::Trajectory_Segment does not + // take + // the ownship of Path and Velocity Profile + KDL::Trajectory_Segment cart_trajectory(path.get(), vp.get(), false); + + moveit_msgs::MoveItErrorCodes error_code; + // sample the Cartesian trajectory and compute joint trajectory using inverse + // kinematics + if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_trajectory, + plan_info.group_name, plan_info.link_name, plan_info.start_joint_position, sampling_time, + joint_trajectory, error_code)) + { + std::ostringstream os; + os << "Failed to generate valid joint trajectory from the Cartesian path"; + throw LinTrajectoryConversionFailure(os.str(), error_code.val); + } +} + +std::unique_ptr TrajectoryGeneratorLIN::setPathLIN(const Eigen::Affine3d& start_pose, + const Eigen::Affine3d& goal_pose) const +{ + ROS_DEBUG("Set Cartesian path for LIN command."); + + KDL::Frame kdl_start_pose, kdl_goal_pose; + tf::transformEigenToKDL(start_pose, kdl_start_pose); + tf::transformEigenToKDL(goal_pose, kdl_goal_pose); + double eqradius = planner_limits_.getCartesianLimits().getMaxTranslationalVelocity() / + planner_limits_.getCartesianLimits().getMaxRotationalVelocity(); + KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); + + return std::unique_ptr(new KDL::Path_Line(kdl_start_pose, kdl_goal_pose, rot_interpo, eqradius, true)); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp new file mode 100644 index 0000000000..45b2157e4a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -0,0 +1,261 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/trajectory_generator_ptp.h" +#include "eigen_conversions/eigen_msg.h" +#include "moveit/robot_state/conversions.h" +#include "ros/ros.h" + +#include +#include + +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const robot_model::RobotModelConstPtr& robot_model, + const LimitsContainer& planner_limits) + : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) +{ + if (!planner_limits_.hasJointLimits()) + { + throw TrajectoryGeneratorInvalidLimitsException("joint limit not set"); + } + + joint_limits_ = planner_limits_.getJointLimitContainer(); + + // collect most strict joint limits for each group in robot model + for (const auto& jmg : robot_model->getJointModelGroups()) + { + JointLimit most_strict_limit = joint_limits_.getCommonLimit(jmg->getActiveJointModelNames()); + + if (!most_strict_limit.has_velocity_limits) + { + ROS_ERROR_STREAM("velocity limit not set for group " << jmg->getName()); + throw TrajectoryGeneratorInvalidLimitsException("velocity limit not set for group " + jmg->getName()); + } + if (!most_strict_limit.has_acceleration_limits) + { + ROS_ERROR_STREAM("acceleration limit not set for group " << jmg->getName()); + throw TrajectoryGeneratorInvalidLimitsException("acceleration limit not set for group " + jmg->getName()); + } + if (!most_strict_limit.has_deceleration_limits) + { + ROS_ERROR_STREAM("deceleration limit not set for group " << jmg->getName()); + throw TrajectoryGeneratorInvalidLimitsException("deceleration limit not set for group " + jmg->getName()); + } + + most_strict_limits_.insert(std::pair(jmg->getName(), most_strict_limit)); + } + + ROS_INFO("Initialized Point-to-Point Trajectory Generator."); +} + +void TrajectoryGeneratorPTP::planPTP(const std::map& start_pos, + const std::map& goal_pos, + trajectory_msgs::JointTrajectory& joint_trajectory, const std::string& group_name, + const double& velocity_scaling_factor, const double& acceleration_scaling_factor, + const double& sampling_time) +{ + // initialize joint names + for (const auto& item : goal_pos) + { + joint_trajectory.joint_names.push_back(item.first); + } + + // check if goal already reached + bool goal_reached = true; + for (auto const& goal : goal_pos) + { + if (fabs(start_pos.at(goal.first) - goal.second) >= MIN_MOVEMENT) + { + goal_reached = false; + break; + } + } + if (goal_reached) + { + ROS_INFO_STREAM("Goal already reached, set one goal point explicitly."); + if (joint_trajectory.points.empty()) + { + trajectory_msgs::JointTrajectoryPoint point; + point.time_from_start = ros::Duration(sampling_time); + for (const std::string& joint_name : joint_trajectory.joint_names) + { + point.positions.push_back(start_pos.at(joint_name)); + point.velocities.push_back(0); + point.accelerations.push_back(0); + } + joint_trajectory.points.push_back(point); + } + return; + } + + // compute the fastest trajectory and choose the slowest joint as leading axis + std::string leading_axis = joint_trajectory.joint_names.front(); + double max_duration = -1.0; + + std::map velocity_profile; + for (const auto& joint_name : joint_trajectory.joint_names) + { + // create vecocity profile if necessary + velocity_profile.insert(std::make_pair( + joint_name, + VelocityProfileATrap(velocity_scaling_factor * most_strict_limits_.at(group_name).max_velocity, + acceleration_scaling_factor * most_strict_limits_.at(group_name).max_acceleration, + acceleration_scaling_factor * most_strict_limits_.at(group_name).max_deceleration))); + + velocity_profile.at(joint_name).SetProfile(start_pos.at(joint_name), goal_pos.at(joint_name)); + if (velocity_profile.at(joint_name).Duration() > max_duration) + { + max_duration = velocity_profile.at(joint_name).Duration(); + leading_axis = joint_name; + } + } + + // Full Synchronization + // This should only work if all axes have same max_vel, max_acc, max_dec + // values + // reset the velocity profile for other joints + double acc_time = velocity_profile.at(leading_axis).firstPhaseDuration(); + double const_time = velocity_profile.at(leading_axis).secondPhaseDuration(); + double dec_time = velocity_profile.at(leading_axis).thirdPhaseDuration(); + + for (const auto& joint_name : joint_trajectory.joint_names) + { + if (joint_name != leading_axis) + { + // make full synchronization + // causes the program to terminate if acc_time<=0 or dec_time<=0 (should + // be prevented by goal_reached block above) + // by using the most strict limit, the following should always return true + if (!velocity_profile.at(joint_name) + .setProfileAllDurations(start_pos.at(joint_name), goal_pos.at(joint_name), acc_time, const_time, + dec_time)) + // LCOV_EXCL_START + { + std::stringstream error_str; + error_str << "TrajectoryGeneratorPTP::planPTP(): Can not synchronize " + "velocity profile of axis " + << joint_name << " with leading axis " << leading_axis; + throw PtpVelocityProfileSyncFailed(error_str.str()); + } + // LCOV_EXCL_STOP + } + } + + // first generate the time samples + std::vector time_samples; + for (double t_sample = 0.0; t_sample < max_duration; t_sample += sampling_time) + { + time_samples.push_back(t_sample); + } + // add last time + time_samples.push_back(max_duration); + + // construct joint trajectory point + for (double time_stamp : time_samples) + { + trajectory_msgs::JointTrajectoryPoint point; + point.time_from_start = ros::Duration(time_stamp); + for (std::string& joint_name : joint_trajectory.joint_names) + { + point.positions.push_back(velocity_profile.at(joint_name).Pos(time_stamp)); + point.velocities.push_back(velocity_profile.at(joint_name).Vel(time_stamp)); + point.accelerations.push_back(velocity_profile.at(joint_name).Acc(time_stamp)); + } + joint_trajectory.points.push_back(point); + } + + // Set last point velocity and acceleration to zero + std::fill(joint_trajectory.points.back().velocities.begin(), joint_trajectory.points.back().velocities.end(), 0.0); + std::fill(joint_trajectory.points.back().accelerations.begin(), joint_trajectory.points.back().accelerations.end(), + 0.0); +} + +void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, + MotionPlanInfo& info) const +{ + info.group_name = req.group_name; + + // extract start state information + info.start_joint_position.clear(); + for (std::size_t i = 0; i < req.start_state.joint_state.name.size(); ++i) + { + info.start_joint_position[req.start_state.joint_state.name[i]] = req.start_state.joint_state.position[i]; + } + + // extract goal + info.goal_joint_position.clear(); + if (!req.goal_constraints.at(0).joint_constraints.empty()) + { + for (const auto& joint_constraint : req.goal_constraints.at(0).joint_constraints) + { + info.goal_joint_position[joint_constraint.joint_name] = joint_constraint.position; + } + } + // slove the ik + else + { + geometry_msgs::Point p = + req.goal_constraints.at(0).position_constraints.at(0).constraint_region.primitive_poses.at(0).position; + p.x -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.x; + p.y -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.y; + p.z -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.z; + + geometry_msgs::Pose pose; + pose.position = p; + pose.orientation = req.goal_constraints.at(0).orientation_constraints.at(0).orientation; + Eigen::Isometry3d pose_eigen; + normalizeQuaternion(pose.orientation); + tf2::convert(pose, pose_eigen); + if (!computePoseIK(robot_model_, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name, + pose_eigen, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position)) + { + throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose"); + } + } +} + +void TrajectoryGeneratorPTP::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) +{ + // plan the ptp trajectory + planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory, plan_info.group_name, + req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, sampling_time); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp b/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp new file mode 100644 index 0000000000..f19da41b8d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp @@ -0,0 +1,451 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner/velocity_profile_atrap.h" + +namespace pilz_industrial_motion_planner +{ +VelocityProfileATrap::VelocityProfileATrap(double max_vel, double max_acc, double max_dec) + : max_vel_(fabs(max_vel)) + , max_acc_(fabs(max_acc)) + , max_dec_(fabs(max_dec)) + , start_pos_(0) + , end_pos_(0) + , start_vel_(0) + , a1_(0) + , a2_(0) + , a3_(0) + , b1_(0) + , b2_(0) + , b3_(0) + , c1_(0) + , c2_(0) + , c3_(0) + , t_a_(0) + , t_b_(0) + , t_c_(0) +{ +} + +void VelocityProfileATrap::SetProfile(double pos1, double pos2) +{ + start_pos_ = pos1; + end_pos_ = pos2; + start_vel_ = 0.0; + + if (start_pos_ == end_pos_) + { + // goal already reached, set everything to zero + setEmptyProfile(); + return; + } + else + { + // get the sign + double s = ((end_pos_ - start_pos_) > 0.0) - ((end_pos_ - start_pos_) < 0.0); + + double dis = fabs(end_pos_ - start_pos_); + double min_dis_max_vel = 0.5 * max_vel_ * max_vel_ / max_acc_ + 0.5 * max_vel_ * max_vel_ / max_dec_; + + // max_vel can be reached + if (dis > min_dis_max_vel) + { + // acceleration phase + a1_ = start_pos_; + a2_ = 0.0; + a3_ = s * max_acc_ / 2.0; + t_a_ = max_vel_ / max_acc_; + + // constant phase + b1_ = a1_ + a3_ * t_a_ * t_a_; + b2_ = s * max_vel_; + b3_ = 0; + t_b_ = (dis - min_dis_max_vel) / max_vel_; + + // deceleration phase + c1_ = b1_ + b2_ * (t_b_); + c2_ = s * max_vel_; + c3_ = -s * max_dec_ / 2.0; + t_c_ = max_vel_ / max_dec_; + } + // max_vel cannot be reached, no constant velocity phase + else + { + // compute the new velocity of constant phase + double new_vel = s * sqrt(2.0 * dis * max_acc_ * max_dec_ / (max_acc_ + max_dec_)); + + // acceleration phase + a1_ = start_pos_; + a2_ = 0.0; + a3_ = s * max_acc_ / 2.0; + t_a_ = fabs(new_vel) / max_acc_; + + // constant phase + b1_ = a1_ + a3_ * t_a_ * t_a_; + b2_ = new_vel; + b3_ = 0; + t_b_ = 0.0; + + // deceleration phase + c1_ = b1_; + c2_ = new_vel; + c3_ = -s * max_dec_ / 2.0; + t_c_ = fabs(new_vel) / max_dec_; + } + } +} + +void VelocityProfileATrap::SetProfileDuration(double pos1, double pos2, double duration) +{ + // compute the fastest case + SetProfile(pos1, pos2); + + // cannot be faster + if (Duration() > duration) + { + return; + } + + double ratio = Duration() / duration; + a2_ *= ratio; + a3_ *= ratio * ratio; + b2_ *= ratio; + b3_ *= ratio * ratio; + c2_ *= ratio; + c3_ *= ratio * ratio; + t_a_ /= ratio; + t_b_ /= ratio; + t_c_ /= ratio; +} + +bool VelocityProfileATrap::setProfileAllDurations(double pos1, double pos2, double duration1, double duration2, + double duration3) +{ + // compute the fastest case + SetProfile(pos1, pos2); + + assert(duration1 > 0); + assert(duration3 > 0); + + // cannot be faster + if (Duration() - (duration1 + duration2 + duration3) > KDL::epsilon) + { + return false; + } + + // get the sign + double s = ((end_pos_ - start_pos_) > 0.0) - ((end_pos_ - start_pos_) < 0.0); + // compute the new velocity/acceleration/decel4eration + double dis = fabs(end_pos_ - start_pos_); + double new_vel = s * dis / (duration2 + duration1 / 2.0 + duration3 / 2.0); + double new_acc = new_vel / duration1; + double new_dec = -new_vel / duration3; + if ((fabs(new_vel) - max_vel_ > KDL::epsilon) || (fabs(new_acc) - max_acc_ > KDL::epsilon) || + (fabs(new_dec) - max_dec_ > KDL::epsilon)) + { + return false; + } + else + { + // set profile + start_pos_ = pos1; + end_pos_ = pos2; + + // acceleration phase + a1_ = start_pos_; + a2_ = 0.0; + a3_ = new_acc / 2.0; + t_a_ = duration1; + + // constant phase + b1_ = a1_ + a3_ * t_a_ * t_a_; + b2_ = new_vel; + b3_ = 0; + t_b_ = duration2; + + // deceleration phase + c1_ = b1_ + b2_ * (t_b_); + c2_ = new_vel; + c3_ = new_dec / 2.0; + t_c_ = duration3; + + return true; + } +} + +bool VelocityProfileATrap::setProfileStartVelocity(double pos1, double pos2, double vel1) +{ + if (vel1 == 0) + { + SetProfile(pos1, pos2); + return true; + } + + // get the sign + double s = ((pos2 - pos1) > 0.0) - ((pos2 - pos1) < 0.0); + + if (s * vel1 <= 0) + { + // TODO initial velocity is in opposite derection of start-end vector + return false; + } + + start_pos_ = pos1; + end_pos_ = pos2; + start_vel_ = vel1; + + // minimum brake distance + double min_brake_dis = 0.5 * vel1 * vel1 / max_dec_; + // minimum distance to reach the maximum velocity + double min_dis_max_vel = + 0.5 * (max_vel_ - start_vel_) * (max_vel_ + start_vel_) / max_acc_ + 0.5 * max_vel_ * max_vel_ / max_dec_; + double dis = fabs(end_pos_ - start_pos_); + + // brake, acceleration in opposite direction, deceleration + if (dis <= min_brake_dis) + { + // brake to zero velocity + t_a_ = fabs(start_vel_ / max_dec_); + a1_ = start_pos_; + a2_ = start_vel_; + a3_ = -0.5 * s * max_dec_; + + // compute the velocity in opposite direction + double new_vel = -s * sqrt(2.0 * fabs(min_brake_dis - dis) * max_acc_ * max_dec_ / (max_acc_ + max_dec_)); + + // acceleration in opposite direction + t_b_ = fabs(new_vel / max_acc_); + b1_ = a1_ + a2_ * t_a_ + a3_ * t_a_ * t_a_; + b2_ = 0; + b3_ = -s * 0.5 * max_acc_; + + // deceleration to zero + t_c_ = fabs(new_vel / max_dec_); + c1_ = b1_ + b2_ * t_b_ + b3_ * t_b_ * t_b_; + c2_ = new_vel; + c3_ = 0.5 * s * max_dec_; + } + else if (dis <= min_dis_max_vel) + { + // compute the reached velocity + double new_vel = + s * sqrt((dis + 0.5 * start_vel_ * start_vel_ / max_acc_) * 2.0 * max_acc_ * max_dec_ / (max_acc_ + max_dec_)); + + // acceleration to new velocity + t_a_ = fabs(new_vel - start_vel_) / max_acc_; + a1_ = start_pos_; + a2_ = start_vel_; + a3_ = 0.5 * s * max_acc_; + + // no constant velocity phase + t_b_ = 0; + b1_ = a1_ + a2_ * t_a_ + a3_ * t_a_ * t_a_; + b2_ = 0; + b3_ = 0; + + // deceleration to zero velocity + t_c_ = fabs(new_vel / max_dec_); + c1_ = b1_; + c2_ = new_vel; + c3_ = -0.5 * s * max_dec_; + } + else + { + // acceleration to max velocity + t_a_ = fabs(max_vel_ - start_vel_) / max_acc_; + a1_ = start_pos_; + a2_ = start_vel_; + a3_ = 0.5 * s * max_acc_; + + // constant velocity + t_b_ = (dis - min_dis_max_vel) / max_vel_; + b1_ = a1_ + a2_ * t_a_ + a3_ * t_a_ * t_a_; + b2_ = max_vel_; + b3_ = 0; + + // deceleration to zero velocity + t_c_ = max_vel_ / max_dec_; + c1_ = b1_ + b2_ * t_b_ + b3_ * t_b_ * t_b_; + c2_ = max_vel_; + c3_ = -0.5 * s * max_dec_; + } + + return true; +} + +double VelocityProfileATrap::Duration() const +{ + return t_a_ + t_b_ + t_c_; +} + +double VelocityProfileATrap::Pos(double time) const +{ + if (time < 0) + { + return start_pos_; + } + else if (time < t_a_) + { + return a1_ + time * (a2_ + a3_ * time); + } + else if (time < (t_a_ + t_b_)) + { + return b1_ + (time - t_a_) * (b2_ + b3_ * (time - t_a_)); + } + else if (time <= (t_a_ + t_b_ + t_c_)) + { + return c1_ + (time - t_a_ - t_b_) * (c2_ + c3_ * (time - t_a_ - t_b_)); + } + else + { + return end_pos_; + } +} + +double VelocityProfileATrap::Vel(double time) const +{ + if (time < 0) + { + return start_vel_; + } + else if (time < t_a_) + { + return a2_ + 2 * a3_ * time; + } + else if (time < (t_a_ + t_b_)) + { + return b2_ + 2 * b3_ * (time - t_a_); + } + else if (time <= (t_a_ + t_b_ + t_c_)) + { + return c2_ + 2 * c3_ * (time - t_a_ - t_b_); + } + else + { + return 0; + } +} + +double VelocityProfileATrap::Acc(double time) const +{ + if (time <= 0) + { + return 0; + } + else if (time <= t_a_) + { + return 2 * a3_; + } + else if (time <= (t_a_ + t_b_)) + { + return 2 * b3_; + } + else if (time <= (t_a_ + t_b_ + t_c_)) + { + return 2 * c3_; + } + else + { + return 0; + } +} + +KDL::VelocityProfile* VelocityProfileATrap::Clone() const +{ + VelocityProfileATrap* trap = new VelocityProfileATrap(max_vel_, max_acc_, max_dec_); + trap->setProfileAllDurations(this->start_pos_, this->end_pos_, this->t_a_, this->t_b_, this->t_c_); + return trap; +} + +// LCOV_EXCL_START // No tests for the print function +void VelocityProfileATrap::Write(std::ostream& os) const +{ + os << *this; +} + +std::ostream& operator<<(std::ostream& os, const VelocityProfileATrap& p) +{ + os << "Asymmetric Trapezoid " << std::endl + << "maximal velocity: " << p.max_vel_ << std::endl + << "maximal acceleration: " << p.max_acc_ << std::endl + << "maximal deceleration: " << p.max_dec_ << std::endl + << "start position: " << p.start_pos_ << std::endl + << "end position: " << p.end_pos_ << std::endl + << "start velocity: " << p.start_vel_ << std::endl + << "a1: " << p.a1_ << std::endl + << "a2: " << p.a2_ << std::endl + << "a3: " << p.a3_ << std::endl + << "b1: " << p.b1_ << std::endl + << "b2: " << p.b2_ << std::endl + << "b3: " << p.b3_ << std::endl + << "c1: " << p.c1_ << std::endl + << "c2: " << p.c2_ << std::endl + << "c3: " << p.c3_ << std::endl + << "firstPhaseDuration " << p.firstPhaseDuration() << std::endl + << "secondPhaseDuration " << p.secondPhaseDuration() << std::endl + << "thirdPhaseDuration " << p.thirdPhaseDuration() << std::endl; + return os; +} +// LCOV_EXCL_STOP + +bool VelocityProfileATrap::operator==(const VelocityProfileATrap& other) const +{ + return (max_vel_ == other.max_vel_ && max_acc_ == other.max_acc_ && max_dec_ == other.max_dec_ && + start_pos_ == other.start_pos_ && end_pos_ == other.end_pos_ && start_vel_ == other.start_vel_ && + a1_ == other.a1_ && a2_ == other.a2_ && a3_ == other.a3_ && b1_ == other.b1_ && b2_ == other.b2_ && + b3_ == other.b3_ && c1_ == other.c1_ && c2_ == other.c2_ && c3_ == other.c3_ && t_a_ == other.t_a_ && + t_b_ == other.t_b_ && t_c_ == other.t_c_); +} + +VelocityProfileATrap::~VelocityProfileATrap() +{ +} + +void VelocityProfileATrap::setEmptyProfile() +{ + a1_ = end_pos_; + a2_ = 0; + a3_ = 0; + b1_ = end_pos_; + b2_ = 0; + c1_ = end_pos_; + c2_ = 0; + c3_ = 0; + + t_a_ = 0; + t_b_ = 0; + t_c_ = 0; +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md new file mode 100644 index 0000000000..4f2abec293 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md @@ -0,0 +1,62 @@ + + +# Acceptance Test LIN Motion using the MoveIt Motion Planning Plugin on the real robot +This test checks that the real robot system is able to perform a LIN Motion to a goal state given by the user. The test is performed using the moveit motion planning plugin. Note that before you can apply the LIN command the robot has +to be moved out of singularities. + +## Prerequisites + - Properly connect and startup the robot. Make sure a emergency stop is within reach. + +## Test Sequence: + 1. Bringup can: `sudo ip link set can0 up type can bitrate 1000000` + 2. Run `roslaunch moveit_resources_prbt_moveit_config moveit_planning_execution.launch sim:=False pipeline:=pilz_industrial_motion_planner` + 3. In the motion planing widget (lower left part of moveit) choose PTP in the dropdown below "Trapezoidal Command Planner" (see image) +![moveit_1](img/acceptance_test_lin_img1.png) + 4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle to select a singularity free position. Click on "plan and execute". +![moveit_2](img/acceptance_test_lin_img2.png) + 5. The motion planning widget (lower left part of moveit) choose LIN in the dropdown below "Trapezoidal Command Planner" (see image) +![moveit_1](img/acceptance_test_lin_img3.png) + 6. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle the select goal pose. + Move the robot about 10-20cm choose a goal where the configuration has no jumps. Set velocity and acceleration scaling to 0.1. Click on "plan and execute". +![moveit_2](img/acceptance_test_lin_img4.png) + +## Expected Results: + 1. Can should be visible with `ifconfig` displayed as can0 + 2. A -click- indicates the enabling of the drives. + 3. PTP was available for selection + 4. The robot should move to the desired position. + 5. LIN was available for selection + 6. The robot should move to the desired position via a straight line. +--- diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md new file mode 100644 index 0000000000..732ec8ec4f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md @@ -0,0 +1,54 @@ + + +# Acceptance Test PTP Motion using the MoveIt Motion Planning Plugin on the real robot +This test checks that the real robot system is able to perform a PTP Motion to a goal state given by the user. The test is performed using the moveit motion planning plugin. + +## Prerequisites + - Properly connect and startup the robot. Make sure a emergency stop is within reach. + +## Test Sequence: + 1. Bringup can: `sudo ip link set can0 up type can bitrate 1000000` + 2. Run `roslaunch moveit_resources_prbt_moveit_config moveit_planning_execution.launch sim:=False pipeline:=pilz_industrial_motion_planner` + 3. The motion planning widget (lower left part of moveit) choose PTP in the dropdown below "Trapezoidal Command Planner" (see image) +![moveit_1](img/acceptance_test_ptp_img1.png) + 4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle the select goal pose. Click on "plan and execute". +![moveit_2](img/acceptance_test_ptp_img2.png) + +## Expected Results: + 1. Can should be visible with `ifconfig` displayed as can0 + 2. A -click- indicates the enabling of the drives. + 3. PTP was available for selection + 4. The robot should move to the desired position. All axis should start and stop at the same time. +--- diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img1.png b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img1.png new file mode 100644 index 0000000000..c0a55f478f Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img1.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img2.png b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img2.png new file mode 100644 index 0000000000..84f533025e Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img2.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img3.png b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img3.png new file mode 100644 index 0000000000..41804fff3b Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img3.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img4.png b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img4.png new file mode 100644 index 0000000000..f666c79907 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_lin_img4.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_ptp_img1.png b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_ptp_img1.png new file mode 100644 index 0000000000..5ecad9d06d Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_ptp_img1.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_ptp_img2.png b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_ptp_img2.png new file mode 100644 index 0000000000..03939c2130 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/img/acceptance_test_ptp_img2.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp new file mode 100644 index 0000000000..9eef3bf195 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp @@ -0,0 +1,661 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "test_utils.h" + +#include "pilz_industrial_motion_planner/command_list_manager.h" +#include "pilz_industrial_motion_planner/tip_frame_getter.h" + +const std::string ROBOT_DESCRIPTION_STR{ "robot_description" }; +const std::string EMPTY_VALUE{ "" }; + +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); + +using testutils::hasStrictlyIncreasingTime; +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner_testutils; + +static std::string createManipulatorJointName(const size_t& joint_number) +{ + return std::string("prbt_joint_") + std::to_string(joint_number + 1); +} + +static std::string createGripperJointName(const size_t& joint_number) +{ + switch (joint_number) + { + case 0: + return "prbt_gripper_finger_left_joint"; + default: + break; + } + throw std::runtime_error("Could not create gripper joint name"); +} + +class IntegrationTestCommandListManager : public testing::Test +{ +protected: + void SetUp() override; + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(ROBOT_DESCRIPTION_STR).getModel() }; + std::shared_ptr manager_; + planning_scene::PlanningScenePtr scene_; + planning_pipeline::PlanningPipelinePtr pipeline_; + + std::unique_ptr data_loader_; +}; + +void IntegrationTestCommandListManager::SetUp() +{ + // get necessary parameters + if (!robot_model_) + { + FAIL() << "Robot model could not be loaded."; + } + + std::string test_data_file_name; + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name)); + + // load the test data provider + data_loader_.reset( + new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name, robot_model_ }); + ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; + + // Define and set the current scene and manager test object + manager_ = std::make_shared(ph_, robot_model_); + scene_ = std::make_shared(robot_model_); + pipeline_ = std::make_shared(robot_model_, ph_); +} + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + * + */ +TEST_F(IntegrationTestCommandListManager, TestExceptionErrorCodeMapping) +{ + std::shared_ptr nbr_ex{ new NegativeBlendRadiusException("") }; + EXPECT_EQ(nbr_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + + std::shared_ptr lbrnz_ex{ new LastBlendRadiusNotZeroException("") }; + EXPECT_EQ(lbrnz_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + + std::shared_ptr sss_ex{ new StartStateSetException("") }; + EXPECT_EQ(sss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + + std::shared_ptr obr_ex{ new OverlappingBlendRadiiException("") }; + EXPECT_EQ(obr_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + + std::shared_ptr pp_ex{ new PlanningPipelineException("") }; + EXPECT_EQ(pp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); +} + +/** + * @brief Tests the concatenation of three motion commands. + * + * Test Sequence: + * 1. Generate request with three trajectories and zero blend radius. + * 2. Generate request with first trajectory and zero blend radius. + * 3. Generate request with second trajectory and zero blend radius. + * 4. Generate request with third trajectory and zero blend radius. + * + * Expected Results: + * 1. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive + * 2. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive + * 3. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive + * 4. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive + * resulting duration in step1 is approximately step2 + step3 + step4 + */ +TEST_F(IntegrationTestCommandListManager, concatThreeSegments) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + ASSERT_GE(seq.size(), 3u); + seq.erase(3, seq.size()); + seq.setAllBlendRadiiToZero(); + + RobotTrajCont res123_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res123_vec.size(), 1u); + EXPECT_GT(res123_vec.front()->getWayPointCount(), 0u); + EXPECT_TRUE(hasStrictlyIncreasingTime(res123_vec.front())) << "Time steps not strictly positively increasing"; + + ROS_INFO("step 2: only first segment"); + moveit_msgs::MotionSequenceRequest req_1; + req_1.items.resize(1); + req_1.items.at(0).req = seq.getCmd(0).toRequest(); + req_1.items.at(0).blend_radius = 0.; + RobotTrajCont res1_vec{ manager_->solve(scene_, pipeline_, req_1) }; + EXPECT_EQ(res1_vec.size(), 1u); + EXPECT_GT(res1_vec.front()->getWayPointCount(), 0u); + EXPECT_EQ(res1_vec.front()->getFirstWayPoint().getVariableCount(), + req_1.items.at(0).req.start_state.joint_state.name.size()); + + ROS_INFO("step 3: only second segment"); + moveit_msgs::MotionSequenceRequest req_2; + req_2.items.resize(1); + req_2.items.at(0).req = seq.getCmd(1).toRequest(); + req_2.items.at(0).blend_radius = 0.; + RobotTrajCont res2_vec{ manager_->solve(scene_, pipeline_, req_2) }; + EXPECT_EQ(res2_vec.size(), 1u); + EXPECT_GT(res2_vec.front()->getWayPointCount(), 0u); + EXPECT_EQ(res2_vec.front()->getFirstWayPoint().getVariableCount(), + req_2.items.at(0).req.start_state.joint_state.name.size()); + + ROS_INFO("step 4: only third segment"); + moveit_msgs::MotionSequenceRequest req_3; + req_3.items.resize(1); + req_3.items.at(0).req = seq.getCmd(2).toRequest(); + req_3.items.at(0).blend_radius = 0.; + RobotTrajCont res3_vec{ manager_->solve(scene_, pipeline_, req_3) }; + EXPECT_EQ(res3_vec.size(), 1u); + EXPECT_GT(res3_vec.front()->getWayPointCount(), 0u); + EXPECT_EQ(res3_vec.front()->getFirstWayPoint().getVariableCount(), + req_3.items.at(0).req.start_state.joint_state.name.size()); + + // durations for the different segments + auto t1_2_3 = res123_vec.front()->getWayPointDurationFromStart(res123_vec.front()->getWayPointCount() - 1); + auto t1 = res1_vec.front()->getWayPointDurationFromStart(res123_vec.front()->getWayPointCount() - 1); + auto t2 = res2_vec.front()->getWayPointDurationFromStart(res123_vec.front()->getWayPointCount() - 1); + auto t3 = res3_vec.front()->getWayPointDurationFromStart(res123_vec.front()->getWayPointCount() - 1); + ROS_DEBUG_STREAM("total time: " << t1_2_3 << " t1:" << t1 << " t2:" << t2 << " t3:" << t3); + EXPECT_LT(fabs((t1_2_3 - t1 - t2 - t3)), 0.4); +} + +/** + * @brief Tests if times are strictly increasing with selective blending + * + * Test Sequence: + * 1. Generate request with three trajectories where only the first has a + * blend radius. + * 1. Generate request with three trajectories where only the second has a + * blend radius. + * + * Expected Results: + * 1. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly increasing in time + * 2. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly increasing in time + */ +TEST_F(IntegrationTestCommandListManager, concatSegmentsSelectiveBlending) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + ASSERT_GE(seq.size(), 3u); + seq.erase(3, seq.size()); + seq.setAllBlendRadiiToZero(); + seq.setBlendRadius(0, 0.1); + RobotTrajCont res1{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res1.size(), 1u); + EXPECT_GT(res1.front()->getWayPointCount(), 0u); + EXPECT_TRUE(hasStrictlyIncreasingTime(res1.front())) << "Time steps not strictly positively increasing"; + + seq.setAllBlendRadiiToZero(); + seq.setBlendRadius(1, 0.1); + RobotTrajCont res2{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res2.size(), 1u); + EXPECT_GT(res2.front()->getWayPointCount(), 0u); + EXPECT_TRUE(hasStrictlyIncreasingTime(res2.front())) << "Time steps not strictly positively increasing"; +} + +/** + * @brief Tests the concatenation of two ptp commands + * + * Test Sequence: + * 1. Generate request with two PTP trajectories and zero blend radius. + * + * Expected Results: + * 1. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive. + */ +TEST_F(IntegrationTestCommandListManager, concatTwoPtpSegments) +{ + Sequence seq{ data_loader_->getSequence("PtpPtpSequence") }; + ASSERT_GE(seq.size(), 2u); + seq.setAllBlendRadiiToZero(); + + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_vec.size(), 1u); + EXPECT_GT(res_vec.front()->getWayPointCount(), 0u); + EXPECT_TRUE(hasStrictlyIncreasingTime(res_vec.front())); +} + +/** + * @brief Tests the concatenation of ptp and a lin command + * + * Test Sequence: + * 1. Generate request with PTP and LIN trajectory and zero blend radius. + * + * Expected Results: + * 1. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive. + */ +TEST_F(IntegrationTestCommandListManager, concatPtpAndLinSegments) +{ + Sequence seq{ data_loader_->getSequence("PtpLinSequence") }; + ASSERT_GE(seq.size(), 2u); + seq.setAllBlendRadiiToZero(); + + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_vec.size(), 1u); + EXPECT_GT(res_vec.front()->getWayPointCount(), 0u); + EXPECT_TRUE(hasStrictlyIncreasingTime(res_vec.front())); +} + +/** + * @brief Tests the concatenation of a lin and a ptp command + * + * Test Sequence: + * 1. Generate request with LIN and PTP trajectory and zero blend radius. + * + * Expected Results: + * 1. Generation of concatenated trajectory is successful. + * All time steps of resulting trajectory are strictly positive. + */ +TEST_F(IntegrationTestCommandListManager, concatLinAndPtpSegments) +{ + Sequence seq{ data_loader_->getSequence("LinPtpSequence") }; + ASSERT_GE(seq.size(), 2u); + seq.setAllBlendRadiiToZero(); + + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_vec.size(), 1u); + EXPECT_GT(res_vec.front()->getWayPointCount(), 0u); + EXPECT_TRUE(hasStrictlyIncreasingTime(res_vec.front())); +} + +/** + * @brief Tests the blending of motion commands + * + * - Test Sequence: + * 1. Generate request with two trajectories and request blending. + * + * - Expected Results: + * 1. blending is successful, result trajectory is not empty + */ +TEST_F(IntegrationTestCommandListManager, blendTwoSegments) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + ASSERT_EQ(seq.size(), 2u); + moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, req) }; + EXPECT_EQ(res_vec.size(), 1u); + EXPECT_GT(res_vec.front()->getWayPointCount(), 0u); + EXPECT_EQ(res_vec.front()->getFirstWayPoint().getVariableCount(), + req.items.at(0).req.start_state.joint_state.name.size()); + + ros::NodeHandle nh; + ros::Publisher pub = nh.advertise("my_planned_path", 1); + ros::Duration duration(1.0); // wait to notify possible subscribers + duration.sleep(); + + moveit_msgs::DisplayTrajectory display_trajectory; + moveit_msgs::RobotTrajectory rob_traj_msg; + res_vec.front()->getRobotTrajectoryMsg(rob_traj_msg); + display_trajectory.trajectory.push_back(rob_traj_msg); + pub.publish(display_trajectory); +} + +// ------------------ +// FAILURE cases +// ------------------ + +/** + * @brief Tests sending an empty blending request. + * + * Test Sequence: + * 1. Generate empty request and request blending. + * + * Expected Results: + * 1. blending is successful, result trajectory container is empty + */ +TEST_F(IntegrationTestCommandListManager, emptyList) +{ + moveit_msgs::MotionSequenceRequest empty_list; + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, empty_list) }; + EXPECT_TRUE(res_vec.empty()); +} + +/** + * @brief Checks that exception is thrown if first goal is not reachable. + * + * Test Sequence: + * 1. Generate request with first goal out of workspace. + * + * Expected Results: + * 1. Exception is thrown. + */ +TEST_F(IntegrationTestCommandListManager, firstGoalNotReachable) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + ASSERT_GE(seq.size(), 2u); + LinCart& lin{ seq.getCmd(0) }; + lin.getGoalConfiguration().getPose().position.y = 2700; + EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), PlanningPipelineException); +} + +/** + * @brief Checks that exception is thrown if second goal has a start state. + * + * Test Sequence: + * 1. Generate request, second goal has an invalid start state set. + * + * Expected Results: + * 1. Exception is thrown. + */ +TEST_F(IntegrationTestCommandListManager, startStateNotFirstGoal) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + ASSERT_GE(seq.size(), 2u); + const LinCart& lin{ seq.getCmd(0) }; + moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + req.items.at(1).req.start_state = lin.getGoalConfiguration().toMoveitMsgsRobotState(); + EXPECT_THROW(manager_->solve(scene_, pipeline_, req), StartStateSetException); +} + +/** + * @brief Checks that exception is thrown in case of blending request with + * negative blend_radius. + * + * Test Sequence: + * 1. Generate request, first goal has negative blend_radius. + * + * Expected Results: + * 1. Exception is thrown. + */ +TEST_F(IntegrationTestCommandListManager, blendingRadiusNegative) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + ASSERT_GE(seq.size(), 2u); + seq.setBlendRadius(0, -0.3); + EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), NegativeBlendRadiusException); +} + +/** + * @brief Checks that exception is thrown if last blend radius is not zero. + * + * + * Test Sequence: + * 1. Generate request, second goal has non-zero blend_radius. + * + * Expected Results: + * 1. Exception is thrown. + */ +TEST_F(IntegrationTestCommandListManager, lastBlendingRadiusNonZero) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + ASSERT_EQ(seq.size(), 2u); + seq.setBlendRadius(1, 0.03); + EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), LastBlendRadiusNotZeroException); +} + +/** + * @brief Checks that exception is thrown if blend radius is greater than the + * segment. + * + * Test Sequence: + * 1. Generate request with huge blending radius, so that trajectories are + * completely inside + * + * Expected Results: + * 2. Exception is thrown. + */ +TEST_F(IntegrationTestCommandListManager, blendRadiusGreaterThanSegment) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + ASSERT_GE(seq.size(), 2u); + seq.setBlendRadius(0, 42.0); + EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), BlendingFailedException); +} + +/** + * @brief Checks that exception is thrown if two consecutive blend radii + * overlap. + * + * Test Sequence: + * 1. Generate request with three trajectories + * 2. Increase second blend radius, so that the radii overlap + * + * Expected Results: + * 1. blending succeeds, result trajectory is not empty + * 2. Exception is thrown. + */ +TEST_F(IntegrationTestCommandListManager, blendingRadiusOverlapping) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + ASSERT_GE(seq.size(), 3u); + seq.erase(3, seq.size()); + + RobotTrajCont res_valid_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_valid_vec.size(), 1u); + EXPECT_GT(res_valid_vec.front()->getWayPointCount(), 0u); + + // calculate distance from first to second goal + const PtpJointCart& ptp{ seq.getCmd(0) }; + const CircInterimCart& circ{ seq.getCmd(1) }; + Eigen::Isometry3d p1, p2; + tf2::fromMsg(ptp.getGoalConfiguration().getPose(), p1); + tf2::fromMsg(circ.getGoalConfiguration().getPose(), p2); + auto distance = (p2.translation() - p1.translation()).norm(); + + seq.setBlendRadius(1, (distance - seq.getBlendRadius(0) + 0.01)); // overlapping radii + EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), OverlappingBlendRadiiException); +} + +/** + * @brief Tests if the planned execution time scales correctly with the number + * of repetitions. + * + * Test Sequence: + * 1. Generate trajectory and save calculated execution time. + * 2. Generate request with repeated path along the points from Test Step 1 + * (repeated two times). + * + * Expected Results: + * 1. Blending succeeds, result trajectory is not empty. + * 2. Blending succeeds, planned execution time should be approx N times + * the single planned execution time. + */ +TEST_F(IntegrationTestCommandListManager, TestExecutionTime) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + ASSERT_GE(seq.size(), 2u); + RobotTrajCont res_single_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_single_vec.size(), 1u); + EXPECT_GT(res_single_vec.front()->getWayPointCount(), 0u); + + moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + // Create large request by making copies of the original sequence commands + // and adding them to the end of the original sequence. + const size_t n{ req.items.size() }; + for (size_t i = 0; i < n; ++i) + { + moveit_msgs::MotionSequenceItem item{ req.items.at(i) }; + if (i == 0) + { + // Remove start state because only the first request + // is allowed to have a start state in a sequence. + item.req.start_state = moveit_msgs::RobotState(); + } + req.items.push_back(item); + } + + RobotTrajCont res_n_vec{ manager_->solve(scene_, pipeline_, req) }; + EXPECT_EQ(res_n_vec.size(), 1u); + EXPECT_GT(res_n_vec.front()->getWayPointCount(), 0u); + + const double trajectory_time_1 = + res_single_vec.front()->getWayPointDurationFromStart(res_single_vec.front()->getWayPointCount() - 1); + const double trajectory_time_n = + res_n_vec.front()->getWayPointDurationFromStart(res_n_vec.front()->getWayPointCount() - 1); + double multiplicator = req.items.size() / n; + EXPECT_LE(trajectory_time_n, trajectory_time_1 * multiplicator); + EXPECT_GE(trajectory_time_n, trajectory_time_1 * multiplicator * 0.5); +} + +/** + * @brief Tests if it possible to send requests which contain more than + * one group. + * + * Please note: This test is still quite trivial. It does not check the + * "correctness" of a calculated trajectory. It only checks that for each + * group and group change there exists a calculated trajectory. + * + */ +TEST_F(IntegrationTestCommandListManager, TestDifferentGroups) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequenceWithGripper") }; + ASSERT_GE(seq.size(), 1u); + // Count the number of group changes in the given sequence + unsigned int num_groups{ 1 }; + std::string last_group_name{ seq.getCmd(0).getPlanningGroup() }; + for (size_t i = 1; i < seq.size(); ++i) + { + if (seq.getCmd(i).getPlanningGroup() != last_group_name) + { + ++num_groups; + last_group_name = seq.getCmd(i).getPlanningGroup(); + } + } + + RobotTrajCont res_single_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_single_vec.size(), num_groups); + + for (const auto& res : res_single_vec) + { + EXPECT_GT(res->getWayPointCount(), 0u); + } +} + +/** + * @brief Checks that no exception is thrown if two gripper commands are + * blended. + * + */ +TEST_F(IntegrationTestCommandListManager, TestGripperCmdBlending) +{ + Sequence seq{ data_loader_->getSequence("PureGripperSequence") }; + ASSERT_GE(seq.size(), 2u); + ASSERT_TRUE(seq.cmdIsOfType(0)); + ASSERT_TRUE(seq.cmdIsOfType(1)); + + // Ensure that blending is requested for gripper commands. + seq.setBlendRadius(0, 1.0); + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_EQ(res_vec.size(), 1u); +} + +/** + * @brief Tests the execution of a sequence in which each group states a start + * state only consisting of joints of the corresonding group. + * + * Test Sequence: + * 1. Create sequence request for which each start state only consists of + * joints of the corresonding group + * + * Expected Results: + * 1. Trajectory generation is successful, result trajectory is not empty. + * + * + */ +TEST_F(IntegrationTestCommandListManager, TestGroupSpecificStartState) +{ + using std::placeholders::_1; + + Sequence seq{ data_loader_->getSequence("ComplexSequenceWithGripper") }; + ASSERT_GE(seq.size(), 4u); + seq.erase(4, seq.size()); + + Gripper& gripper{ seq.getCmd(0) }; + gripper.getStartConfiguration().setCreateJointNameFunc(std::bind(&createGripperJointName, _1)); + // By deleting the model we guarantee that the start state only consists + // of joints of the gripper group without the manipulator + gripper.getStartConfiguration().clearModel(); + + PtpJointCart& ptp{ seq.getCmd(1) }; + ptp.getStartConfiguration().setCreateJointNameFunc(std::bind(&createManipulatorJointName, _1)); + // By deleting the model we guarantee that the start state only consists + // of joints of the manipulator group without the gripper + ptp.getStartConfiguration().clearModel(); + + RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) }; + EXPECT_GE(res_vec.size(), 1u); + EXPECT_GT(res_vec.front()->getWayPointCount(), 0u); +} + +/** + * @brief Checks that exception is thrown if Tip-Frame is requested for + * a group without a solver. + */ +TEST_F(IntegrationTestCommandListManager, TestGetSolverTipFrameForSolverlessGroup) +{ + Gripper gripper_cmd{ data_loader_->getGripper("open_gripper") }; + EXPECT_THROW(getSolverTipFrame(robot_model_->getJointModelGroup(gripper_cmd.getPlanningGroup())), NoSolverException); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_command_list_manager"); + testing::InitGoogleTest(&argc, argv); + + ros::NodeHandle nh; + + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.test new file mode 100644 index 0000000000..1f4eb9489d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.test @@ -0,0 +1,68 @@ + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp new file mode 100644 index 0000000000..c724595bd8 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.cpp @@ -0,0 +1,501 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "test_utils.h" + +const double EPSILON = 1.0e-6; +const std::string PLAN_SERVICE_NAME = "/plan_kinematic_path"; + +// Parameters from parameter server +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance"); +const std::string ORIENTATION_NORM_TOLERANCE("orientation_norm_tolerance"); +const std::string PARAM_TARGET_LINK_NAME("target_link"); +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); + +using namespace pilz_industrial_motion_planner_testutils; + +/** + * PLEASE NOTE: + * More detailed lin tests are done via unit tests. With the help of the + * integration tests, it is only checked that a linear command actually + * performs a linear command. + */ +class IntegrationTestCommandPlanning : public ::testing::Test +{ +protected: + void SetUp() override; + +protected: + ros::NodeHandle ph_{ "~" }; + + robot_model::RobotModelPtr robot_model_; + + double pose_norm_tolerance_, orientation_norm_tolerance_; + std::string planning_group_, target_link_, test_data_file_name_; + + std::unique_ptr test_data_; + + unsigned int num_joints_{ 0 }; +}; + +void IntegrationTestCommandPlanning::SetUp() +{ + // create robot model + robot_model_loader::RobotModelLoader model_loader; + robot_model_ = model_loader.getModel(); + + // get the parameters + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(POSE_TRANSFORM_MATRIX_NORM_TOLERANCE, pose_norm_tolerance_)); + ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + ASSERT_TRUE(ph_.getParam(ORIENTATION_NORM_TOLERANCE, orientation_norm_tolerance_)); + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + + ASSERT_TRUE(ros::service::waitForService(PLAN_SERVICE_NAME, ros::Duration(testutils::DEFAULT_SERVICE_TIMEOUT))); + + // load the test data provider + test_data_.reset( + new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_, robot_model_ }); + ASSERT_NE(nullptr, test_data_) << "Failed to load test data by provider."; + + num_joints_ = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().size(); +} + +/** + * @brief Tests if ptp motions with start & goal state given as + * joint configuration are executed correctly. + * + * Test Sequence: + * 1. Generate request with joint goal and start state call planning service. + * + * Expected Results: + * 1. Last point of the resulting trajectory is at the goal + */ +TEST_F(IntegrationTestCommandPlanning, PtpJoint) +{ + ros::NodeHandle node_handle("~"); + auto ptp{ test_data_->getPtpJoint("Ptp1") }; + + moveit_msgs::GetMotionPlan srv; + moveit_msgs::MotionPlanRequest req = ptp.toRequest(); + srv.request.motion_plan_request = req; + + ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); + + ASSERT_TRUE(client.call(srv)); + const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + + // Check the result + ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory; + + EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames"; + EXPECT_GT(trajectory.points.size(), 0u) << "There are no points in the trajectory"; + + // Check that every point has position, velocity, acceleration + for (const auto& point : trajectory.points) + { + EXPECT_EQ(point.positions.size(), num_joints_); + EXPECT_EQ(point.velocities.size(), num_joints_); + EXPECT_EQ(point.accelerations.size(), num_joints_); + } + + for (size_t i = 0; i < num_joints_; ++i) + { + EXPECT_NEAR(trajectory.points.back().positions.at(i), req.goal_constraints.back().joint_constraints.at(i).position, + 10e-10); + EXPECT_NEAR(trajectory.points.back().velocities.at(i), 0, 10e-10); + // EXPECT_NEAR(trajectory.points.back().accelerations.at(i), 0, 10e-10); // + // TODO what is expected + } +} + +/** + * @brief Tests if ptp motions with start state given as joint configuration + * and goal state given as cartesian configuration are executed correctly. + * + * Test Sequence: + * 1. Generate request with pose goal and start state call planning service. + * + * Expected Results: + * 1. Last point of the resulting trajectory is at the goal + */ +TEST_F(IntegrationTestCommandPlanning, PtpJointCart) +{ + ros::NodeHandle node_handle("~"); + + PtpJointCart ptp{ test_data_->getPtpJointCart("Ptp1") }; + ptp.getGoalConfiguration().setPoseTolerance(0.01); + ptp.getGoalConfiguration().setAngleTolerance(0.01); + + moveit_msgs::GetMotionPlan srv; + srv.request.motion_plan_request = ptp.toRequest(); + + ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); + + ASSERT_TRUE(client.call(srv)); + const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + + // Make sure the planning succeeded + ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + + // Check the result + trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory; + + EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames"; + EXPECT_GT(trajectory.points.size(), 0u) << "There are no points in the trajectory"; + + // Check that every point has position, velocity, acceleration + for (const auto& point : trajectory.points) + { + EXPECT_EQ(point.positions.size(), num_joints_); + EXPECT_EQ(point.velocities.size(), num_joints_); + EXPECT_EQ(point.accelerations.size(), num_joints_); + } + + robot_state::RobotState rstate(robot_model_); + rstate.setToDefaultValues(); + rstate.setJointGroupPositions(planning_group_, response.trajectory.joint_trajectory.points.back().positions); + Eigen::Isometry3d tf = rstate.getFrameTransform(target_link_); + + const geometry_msgs::Pose& expected_pose{ ptp.getGoalConfiguration().getPose() }; + + EXPECT_NEAR(tf(0, 3), expected_pose.position.x, EPSILON); + EXPECT_NEAR(tf(1, 3), expected_pose.position.y, EPSILON); + EXPECT_NEAR(tf(2, 3), expected_pose.position.z, EPSILON); + + Eigen::Isometry3d exp_iso3d_pose; + tf2::convert(expected_pose, exp_iso3d_pose); + + EXPECT_TRUE(Eigen::Quaterniond(tf.rotation()).isApprox(Eigen::Quaterniond(exp_iso3d_pose.rotation()), EPSILON)); +} + +/** + * @brief Tests if linear motions with start and goal state given + * as joint configuration are executed correctly. + * + * Test Sequence: + * 1. Generate request and make service request. + * 2. Check if target position correct. + * 3. Check if trajectory is linear. + * + * Expected Results: + * 1. Planning request is successful. + * 2. Goal position correponds with the given goal position. + * 3. Trajectory is a straight line. + */ +TEST_F(IntegrationTestCommandPlanning, LinJoint) +{ + planning_interface::MotionPlanRequest req{ test_data_->getLinJoint("lin2").toRequest() }; + + std::cout << "++++++++++" << std::endl; + std::cout << "+ Step 1 +" << std::endl; + std::cout << "++++++++++" << std::endl; + + moveit_msgs::GetMotionPlan srv; + srv.request.motion_plan_request = req; + + ros::NodeHandle node_handle("~"); + ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); + + ASSERT_TRUE(client.call(srv)); + const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + + ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + + std::cout << "++++++++++" << std::endl; + std::cout << "+ Step 2 +" << std::endl; + std::cout << "++++++++++" << std::endl; + + ASSERT_TRUE(testutils::isGoalReached(robot_model_, response.trajectory.joint_trajectory, req, pose_norm_tolerance_, + orientation_norm_tolerance_)) + << "Goal not reached."; + + std::cout << "++++++++++" << std::endl; + std::cout << "+ Step 3 +" << std::endl; + std::cout << "++++++++++" << std::endl; + + ASSERT_TRUE(testutils::checkCartesianLinearity(robot_model_, response.trajectory.joint_trajectory, req, + pose_norm_tolerance_, orientation_norm_tolerance_)) + << "Trajectory violates cartesian linearity."; +} + +/** + * @brief Tests if linear motions with start state given as joint + * configuration and goal state given as cartesian configuration + * are executed correctly. + * + * Test Sequence: + * 1. Generate request and make service request. + * 2. Check if target position correct. + * 3. Check if trajectory is linear. + * + * Expected Results: + * 1. Planning request is successful. + * 2. Goal position correponds with the given goal position. + * 3. Trajectory is a straight line. + */ +TEST_F(IntegrationTestCommandPlanning, LinJointCart) +{ + ros::NodeHandle node_handle("~"); + planning_interface::MotionPlanRequest req{ test_data_->getLinJointCart("lin2").toRequest() }; + + std::cout << "++++++++++" << std::endl; + std::cout << "+ Step 1 +" << std::endl; + std::cout << "++++++++++" << std::endl; + + moveit_msgs::GetMotionPlan srv; + srv.request.motion_plan_request = req; + + ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); + + ASSERT_TRUE(client.call(srv)); + const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + + ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + + std::cout << "++++++++++" << std::endl; + std::cout << "+ Step 2 +" << std::endl; + std::cout << "++++++++++" << std::endl; + + ASSERT_TRUE(testutils::isGoalReached(robot_model_, response.trajectory.joint_trajectory, req, pose_norm_tolerance_, + orientation_norm_tolerance_)) + << "Goal not reached."; + + std::cout << "++++++++++" << std::endl; + std::cout << "+ Step 3 +" << std::endl; + std::cout << "++++++++++" << std::endl; + + ASSERT_TRUE(testutils::checkCartesianLinearity(robot_model_, response.trajectory.joint_trajectory, req, + pose_norm_tolerance_, orientation_norm_tolerance_)) + << "Trajectory violates cartesian linearity."; +} + +/** + * @brief Tests if circular motions with start & goal state given as joint + * configuration and center point given as cartesian configuration + * are executed correctly. + * + * Test Sequence: + * 1. Generate request with JOINT goal and start state call planning service. + * + * Expected Results: + * 1. Last point of the resulting trajectory is at the goal + * 2. Waypoints are on the desired circle + */ +TEST_F(IntegrationTestCommandPlanning, CircJointCenterCart) +{ + ros::NodeHandle node_handle("~"); + + CircJointCenterCart circ{ test_data_->getCircJointCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req{ circ.toRequest() }; + + moveit_msgs::GetMotionPlan srv; + srv.request.motion_plan_request = req; + + ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); + + ASSERT_TRUE(client.call(srv)); + const moveit_msgs::MotionPlanResponse& response{ srv.response.motion_plan_response }; + + // Check the result + ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory; + + EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames"; + EXPECT_GT(trajectory.points.size(), 0u) << "There are no points in the trajectory"; + + // Check that every point has position, velocity, acceleration + for (const auto& point : trajectory.points) + { + EXPECT_EQ(point.positions.size(), num_joints_); + EXPECT_EQ(point.velocities.size(), num_joints_); + EXPECT_EQ(point.accelerations.size(), num_joints_); + } + + // check goal is reached + ASSERT_TRUE(testutils::isGoalReached(robot_model_, response.trajectory.joint_trajectory, req, pose_norm_tolerance_, + orientation_norm_tolerance_)) + << "Goal not reached."; + + // check all waypoints are on the circle and SLERP + robot_state::RobotState waypoint_state(robot_model_); + Eigen::Isometry3d waypoint_pose; + double x_dist, y_dist, z_dist; + + const geometry_msgs::Pose& aux_pose{ circ.getAuxiliaryConfiguration().getConfiguration().getPose() }; + + CircCenterCart circ_cart{ test_data_->getCircCartCenterCart("circ1_center_2") }; + const geometry_msgs::Pose& start_pose{ circ_cart.getStartConfiguration().getPose() }; + const geometry_msgs::Pose& goal_pose{ circ_cart.getGoalConfiguration().getPose() }; + + x_dist = aux_pose.position.x - start_pose.position.x; + y_dist = aux_pose.position.y - start_pose.position.y; + z_dist = aux_pose.position.z - start_pose.position.z; + double expected_radius = sqrt(x_dist * x_dist + y_dist * y_dist + z_dist * z_dist); + for (const auto& waypoint : trajectory.points) + { + waypoint_state.setToDefaultValues(); + waypoint_state.setJointGroupPositions(planning_group_, waypoint.positions); + waypoint_pose = waypoint_state.getFrameTransform(target_link_); + + // Calculate (and check) distance of current trajectory waypoint from circ + // center + x_dist = aux_pose.position.x - waypoint_pose(0, 3); + y_dist = aux_pose.position.y - waypoint_pose(1, 3); + z_dist = aux_pose.position.z - waypoint_pose(2, 3); + double actual_radius = sqrt(x_dist * x_dist + y_dist * y_dist + z_dist * z_dist); + EXPECT_NEAR(actual_radius, expected_radius, pose_norm_tolerance_) << "Trajectory way point is not on the circle."; + + // Check orientation + Eigen::Isometry3d start_pose_iso3d, goal_pose_iso3d; + tf2::convert(start_pose, start_pose_iso3d); + tf2::convert(goal_pose, goal_pose_iso3d); + EXPECT_TRUE(testutils::checkSLERP(start_pose_iso3d, goal_pose_iso3d, waypoint_pose, orientation_norm_tolerance_)); + } +} + +/** + * @brief Tests if linear motions with start state given as cartesian + * configuration and goal state given as cartesian configuration + * are executed correctly. + * + * - Test Sequence: + * 1. Generate request with POSE goal and start state call planning service. + * + * - Expected Results: + * 1. Last point of the resulting trajectory is at the goal + * 2. Waypoints are on the desired circle + */ +TEST_F(IntegrationTestCommandPlanning, CircCartCenterCart) +{ + ros::NodeHandle node_handle("~"); + + CircCenterCart circ{ test_data_->getCircCartCenterCart("circ1_center_2") }; + moveit_msgs::MotionPlanRequest req{ circ.toRequest() }; + moveit_msgs::GetMotionPlan srv; + srv.request.motion_plan_request = req; + + ros::ServiceClient client = node_handle.serviceClient(PLAN_SERVICE_NAME); + + ASSERT_TRUE(client.call(srv)); + const moveit_msgs::MotionPlanResponse& response = srv.response.motion_plan_response; + + // Check the result + ASSERT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!"; + trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory; + + EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames"; + EXPECT_GT(trajectory.points.size(), 0u) << "There are no points in the trajectory"; + + // Check that every point has position, velocity, acceleration + for (const auto& point : trajectory.points) + { + EXPECT_EQ(point.positions.size(), num_joints_); + EXPECT_EQ(point.velocities.size(), num_joints_); + EXPECT_EQ(point.accelerations.size(), num_joints_); + } + + // check goal is reached + ASSERT_TRUE(testutils::isGoalReached(robot_model_, response.trajectory.joint_trajectory, req, pose_norm_tolerance_, + orientation_norm_tolerance_)) + << "Goal not reached."; + + // check all waypoints are on the cricle and SLERP + robot_state::RobotState waypoint_state(robot_model_); + Eigen::Isometry3d waypoint_pose; + double x_dist, y_dist, z_dist; + + const geometry_msgs::Pose& start_pose{ circ.getStartConfiguration().getPose() }; + const geometry_msgs::Pose& aux_pose{ circ.getAuxiliaryConfiguration().getConfiguration().getPose() }; + const geometry_msgs::Pose& goal_pose{ circ.getGoalConfiguration().getPose() }; + + x_dist = aux_pose.position.x - start_pose.position.x; + y_dist = aux_pose.position.y - start_pose.position.y; + z_dist = aux_pose.position.z - start_pose.position.z; + double expected_radius = sqrt(x_dist * x_dist + y_dist * y_dist + z_dist * z_dist); + for (const auto& waypoint : trajectory.points) + { + waypoint_state.setToDefaultValues(); + waypoint_state.setJointGroupPositions(planning_group_, waypoint.positions); + waypoint_pose = waypoint_state.getFrameTransform(target_link_); + + // Calculate (and check) distance of current trajectory waypoint from circ + // center + x_dist = aux_pose.position.x - waypoint_pose(0, 3); + y_dist = aux_pose.position.y - waypoint_pose(1, 3); + z_dist = aux_pose.position.z - waypoint_pose(2, 3); + double actual_radius = sqrt(x_dist * x_dist + y_dist * y_dist + z_dist * z_dist); + EXPECT_NEAR(actual_radius, expected_radius, pose_norm_tolerance_) << "Trajectory way point is not on the circle."; + + // Check orientation + Eigen::Isometry3d start_pose_iso3d, goal_pose_iso3d; + tf2::convert(start_pose, start_pose_iso3d); + tf2::convert(goal_pose, goal_pose_iso3d); + EXPECT_TRUE(testutils::checkSLERP(start_pose_iso3d, goal_pose_iso3d, waypoint_pose, orientation_norm_tolerance_)); + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_command_planning"); + ros::NodeHandle nh; // For output via ROS_ERROR etc during test + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test new file mode 100644 index 0000000000..ba73e8b56b --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test @@ -0,0 +1,62 @@ + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test new file mode 100644 index 0000000000..d360fbcafa --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test @@ -0,0 +1,63 @@ + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test new file mode 100644 index 0000000000..64fb904bde --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test @@ -0,0 +1,61 @@ + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.cpp new file mode 100644 index 0000000000..0cd4ac3bcc --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.cpp @@ -0,0 +1,116 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/tip_frame_getter.h" + +static const std::string ROBOT_DESCRIPTION_PARAM{ "robot_description" }; + +namespace pilz_industrial_motion_planner +{ +class GetSolverTipFrameIntegrationTest : public testing::Test +{ +protected: + void SetUp() override; + +protected: + robot_model::RobotModelConstPtr robot_model_{ + robot_model_loader::RobotModelLoader(ROBOT_DESCRIPTION_PARAM).getModel() + }; +}; + +void GetSolverTipFrameIntegrationTest::SetUp() +{ + if (!robot_model_) + { + FAIL() << "Robot model could not be loaded."; + } +} + +/** + * @brief Check if hasSolver() can be called successfully for the manipulator + * group. + */ +TEST_F(GetSolverTipFrameIntegrationTest, TestHasSolverManipulator) +{ + EXPECT_TRUE(hasSolver(robot_model_->getJointModelGroup("manipulator"))) << "hasSolver returns false for manipulator"; +} + +/** + * @brief Check if hasSolver() can be called successfully for the gripper group. + */ +TEST_F(GetSolverTipFrameIntegrationTest, TestHasSolverGripperGroup) +{ + EXPECT_FALSE(hasSolver(robot_model_->getJointModelGroup("gripper"))) << "hasSolver returns true for gripper"; +} + +/** + * @brief Check if getSolverTipFrame() can be called successfully for the + * manipulator group. + */ +TEST_F(GetSolverTipFrameIntegrationTest, TestGetTipSolverFrameManipulator) +{ + getSolverTipFrame(robot_model_->getJointModelGroup("manipulator")); +} + +/** + * @brief Check if getSolverTipFrame() fails for gripper group. + */ +TEST_F(GetSolverTipFrameIntegrationTest, TestGetTipSolverFrameGripper) +{ + EXPECT_THROW(getSolverTipFrame(robot_model_->getJointModelGroup("gripper")), NoSolverException); +} + +} // namespace pilz_industrial_motion_planner + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_get_solver_tip_frame"); + testing::InitGoogleTest(&argc, argv); + + ros::NodeHandle nh; + + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.test new file mode 100644 index 0000000000..6f5425d85a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_get_solver_tip_frame.test @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp new file mode 100644 index 0000000000..43f0415771 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp @@ -0,0 +1,132 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include + +#include + +#include + +#include +#include + +#include "pilz_industrial_motion_planner/plan_components_builder.h" +#include "pilz_industrial_motion_planner/trajectory_blender_transition_window.h" + +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string ROBOT_DESCRIPTION_STR{ "robot_description" }; +const std::string EMPTY_VALUE{ "" }; + +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner; + +class IntegrationTestPlanComponentBuilder : public testing::Test +{ +protected: + void SetUp() override; + +protected: + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(ROBOT_DESCRIPTION_STR).getModel() }; + + std::string planning_group_; +}; + +void IntegrationTestPlanComponentBuilder::SetUp() +{ + if (!robot_model_) + { + FAIL() << "Robot model could not be loaded."; + } + + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); +} + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + * + */ +TEST_F(IntegrationTestPlanComponentBuilder, TestExceptionErrorCodeMapping) +{ + std::shared_ptr nbs_ex{ new NoBlenderSetException("") }; + EXPECT_EQ(nbs_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + + std::shared_ptr ntffse_ex{ new NoTipFrameFunctionSetException("") }; + EXPECT_EQ(ntffse_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + + std::shared_ptr nrms_ex{ new NoRobotModelSetException("") }; + EXPECT_EQ(nrms_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + + std::shared_ptr bf_ex{ new BlendingFailedException("") }; + EXPECT_EQ(bf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); +} + +/** + * @brief Checks that exception is thrown if no robot model is set. + * + */ +TEST_F(IntegrationTestPlanComponentBuilder, TestModelSet) +{ + robot_trajectory::RobotTrajectoryPtr traj{ new robot_trajectory::RobotTrajectory(robot_model_, planning_group_) }; + PlanComponentsBuilder builder; + + EXPECT_THROW(builder.append(traj, 1.0), NoRobotModelSetException); +} + +/** + * @brief Checks that exception is thrown if no blender is set. + * + */ +TEST_F(IntegrationTestPlanComponentBuilder, TestNoBlenderSet) +{ + robot_trajectory::RobotTrajectoryPtr traj{ new robot_trajectory::RobotTrajectory(robot_model_, planning_group_) }; + PlanComponentsBuilder builder; + builder.setModel(robot_model_); + + builder.append(traj, 0.0); + + EXPECT_THROW(builder.append(traj, 1.0), NoBlenderSetException); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_plan_components_builder"); + testing::InitGoogleTest(&argc, argv); + + ros::NodeHandle nh; + + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.test new file mode 100644 index 0000000000..447c618525 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.test @@ -0,0 +1,65 @@ + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp new file mode 100644 index 0000000000..a5883e71cc --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.cpp @@ -0,0 +1,628 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "moveit_msgs/MoveGroupSequenceAction.h" + +static constexpr int WAIT_FOR_ACTION_SERVER_TIME_OUT{ 10 }; // seconds + +const std::string SEQUENCE_ACTION_NAME("/sequence_move_group"); + +// Parameters from parameter server +const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance"); + +// events for callback tests +const std::string GOAL_SUCCEEDED_EVENT = "GOAL_SUCCEEDED"; +const std::string SERVER_IDLE_EVENT = "SERVER_IDLE"; + +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); +const std::string GROUP_NAME("group_name"); + +using namespace pilz_industrial_motion_planner_testutils; + +class IntegrationTestSequenceAction : public testing::Test, public testing::AsyncTest +{ +protected: + void SetUp() override; + +public: + MOCK_METHOD0(active_callback, void()); + MOCK_METHOD1(feedback_callback, void(const moveit_msgs::MoveGroupSequenceFeedbackConstPtr& feedback)); + MOCK_METHOD2(done_callback, void(const actionlib::SimpleClientGoalState& state, + const moveit_msgs::MoveGroupSequenceResultConstPtr& result)); + +protected: + ros::NodeHandle ph_{ "~" }; + actionlib::SimpleActionClient ac_{ ph_, SEQUENCE_ACTION_NAME, true }; + std::shared_ptr move_group_; + + robot_model_loader::RobotModelLoader model_loader_; + robot_model::RobotModelPtr robot_model_; + double joint_position_tolerance_; + + std::string test_data_file_name_; + std::string group_name_; + TestdataLoaderUPtr data_loader_; + + //! The configuration at which the robot stays at the beginning of each test. + JointConfiguration start_config; +}; + +void IntegrationTestSequenceAction::SetUp() +{ + // get necessary parameters + ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_)); + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + ph_.param(GROUP_NAME, group_name_, "manipulator"); + + robot_model_ = model_loader_.getModel(); + + data_loader_.reset(new XmlTestdataLoader(test_data_file_name_, robot_model_)); + ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; + + // wait for action server + ASSERT_TRUE(ac_.waitForServer(ros::Duration(WAIT_FOR_ACTION_SERVER_TIME_OUT))) << "Action server is not active."; + + // move to default position + start_config = data_loader_->getJoints("ZeroPose", group_name_); + robot_state::RobotState robot_state{ start_config.toRobotState() }; + + move_group_ = std::make_shared(start_config.getGroupName()); + move_group_->setPlannerId("PTP"); + move_group_->setGoalTolerance(joint_position_tolerance_); + move_group_->setJointValueTarget(robot_state); + move_group_->move(); + + ASSERT_TRUE( + isAtExpectedPosition(robot_state, *(move_group_->getCurrentState()), joint_position_tolerance_, group_name_)); +} + +/** + * @brief Test behavior of sequence action server when empty sequence is sent. + * + * Test Sequence: + * 1. Send empty sequence. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Empty sequence is sent to the action server. + * 2. Error code of the blend result is SUCCESS. + */ +TEST_F(IntegrationTestSequenceAction, TestSendingOfEmptySequence) +{ + moveit_msgs::MoveGroupSequenceGoal seq_goal; + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Execution of sequence failed."; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests that invalid (differing) group names are detected. + * + * Test Sequence: + * 1. Invalidate first request (change group_name) and send goal for planning + * and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is failure. + */ +TEST_F(IntegrationTestSequenceAction, TestDifferingGroupNames) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + MotionCmd& cmd{ seq.getCmd(1) }; + cmd.setPlanningGroup("WrongGroupName"); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME) << "Incorrect error code."; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests that negative blend radii are detected. + * + * Test Sequence: + * 1. Send goal for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is not success and the planned trajectory is + * empty. + */ +TEST_F(IntegrationTestSequenceAction, TestNegativeBlendRadius) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setBlendRadius(0, -1.0); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN) + << "Incorrect error code."; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests that overlapping blend radii are detected. + * + * Test Sequence: + * 1. Generate request with overlapping blend radii. + * 2. Send goal for planning and execution. + * 3. Evaluate the result. + * + * Expected Results: + * 1. - + * 2. Goal is sent to the action server. + * 3. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceAction, TestOverlappingBlendRadii) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setBlendRadius(0, 10 * seq.getBlendRadius(0)); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN) << "Incorrect error code"; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests that too large blend radii are detected. + * + * Test Sequence: + * 1. Generate request with too large blend radii. + * 2. Send goal for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. - + * 2. Goal is sent to the action server. + * 3. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceAction, TestTooLargeBlendRadii) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.erase(2, seq.size()); + seq.setBlendRadius(0, 10 * seq.getBlendRadius(seq.size() - 2)); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::FAILURE) << "Incorrect error code"; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests what happens if sequence contains not executable (invalid) + * command. + * + * Test Sequence: + * 1. Create sequence containing at least one invalid command. + * 2. Send goal for planning and execution. + * 3. Evaluate the result. + * + * Expected Results: + * 1. - + * 2. Goal is sent to the action server. + * 3. Error code indicates an error + planned trajectory is empty. + */ +TEST_F(IntegrationTestSequenceAction, TestInvalidCmd) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + // Erase certain command to invalid command following the command in sequence. + seq.erase(3, 4); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_NE(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Incorrect error code."; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests that incorrect link_names are detected. + * + * Test Sequence: + * 1. Create sequence and send it via ActionClient. + * 2. Wait for successful completion of command. + * + * Expected Results: + * 1. - + * 2. ActionClient reports successful completion of command. + */ +TEST_F(IntegrationTestSequenceAction, TestInvalidLinkName) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setAllBlendRadiiToZero(); + + // Invalidate link name + CircInterimCart& circ{ seq.getCmd(1) }; + circ.getGoalConfiguration().setLinkName("InvalidLinkName"); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_NE(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Incorrect error code."; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +//******************************************************* +//*** matcher for callback functions of action server *** +//******************************************************* +MATCHER_P(FeedbackStateEq, state, "") +{ + return arg->state == state; +} +MATCHER(IsResultSuccess, "") +{ + return arg->response.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS; +} +MATCHER(IsResultNotEmpty, "") +{ + return !arg->response.planned_trajectories.empty(); +} + +/** + * @brief Tests that action server callbacks are called correctly. + * + * Test Sequence: + * 1. Send goal for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is success. Active-, feedback- and + * done-callbacks are called. + */ +TEST_F(IntegrationTestSequenceAction, TestActionServerCallbacks) +{ + using ::testing::_; + using ::testing::AllOf; + using ::testing::AtLeast; + using ::testing::InSequence; + + namespace ph = std::placeholders; + + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + // We do not need the complete sequence, just two commands. + seq.erase(2, seq.size()); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + // set expectations (no guarantee, that done callback is called before idle + // feedback) + EXPECT_CALL(*this, active_callback()).Times(1).RetiresOnSaturation(); + + EXPECT_CALL(*this, done_callback(_, AllOf(IsResultSuccess(), IsResultNotEmpty()))) + .Times(1) + .WillOnce(ACTION_OPEN_BARRIER_VOID(GOAL_SUCCEEDED_EVENT)) + .RetiresOnSaturation(); + + // the feedbacks are expected in order + { + InSequence dummy; + + EXPECT_CALL(*this, feedback_callback(FeedbackStateEq("PLANNING"))).Times(AtLeast(1)); + EXPECT_CALL(*this, feedback_callback(FeedbackStateEq("MONITOR"))).Times(AtLeast(1)); + EXPECT_CALL(*this, feedback_callback(FeedbackStateEq("IDLE"))) + .Times(AtLeast(1)) + .WillOnce(ACTION_OPEN_BARRIER_VOID(SERVER_IDLE_EVENT)) + .RetiresOnSaturation(); + } + + // send goal using mocked callback methods + ac_.sendGoal(seq_goal, std::bind(&IntegrationTestSequenceAction::done_callback, this, ph::_1, ph::_2), + std::bind(&IntegrationTestSequenceAction::active_callback, this), + std::bind(&IntegrationTestSequenceAction::feedback_callback, this, ph::_1)); + + // wait for the ecpected events + BARRIER({ GOAL_SUCCEEDED_EVENT, SERVER_IDLE_EVENT }); +} + +/** + * @brief Tests the "only planning" flag. + * + * Test Sequence: + * 1. Send goal for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is success. + */ +TEST_F(IntegrationTestSequenceAction, TestPlanOnlyFlag) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + // We do not need the complete sequence, just two commands. + seq.erase(2, seq.size()); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.planning_options.plan_only = true; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Sequence execution failed."; + EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; + + ASSERT_TRUE(isAtExpectedPosition(*(move_group_->getCurrentState()), start_config.toRobotState(), + joint_position_tolerance_, group_name_)) + << "Robot did move although \"PlanOnly\" flag set."; +} + +/** + * @brief Tests that robot state in planning_scene_diff is + * ignored (Mainly for full coverage) in case "plan only" flag is set. + * + * Test Sequence: + * 1. Send goal with "empty" planning scene for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is success. + */ +TEST_F(IntegrationTestSequenceAction, TestIgnoreRobotStateForPlanOnly) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + // We do not need the complete sequence, just two commands. + seq.erase(2, seq.size()); + + // create request + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.planning_options.plan_only = true; + seq_goal.request = seq.toRequest(); + + seq_goal.planning_options.planning_scene_diff.robot_state.is_diff = true; + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Execution of sequence failed."; + EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; + + ASSERT_TRUE(isAtExpectedPosition(*(move_group_->getCurrentState()), start_config.toRobotState(), + joint_position_tolerance_, group_name_)) + << "Robot did move although \"PlanOnly\" flag set."; +} + +/** + * @brief Tests that negative blend radii are detected + * (Mainly for full coverage) in case "plan only" flag is set. + * + * Test Sequence: + * 1. Send goal for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is not success and the planned trajectory is + * empty. + */ +TEST_F(IntegrationTestSequenceAction, TestNegativeBlendRadiusForPlanOnly) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setBlendRadius(0, -1.0); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + seq_goal.planning_options.plan_only = true; + + ac_.sendGoalAndWait(seq_goal); + + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN) + << "Incorrect error code."; + EXPECT_TRUE(res->response.planned_trajectories.empty()); +} + +/** + * @brief Tests that robot state in planning_scene_diff is + * ignored (Mainly for full coverage). + * + * Test Sequence: + * 1. Send goal with "empty" planning scene for planning and execution. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Error code of the result is success. + */ +TEST_F(IntegrationTestSequenceAction, TestIgnoreRobotState) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + // We do not need the complete sequence, just two commands. + seq.erase(2, seq.size()); + + // create request + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + seq_goal.planning_options.planning_scene_diff.robot_state.is_diff = true; + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Execution of sequence failed."; + EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; +} + +/** + * @brief Tests the execution of a sequence with more than two commands. + * + * Test Sequence: + * 1. Create large sequence requests and sent it to action server. + * 2. Evaluate the result. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Command succeeds, result trajectory is not empty. + */ +TEST_F(IntegrationTestSequenceAction, TestLargeRequest) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + // Create large request by making copies of the original sequence commands + // and adding them to the end of the original sequence. + size_t n{ req.items.size() }; + for (size_t i = 0; i < n; ++i) + { + moveit_msgs::MotionSequenceItem item{ req.items.at(i) }; + if (i == 0) + { + // Remove start state because only the first request + // is allowed to have a start state in a sequence. + item.req.start_state = moveit_msgs::RobotState(); + } + req.items.push_back(item); + } + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = req; + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS) << "Incorrect error code."; + EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; +} + +/** + * @brief Tests the execution of a sequence command (without blending) + * consisting of most of the possible command type combination. + * + * Test Sequence: + * 1. Create sequence goal and send it via ActionClient. + * 2. Wait for successful completion of command. + * + * Expected Results: + * 1. - + * 2. ActionClient reports successful completion of command. + */ +TEST_F(IntegrationTestSequenceAction, TestComplexSequenceWithoutBlending) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + + seq.setAllBlendRadiiToZero(); + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; +} + +/** + * @brief Tests the execution of a sequence command (with blending) + * consisting of most of the possible command type combination. + * + * Test Sequence: + * 1. Create sequence goal and send it via ActionClient. + * 2. Wait for successful completion of command. + * + * Expected Results: + * 1. - + * 2. ActionClient reports successful completion of command. + */ +TEST_F(IntegrationTestSequenceAction, TestComplexSequenceWithBlending) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoalAndWait(seq_goal); + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty"; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_sequence_action_capability"); + ros::NodeHandle nh; + + ros::AsyncSpinner spinner{ 1 }; + spinner.start(); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test new file mode 100644 index 0000000000..df51f007b1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test @@ -0,0 +1,59 @@ + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test new file mode 100644 index 0000000000..bc6afb0a8e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test @@ -0,0 +1,65 @@ + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test new file mode 100644 index 0000000000..c45f964dbc --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test @@ -0,0 +1,60 @@ + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp new file mode 100644 index 0000000000..bf9198d52a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.cpp @@ -0,0 +1,180 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "moveit_msgs/MoveGroupSequenceAction.h" + +static constexpr double WAIT_FOR_RESULT_TIME_OUT{ 5. }; // seconds +static constexpr double TIME_BEFORE_CANCEL_GOAL{ 1.0 }; // seconds +static constexpr double WAIT_FOR_ACTION_SERVER_TIME_OUT{ 10. }; // seconds + +const std::string SEQUENCE_ACTION_NAME("/sequence_move_group"); + +// Parameters from parameter server +const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance"); + +// events for callback tests +const std::string GOAL_SUCCEEDED_EVENT = "GOAL_SUCCEEDED"; +const std::string SERVER_IDLE_EVENT = "SERVER_IDLE"; + +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); +const std::string GROUP_NAME("group_name"); + +using namespace pilz_industrial_motion_planner_testutils; + +class IntegrationTestSequenceAction : public testing::Test, public testing::AsyncTest +{ +protected: + void SetUp() override; + +public: + MOCK_METHOD0(active_callback, void()); + MOCK_METHOD1(feedback_callback, void(const moveit_msgs::MoveGroupSequenceFeedbackConstPtr& feedback)); + MOCK_METHOD2(done_callback, void(const actionlib::SimpleClientGoalState& state, + const moveit_msgs::MoveGroupSequenceResultConstPtr& result)); + +protected: + ros::NodeHandle ph_{ "~" }; + actionlib::SimpleActionClient ac_{ ph_, SEQUENCE_ACTION_NAME, true }; + std::shared_ptr move_group_; + + robot_model_loader::RobotModelLoader model_loader_; + robot_model::RobotModelPtr robot_model_; + double joint_position_tolerance_; + + std::string test_data_file_name_; + std::string group_name_; + TestdataLoaderUPtr data_loader_; + + //! The configuration at which the robot stays at the beginning of each test. + JointConfiguration start_config; +}; + +void IntegrationTestSequenceAction::SetUp() +{ + // get necessary parameters + ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_)); + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + ph_.param(GROUP_NAME, group_name_, "manipulator"); + + robot_model_ = model_loader_.getModel(); + + data_loader_.reset(new XmlTestdataLoader(test_data_file_name_, robot_model_)); + ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; + + // wait for action server + ASSERT_TRUE(ac_.waitForServer(ros::Duration(WAIT_FOR_ACTION_SERVER_TIME_OUT))) << "Action server is not active."; + + // move to default position + start_config = data_loader_->getJoints("ZeroPose", group_name_); + robot_state::RobotState robot_state{ start_config.toRobotState() }; + + move_group_ = std::make_shared(start_config.getGroupName()); + move_group_->setPlannerId("PTP"); + move_group_->setGoalTolerance(joint_position_tolerance_); + move_group_->setJointValueTarget(robot_state); + move_group_->setMaxVelocityScalingFactor(1.0); + move_group_->setMaxAccelerationScalingFactor(1.0); + move_group_->move(); + + ASSERT_TRUE(isAtExpectedPosition(robot_state, *(move_group_->getCurrentState()), joint_position_tolerance_)); +} + +/** + * @brief Tests that goal can be cancelled. + * + * Test Sequence: + * 1. Send goal for planning and execution. + * 2. Cancel goal before it finishes. + * + * Expected Results: + * 1. Goal is sent to the action server. + * 2. Goal is cancelled. Execution stops. + */ +TEST_F(IntegrationTestSequenceAction, TestCancellingOfGoal) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + + moveit_msgs::MoveGroupSequenceGoal seq_goal; + seq_goal.request = seq.toRequest(); + + ac_.sendGoal(seq_goal); + // wait for 1 second + ros::Duration(TIME_BEFORE_CANCEL_GOAL).sleep(); + + ac_.cancelGoal(); + ac_.waitForResult(ros::Duration(WAIT_FOR_RESULT_TIME_OUT)); + + moveit_msgs::MoveGroupSequenceResultConstPtr res = ac_.getResult(); + EXPECT_EQ(res->response.error_code.val, moveit_msgs::MoveItErrorCodes::PREEMPTED) + << "Error code should be preempted."; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_sequence_action_preemption"); + ros::NodeHandle nh; + + ros::AsyncSpinner spinner{ 1 }; + spinner.start(); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test new file mode 100644 index 0000000000..614b5ffe1d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test @@ -0,0 +1,59 @@ + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp new file mode 100644 index 0000000000..02f4bf5aa9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp @@ -0,0 +1,426 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "moveit_msgs/GetMotionSequence.h" +#include "moveit_msgs/MotionSequenceRequest.h" +#include "pilz_industrial_motion_planner/capability_names.h" + +// Parameters from parameter server +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); + +using namespace pilz_industrial_motion_planner_testutils; + +static std::string createJointName(const size_t& joint_number) +{ + return std::string("prbt_joint_") + std::to_string(joint_number + 1); +} + +class IntegrationTestSequenceService : public ::testing::Test +{ +protected: + void SetUp() override; + +protected: + ros::NodeHandle ph_{ "~" }; + ros::ServiceClient client_; + robot_model::RobotModelPtr robot_model_; + + std::string test_data_file_name_; + TestdataLoaderUPtr data_loader_; +}; + +void IntegrationTestSequenceService::SetUp() +{ + // get necessary parameters + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + + robot_model_loader::RobotModelLoader model_loader; + robot_model_ = model_loader.getModel(); + + data_loader_.reset(new XmlTestdataLoader(test_data_file_name_, robot_model_)); + ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; + + ASSERT_TRUE(ros::service::waitForService(pilz_industrial_motion_planner::SEQUENCE_SERVICE_NAME, ros::Duration(10))) + << "Service not available."; + ros::NodeHandle nh; // connect to service in global namespace, not in ph_ + client_ = nh.serviceClient(pilz_industrial_motion_planner::SEQUENCE_SERVICE_NAME); +} + +/** + * @brief Test behavior of service when empty sequence is sent. + * + * Test Sequence: + * 1. Generate empty request and call sequence service. + * 2. Evaluate the result. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command is successful, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestSendingOfEmptySequence) +{ + moveit_msgs::MotionSequenceRequest empty_list; + + moveit_msgs::GetMotionSequence srv; + srv.request.request = empty_list; + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Planning failed."; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests that invalid (differing) group names are detected. + * + * Test Sequence: + * 1. Generate request, first request has invalid group_name + Call sequence + * service. + * 2. Invalidate first request (change group_name) and send goal for planning + * and execution. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestDifferingGroupNames) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + MotionCmd& cmd{ seq.getCmd(1) }; + cmd.setPlanningGroup("WrongGroupName"); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME, srv.response.response.error_code.val) + << "Planning should have failed but did not."; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests that negative blend radii are detected. + * + * Test Sequence: + * 1. Generate request with negative blend_radius + Call sequence service. + * 2. Evaluate the result. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestNegativeBlendRadius) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setBlendRadius(0, -1.0); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN, srv.response.response.error_code.val) + << "Planning should have failed but did not."; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests that overlapping blend radii are detected. + * + * Test Sequence: + * 1. Generate request with overlapping blend radii + Call sequence service. + * 2. Evaluate the result. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestOverlappingBlendRadii) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setBlendRadius(0, 10 * seq.getBlendRadius(0)); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN, srv.response.response.error_code.val) + << "Incorrect error code"; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests that too large blend radii are detected. + * + * Test Sequence: + * 1. Generate request with too large blend radii + Call sequence service. + * 2. Evaluate the result. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestTooLargeBlendRadii) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.erase(2, seq.size()); + seq.setBlendRadius(0, 10 * seq.getBlendRadius(seq.size() - 2)); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::FAILURE, srv.response.response.error_code.val) << "Incorrect error code"; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests behavior of service when sequence with invalid second + * start state is sent. + * + * Test Sequence: + * 1. Generate request (second goal has invalid start state) + Call sequence + * service. + * 2. Evaluate the result + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestSecondTrajInvalidStartState) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + moveit_msgs::MotionSequenceRequest req_list{ seq.toRequest() }; + + // Set start state + using std::placeholders::_1; + JointConfiguration config{ "MyGroupName", { -1., 2., -3., 4., -5., 0. }, std::bind(&createJointName, _1) }; + req_list.items[1].req.start_state.joint_state = config.toSensorMsg(); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = req_list; + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE, srv.response.response.error_code.val) + << "Incorrect error code."; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests behavior of service when sequence with invalid first goal + * is sent. + * + * Test Sequence: + * 1. Generate request with first goal out of workspace + Call sequence + * service. + * 2. Evaluate the result + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestFirstGoalNotReachable) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + PtpJointCart& cmd{ seq.getCmd(0) }; + cmd.getGoalConfiguration().getPose().position.y = 27; + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION, srv.response.response.error_code.val) + << "Incorrect error code."; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests that incorrect link_names are detected. + * + * Test Sequence: + * 1. Create sequence and send it. + * 2. Wait for successful completion of command. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command fails, result trajectory is empty. + */ +TEST_F(IntegrationTestSequenceService, TestInvalidLinkName) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + seq.setAllBlendRadiiToZero(); + + // Invalidate link name + CircInterimCart& circ{ seq.getCmd(1) }; + circ.getGoalConfiguration().setLinkName("InvalidLinkName"); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_NE(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Incorrect error code."; + EXPECT_TRUE(srv.response.response.planned_trajectories.empty()); +} + +/** + * @brief Tests the execution of a sequence with more than two commands. + * + * Test Sequence: + * 1. Call service with serveral requests. + * 2. Evaluate the result. + * + * Expected Results: + * 1. MotionPlanResponse is received. + * 2. Command succeeds, result trajectory is not empty. + */ +TEST_F(IntegrationTestSequenceService, TestLargeRequest) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + moveit_msgs::MotionSequenceRequest req{ seq.toRequest() }; + // Make copy of sequence commands and add them to the end of sequence. + // Create large request by making copies of the original sequence commands + // and adding them to the end of the original sequence. + size_t n{ req.items.size() }; + for (size_t i = 0; i < n; ++i) + { + moveit_msgs::MotionSequenceItem item{ req.items.at(i) }; + if (i == 0) + { + // Remove start state because only the first request + // is allowed to have a start state in a sequence. + item.req.start_state = moveit_msgs::RobotState(); + } + req.items.push_back(item); + } + + moveit_msgs::GetMotionSequence srv; + srv.request.request = req; + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Incorrect error code."; + EXPECT_EQ(srv.response.response.planned_trajectories.size(), 1u); + EXPECT_GT(srv.response.response.planned_trajectories.front().joint_trajectory.points.size(), 0u) + << "Trajectory should contain points."; +} + +/** + * @brief Tests the execution of a sequence command (without blending) + * consisting of most of the possible command type combination. + * + * Test Sequence: + * 1. Create sequence goal and send it via ActionClient. + * 2. Wait for successful completion of command. + * + * Expected Results: + * 1. - + * 2. ActionClient reports successful completion of command. + */ +TEST_F(IntegrationTestSequenceService, TestComplexSequenceWithoutBlending) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + + seq.setAllBlendRadiiToZero(); + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Incorrect error code."; + EXPECT_EQ(srv.response.response.planned_trajectories.size(), 1u); + EXPECT_GT(srv.response.response.planned_trajectories.front().joint_trajectory.points.size(), 0u) + << "Trajectory should contain points."; +} + +/** + * @brief Tests the execution of a sequence command (with blending) + * consisting of most of the possible command type combination. + * + * Test Sequence: + * 1. Create sequence goal and send it via ActionClient. + * 2. Wait for successful completion of command. + * + * Expected Results: + * 1. - + * 2. ActionClient reports successful completion of command. + */ +TEST_F(IntegrationTestSequenceService, TestComplexSequenceWithBlending) +{ + Sequence seq{ data_loader_->getSequence("ComplexSequence") }; + + moveit_msgs::GetMotionSequence srv; + srv.request.request = seq.toRequest(); + + ASSERT_TRUE(client_.call(srv)); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Incorrect error code."; + EXPECT_EQ(srv.response.response.planned_trajectories.size(), 1u); + EXPECT_GT(srv.response.response.planned_trajectories.front().joint_trajectory.points.size(), 0u) + << "Trajectory should contain points."; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "integrationtest_sequence_service_capability"); + ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test new file mode 100644 index 0000000000..9eec6f308b --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test @@ -0,0 +1,57 @@ + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test new file mode 100644 index 0000000000..dbec926599 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test @@ -0,0 +1,59 @@ + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test new file mode 100644 index 0000000000..f214d4baf7 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test @@ -0,0 +1,57 @@ + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.odp b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.odp new file mode 100644 index 0000000000..1e437d4204 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.odp differ diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.png b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.png new file mode 100644 index 0000000000..0dbf6f8d88 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/concept_testdata.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch new file mode 100644 index 0000000000..af5a15e896 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml new file mode 100644 index 0000000000..9699c5caad --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml @@ -0,0 +1,173 @@ + + + + + + + + 0.0 -0.785 0.0 -2.356 0.0 1.571 0.785 + + + + -0.022096 -0.753529 0.007612 -1.716165 0.0063464 0.962660 0.771123 + 0.2 0 0.8 0.924138 -0.382057 0.0 0.0 + -0.022096 -0.753529 0.007612 -1.716165 0.0063464 0.962660 0.771123 + + + + + -0.061035 -0.301221 0.410282 -1.665777 0.120602 1.389535 1.138547 + 0.4 0.2 0.7 0.924138 -0.382057 0.0 0.0 + -0.061035 -0.301221 0.410282 -1.665777 0.120602 1.389535 1.138547 + + + + + 0.0875228 -0.540765 0.770139 -1.990062 0.366596 1.582282 1.560837 + 0.2 0.4 0.6 0.924138 -0.382057 0.0 0.0 + 0.0875228 -0.540765 0.770139 -1.990062 0.366596 1.582282 1.560837 + + + + + 0.164988 -0.805223 0.304894 -2.020563 0.230321 1.247100 1.238191 + 0.2 0.2 0.7 0.924138 -0.382057 0.0 0.0 + 0.164988 -0.805223 0.304894 -2.020563 0.230321 1.247100 1.238191 + + + + + + + + panda_arm + panda_link8 + ZeroPose + P1 + 1.0 + 0.2 + + + + panda_arm + panda_link8 + P3 + P2 + 1.0 + 0.2 + + + + panda_arm + panda_link8 + P2 + P3 + 1.0 + 0.2 + + + + + + panda_arm + panda_link8 + P2 + P3 + 0.1 + 0.05 + + + + panda_arm + panda_link8 + P1 + P2 + 0.1 + 0.05 + + + + panda_arm + panda_link8 + P2 + P4 + 0.1 + 0.05 + + + + + + panda_arm + panda_link8 + P1 + P2 + P3 + 0.1 + 0.1 + + + + panda_arm + panda_link8 + P3 + P2 + P1 + 0.1 + 0.1 + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch new file mode 100644 index 0000000000..4c43af1420 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_deprecated.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_deprecated.xml new file mode 100644 index 0000000000..65d6b388ce --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_deprecated.xml @@ -0,0 +1,434 @@ + + + + + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0 + + + + + + 0.5 0.0 0.0 0.0 0.0 0.0 + + + 0 + + + + + + 0.0 0.785398 0.785398 0.0 1.570796 0.0 + + + + + + 0.45 -0.1 0.62 0.0 0.0 0.0 1.0 + + + + + + 0.0 -0.52 1.13 0.0 -1.57 0.0 + + + 0 + + + + + + 0.0 -0.63 1.4 0.0 1.19 0.0 + + + 0 + + + + + + 0.463648 -0.418738 1.03505 0.0 -1.45379 -0.149488 + + + 0 + + + + + + 0.0 0.278259 2.11585 0.0 -1.83759 0.785398 + + + 0 + + + + + + 0.0 1.52001 1.13 0.0 -1.57 0.0 + + + 0 + + + + + + + 0.244979 0.333691 -1.48422 0.0 1.81791 -0.244979 + 0.4 0.1 0.6 0. 0. 0. 1.0 + + + 0 + + + + + + + 1.32582 0.333691 -1.48422 0.0 1.81791 -1.32582 + 0.1 0.4 0.7 0 0 0.923916 -0.382596 + + + 0 + + + + + + + 0.1 0.1 0.6 0 0 0 1.0 + + + 0 + + + + + + + -0.463648 -1.64103 -2.06227 0 0.421241 0.463648 + -0.2 0.1 0.6 0 0 0 1.0 + + + 0 + + + + + + + 0 0.4 0.5 0 0 0 1 + + + 0 + + + + + + 0.0 -0.0107869 -1.72665 0.0 1.71586 0.0 + 0.3 0.0 0.65 0.0 0.0 0.0 1.0 + + + 0 + + + + + + 0.300000001 0.0 0.65 0.0 0.0 0.0 1.0 + + + 0 + + + + + + 0.0 -0.010787 -1.72666 0.0 1.71586 0.0 + 0.300000001 0.0 0.650000001 0.0 0.0 0.0 1.0 + + + 0 + + + + + + 0.0 -1.56309 -1.72665 0.0 0.163561 0.0 + -0.3 0.0 0.65 0.0 0.0 0.0 1.0 + + + 0 + + + + + + 2.61799 -0.0107869 -1.72665 0.0 1.71586 -1.0472 + -0.25980762113533159 0.15 0.65 0 0 0.7071067811865475 0.7071067811865475 + + + 0 + + + + + + 0.0 0.0 0.65 0.0 0.0 0.0 1.0 + + + 0 + + + + + + 0.0 0.3 0.65 0.0 0.0 0.0 1.0 + + + 0 + + + + + + + 0.244979 0.333691 -1.48422 0.0 1.81791 -0.244979 + 0.4 0.1 0.7 0. 0. 0.923916 -0.382596 + + + 0 + + + + + + + 1.32582 0.333691 -1.48422 0.0 1.81791 -1.32582 + 0.3 0.2 0.7 0 0 0.923916 -0.382596 + + + 0 + + + + + + + -0.463648 -1.64103 -2.06227 0 0.421241 0.463648 + 0.2 0.1 0.7 0 0 0.923916 -0.382596 + + + 0 + + + + + + + + + manipulator + prbt_tcp + CIRCPose1 + CIRCAuxPose1 + CIRCPose6 + 0.05 + 0.05 + + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCAuxPose3 + CIRCPose6 + 0.1 + 0.1 + + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCAuxPose4 + CIRCPose6 + 0.1 + 0.1 + + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCPose4_delta1 + CIRCPose4_delta2 + 0.1 + 0.1 + + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCAuxPose4 + CIRCPose6 + 1.0 + 1.0 + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCAuxPose3 + CIRCPose5 + 0.1 + 0.1 + + + + + manipulator + prbt_tcp + CIRCPose1 + CIRCAuxPose2 + CIRCAuxPose2 + CIRCPose2 + 0.05 + 0.05 + + + + + manipulator + prbt_tcp + CIRCPose7 + CIRCPose8 + CIRCPose9 + 0.1 + 0.1 + + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCAuxPose3 + CIRCPose5 + 0.1 + 0.1 + + + + manipulator + prbt_tcp + CIRCPose4 + CIRCAuxPose4 + CIRCPose5 + 0.01 + 0.01 + + + + + + + + + manipulator + prbt_tcp + LINPose1 + LINPose2 + 0.4 + 0.3 + + + + manipulator + prbt_tcp + LINPose3 + LINPose4 + 0.2 + 0.2 + + + + manipulator + prbt_tcp + LINPose1 + LINPose1Opposite + 1.0 + 1.0 + + + + manipulator + prbt_tcp + LINPose1 + LINPose1 + 0.4 + 0.4 + + + + + + + + manipulator + prbt_tcp + ZeroPose + PTPJointValid + 1.0 + 1.0 + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_sequence.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_sequence.xml new file mode 100644 index 0000000000..3b87ef0328 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_sequence.xml @@ -0,0 +1,227 @@ + + + + + + + + 0.0, 0.0, -0.4353981633974483, 0.0, -1.5700000000000003, 0.0 + + + + 0, -0.016763700542892668, -1.673499069949556, 0, -1.4848572841831293, 2.3561944901923377 + 0.3 0.0 0.5 0.923879532509 0.38268343237 0.0 0.0 + + + + 0.380506 0.731299 -0.960008 0.0 -1.450284 1.951302 + 0.5 0.2 0.4 0.707106 0.707106 0.0 0.0 + + + + 0.927295 0.708913 -1.343015 0.0 -1.089664 2.498091 + 0.3 0.4 0.3 0.707106 0.707106 0.0 0.0 + + 0.927295 0.708913 -1.343015 0.0 -1.089664 2.498091 + + + + + 0.588002 0.221990 -1.742660 0.0 -1.176941 2.158798 + 0.3 0.2 0.4 0.707106 0.707106 0.0 0.0 + + + + 0.0 + + + + 0.03 + + + + + + + manipulator + prbt_tcp + ZeroPose + P1 + 0.1 + 0.1 + + + + manipulator + prbt_tcp + P3 + P2 + 1.0 + 0.4 + + + + manipulator + prbt_tcp + P2 + P3 + 1.0 + 0.4 + + + + manipulator + prbt_tcp + P2 + P1 + 1.0 + 0.4 + + + + + + manipulator + prbt_tcp + P2 + P3 + 0.1 + 0.2 + + + + manipulator + prbt_tcp + P1 + P2 + 0.1 + 0.2 + + + + manipulator + prbt_tcp + P2 + P4 + 0.1 + 0.2 + + + + + + manipulator + prbt_tcp + P1 + P2 + P3 + 0.1 + 0.2 + + + + manipulator + prbt_tcp + P3 + P2 + P1 + 0.1 + 0.2 + + + + + + gripper + gripper_closed + gripper_open + + + gripper + gripper_open + gripper_closed + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_with_gripper.xml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_with_gripper.xml new file mode 100644 index 0000000000..cece30dc4b --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/testdata_with_gripper.xml @@ -0,0 +1,95 @@ + + + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + 0.0 0.005583 -1.323096 0.0 -1.812912 -2.356194 + 0.3 0.0 0.5 0.707106 0.707106 0.0 0.0 + + + + 0.380506 0.752267 -0.595009 0.0 -1.794315 -1.975688 + 0.5 0.2 0.4 0.707106 0.707106 0.0 0.0 + + + + 0.927295 0.607232 -1.173008 0.0 -1.361351 -1.428899 + 0.3 0.4 0.3 0.707106 0.707106 0.0 0.0 + + + + + + + + manipulator + prbt_tcp + ZeroPose + P1 + 1.0 + 0.4 + + + + + + + manipulator + prbt_tcp + P1 + P2 + 0.1 + 0.2 + + + + + + manipulator + prbt_tcp + P3 + P2 + P1 + 0.1 + 0.2 + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_all.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_all.yaml new file mode 100644 index 0000000000..bd165e6ebc --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_all.yaml @@ -0,0 +1,37 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Pilz GmbH & Co. KG nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2 + max_trans_dec: -3 + max_rot_vel: 4 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_only_vel.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_only_vel.yaml new file mode 100644 index 0000000000..4ac9ff1ae8 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_only_vel.yaml @@ -0,0 +1,34 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Pilz GmbH & Co. KG nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +cartesian_limits: + max_trans_vel: 10 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_valid_1.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_valid_1.yaml new file mode 100644 index 0000000000..dd953add3d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_valid_1.yaml @@ -0,0 +1,52 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Pilz GmbH & Co. KG nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +joint_limits: + prbt_joint_1: + has_position_limits: true + min_position: -2 + max_position: 2 + prbt_joint_2: + has_position_limits: false + min_position: -2 + max_position: 2 + prbt_joint_3: + has_velocity_limits: true + max_velocity: 1.1 + has_acceleration_limits: false + max_acceleration: 5.5 + prbt_joint_4: + has_acceleration_limits: true + max_acceleration: 5.5 + prbt_joint_5: + has_deceleration_limits: true + max_deceleration: -6.6 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_max.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_max.yaml new file mode 100644 index 0000000000..7aecb01bc7 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_max.yaml @@ -0,0 +1,37 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Pilz GmbH & Co. KG nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +joint_limits: + prbt_joint_3: + has_position_limits: true + min_position: -1 + max_position: 4 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_min.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_min.yaml new file mode 100644 index 0000000000..50612ed284 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_min.yaml @@ -0,0 +1,37 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Pilz GmbH & Co. KG nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +joint_limits: + prbt_joint_2: + has_position_limits: true + min_position: -4 + max_position: 2 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_velocity.yaml b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_velocity.yaml new file mode 100644 index 0000000000..8373e05b28 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_velocity.yaml @@ -0,0 +1,36 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Pilz GmbH & Co. KG nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +joint_limits: + prbt_joint_3: + has_velocity_limits: true + max_velocity: -90 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/testpoints.py b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/testpoints.py new file mode 100755 index 0000000000..e0056728f3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/testpoints.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2018 Pilz GmbH & Co. KG +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Pilz GmbH & Co. KG nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from geometry_msgs.msg import Point +from pilz_robot_programming.robot import * +from pilz_robot_programming.commands import * +import math +import rospy + +__REQUIRED_API_VERSION__ = "1" + +robot_configs = {} +robot_configs['prbt'] = { + 'initJointPose': [0.0, 0.0, math.radians(-25), 0.0, -1.57, 0], + 'L': 0.2, + 'M': 0.1, + 'planning_group': 'manipulator', + 'target_link': 'prbt_tcp', + 'reference_frame': 'prbt_base', + 'default_or': from_euler(0, math.radians(180), math.radians(90)), + 'P1_position': Point(0.3, 0.0, 0.5), + 'P1_orientation': from_euler(0, math.radians(180), math.radians(135)) +} + +robot_configs['panda'] = { + 'initJointPose': [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785], + 'L': 0.2, + 'M': 0.1, + 'planning_group': 'panda_arm', + 'target_link': 'panda_link8', + 'reference_frame': 'panda_link0', + 'default_or': Quaternion(0.924, -0.382, 0.000, 0.000), + 'P1_position': Point(0.2, 0.0, 0.8) +} + + +def start_program(robot_name): + print("Executing " + __file__) + + test_sequence(**robot_configs[robot_name]) + +def test_sequence(initJointPose, L, M, planning_group, target_link, reference_frame, default_or, P1_position, P1_orientation): + r = Robot(__REQUIRED_API_VERSION__) + + r.move(Ptp(goal=initJointPose, planning_group=planning_group)) + + P1 = Pose(position=P1_position, orientation=P1_orientation) + + + P2 = Pose(position=Point(P1.position.x+L, P1.position.y+L, P1.position.z-M), orientation=default_or) + P3 = Pose(position=Point(P1.position.x, P1.position.y+2*L, P1.position.z-2*M), orientation=default_or) + P4 = Pose(position=Point(P1.position.x, P1.position.y+L, P1.position.z-M), orientation=default_or) + + ptp1 = Ptp(goal=P1, acc_scale=0.3, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + ptp2 = Ptp(goal=P2, acc_scale=0.3, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + ptp3 = Ptp(goal=P3, acc_scale=0.3, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + ptp4 = Ptp(goal=P4, acc_scale=0.3, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + lin1 = Lin(goal=P1, acc_scale=0.1, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + lin2 = Lin(goal=P2, acc_scale=0.1, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + lin3 = Lin(goal=P3, acc_scale=0.1, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + lin4 = Lin(goal=P4, acc_scale=0.1, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + + r.move(ptp1) #PTP_12 test + r.move(ptp2) #PTP_23 + r.move(ptp3) #PTP_34 + r.move(ptp4) #PTP_41 + r.move(ptp1) + + r.move(lin1) #LIN_12 + r.move(lin2) #LIN_23 + r.move(lin3) #LIN_34 + r.move(lin4) #LIN_41 + r.move(lin1) + + circ3_interim_2 = Circ(goal=P3, interim=P2.position, acc_scale=0.2, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + circ1_center_2 = Circ(goal=P1, center=P2.position, acc_scale=0.2, planning_group=planning_group, target_link=target_link, reference_frame=reference_frame) + + for radius in [0, 0.1]: + r.move(Ptp(goal=initJointPose, planning_group=planning_group)) + + seq = Sequence() + seq.append(ptp1, blend_radius=radius) + seq.append(circ3_interim_2, blend_radius=radius) + seq.append(ptp2, blend_radius=radius) + seq.append(lin3, blend_radius=radius) + seq.append(circ1_center_2, blend_radius=radius) + seq.append(lin2, blend_radius=radius) + seq.append(ptp3) + + r.move(seq) + +if __name__ == "__main__": + # Init a ros node + rospy.init_node('robot_program_node') + + import sys + + robots = list(robot_configs.keys()) + + if(len(sys.argv) < 2): + print("Please specify the robot you want to use." + ', '.join('"{0}"'.format(r) for r in robots)) + sys.exit() + + + if(sys.argv[1] not in robots): + print("Robot " + sys.argv[1] + " not available. Use one of " + ', '.join('"{0}"'.format(r) for r in robots)) + sys.exit() + + + start_program(sys.argv[1]) + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp new file mode 100644 index 0000000000..2c9b859e56 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -0,0 +1,1430 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include +#include +#include +#include + +#include "test_utils.h" + +pilz_industrial_motion_planner::JointLimitsContainer +testutils::createFakeLimits(const std::vector& joint_names) +{ + pilz_industrial_motion_planner::JointLimitsContainer container; + + for (const std::string& name : joint_names) + { + JointLimit limit; + limit.has_position_limits = true; + limit.max_position = 2.967; + limit.min_position = -2.967; + limit.has_velocity_limits = true; + limit.max_velocity = 1; + limit.has_acceleration_limits = true; + limit.max_acceleration = 0.5; + limit.has_deceleration_limits = true; + limit.max_deceleration = -1; + + container.addLimit(name, limit); + } + + return container; +} + +bool testutils::getExpectedGoalPose(const moveit::core::RobotModelConstPtr& robot_model, + const planning_interface::MotionPlanRequest& req, std::string& link_name, + Eigen::Isometry3d& goal_pose_expect) +{ + // ++++++++++++++++++++++++++++++++++ + // + Get goal from joint constraint + + // ++++++++++++++++++++++++++++++++++ + if (!req.goal_constraints.front().joint_constraints.empty()) + { + std::map goal_joint_position; + + // initializing all joints of the model + for (const auto& joint_name : robot_model->getVariableNames()) + { + goal_joint_position[joint_name] = 0; + } + + for (const auto& joint_item : req.goal_constraints.front().joint_constraints) + { + goal_joint_position[joint_item.joint_name] = joint_item.position; + } + + link_name = robot_model->getJointModelGroup(req.group_name)->getSolverInstance()->getTipFrame(); + + if (!computeLinkFK(robot_model, link_name, goal_joint_position, goal_pose_expect)) + { + std::cerr << "Failed to compute forward kinematics for link in goal " + "constraints \n"; + return false; + } + return true; + } + + // ++++++++++++++++++++++++++++++++++++++ + // + Get goal from cartesian constraint + + // ++++++++++++++++++++++++++++++++++++++ + // TODO frame id + link_name = req.goal_constraints.front().position_constraints.front().link_name; + geometry_msgs::Pose goal_pose_msg; + goal_pose_msg.position = + req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position; + goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation; + normalizeQuaternion(goal_pose_msg.orientation); + tf2::convert(goal_pose_msg, goal_pose_expect); + return true; +} + +bool testutils::isGoalReached(const trajectory_msgs::JointTrajectory& trajectory, + const std::vector& goal, + const double joint_position_tolerance, const double joint_velocity_tolerance) +{ + trajectory_msgs::JointTrajectoryPoint last_point = trajectory.points.back(); + + for (unsigned int i = 0; i < trajectory.joint_names.size(); ++i) + { + if (fabs(last_point.velocities.at(i)) > joint_velocity_tolerance) + { + std::cerr << "[ Fail ] goal has non zero velocity." + << " Joint Name: " << trajectory.joint_names.at(i) << "; Velocity: " << last_point.velocities.at(i) + << std::endl; + return false; + } + + for (const auto& joint_goal : goal) + { + if (trajectory.joint_names.at(i) == joint_goal.joint_name) + { + if (fabs(last_point.positions.at(i) - joint_goal.position) > joint_position_tolerance) + { + std::cerr << "[ Fail ] goal position not reached." + << " Joint Name: " << trajectory.joint_names.at(i) + << "; Actual Position: " << last_point.positions.at(i) + << "; Expected Position: " << joint_goal.position << std::endl; + return false; + } + } + } + } + + return true; +} + +bool testutils::isGoalReached(const moveit::core::RobotModelConstPtr& robot_model, + const trajectory_msgs::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, const double pos_tolerance, + const double orientation_tolerance) +{ + std::string link_name; + Eigen::Isometry3d goal_pose_expect; + if (!testutils::getExpectedGoalPose(robot_model, req, link_name, goal_pose_expect)) + { + return false; + } + + // compute the actual goal pose in model frame + trajectory_msgs::JointTrajectoryPoint last_point = trajectory.points.back(); + Eigen::Isometry3d goal_pose_actual; + std::map joint_state; + + // initializing all joints of the model + for (const auto& joint_name : robot_model->getVariableNames()) + { + joint_state[joint_name] = 0; + } + + for (std::size_t i = 0; i < trajectory.joint_names.size(); ++i) + { + joint_state[trajectory.joint_names.at(i)] = last_point.positions.at(i); + } + + if (!computeLinkFK(robot_model, link_name, joint_state, goal_pose_actual)) + { + std::cerr << "[ Fail ] forward kinematics computation failed for link: " << link_name << std::endl; + } + + auto actual_rotation{ goal_pose_actual.rotation() }; + auto expected_rotation{ goal_pose_expect.rotation() }; + auto rot_diff{ actual_rotation - expected_rotation }; + if (rot_diff.norm() > orientation_tolerance) + { + std::cout << "\nOrientation difference = " << rot_diff.norm() << " (eps=" << orientation_tolerance << ")" + << "\n### Expected goal orientation: ### \n" + << expected_rotation << std::endl + << "### Actual goal orientation ### \n" + << actual_rotation << std::endl; + return false; + } + + auto actual_position{ goal_pose_actual.translation() }; + auto expected_position{ goal_pose_expect.translation() }; + auto pos_diff{ actual_position - expected_position }; + if (pos_diff.norm() > pos_tolerance) + { + std::cout << "\nPosition difference = " << pos_diff.norm() << " (eps=" << pos_tolerance << ")" + << "\n### Expected goal position: ### \n" + << expected_position << std::endl + << "### Actual goal position ### \n" + << actual_position << std::endl; + return false; + } + + return true; +} + +bool testutils::isGoalReached(const moveit::core::RobotModelConstPtr& robot_model, + const trajectory_msgs::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, const double tolerance) +{ + return isGoalReached(robot_model, trajectory, req, tolerance, tolerance); +} + +bool testutils::checkCartesianLinearity(const moveit::core::RobotModelConstPtr& robot_model, + const trajectory_msgs::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, + const double translation_norm_tolerance, const double rot_axis_norm_tolerance, + const double rot_angle_tolerance) +{ + std::string link_name; + Eigen::Isometry3d goal_pose_expect; + if (!testutils::getExpectedGoalPose(robot_model, req, link_name, goal_pose_expect)) + { + return false; + } + + // compute start pose + robot_state::RobotState rstate(robot_model); + rstate.setToDefaultValues(); + moveit::core::jointStateToRobotState(req.start_state.joint_state, rstate); + Eigen::Isometry3d start_pose = rstate.getFrameTransform(link_name); + + // start goal angle axis + Eigen::AngleAxisd start_goal_aa(start_pose.rotation().transpose() * goal_pose_expect.rotation()); + + // check the linearity + for (trajectory_msgs::JointTrajectoryPoint way_point : trajectory.points) + { + Eigen::Isometry3d way_point_pose; + std::map way_point_joint_state; + + // initializing all joints of the model + for (const auto& joint_name : robot_model->getVariableNames()) + { + way_point_joint_state[joint_name] = 0; + } + + for (std::size_t i = 0; i < trajectory.joint_names.size(); ++i) + { + way_point_joint_state[trajectory.joint_names.at(i)] = way_point.positions.at(i); + } + + if (!computeLinkFK(robot_model, link_name, way_point_joint_state, way_point_pose)) + { + std::cerr << "Failed to compute forward kinematics for link in goal " + "constraints \n"; + return false; + } + + // translational linearity + Eigen::Vector3d goal_start_translation = goal_pose_expect.translation() - start_pose.translation(); + // https://de.wikipedia.org/wiki/Geradengleichung + // Determined form of a straight line in 3D space + if (fabs((goal_start_translation.cross(way_point_pose.translation()) - + goal_start_translation.cross(start_pose.translation())) + .norm()) > fabs(translation_norm_tolerance)) + { + std::cout << "Translational linearity is violated. \n" + << "goal tanslation: " << goal_pose_expect.translation() << std::endl + << "start translation: " << start_pose.translation() << std::endl + << "acutual translation " << way_point_pose.translation() << std::endl; + return false; + } + + if (!checkSLERP(start_pose, goal_pose_expect, way_point_pose, rot_axis_norm_tolerance)) + { + return false; + } + } + + return true; +} + +bool testutils::checkSLERP(const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& goal_pose, + const Eigen::Isometry3d& wp_pose, const double rot_axis_norm_tolerance, + const double rot_angle_tolerance) +{ + // rotational linearity + // start way point angle axis + Eigen::AngleAxisd start_goal_aa(start_pose.rotation().transpose() * goal_pose.rotation()); + Eigen::AngleAxisd start_wp_aa(start_pose.rotation().transpose() * wp_pose.rotation()); + + // parallel rotation axis + // it is possible the axis opposite is + // if the angle is zero, axis is arbitrary + if (!(((start_goal_aa.axis() - start_wp_aa.axis()).norm() < fabs(rot_axis_norm_tolerance)) || + ((start_goal_aa.axis() + start_wp_aa.axis()).norm() < fabs(rot_axis_norm_tolerance)) || + (fabs(start_wp_aa.angle()) < fabs(rot_angle_tolerance)))) + { + std::cout << "Rotational linearity is violated. \n" + << std::endl + << "start goal angle: " << start_goal_aa.angle() << "; axis: " << std::endl + << start_goal_aa.axis() << std::endl + << "start waypoint angle: " << start_wp_aa.angle() << "; axis: " << std::endl + << start_wp_aa.axis() << std::endl; + + return false; + } + + return true; +} + +bool testutils::computeLinkFK(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name, + const std::map& joint_state, Eigen::Isometry3d& pose) +{ + // create robot state + robot_state::RobotState rstate(robot_model); + rstate.setToDefaultValues(); + + // check the reference frame of the target pose + if (!rstate.knowsFrameTransform(link_name)) + { + ROS_ERROR_STREAM("The target link " << link_name << " is not known by robot."); + return false; + } + + // set the joint positions + try + { + rstate.setVariablePositions(joint_state); + } + catch (const std::exception& e) + { + std::cerr << e.what() << '\n'; + return false; + } + + // update the frame + rstate.update(); + pose = rstate.getFrameTransform(link_name); + + return true; +} + +bool testutils::isVelocityBounded(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + for (const auto& point : trajectory.points) + { + for (std::size_t i = 0; i < point.velocities.size(); ++i) + { + if (fabs(point.velocities.at(i)) > fabs(joint_limits.getLimit(trajectory.joint_names.at(i)).max_velocity)) + { + std::cerr << "[ Fail ] Joint velocity limit violated in " << i << "th waypoint of joint: " + << " Joint Name: " << trajectory.joint_names.at(i) << "; Position: " << point.positions.at(i) + << "; Velocity: " << point.velocities.at(i) << "; Acceleration: " << point.accelerations.at(i) + << "; Time from start: " << point.time_from_start.toSec() + << "; Velocity Limit: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_velocity + << std::endl; + + return false; + } + } + } + + return true; +} + +bool testutils::isTrajectoryConsistent(const trajectory_msgs::JointTrajectory& trajectory) +{ + for (const auto& point : trajectory.points) + { + if (trajectory.joint_names.size() != point.positions.size() || + trajectory.joint_names.size() != point.velocities.size() || + trajectory.joint_names.size() != point.accelerations.size()) + { + return false; + } + } + + // TODO check value is consistant (time, position, velocity, acceleration) + + return true; +} + +bool testutils::isAccelerationBounded(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + for (const auto& point : trajectory.points) + { + for (std::size_t i = 0; i < point.velocities.size(); ++i) + { + // deceleration + if (point.velocities.at(i) * point.accelerations.at(i) <= 0) + { + if (fabs(point.accelerations.at(i)) > fabs(joint_limits.getLimit(trajectory.joint_names.at(i)).max_deceleration)) + { + std::cerr << "[ Fail ] Deceleration limit violated of joint: " << trajectory.joint_names.at(i) + << ": Position: " << point.positions.at(i) << "; Velocity: " << point.velocities.at(i) + << "; Acceleration: " << point.accelerations.at(i) + << "; Time from start: " << point.time_from_start.toSec() + << ". Deceleration Limit: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_deceleration + << std::endl; + return false; + } + } + // acceleration + else + { + if (fabs(point.accelerations.at(i)) > fabs(joint_limits.getLimit(trajectory.joint_names.at(i)).max_acceleration)) + { + std::cerr << "[ Fail ] Acceleration limit violated of joint: " << trajectory.joint_names.at(i) + << ": Position: " << point.positions.at(i) << "; Velocity: " << point.velocities.at(i) + << "; Acceleration: " << point.accelerations.at(i) + << "; Time from start: " << point.time_from_start.toSec() + << ". Acceleration Limit: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_acceleration + << std::endl; + + return false; + } + } + } + } + + return true; +} + +bool testutils::isPositionBounded(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + for (const auto& point : trajectory.points) + { + for (std::size_t i = 0; i < point.positions.size(); ++i) + { + if (point.positions.at(i) > joint_limits.getLimit(trajectory.joint_names.at(i)).max_position || + point.positions.at(i) < joint_limits.getLimit(trajectory.joint_names.at(i)).min_position) + { + std::cerr << "[ Fail ] Joint position limit violated in " << i << "th waypoint of joint: " + << " Joint Name: " << trajectory.joint_names.at(i) << "; Position: " << point.positions.at(i) + << "; Velocity: " << point.velocities.at(i) << "; Acceleration: " << point.accelerations.at(i) + << "; Time from start: " << point.time_from_start.toSec() + << "; Max Position: " << joint_limits.getLimit(trajectory.joint_names.at(i)).max_position + << "; Min Position: " << joint_limits.getLimit(trajectory.joint_names.at(i)).min_position + << std::endl; + + return false; + } + } + } + + return true; +} + +bool testutils::checkJointTrajectory(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) +{ + if (!testutils::isTrajectoryConsistent(trajectory)) + { + std::cout << "Joint trajectory is not consistent." << std::endl; + return false; + } + if (!testutils::isPositionBounded(trajectory, joint_limits)) + { + std::cout << "Joint poisiton violated the limits." << std::endl; + return false; + } + if (!testutils::isVelocityBounded(trajectory, joint_limits)) + { + std::cout << "Joint velocity violated the limits." << std::endl; + return false; + } + if (!testutils::isAccelerationBounded(trajectory, joint_limits)) + { + std::cout << "Joint acceleration violated the limits." << std::endl; + return false; + } + + return true; +} + +::testing::AssertionResult testutils::hasStrictlyIncreasingTime(const robot_trajectory::RobotTrajectoryPtr& trajectory) +{ + // Check for strictly positively increasing time steps + for (unsigned int i = 1; i < trajectory->getWayPointCount(); ++i) + { + if (trajectory->getWayPointDurationFromPrevious(i) <= 0.0) + { + return ::testing::AssertionFailure() + << "Waypoint " << (i) << " has " << trajectory->getWayPointDurationFromPrevious(i) + << " time between itself and its predecessor." + << " Total points in trajectory: " << trajectory->getWayPointCount() << "."; + } + } + + return ::testing::AssertionSuccess(); +} + +void testutils::createDummyRequest(const moveit::core::RobotModelConstPtr& robot_model, + const std::string& planning_group, planning_interface::MotionPlanRequest& req) +{ + // valid motion plan request with goal in joint space + req.group_name = planning_group; + req.max_velocity_scaling_factor = 1.0; + req.max_acceleration_scaling_factor = 1.0; + robot_state::RobotState rstate(robot_model); + rstate.setToDefaultValues(); + moveit::core::robotStateToRobotStateMsg(rstate, req.start_state, false); +} + +void testutils::createPTPRequest(const std::string& planning_group, const robot_state::RobotState& start_state, + const robot_state::RobotState& goal_state, planning_interface::MotionPlanRequest& req) +{ + // valid motion plan request with goal in joint space + req.planner_id = "PTP"; + req.group_name = planning_group; + req.max_velocity_scaling_factor = 0.5; + req.max_acceleration_scaling_factor = 0.5; + // start state + moveit::core::robotStateToRobotStateMsg(start_state, req.start_state, false); + // goal state + req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + goal_state, goal_state.getRobotModel()->getJointModelGroup(planning_group))); +} + +bool testutils::toTCPPose(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name, + const std::vector& joint_values, geometry_msgs::Pose& pose, + const std::string& joint_prefix) +{ + { + std::map joint_state; + auto joint_values_it = joint_values.begin(); + + // initializing all joints of the model + for (const auto& joint_name : robot_model->getVariableNames()) + { + joint_state[joint_name] = 0; + } + + for (std::size_t i = 0; i < joint_values.size(); ++i) + { + joint_state[testutils::getJointName(i + 1, joint_prefix)] = *joint_values_it; + ++joint_values_it; + } + + Eigen::Isometry3d eig_pose; + if (!testutils::computeLinkFK(robot_model, link_name, joint_state, eig_pose)) + { + return false; + } + tf2::convert(eig_pose, pose); + return true; + } +} + +bool testutils::checkOriginalTrajectoryAfterBlending(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, + const double time_tolerance) +{ + for (std::size_t i = 0; i < res.first_trajectory->getWayPointCount(); ++i) + { + for (const std::string& joint_name : + res.first_trajectory->getWayPoint(i).getJointModelGroup(req.group_name)->getActiveJointModelNames()) + { + // check joint position + if (res.first_trajectory->getWayPoint(i).getVariablePosition(joint_name) != + req.first_trajectory->getWayPoint(i).getVariablePosition(joint_name)) + { + std::cout << i << "th position of the first trajectory is not same." << std::endl; + return false; + } + + // check joint velocity + if (res.first_trajectory->getWayPoint(i).getVariableVelocity(joint_name) != + req.first_trajectory->getWayPoint(i).getVariableVelocity(joint_name)) + { + std::cout << i << "th velocity of the first trajectory is not same." << std::endl; + return false; + } + + // check joint acceleration + if (res.first_trajectory->getWayPoint(i).getVariableAcceleration(joint_name) != + req.first_trajectory->getWayPoint(i).getVariableAcceleration(joint_name)) + { + std::cout << i << "th acceleration of the first trajectory is not same." << std::endl; + return false; + } + } + + // check time from start + if (res.first_trajectory->getWayPointDurationFromStart(i) != req.first_trajectory->getWayPointDurationFromStart(i)) + { + std::cout << i << "th time_from_start of the first trajectory is not same." << std::endl; + return false; + } + } + + size_t size_second = res.second_trajectory->getWayPointCount(); + size_t size_second_original = req.second_trajectory->getWayPointCount(); + for (std::size_t i = 0; i < size_second; ++i) + { + for (const std::string& joint_name : res.second_trajectory->getWayPoint(size_second - i - 1) + .getJointModelGroup(req.group_name) + ->getActiveJointModelNames()) + { + // check joint position + if (res.second_trajectory->getWayPoint(size_second - i - 1).getVariablePosition(joint_name) != + req.second_trajectory->getWayPoint(size_second_original - i - 1).getVariablePosition(joint_name)) + { + std::cout << i - 1 << "th position of the second trajectory is not same." << std::endl; + return false; + } + + // check joint velocity + if (res.second_trajectory->getWayPoint(size_second - i - 1).getVariableVelocity(joint_name) != + req.second_trajectory->getWayPoint(size_second_original - i - 1).getVariableVelocity(joint_name)) + { + std::cout << i - 1 << "th position of the second trajectory is not same." << std::endl; + return false; + } + + // check joint acceleration + if (res.second_trajectory->getWayPoint(size_second - i - 1).getVariableAcceleration(joint_name) != + req.second_trajectory->getWayPoint(size_second_original - i - 1).getVariableAcceleration(joint_name)) + { + std::cout << i - 1 << "th position of the second trajectory is not same." << std::endl; + return false; + } + } + + // check time from start + if (i < size_second - 1) + { + if (fabs((res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1) - + res.second_trajectory->getWayPointDurationFromStart(size_second - i - 2)) - + (req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) - + req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2))) > time_tolerance) + { + std::cout << size_second - i - 1 << "th time from start of the second trajectory is not same." + << res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1) << ", " + << res.second_trajectory->getWayPointDurationFromStart(size_second - i - 2) << ", " + << req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) << ", " + << req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2) << std::endl; + return false; + } + } + else + { + if (fabs((res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1)) - + (req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) - + req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2))) > time_tolerance) + { + std::cout << size_second - i - 1 << "th time from start of the second trajectory is not same." + << res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1) << ", " + << req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) - + req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2) + << std::endl; + return false; + } + } + } + + return true; +} + +bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, + double joint_velocity_tolerance, double joint_accleration_tolerance) +{ + // convert to msgs + moveit_msgs::RobotTrajectory first_traj, blend_traj, second_traj; + res.first_trajectory->getRobotTrajectoryMsg(first_traj); + res.blend_trajectory->getRobotTrajectoryMsg(blend_traj); + res.second_trajectory->getRobotTrajectoryMsg(second_traj); + + // check the continuity between first trajectory and blend trajectory + trajectory_msgs::JointTrajectoryPoint first_end, blend_start; + first_end = first_traj.joint_trajectory.points.back(); + blend_start = blend_traj.joint_trajectory.points.front(); + + // check the dimensions + if (first_end.positions.size() != blend_start.positions.size() || + first_end.velocities.size() != blend_start.velocities.size() || + first_end.accelerations.size() != blend_start.accelerations.size()) + { + std::cout << "Different sizes of the position/velocity/acceleration " + "between first trajectory and blend trajectory." + << std::endl; + return false; + } + + // check the velocity and acceleration + for (std::size_t i = 0; i < first_end.positions.size(); ++i) + { + double blend_start_velo = + (blend_start.positions.at(i) - first_end.positions.at(i)) / blend_start.time_from_start.toSec(); + if (fabs(blend_start_velo - blend_start.velocities.at(i)) > joint_velocity_tolerance) + { + std::cout << "Velocity computed from positions are different from the " + "value in trajectory." + << std::endl + << "position: " << blend_start.positions.at(i) << "; " << first_end.positions.at(i) + << "computed: " << blend_start_velo << "; " + << "in trajectory: " << blend_start.velocities.at(i) << std::endl; + return false; + } + + double blend_start_acc = (blend_start_velo - first_end.velocities.at(i)) / blend_start.time_from_start.toSec(); + if (fabs(blend_start_acc - blend_start.accelerations.at(i)) > joint_velocity_tolerance) + { + std::cout << "Acceleration computed from positions/velocities are " + "different from the value in trajectory." + << std::endl + << "computed: " << blend_start_acc << "; " + << "in trajectory: " << blend_start.accelerations.at(i) << std::endl; + return false; + } + } + + // check the continuity between blend trajectory and second trajectory + trajectory_msgs::JointTrajectoryPoint blend_end, second_start; + blend_end = blend_traj.joint_trajectory.points.back(); + second_start = second_traj.joint_trajectory.points.front(); + + // check the dimensions + if (blend_end.positions.size() != second_start.positions.size() || + blend_end.velocities.size() != second_start.velocities.size() || + blend_end.accelerations.size() != second_start.accelerations.size()) + { + std::cout << "Different sizes of the position/velocity/acceleration " + "between first trajectory and blend trajectory." + << std::endl + << blend_end.positions.size() << ", " << second_start.positions.size() << std::endl + << blend_end.velocities.size() << ", " << second_start.positions.size() << std::endl + << blend_end.accelerations.size() << ", " << second_start.accelerations.size() << std::endl; + return false; + } + + // check the velocity and acceleration + for (std::size_t i = 0; i < blend_end.positions.size(); ++i) + { + double second_start_velo = + (second_start.positions.at(i) - blend_end.positions.at(i)) / second_start.time_from_start.toSec(); + if (fabs(second_start_velo - second_start.velocities.at(i)) > joint_accleration_tolerance) + { + std::cout << "Velocity computed from positions are different from the " + "value in trajectory." + << std::endl + << "computed: " << second_start_velo << "; " + << "in trajectory: " << second_start.velocities.at(i) << std::endl; + return false; + } + + double second_start_acc = (second_start_velo - blend_end.velocities.at(i)) / second_start.time_from_start.toSec(); + if (fabs(second_start_acc - second_start.accelerations.at(i)) > joint_accleration_tolerance) + { + std::cout << "Acceleration computed from positions/velocities are " + "different from the value in trajectory." + << std::endl + << "computed: " << second_start_acc << "; " + << "in trajectory: " << second_start.accelerations.at(i) << std::endl; + return false; + } + } + + return true; +} + +bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits) +{ + // sampling time + double duration = res.blend_trajectory->getWayPointDurationFromPrevious(res.blend_trajectory->getWayPointCount() - 1); + if (duration == 0.0) + { + std::cout << "Cannot perform check of cartesian space continuity with " + "sampling time 0.0" + << std::endl; + return false; + } + + // limits + double max_trans_velo = planner_limits.getCartesianLimits().getMaxTranslationalVelocity(); + double max_trans_acc = planner_limits.getCartesianLimits().getMaxTranslationalAcceleration(); + double max_rot_velo = planner_limits.getCartesianLimits().getMaxRotationalVelocity(); + double max_rot_acc = max_trans_acc / max_trans_velo * max_rot_velo; + + // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + // +++ check the connection points between three trajectories +++ + // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + + // connection points + Eigen::Isometry3d pose_first_end, pose_first_end_1, pose_blend_start, pose_blend_start_1, pose_blend_end, + pose_blend_end_1, pose_second_start, pose_second_start_1; + // one sample before last point of first trajectory + pose_first_end_1 = res.first_trajectory->getWayPointPtr(res.first_trajectory->getWayPointCount() - 2) + ->getFrameTransform(req.link_name); + // last point of first trajectory + pose_first_end = res.first_trajectory->getLastWayPointPtr()->getFrameTransform(req.link_name); + // first point of blend trajectory + pose_blend_start = res.blend_trajectory->getFirstWayPointPtr()->getFrameTransform(req.link_name); + // second point of blend trajectory + pose_blend_start_1 = res.blend_trajectory->getWayPointPtr(1)->getFrameTransform(req.link_name); + // one sample before last point of blend trajectory + pose_blend_end_1 = res.blend_trajectory->getWayPointPtr(res.blend_trajectory->getWayPointCount() - 2) + ->getFrameTransform(req.link_name); + // last point of blend trajectory + pose_blend_end = res.blend_trajectory->getLastWayPointPtr()->getFrameTransform(req.link_name); + // first point of second trajectory + pose_second_start = res.second_trajectory->getFirstWayPointPtr()->getFrameTransform(req.link_name); + // second point of second trajectory + pose_second_start_1 = res.second_trajectory->getWayPointPtr(1)->getFrameTransform(req.link_name); + + // std::cout << "### sample duration: " << duration << " ###" << std::endl; + // std::cout << "### end pose of first trajectory ###" << std::endl; + // std::cout << pose_first_end.matrix() << std::endl; + // std::cout << "### start pose of blend trajectory ###" << std::endl; + // std::cout << pose_blend_start.matrix() << std::endl; + // std::cout << "### end pose of blend trajectory ###" << std::endl; + // std::cout << pose_blend_end.matrix() << std::endl; + // std::cout << "### start pose of second trajectory ###" << std::endl; + // std::cout << pose_second_start.matrix() << std::endl; + + // std::cout << "### v_1 ###" << std::endl; + // std::cout << v_1 << std::endl; + // std::cout << "### w_1 ###" << std::endl; + // std::cout << w_1 << std::endl; + // std::cout << "### v_2 ###" << std::endl; + // std::cout << v_2 << std::endl; + // std::cout << "### w_2 ###" << std::endl; + // std::cout << w_2 << std::endl; + // std::cout << "### v_3 ###" << std::endl; + // std::cout << v_3 << std::endl; + // std::cout << "### w_3 ###" << std::endl; + // std::cout << w_3 << std::endl; + + // check the connection points between first trajectory and blend trajectory + Eigen::Vector3d v_1, w_1, v_2, w_2, v_3, w_3; + computeCartVelocity(pose_first_end_1, pose_first_end, duration, v_1, w_1); + computeCartVelocity(pose_first_end, pose_blend_start, duration, v_2, w_2); + computeCartVelocity(pose_blend_start, pose_blend_start_1, duration, v_3, w_3); + + // translational velocity + if (v_2.norm() > max_trans_velo) + { + std::cout << "Translational velocity between first trajectory and blend " + "trajectory exceeds the limit." + << "Actual velocity (norm): " << v_2.norm() << "; " + << "Limits: " << max_trans_velo << std::endl; + } + // rotational velocity + if (w_2.norm() > max_rot_velo) + { + std::cout << "Rotational velocity between first trajectory and blend " + "trajectory exceeds the limit." + << "Actual velocity (norm): " << w_2.norm() << "; " + << "Limits: " << max_rot_velo << std::endl; + } + // translational acceleration + Eigen::Vector3d a_1 = (v_2 - v_1) / duration; + Eigen::Vector3d a_2 = (v_3 - v_2) / duration; + if (a_1.norm() > max_trans_acc || a_2.norm() > max_trans_acc) + { + std::cout << "Translational acceleration between first trajectory and " + "blend trajectory exceeds the limit." + << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; " + << "Limits: " << max_trans_acc << std::endl; + } + + // rotational acceleration + a_1 = (w_2 - w_1) / duration; + a_2 = (w_3 - w_2) / duration; + if (a_1.norm() > max_rot_acc || a_2.norm() > max_rot_acc) + { + std::cout << "Rotational acceleration between first trajectory and blend " + "trajectory exceeds the limit." + << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; " + << "Limits: " << max_rot_acc << std::endl; + } + + computeCartVelocity(pose_blend_end_1, pose_blend_end, duration, v_1, w_1); + computeCartVelocity(pose_blend_end, pose_second_start, duration, v_2, w_2); + computeCartVelocity(pose_second_start, pose_second_start_1, duration, v_3, w_3); + + if (v_2.norm() > max_trans_velo) + { + std::cout << "Translational velocity between blend trajectory and second " + "trajectory exceeds the limit." + << "Actual velocity (norm): " << v_2.norm() << "; " + << "Limits: " << max_trans_velo << std::endl; + } + if (w_2.norm() > max_rot_velo) + { + std::cout << "Rotational velocity between blend trajectory and second " + "trajectory exceeds the limit." + << "Actual velocity (norm): " << w_2.norm() << "; " + << "Limits: " << max_rot_velo << std::endl; + } + a_1 = (v_2 - v_1) / duration; + a_2 = (v_3 - v_2) / duration; + if (a_1.norm() > max_trans_acc || a_2.norm() > max_trans_acc) + { + std::cout << "Translational acceleration between blend trajectory and " + "second trajectory exceeds the limit." + << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; " + << "Limits: " << max_trans_acc << std::endl; + } + // check rotational acceleration + a_1 = (w_2 - w_1) / duration; + a_2 = (w_3 - w_2) / duration; + if (a_1.norm() > max_rot_acc || a_2.norm() > max_rot_acc) + { + std::cout << "Rotational acceleration between blend trajectory and second " + "trajectory exceeds the limit." + << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; " + << "Limits: " << max_rot_acc << std::endl; + } + + return true; +} + +bool testutils::checkThatPointsInRadius(const std::string& link_name, const double& r, Eigen::Isometry3d& circ_pose, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& res) +{ + bool result = true; + for (size_t i = 1; i < res.blend_trajectory->getWayPointCount() - 1; ++i) + { + Eigen::Isometry3d curr_pos(res.blend_trajectory->getWayPointPtr(i)->getFrameTransform(link_name)); + if ((curr_pos.translation() - circ_pose.translation()).norm() > r) + { + std::cout << "Point " << i << " does not lie within blending radius (dist: " + << ((curr_pos.translation() - circ_pose.translation()).norm() - r) << ")." << std::endl; + result = false; + } + } + return result; +} + +void testutils::computeCartVelocity(const Eigen::Isometry3d& pose_1, const Eigen::Isometry3d& pose_2, double duration, + Eigen::Vector3d& v, Eigen::Vector3d& w) +{ + // translational velocity + v = (pose_2.translation() - pose_1.translation()) / duration; + + // angular velocity + // reference: A Mathematical Introduction to Robotics Manipulation 2.4.1 + // Rotational velocity + Eigen::Matrix3d rm_1 = pose_1.linear(); + Eigen::Matrix3d rm_2 = pose_2.linear(); + Eigen::Matrix3d rm_dot = (rm_2 - rm_1) / duration; + Eigen::Matrix3d w_hat = rm_dot * rm_1.transpose(); + w(0) = w_hat(2, 1); + w(1) = w_hat(0, 2); + w(2) = w_hat(1, 0); +} + +void testutils::getLinLinPosesWithoutOriChange(const std::string& frame_id, sensor_msgs::JointState& initialJointState, + geometry_msgs::PoseStamped& p1, geometry_msgs::PoseStamped& p2) +{ + initialJointState = + testutils::generateJointState({ 0., 0.007881892504574495, -1.8157263253868452, 0., 1.8236082178909834, 0. }); + + p1.header.frame_id = frame_id; + p1.pose.position.x = 0.25; + p1.pose.position.y = 0.3; + p1.pose.position.z = 0.65; + p1.pose.orientation.x = 0.; + p1.pose.orientation.y = 0.; + p1.pose.orientation.z = 0.; + p1.pose.orientation.w = 1.; + + p2 = p1; + p2.pose.position.x -= 0.15; +} + +void testutils::getOriChange(Eigen::Matrix3d& ori1, Eigen::Matrix3d& ori2) +{ + ori1 = Eigen::AngleAxisd(0.2 * M_PI, Eigen::Vector3d::UnitZ()); + ori2 = Eigen::AngleAxisd(0.4 * M_PI, Eigen::Vector3d::UnitZ()); +} + +void testutils::createFakeCartTraj(const robot_trajectory::RobotTrajectoryPtr& traj, const std::string& link_name, + moveit_msgs::RobotTrajectory& fake_traj) +{ + fake_traj.joint_trajectory.joint_names.push_back("x"); + fake_traj.joint_trajectory.joint_names.push_back("y"); + fake_traj.joint_trajectory.joint_names.push_back("z"); + // fake_traj.joint_trajectory.joint_names.push_back("a"); + // fake_traj.joint_trajectory.joint_names.push_back("b"); + // fake_traj.joint_trajectory.joint_names.push_back("c"); + + for (size_t i = 0; i < traj->getWayPointCount(); ++i) + { + trajectory_msgs::JointTrajectoryPoint waypoint; + waypoint.time_from_start = ros::Duration(traj->getWayPointDurationFromStart(i)); + Eigen::Isometry3d waypoint_pose = traj->getWayPointPtr(i)->getFrameTransform(link_name); + Eigen::Vector3d waypoint_position = waypoint_pose.translation(); + waypoint.positions.push_back(waypoint_position(0)); + waypoint.positions.push_back(waypoint_position(1)); + waypoint.positions.push_back(waypoint_position(2)); + waypoint.velocities.push_back(0); + waypoint.velocities.push_back(0); + waypoint.velocities.push_back(0); + waypoint.accelerations.push_back(0); + waypoint.accelerations.push_back(0); + waypoint.accelerations.push_back(0); + fake_traj.joint_trajectory.points.push_back(waypoint); + } +} + +bool testutils::getBlendTestData(const ros::NodeHandle& nh, const size_t& dataset_num, const std::string& name_prefix, + std::vector& datasets) +{ + datasets.clear(); + testutils::BlendTestData dataset; + for (size_t i = 1; i < dataset_num + 1; ++i) + { + std::string data_set_name = "blend_set_" + std::to_string(i); + if (nh.getParam(name_prefix + data_set_name + "/start_position", dataset.start_position) && + nh.getParam(name_prefix + data_set_name + "/mid_position", dataset.mid_position) && + nh.getParam(name_prefix + data_set_name + "/end_position", dataset.end_position)) + { + datasets.push_back(dataset); + } + else + { + return false; + } + } + return true; +} + +bool testutils::generateTrajFromBlendTestData( + const robot_model::RobotModelConstPtr& robot_model, + const std::shared_ptr& tg, const std::string& group_name, + const std::string& link_name, const testutils::BlendTestData& data, const double& sampling_time_1, + const double& sampling_time_2, planning_interface::MotionPlanResponse& res_1, + planning_interface::MotionPlanResponse& res_2, double& dis_1, double& dis_2) +{ + // generate first trajectory + planning_interface::MotionPlanRequest req_1; + req_1.group_name = group_name; + req_1.max_velocity_scaling_factor = 0.1; + req_1.max_acceleration_scaling_factor = 0.1; + // start state + robot_state::RobotState start_state(robot_model); + start_state.setToDefaultValues(); + start_state.setJointGroupPositions(group_name, data.start_position); + moveit::core::robotStateToRobotStateMsg(start_state, req_1.start_state, false); + // goal constraint + robot_state::RobotState goal_state_1(robot_model); + goal_state_1.setToDefaultValues(); + goal_state_1.setJointGroupPositions(group_name, data.mid_position); + req_1.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + goal_state_1, goal_state_1.getRobotModel()->getJointModelGroup(group_name))); + + // trajectory generation + if (!tg->generate(req_1, res_1, sampling_time_1)) + { + std::cout << "Failed to generate first trajectory." << std::endl; + return false; + } + + // generate second LIN trajectory + planning_interface::MotionPlanRequest req_2; + req_2.group_name = group_name; + req_2.max_velocity_scaling_factor = 0.1; + req_2.max_acceleration_scaling_factor = 0.1; + // start state + moveit::core::robotStateToRobotStateMsg(goal_state_1, req_2.start_state, false); + // goal state + robot_state::RobotState goal_state_2(robot_model); + goal_state_2.setToDefaultValues(); + goal_state_2.setJointGroupPositions(group_name, data.end_position); + req_2.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + goal_state_2, goal_state_2.getRobotModel()->getJointModelGroup(group_name))); + // trajectory generation + if (!tg->generate(req_2, res_2, sampling_time_2)) + { + std::cout << "Failed to generate second trajectory." << std::endl; + return false; + } + + // estimate a proper blend radius + dis_1 = + (start_state.getFrameTransform(link_name).translation() - goal_state_1.getFrameTransform(link_name).translation()) + .norm(); + + dis_2 = (goal_state_1.getFrameTransform(link_name).translation() - + goal_state_2.getFrameTransform(link_name).translation()) + .norm(); + + return true; +} + +bool testutils::checkBlendResult(const pilz_industrial_motion_planner::TrajectoryBlendRequest& blend_req, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& blend_res, + const pilz_industrial_motion_planner::LimitsContainer& limits, + double joint_velocity_tolerance, double joint_acceleration_tolerance, + double cartesian_velocity_tolerance, double cartesian_angular_velocity_tolerance) +{ + // ++++++++++++++++++++++ + // + Check trajectories + + // ++++++++++++++++++++++ + moveit_msgs::RobotTrajectory traj_msg; + blend_res.first_trajectory->getRobotTrajectoryMsg(traj_msg); + if (!testutils::checkJointTrajectory(traj_msg.joint_trajectory, limits.getJointLimitContainer())) + { + return false; + } + + blend_res.blend_trajectory->getRobotTrajectoryMsg(traj_msg); + if (!testutils::checkJointTrajectory(traj_msg.joint_trajectory, limits.getJointLimitContainer())) + { + return false; + }; + + blend_res.second_trajectory->getRobotTrajectoryMsg(traj_msg); + if (!testutils::checkJointTrajectory(traj_msg.joint_trajectory, limits.getJointLimitContainer())) + { + return false; + }; + + Eigen::Isometry3d circ_pose = + blend_req.first_trajectory->getLastWayPointPtr()->getFrameTransform(blend_req.link_name); + if (!testutils::checkThatPointsInRadius(blend_req.link_name, blend_req.blend_radius, circ_pose, blend_res)) + { + return false; + } + + // check the first and second trajectories, if they are still the same before + // and after the bendling phase + if (!testutils::checkOriginalTrajectoryAfterBlending(blend_req, blend_res, 10e-5)) + { + return false; + } + + // check the continuity between the trajectories in joint space + if (!testutils::checkBlendingJointSpaceContinuity(blend_res, joint_velocity_tolerance, joint_acceleration_tolerance)) + { + return false; + } + + // check the continuity between the trajectories in cart space + if (!testutils::checkBlendingCartSpaceContinuity(blend_req, blend_res, limits)) + { + return false; + } + + // ++++++++++++++++++++++++ + // + Visualize the result + + // ++++++++++++++++++++++++ + // ros::NodeHandle nh; + // ros::Publisher pub = + // nh.advertise("my_planned_path", 1); + // ros::Duration duration(1.0); + // duration.sleep(); + + // // visualize the joint trajectory + // moveit_msgs::DisplayTrajectory displayTrajectory; + // moveit_msgs::RobotTrajectory res_first_traj_msg, res_blend_traj_msg, + // res_second_traj_msg; + // blend_res.first_trajectory->getRobotTrajectoryMsg(res_first_traj_msg); + // blend_res.blend_trajectory->getRobotTrajectoryMsg(res_blend_traj_msg); + // blend_res.second_trajectory->getRobotTrajectoryMsg(res_second_traj_msg); + // displayTrajectory.trajectory.push_back(res_first_traj_msg); + // displayTrajectory.trajectory.push_back(res_blend_traj_msg); + // displayTrajectory.trajectory.push_back(res_second_traj_msg); + + // pub.publish(displayTrajectory); + + return true; +} + +void testutils::generateRequestMsgFromBlendTestData(const moveit::core::RobotModelConstPtr& robot_model, + const testutils::BlendTestData& data, const std::string& planner_id, + const std::string& group_name, const std::string& link_name, + moveit_msgs::MotionSequenceRequest& req_list) +{ + // motion plan request of first trajectory + planning_interface::MotionPlanRequest req_1; + req_1.planner_id = planner_id; + req_1.group_name = group_name; + req_1.max_velocity_scaling_factor = 0.1; + req_1.max_acceleration_scaling_factor = 0.1; + // start state + robot_state::RobotState start_state(robot_model); + start_state.setToDefaultValues(); + start_state.setJointGroupPositions(group_name, data.start_position); + moveit::core::robotStateToRobotStateMsg(start_state, req_1.start_state, false); + // goal constraint + robot_state::RobotState goal_state_1(robot_model); + goal_state_1.setToDefaultValues(); + goal_state_1.setJointGroupPositions(group_name, data.mid_position); + req_1.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + goal_state_1, goal_state_1.getRobotModel()->getJointModelGroup(group_name))); + + // motion plan request of second trajectory + planning_interface::MotionPlanRequest req_2; + req_2.planner_id = planner_id; + req_2.group_name = group_name; + req_2.max_velocity_scaling_factor = 0.1; + req_2.max_acceleration_scaling_factor = 0.1; + // start state + // moveit::core::robotStateToRobotStateMsg(goal_state_1, req_2.start_state, + // false); + // goal state + robot_state::RobotState goal_state_2(robot_model); + goal_state_2.setToDefaultValues(); + goal_state_2.setJointGroupPositions(group_name, data.end_position); + req_2.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + goal_state_2, goal_state_2.getRobotModel()->getJointModelGroup(group_name))); + + // select a proper blending radius + // estimate a proper blend radius + double dis_1 = + (start_state.getFrameTransform(link_name).translation() - goal_state_1.getFrameTransform(link_name).translation()) + .norm(); + + double dis_2 = (goal_state_1.getFrameTransform(link_name).translation() - + goal_state_2.getFrameTransform(link_name).translation()) + .norm(); + + double blend_radius = 0.5 * std::min(dis_1, dis_2); + + moveit_msgs::MotionSequenceItem blend_req_1, blend_req_2; + blend_req_1.req = req_1; + blend_req_1.blend_radius = blend_radius; + blend_req_2.req = req_2; + blend_req_2.blend_radius = 0.0; + + req_list.items.push_back(blend_req_1); + req_list.items.push_back(blend_req_2); +} + +void testutils::checkRobotModel(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, + const std::string& link_name) +{ + ASSERT_TRUE(robot_model != nullptr) << "failed to load robot model"; + ASSERT_FALSE(robot_model->isEmpty()) << "robot model is empty"; + ASSERT_TRUE(robot_model->hasJointModelGroup(group_name)) << group_name << " is not known to robot"; + ASSERT_TRUE(robot_model->hasLinkModel(link_name)) << link_name << " is not known to robot"; + ASSERT_TRUE(robot_state::RobotState(robot_model).knowsFrameTransform(link_name)) + << "Transform of " << link_name << " is unknown"; +} + +::testing::AssertionResult testutils::hasTrapezoidVelocity(std::vector accelerations, const double acc_tol) +{ + // Check that acceleration is monotonously decreasing + if (!isMonotonouslyDecreasing(accelerations, acc_tol)) + { + return ::testing::AssertionFailure() << "Cannot be a trapezoid since " + "acceleration is not monotonously " + "decreasing!"; + } + + // Check accelerations + double first_acc = accelerations.front(); + auto it_last_acc = std::find_if(accelerations.begin(), accelerations.end(), + [first_acc, acc_tol](double el) { return (std::abs(el - first_acc) > acc_tol); }) - + 1; + + auto it_last_intermediate = + std::find_if(it_last_acc + 1, accelerations.end(), [acc_tol](double el) { return (el < acc_tol); }) - 1; + + // Check that there are 1 or 2 intermediate points + auto n_intermediate_1 = std::distance(it_last_acc, it_last_intermediate); + + if (n_intermediate_1 != 1 && n_intermediate_1 != 2) + { + return ::testing::AssertionFailure() << "Expected 1 or 2 intermediate points between acceleration and " + "constant " + "velocity phase but found: " + << n_intermediate_1; + } + + // Const phase (vel == 0) + auto it_first_const = it_last_intermediate + 1; + auto it_last_const = + std::find_if(it_first_const, accelerations.end(), [acc_tol](double el) { return (std::abs(el) > acc_tol); }) - 1; + // This test makes sense only if the generated traj has enough points such + // that the trapezoid is not degenerated. + if (std::distance(it_first_const, it_last_const) < 3) + { + return ::testing::AssertionFailure() << "Exptected the have at least 3 points during the phase of " + "constant " + "velocity."; + } + + // Deceleration + double deceleration = accelerations.back(); + auto it_first_dec = + std::find_if(it_last_const + 1, accelerations.end(), + [deceleration, acc_tol](double el) { return (std::abs(el - deceleration) > acc_tol); }) + + 1; + + // Points between const and deceleration (again 1 or 2) + auto n_intermediate_2 = std::distance(it_last_const, it_first_dec); + if (n_intermediate_2 != 1 && n_intermediate_2 != 2) + { + return ::testing::AssertionFailure() << "Expected 1 or 2 intermediate points between constant velocity " + "and " + "deceleration phase but found: " + << n_intermediate_2; + } + + std::stringstream debug_stream; + for (auto it = accelerations.begin(); it != it_last_acc + 1; it++) + { + debug_stream << *it << "(Acc)" << std::endl; + } + + for (auto it = it_last_acc + 1; it != it_last_intermediate + 1; it++) + { + debug_stream << *it << "(Inter1)" << std::endl; + } + + for (auto it = it_first_const; it != it_last_const + 1; it++) + { + debug_stream << *it << "(Const)" << std::endl; + } + + for (auto it = it_last_const + 1; it != it_first_dec; it++) + { + debug_stream << *it << "(Inter2)" << std::endl; + } + + for (auto it = it_first_dec; it != accelerations.end(); it++) + { + debug_stream << *it << "(Dec)" << std::endl; + } + + ROS_DEBUG_STREAM(debug_stream.str()); + + return ::testing::AssertionSuccess(); +} + +::testing::AssertionResult +testutils::checkCartesianTranslationalPath(const robot_trajectory::RobotTrajectoryConstPtr& trajectory, + const std::string& link_name, const double acc_tol) +{ + // We require the following such that the test makes sense, else the traj + // would have a degenerated velocity profile + EXPECT_GT(trajectory->getWayPointCount(), 9u); + + std::vector accelerations; + + // Iterate over waypoints collect accelerations + for (size_t i = 2; i < trajectory->getWayPointCount(); ++i) + { + auto waypoint_pose_0 = trajectory->getWayPoint(i - 2).getFrameTransform(link_name); + auto waypoint_pose_1 = trajectory->getWayPoint(i - 1).getFrameTransform(link_name); + auto waypoint_pose_2 = trajectory->getWayPoint(i).getFrameTransform(link_name); + + auto t1 = trajectory->getWayPointDurationFromPrevious(i - 1); + auto t2 = trajectory->getWayPointDurationFromPrevious(i); + + auto vel1 = (waypoint_pose_1.translation() - waypoint_pose_0.translation()).norm() / t1; + auto vel2 = (waypoint_pose_2.translation() - waypoint_pose_1.translation()).norm() / t2; + auto acc_transl = (vel2 - vel1) / (t1 + t2); + accelerations.push_back(acc_transl); + } + + return hasTrapezoidVelocity(accelerations, acc_tol); +} + +::testing::AssertionResult +testutils::checkCartesianRotationalPath(const robot_trajectory::RobotTrajectoryConstPtr& trajectory, + const std::string& link_name, const double rot_axis_tol, const double acc_tol) +{ + // skip computations if rotation angle is zero + if (trajectory->getFirstWayPoint().getFrameTransform(link_name).rotation().isApprox( + trajectory->getLastWayPoint().getFrameTransform(link_name).rotation(), rot_axis_tol)) + { + return ::testing::AssertionSuccess(); + } + + // We require the following such that the test makes sense, else the traj + // would have a degenerated velocity profile + EXPECT_GT(trajectory->getWayPointCount(), 9u); + + std::vector accelerations; + std::vector rotation_axes; + + // Iterate over waypoints collect accelerations and rotation axes + for (size_t i = 2; i < trajectory->getWayPointCount(); ++i) + { + auto waypoint_pose_0 = trajectory->getWayPoint(i - 2).getFrameTransform(link_name); + auto waypoint_pose_1 = trajectory->getWayPoint(i - 1).getFrameTransform(link_name); + auto waypoint_pose_2 = trajectory->getWayPoint(i).getFrameTransform(link_name); + + auto t1 = trajectory->getWayPointDurationFromPrevious(i - 1); + auto t2 = trajectory->getWayPointDurationFromPrevious(i); + + Eigen::Quaterniond orientation0(waypoint_pose_0.rotation()); + Eigen::Quaterniond orientation1(waypoint_pose_1.rotation()); + Eigen::Quaterniond orientation2(waypoint_pose_2.rotation()); + + Eigen::AngleAxisd axis1(orientation0 * orientation1.inverse()); + Eigen::AngleAxisd axis2(orientation1 * orientation2.inverse()); + if (i == 2) + { + rotation_axes.push_back(axis1); + } + rotation_axes.push_back(axis2); + + double angular_vel1 = axis1.angle() / t1; + double angular_vel2 = axis2.angle() / t2; + double angular_acc = (angular_vel2 - angular_vel1) / (t1 + t2); + accelerations.push_back(angular_acc); + } + + // Check that rotation axis is fixed + if (std::adjacent_find(rotation_axes.begin(), rotation_axes.end(), + [rot_axis_tol](const Eigen::AngleAxisd& axis1, const Eigen::AngleAxisd& axis2) { + return ((axis1.axis() - axis2.axis()).norm() > rot_axis_tol); + }) != rotation_axes.end()) + { + return ::testing::AssertionFailure() << "Rotational path of trajectory " + "does not have a fixed rotation " + "axis"; + } + + return hasTrapezoidVelocity(accelerations, acc_tol); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h new file mode 100644 index 0000000000..2937ff1dae --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h @@ -0,0 +1,484 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#ifndef INSTANTIATE_TEST_SUITE_P // prior to gtest 1.10 +#define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__) +#endif +#ifndef TYPED_TEST_SUITE // prior to gtest 1.10 +#define TYPED_TEST_SUITE(...) TYPED_TEST_CASE(__VA_ARGS__) +#endif + +#include "moveit_msgs/MotionSequenceRequest.h" +#include "pilz_industrial_motion_planner/limits_container.h" +#include "pilz_industrial_motion_planner/trajectory_blend_request.h" +#include "pilz_industrial_motion_planner/trajectory_blend_response.h" +#include "pilz_industrial_motion_planner/trajectory_generator.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace testutils +{ +const std::string JOINT_NAME_PREFIX("prbt_joint_"); + +static constexpr int32_t DEFAULT_SERVICE_TIMEOUT(10); +static constexpr double DEFAULT_ACCELERATION_EQUALITY_TOLERANCE{ 1e-6 }; +static constexpr double DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE{ 1e-8 }; + +/** + * @brief Convert degree to rad. + */ +inline static constexpr double deg2Rad(double angle) +{ + return (angle / 180.0) * M_PI; +} + +inline std::string getJointName(size_t joint_number, const std::string& joint_prefix) +{ + return joint_prefix + std::to_string(joint_number); +} + +/** + * @brief Create limits for tests to avoid the need to get the limits from the + * parameter server + */ +pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits(const std::vector& joint_names); + +inline std::string demangel(char const* name) +{ + return boost::core::demangle(name); +} + +//******************************************** +// Motion plan requests +//******************************************** + +inline sensor_msgs::JointState generateJointState(const std::vector& pos, const std::vector& vel, + const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX) +{ + sensor_msgs::JointState state; + auto posit = pos.cbegin(); + size_t i = 0; + + while (posit != pos.cend()) + { + state.name.push_back(testutils::getJointName(i + 1, joint_prefix)); + state.position.push_back(*posit); + + i++; + posit++; + } + for (const auto& one_vel : vel) + { + state.velocity.push_back(one_vel); + } + return state; +} + +inline sensor_msgs::JointState generateJointState(const std::vector& pos, + const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX) +{ + return generateJointState(pos, std::vector(), joint_prefix); +} + +inline moveit_msgs::Constraints generateJointConstraint(const std::vector& pos_list, + const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX) +{ + moveit_msgs::Constraints gc; + + auto pos_it = pos_list.begin(); + + for (size_t i = 0; i < pos_list.size(); ++i) + { + moveit_msgs::JointConstraint jc; + jc.joint_name = testutils::getJointName(i + 1, joint_prefix); + jc.position = *pos_it; + gc.joint_constraints.push_back(jc); + + ++pos_it; + } + + return gc; +} + +/** + * @brief Determines the goal position from the given request. + * TRUE if successful, FALSE otherwise. + */ +bool getExpectedGoalPose(const moveit::core::RobotModelConstPtr& robot_model, + const planning_interface::MotionPlanRequest& req, std::string& link_name, + Eigen::Isometry3d& goal_pose_expect); + +/** + * @brief create a dummy motion plan request with zero start state + * No goal constraint is given. + * @param robot_model + * @param planning_group + * @param req + */ +void createDummyRequest(const moveit::core::RobotModelConstPtr& robot_model, const std::string& planning_group, + planning_interface::MotionPlanRequest& req); + +void createPTPRequest(const std::string& planning_group, const moveit::core::RobotState& start_state, + const moveit::core::RobotState& goal_state, planning_interface::MotionPlanRequest& req); + +/** + * @brief check if the goal given in joint space is reached + * Only the last point in the trajectory is veryfied. + * @param trajectory: generated trajectory + * @param goal: goal in joint space + * @param joint_position_tolerance + * @param joint_velocity_tolerance + * @return true if satisfied + */ +bool isGoalReached(const trajectory_msgs::JointTrajectory& trajectory, + const std::vector& goal, const double joint_position_tolerance, + const double joint_velocity_tolerance = 1.0e-6); + +/** + * @brief check if the goal given in cartesian space is reached + * Only the last point in the trajectory is veryfied. + * @param robot_model + * @param trajectory + * @param req + * @param matrix_norm_tolerance: used to compare the transformation matrix + * @param joint_velocity_tolerance + * @return + */ +bool isGoalReached(const robot_model::RobotModelConstPtr& robot_model, + const trajectory_msgs::JointTrajectory& trajectory, const planning_interface::MotionPlanRequest& req, + const double pos_tolerance, const double orientation_tolerance); + +bool isGoalReached(const moveit::core::RobotModelConstPtr& robot_model, + const trajectory_msgs::JointTrajectory& trajectory, const planning_interface::MotionPlanRequest& req, + const double tolerance); + +/** + * @brief Check that given trajectory is straight line. + */ +bool checkCartesianLinearity(const robot_model::RobotModelConstPtr& robot_model, + const trajectory_msgs::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, const double translation_norm_tolerance, + const double rot_axis_norm_tolerance, const double rot_angle_tolerance = 10e-5); + +/** + * @brief check SLERP. The orientation should rotate around the same axis + * linearly. + * @param start_pose + * @param goal_pose + * @param wp_pose + * @param rot_axis_norm_tolerance + * @return + */ +bool checkSLERP(const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& goal_pose, + const Eigen::Isometry3d& wp_pose, const double rot_axis_norm_tolerance, + const double rot_angle_tolerance = 10e-5); + +/** + * @brief get the waypoint index from time from start + * @param trajectory + * @param time_from_start + * @return + */ +inline int getWayPointIndex(const robot_trajectory::RobotTrajectoryPtr& trajectory, const double time_from_start) +{ + int index_before, index_after; + double blend; + trajectory->findWayPointIndicesForDurationAfterStart(time_from_start, index_before, index_after, blend); + return blend > 0.5 ? index_after : index_before; +} + +/** + * @brief check joint trajectory of consistency, position, velocity and + * acceleration limits + * @param trajectory + * @param joint_limits + * @return + */ +bool checkJointTrajectory(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + +/** + * @brief Checks that every waypoint in the trajectory has a non-zero duration + * between itself and its predecessor + * + * Usage in tests: + * @code + * EXPECT_TRUE(HasStrictlyIncreasingTime(trajectory)); + * @endcode + */ +::testing::AssertionResult hasStrictlyIncreasingTime(const robot_trajectory::RobotTrajectoryPtr& trajectory); + +/** + * @brief check if the sizes of the joint position/veloicty/acceleration are + * correct + * @param trajectory + * @return + */ +bool isTrajectoryConsistent(const trajectory_msgs::JointTrajectory& trajectory); + +/** + * @brief is Position Bounded + * @param trajectory + * @param joint_limits + * @return + */ +bool isPositionBounded(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + +/** + * @brief is Velocity Bounded + * @param trajectory + * @param joint_limits + * @return + */ +bool isVelocityBounded(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + +/** + * @brief is Acceleration Bounded + * @param trajectory + * @param joint_limits + * @return + */ +bool isAccelerationBounded(const trajectory_msgs::JointTrajectory& trajectory, + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + +/** + * @brief compute the tcp pose from joint values + * @param robot_model + * @param link_name + * @param joint_values + * @param pose + * @param joint_prefix Prefix of the joint names + * @return false if forward kinematics failed + */ +bool toTCPPose(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name, + const std::vector& joint_values, geometry_msgs::Pose& pose, + const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX); + +/** + * @brief computeLinkFK + * @param robot_model + * @param link_name + * @param joint_state + * @param pose + * @return + */ +bool computeLinkFK(const robot_model::RobotModelConstPtr& robot_model, const std::string& link_name, + const std::map& joint_state, Eigen::Isometry3d& pose); + +/** + * @brief checkOriginalTrajectoryAfterBlending + * @param blend_res + * @param traj_1_res + * @param traj_2_res + * @param time_tolerance + * @return + */ +bool checkOriginalTrajectoryAfterBlending(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, + const double time_tolerance); + +/** + * @brief check the blending result, if the joint space continuity is fulfilled + * @param res: trajectory blending response, contains three trajectories. + * Between these three trajectories should be continuous. + * @return true if joint position/velocity is continuous. joint acceleration can + * have jumps. + */ +bool checkBlendingJointSpaceContinuity(const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, + double joint_velocity_tolerance, double joint_accleration_tolerance); + +bool checkBlendingCartSpaceContinuity(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits); + +/** + * @brief Checks if all points of the blending trajectory lie within the + * blending radius. + */ +bool checkThatPointsInRadius(const std::string& link_name, const double& r, Eigen::Isometry3d& circ_pose, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& res); + +/** + * @brief compute Cartesian velocity + * @param pose_1 + * @param pose_2 + * @param duration + * @param v + * @param w + */ +void computeCartVelocity(const Eigen::Isometry3d& pose_1, const Eigen::Isometry3d& pose_2, double duration, + Eigen::Vector3d& v, Eigen::Vector3d& w); + +/** + * @brief Returns an initial joint state and two poses which + * can be used to perform a Lin-Lin movement. + * + * @param frame_id + * @param initialJointState As cartesian position: (0.3, 0, 0.65, 0, 0, 0) + * @param p1 (0.05, 0, 0.65, 0, 0, 0) + * @param p2 (0.05, 0.4, 0.65, 0, 0, 0) + */ +void getLinLinPosesWithoutOriChange(const std::string& frame_id, sensor_msgs::JointState& initialJointState, + geometry_msgs::PoseStamped& p1, geometry_msgs::PoseStamped& p2); + +void getOriChange(Eigen::Matrix3d& ori1, Eigen::Matrix3d& ori2); + +void createFakeCartTraj(const robot_trajectory::RobotTrajectoryPtr& traj, const std::string& link_name, + moveit_msgs::RobotTrajectory& fake_traj); + +inline geometry_msgs::Quaternion fromEuler(double a, double b, double c) +{ + tf2::Vector3 qvz(0., 0., 1.); + tf2::Vector3 qvy(0., 1., 0.); + tf2::Quaternion q1(qvz, a); + + q1 = q1 * tf2::Quaternion(qvy, b); + q1 = q1 * tf2::Quaternion(qvz, c); + + geometry_msgs::Quaternion msg; + tf2::convert(q1, msg); + return msg; +} + +/** + * @brief Test data for blending, which contains three joint position vectors of + * three robot state. + */ +struct BlendTestData +{ + std::vector start_position; + std::vector mid_position; + std::vector end_position; +}; + +/** + * @brief fetch test datasets from parameter server + * @param nh + * @return + */ +bool getBlendTestData(const ros::NodeHandle& nh, const size_t& dataset_num, const std::string& name_prefix, + std::vector& datasets); + +/** + * @brief check the blending result of lin-lin + * @param lin_res_1 + * @param lin_res_2 + * @param blend_req + * @param blend_res + * @param checkAcceleration + */ +bool checkBlendResult(const pilz_industrial_motion_planner::TrajectoryBlendRequest& blend_req, + const pilz_industrial_motion_planner::TrajectoryBlendResponse& blend_res, + const pilz_industrial_motion_planner::LimitsContainer& limits, double joint_velocity_tolerance, + double joint_acceleration_tolerance, double cartesian_velocity_tolerance, + double cartesian_angular_velocity_tolerance); + +/** + * @brief generate two LIN trajectories from test data set + * @param data: contains joint poisitons of start/mid/end states + * @param sampling_time_1: sampling time for first LIN + * @param sampling_time_2: sampling time for second LIN + * @param[out] res_lin_1: result of the first LIN motion planning + * @param[out] res_lin_2: result of the second LIN motion planning + * @param[out] dis_lin_1: translational distance of the first LIN + * @param[out] dis_lin_2: translational distance of the second LIN + * @return true if succeed + */ +bool generateTrajFromBlendTestData(const moveit::core::RobotModelConstPtr& robot_model, + const std::shared_ptr& tg, + const std::string& group_name, const std::string& link_name, + const BlendTestData& data, const double& sampling_time_1, + const double& sampling_time_2, planning_interface::MotionPlanResponse& res_lin_1, + planning_interface::MotionPlanResponse& res_lin_2, double& dis_lin_1, + double& dis_lin_2); + +void generateRequestMsgFromBlendTestData(const moveit::core::RobotModelConstPtr& robot_model, const BlendTestData& data, + const std::string& planner_id, const std::string& group_name, + const std::string& link_name, moveit_msgs::MotionSequenceRequest& req_list); + +void checkRobotModel(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, + const std::string& link_name); + +/** @brief Check that a given vector of accelerations represents a trapezoid + * velocity profile + * @param acc_tol: tolerance for comparing acceleration values + */ +::testing::AssertionResult hasTrapezoidVelocity(std::vector accelerations, const double acc_tol); + +/** + * @brief Check that the translational path of a given trajectory has a + * trapezoid velocity profile + * @param acc_tol: tolerance for comparing acceleration values + */ +::testing::AssertionResult +checkCartesianTranslationalPath(const robot_trajectory::RobotTrajectoryConstPtr& trajectory, + const std::string& link_name, + const double acc_tol = DEFAULT_ACCELERATION_EQUALITY_TOLERANCE); + +/** + * @brief Check that the rotational path of a given trajectory is a quaternion + * slerp. + * @param rot_axis_tol: tolerance for comparing rotation axes (in the L2 norm) + * @param acc_tol: tolerance for comparing angular acceleration values + */ +::testing::AssertionResult +checkCartesianRotationalPath(const robot_trajectory::RobotTrajectoryConstPtr& trajectory, const std::string& link_name, + const double rot_axis_tol = DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE, + const double acc_tol = DEFAULT_ACCELERATION_EQUALITY_TOLERANCE); + +inline bool isMonotonouslyDecreasing(const std::vector& vec, const double& tol) +{ + return std::is_sorted(vec.begin(), vec.end(), [tol](double a, double b) { + return !(std::abs(a - b) < tol || a < b); // true -> a is ordered before b -> list is not sorted + }); +} + +} // namespace testutils diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.cpp new file mode 100644 index 0000000000..1809ada9c7 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.cpp @@ -0,0 +1,92 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include + +#include "pilz_industrial_motion_planner/cartesian_limit.h" +#include "pilz_industrial_motion_planner/cartesian_limits_aggregator.h" + +/** + * @brief Unittest of the CartesianLimitsAggregator class + */ +class CartesianLimitsAggregator : public ::testing::Test +{ +}; + +/** + * @brief Check if only velocity is set + */ +TEST_F(CartesianLimitsAggregator, OnlyVelocity) +{ + ros::NodeHandle nh("~/vel_only"); + + pilz_industrial_motion_planner::CartesianLimit limit = + pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(nh); + EXPECT_TRUE(limit.hasMaxTranslationalVelocity()); + EXPECT_EQ(limit.getMaxTranslationalVelocity(), 10); + EXPECT_FALSE(limit.hasMaxTranslationalAcceleration()); + EXPECT_FALSE(limit.hasMaxTranslationalDeceleration()); + EXPECT_FALSE(limit.hasMaxRotationalVelocity()); +} + +/** + * @brief Check if all values are set correctly + */ +TEST_F(CartesianLimitsAggregator, AllValues) +{ + ros::NodeHandle nh("~/all"); + + pilz_industrial_motion_planner::CartesianLimit limit = + pilz_industrial_motion_planner::CartesianLimitsAggregator::getAggregatedLimits(nh); + EXPECT_TRUE(limit.hasMaxTranslationalVelocity()); + EXPECT_EQ(limit.getMaxTranslationalVelocity(), 1); + + EXPECT_TRUE(limit.hasMaxTranslationalAcceleration()); + EXPECT_EQ(limit.getMaxTranslationalAcceleration(), 2); + + EXPECT_TRUE(limit.hasMaxTranslationalDeceleration()); + EXPECT_EQ(limit.getMaxTranslationalDeceleration(), -3); + + EXPECT_TRUE(limit.hasMaxRotationalVelocity()); + EXPECT_EQ(limit.getMaxRotationalVelocity(), 4); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_cartesian_limits_aggregator"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.test new file mode 100644 index 0000000000..382a5a5924 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_cartesian_limits_aggregator.test @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp new file mode 100644 index 0000000000..b749d48ae3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_get_solver_tip_frame.cpp @@ -0,0 +1,145 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include + +#include +#include + +#include + +#include "pilz_industrial_motion_planner/tip_frame_getter.h" + +namespace pilz_industrial_motion_planner +{ +using ::testing::AtLeast; +using ::testing::Return; +using ::testing::ReturnRef; + +class SolverMock +{ +public: + MOCK_CONST_METHOD0(getTipFrames, const std::vector&()); +}; + +class JointModelGroupMock +{ +public: + MOCK_CONST_METHOD0(getSolverInstance, SolverMock const*()); + MOCK_CONST_METHOD0(getName, const std::string&()); +}; + +/** + * @brief Test fixture for getSolverTipFrame tests. + */ +class GetSolverTipFrameTest : public testing::Test +{ +protected: + void SetUp() override; + +protected: + SolverMock solver_mock_; + JointModelGroupMock jmg_mock_; + const std::string group_name_{ "fake_group" }; +}; + +void GetSolverTipFrameTest::SetUp() +{ + EXPECT_CALL(jmg_mock_, getName()).WillRepeatedly(ReturnRef(group_name_)); +} + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + */ +TEST_F(GetSolverTipFrameTest, TestExceptionErrorCodeMapping) +{ + { + std::shared_ptr nse_ex{ new NoSolverException("") }; + EXPECT_EQ(nse_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + } + + { + std::shared_ptr ex{ new MoreThanOneTipFrameException("") }; + EXPECT_EQ(ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + } +} + +/** + * @brief Checks that an exceptions is thrown in case a group has more than + * one tip frame. + */ +TEST_F(GetSolverTipFrameTest, TestExceptionMoreThanOneTipFrame) +{ + std::vector tip_frames{ "fake_tip_frame1", "fake_tip_frame2" }; + + EXPECT_CALL(jmg_mock_, getSolverInstance()).Times(AtLeast(1)).WillRepeatedly(Return(&solver_mock_)); + + EXPECT_CALL(solver_mock_, getTipFrames()).Times(AtLeast(1)).WillRepeatedly(ReturnRef(tip_frames)); + + EXPECT_THROW(getSolverTipFrame(&jmg_mock_), MoreThanOneTipFrameException); +} + +/** + * @brief Checks that an exceptions is thrown in case a group does not + * possess a solver. + */ +TEST_F(GetSolverTipFrameTest, TestExceptionNoSolver) +{ + EXPECT_CALL(jmg_mock_, getSolverInstance()).WillOnce(Return(nullptr)); + + EXPECT_THROW(getSolverTipFrame(&jmg_mock_), NoSolverException); +} + +/** + * @brief Checks that an exceptions is thrown in case a nullptr is + * specified as JointModelGroup. + */ +TEST_F(GetSolverTipFrameTest, NullptrJointGroup) +{ + moveit::core::JointModelGroup* group = nullptr; + EXPECT_THROW(hasSolver(group), std::invalid_argument); +} + +} // namespace pilz_industrial_motion_planner + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.cpp new file mode 100644 index 0000000000..488a27bb00 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.cpp @@ -0,0 +1,111 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "ros/ros.h" + +#include + +#include + +#include "pilz_industrial_motion_planner/joint_limits_extension.h" +#include "pilz_industrial_motion_planner/joint_limits_interface_extension.h" + +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner::joint_limits_interface; + +namespace pilz_extensions_tests +{ +class JointLimitTest : public ::testing::Test +{ +}; + +TEST_F(JointLimitTest, SimpleRead) +{ + ros::NodeHandle node_handle("~"); + + // Joints limits interface + JointLimits joint_limits_extended; + JointLimits joint_limits; + + getJointLimits("joint_1", node_handle, joint_limits_extended); + + EXPECT_EQ(1, joint_limits_extended.max_acceleration); + EXPECT_EQ(-1, joint_limits_extended.max_deceleration); +} + +TEST_F(JointLimitTest, readNonExistingJointLimit) +{ + ros::NodeHandle node_handle("~"); + + // Joints limits interface + JointLimits joint_limits_extended; + JointLimits joint_limits; + + EXPECT_FALSE(getJointLimits("anything", node_handle, joint_limits_extended)); +} + +/** + * @brief Test reading a joint limit representing an invalid parameter key + * + * For full coverage. + */ +TEST_F(JointLimitTest, readInvalidParameterName) +{ + ros::NodeHandle node_handle("~"); + + // Joints limits interface + JointLimits joint_limits_extended; + JointLimits joint_limits; + + EXPECT_FALSE(getJointLimits("~anything", node_handle, joint_limits_extended)); +} + +TEST_F(JointLimitTest, OldRead) +{ + ros::NodeHandle node_handle("~"); + + // Joints limits interface + JointLimits joint_limits; + getJointLimits("joint_1", node_handle, joint_limits); + + EXPECT_EQ(1, joint_limits.max_acceleration); +} +} // namespace pilz_extensions_tests + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_joint_limits_extended"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.test new file mode 100644 index 0000000000..361e6f5102 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit.test @@ -0,0 +1,40 @@ + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit_config.yaml b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit_config.yaml new file mode 100644 index 0000000000..4f4bfffd6e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limit_config.yaml @@ -0,0 +1,31 @@ +joint_limits: + joint_1: + has_acceleration_limits: true + max_acceleration: 1 + has_deceleration_limits: true + max_deceleration: -1 + joint_2: + has_acceleration_limits: true + max_acceleration: 2 + has_deceleration_limits: true + max_deceleration: -2 + joint_3: + has_acceleration_limits: true + max_acceleration: 3 + has_deceleration_limits: true + max_deceleration: -3 + joint_4: + has_acceleration_limits: true + max_acceleration: 4 + has_deceleration_limits: true + max_deceleration: -4 + joint_5: + has_acceleration_limits: true + max_acceleration: 5 + has_deceleration_limits: true + max_deceleration: -5 + joint_6: + has_acceleration_limits: true + max_acceleration: 6 + has_deceleration_limits: true + max_deceleration: -6 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.cpp new file mode 100644 index 0000000000..06827e3284 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.cpp @@ -0,0 +1,208 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include +#include + +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/joint_limits_extension.h" +#include "pilz_industrial_motion_planner/joint_limits_interface_extension.h" + +/** + * @brief Unittest of the JointLimitsAggregator class + */ +class JointLimitsAggregator : public ::testing::Test +{ +protected: + void SetUp() override + { + ros::NodeHandle node_handle("~"); + + // Load robot module + robot_model_loader::RobotModelLoader::Options opt("robot_description"); + model_loader_.reset(new robot_model_loader::RobotModelLoader(opt)); + robot_model_ = model_loader_->getModel(); + + return; + } + + /// The robot model loader + robot_model_loader::RobotModelLoaderPtr model_loader_; + + /// The robot model + robot_model::RobotModelConstPtr robot_model_; +}; + +/** + * @brief Check for that the size of the map and the size of the given joint + * models is equal + */ +TEST_F(JointLimitsAggregator, ExpectedMapSize) +{ + ros::NodeHandle nh("~"); + + pilz_industrial_motion_planner::JointLimitsContainer container = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh, + robot_model_->getActiveJointModels()); + + EXPECT_EQ(robot_model_->getActiveJointModels().size(), container.getCount()); +} + +/** + * @brief Check that the value in the parameter server correctly overrides the + * position(if within limits) + */ +TEST_F(JointLimitsAggregator, CorrectOverwriteByParamterPosition) +{ + ros::NodeHandle nh("~/valid_1"); + + pilz_industrial_motion_planner::JointLimitsContainer container = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh, + robot_model_->getActiveJointModels()); + + for (std::pair lim : container) + { + // Check for the overwrite + if (lim.first == "prbt_joint_1") + { + EXPECT_EQ(2, container.getLimit(lim.first).max_position); + EXPECT_EQ(-2, container.getLimit(lim.first).min_position); + } + // Check that nothing else changed + else + { + EXPECT_EQ(robot_model_->getJointModel(lim.first)->getVariableBounds()[0].min_position_, + container.getLimit(lim.first).min_position); + EXPECT_EQ(robot_model_->getJointModel(lim.first)->getVariableBounds()[0].max_position_, + container.getLimit(lim.first).max_position); + } + } +} + +/** + * @brief Check that the value in the parameter server correctly overrides the + * velocity(if within limits) + */ +TEST_F(JointLimitsAggregator, CorrectOverwriteByParamterVelocity) +{ + ros::NodeHandle nh("~/valid_1"); + + pilz_industrial_motion_planner::JointLimitsContainer container = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh, + robot_model_->getActiveJointModels()); + + for (std::pair lim : container) + { + // Check that velocity was only changed in joint "prbt_joint_3" + if (lim.first == "prbt_joint_3") + { + EXPECT_EQ(1.1, container.getLimit(lim.first).max_velocity); + } + else + { + EXPECT_EQ(robot_model_->getJointModel(lim.first)->getVariableBounds()[0].max_velocity_, + container.getLimit(lim.first).max_velocity); + } + } +} + +/** + * @brief Check that the acceleration and deceleration are set properly + */ +TEST_F(JointLimitsAggregator, CorrectSettingAccelerationAndDeceleration) +{ + ros::NodeHandle nh("~/valid_1"); + + pilz_industrial_motion_planner::JointLimitsContainer container = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(nh, + robot_model_->getActiveJointModels()); + + for (std::pair lim : container) + { + if (lim.first == "prbt_joint_4") + { + EXPECT_EQ(5.5, container.getLimit(lim.first).max_acceleration) << lim.first; + EXPECT_EQ(-5.5, container.getLimit(lim.first).max_deceleration) << lim.first; + } + else if (lim.first == "prbt_joint_5") + { + EXPECT_EQ(0, container.getLimit(lim.first).max_acceleration) << lim.first; + EXPECT_EQ(-6.6, container.getLimit(lim.first).max_deceleration) << lim.first; + } + else + { + EXPECT_EQ(0, container.getLimit(lim.first).max_acceleration) << lim.first; + EXPECT_EQ(0, container.getLimit(lim.first).max_deceleration) << lim.first; + } + } +} + +/** + * @brief Check that position limit violations are detected properly + */ +TEST_F(JointLimitsAggregator, LimitsViolationPosition) +{ + ros::NodeHandle nh_min("~/violate_position_min"); + + EXPECT_THROW(pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + nh_min, robot_model_->getActiveJointModels()), + pilz_industrial_motion_planner::AggregationBoundsViolationException); + + ros::NodeHandle nh_max("~/violate_position_max"); + + EXPECT_THROW(pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + nh_max, robot_model_->getActiveJointModels()), + pilz_industrial_motion_planner::AggregationBoundsViolationException); +} + +/** + * @brief Check that velocity limit violations are detected properly + */ +TEST_F(JointLimitsAggregator, LimitsViolationVelocity) +{ + ros::NodeHandle nh("~/violate_velocity"); + + EXPECT_THROW(pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + nh, robot_model_->getActiveJointModels()), + pilz_industrial_motion_planner::AggregationBoundsViolationException); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_joint_limits_aggregator"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.test new file mode 100644 index 0000000000..28177da403 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_aggregator.test @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp new file mode 100644 index 0000000000..2f7f7b80a4 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp @@ -0,0 +1,229 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include "pilz_industrial_motion_planner/joint_limits_container.h" +#include "pilz_industrial_motion_planner/joint_limits_extension.h" + +using namespace pilz_industrial_motion_planner; + +class JointLimitsContainerTest : public ::testing::Test +{ +protected: + void SetUp() override + { + JointLimit lim1; + lim1.has_position_limits = true; + lim1.min_position = -2; + lim1.max_position = 2; + lim1.has_acceleration_limits = true; + lim1.max_acceleration = 3; //<- Expected for common_limit_.max_acceleration + + JointLimit lim2; + lim2.has_position_limits = true; + lim2.min_position = -1; //<- Expected for common_limit_.min_position + lim2.max_position = 1; //<- Expected for common_limit_.max_position + lim2.has_deceleration_limits = true; + lim2.max_deceleration = -5; //<- Expected for common_limit_.max_deceleration + + JointLimit lim3; + lim3.has_velocity_limits = true; + lim3.max_velocity = 10; + + JointLimit lim4; + lim4.has_position_limits = true; + lim4.min_position = -1; + lim4.max_position = 1; + lim4.has_acceleration_limits = true; + lim4.max_acceleration = 400; + lim4.has_deceleration_limits = false; + lim4.max_deceleration = -1; + + JointLimit lim5; + lim5.has_position_limits = true; + lim5.min_position = -1; + lim5.max_position = 1; + lim5.has_acceleration_limits = false; + lim5.max_acceleration = 1; + + JointLimit lim6; + lim6.has_velocity_limits = true; + lim6.max_velocity = 2; //<- Expected for common_limit_.max_velocity + lim6.has_deceleration_limits = true; + lim6.max_deceleration = -100; + + container_.addLimit("joint1", lim1); + container_.addLimit("joint2", lim2); + container_.addLimit("joint3", lim3); + container_.addLimit("joint4", lim4); + container_.addLimit("joint5", lim5); + container_.addLimit("joint6", lim6); + + common_limit_ = container_.getCommonLimit(); + } + + pilz_industrial_motion_planner::JointLimitsContainer container_; + JointLimit common_limit_; +}; + +/** + * @brief Check postion + */ +TEST_F(JointLimitsContainerTest, CheckPositionUnification) +{ + EXPECT_EQ(-1, common_limit_.min_position); + EXPECT_EQ(1, common_limit_.max_position); +} + +/** + * @brief Check velocity + */ +TEST_F(JointLimitsContainerTest, CheckVelocityUnification) +{ + EXPECT_EQ(2, common_limit_.max_velocity); +} + +/** + * @brief Check acceleration + */ +TEST_F(JointLimitsContainerTest, CheckAccelerationUnification) +{ + EXPECT_EQ(3, common_limit_.max_acceleration); +} + +/** + * @brief Check deceleration + */ +TEST_F(JointLimitsContainerTest, CheckDecelerationUnification) +{ + EXPECT_EQ(-5, common_limit_.max_deceleration); +} + +/** + * @brief Check AddLimit for positive and null deceleration + */ +TEST_F(JointLimitsContainerTest, CheckAddLimitDeceleration) +{ + JointLimit lim_invalid1; + lim_invalid1.has_deceleration_limits = true; + lim_invalid1.max_deceleration = 0; + + JointLimit lim_invalid2; + lim_invalid2.has_deceleration_limits = true; + lim_invalid2.max_deceleration = 1; + + JointLimit lim_valid; + lim_valid.has_deceleration_limits = true; + lim_valid.max_deceleration = -1; + + pilz_industrial_motion_planner::JointLimitsContainer container; + + EXPECT_EQ(false, container.addLimit("joint_invalid1", lim_invalid1)); + EXPECT_EQ(false, container.addLimit("joint_invalid2", lim_invalid2)); + EXPECT_EQ(true, container.addLimit("joint_valid", lim_valid)); +} + +/** + * @brief Check AddLimit for already contained limit + */ +TEST_F(JointLimitsContainerTest, CheckAddLimitAlreadyContained) +{ + JointLimit lim_valid; + lim_valid.has_deceleration_limits = true; + lim_valid.max_deceleration = -1; + + pilz_industrial_motion_planner::JointLimitsContainer container; + ASSERT_TRUE(container.addLimit("joint_valid", lim_valid)); + EXPECT_FALSE(container.addLimit("joint_valid", lim_valid)); +} + +/** + * @brief An uninitialized container should not have any limits set. + */ +TEST_F(JointLimitsContainerTest, CheckEmptyContainer) +{ + pilz_industrial_motion_planner::JointLimitsContainer container; + JointLimit limits = container.getCommonLimit(); + EXPECT_FALSE(limits.has_position_limits); + EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_FALSE(limits.has_acceleration_limits); +} + +/** + * @brief empty position limits for first joint, second one should be returned + */ +TEST_F(JointLimitsContainerTest, FirstPositionEmpty) +{ + JointLimit lim1; + + JointLimit lim2; + lim2.has_position_limits = true; + lim2.min_position = -1; //<- Expected for common_limit_.min_position + lim2.max_position = 1; //<- Expected for common_limit_.max_position + + pilz_industrial_motion_planner::JointLimitsContainer container; + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + JointLimit limits = container.getCommonLimit(); + EXPECT_TRUE(limits.has_position_limits); + EXPECT_EQ(1, limits.max_position); + EXPECT_EQ(-1, limits.min_position); +} + +/** + * @brief Check position limits + */ +TEST_F(JointLimitsContainerTest, CheckVerifyPositionLimits) +{ + // positive check: inside limits + std::vector joint_names{ "joint1", "joint2" }; + std::vector joint_positions{ 0.5, 0.5 }; + EXPECT_TRUE(container_.verifyPositionLimits(joint_names, joint_positions)); + + // outside limit2 + joint_positions[1] = 7; + EXPECT_FALSE(container_.verifyPositionLimits(joint_names, joint_positions)); + + // invalid size + std::vector joint_positions1{ 0. }; + EXPECT_THROW(container_.verifyPositionLimits(joint_names, joint_positions1), std::out_of_range); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_validator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_validator.cpp new file mode 100644 index 0000000000..f622cdf8f1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_validator.cpp @@ -0,0 +1,490 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include "pilz_industrial_motion_planner/joint_limits_extension.h" + +#include "pilz_industrial_motion_planner/joint_limits_validator.h" + +using namespace pilz_industrial_motion_planner; + +class JointLimitsValidatorTest : public ::testing::Test +{ +}; + +/** + * @brief Check postion equality + */ +TEST_F(JointLimitsValidatorTest, CheckPositionEquality) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_position_limits = true; + lim1.min_position = -1; + lim1.max_position = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim1); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check postion inequality in min_position detection + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMinPosition) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_position_limits = true; + lim1.min_position = -1; + lim1.max_position = 1; + + JointLimit lim2; + lim2.has_position_limits = true; + lim2.min_position = -2; + lim2.max_position = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(false, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check postion inequality in max_position detection + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxPosition1) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_position_limits = true; + lim1.min_position = -1; + lim1.max_position = 2; + + JointLimit lim2; + lim2.has_position_limits = true; + lim2.min_position = -1; + lim2.max_position = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(false, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check postion inequality in max_position detection (using 3 limits) + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxPosition2) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_position_limits = true; + lim1.min_position = -1; + lim1.max_position = 2; + + JointLimit lim2; + lim2.has_position_limits = true; + lim2.min_position = -1; + lim2.max_position = 1; + + JointLimit lim3; + lim3.has_position_limits = true; + lim3.min_position = -1; + lim3.max_position = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + container.addLimit("joint3", lim3); + + EXPECT_EQ(false, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check postion inequality in has_position_limits false detection + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityHasPositionLimits) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_position_limits = true; + lim1.min_position = -1; + lim1.max_position = 1; + + JointLimit lim2; + lim2.has_position_limits = false; + lim2.min_position = -1; + lim2.max_position = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(false, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- +// VELOCITY +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- + +/** + * @brief Check velocity equality + */ +TEST_F(JointLimitsValidatorTest, CheckVelocityEquality) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_velocity_limits = true; + lim1.max_velocity = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim1); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check max_velocity inequality + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxVelocity1) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_velocity_limits = true; + lim1.max_velocity = 1; + + JointLimit lim2; + lim2.has_velocity_limits = true; + lim2.max_velocity = 2; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check max_velocity inequality (using 3Limits) + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxVelocity2) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_velocity_limits = true; + lim1.max_velocity = 1; + + JointLimit lim2; + lim2.has_velocity_limits = true; + lim2.max_velocity = 2; + + JointLimit lim3; + lim3.has_velocity_limits = true; + lim3.max_velocity = 2; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + container.addLimit("joint3", lim3); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check postion inequality in has_position_limits false detection + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityHasVelocityLimits) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_velocity_limits = true; + + JointLimit lim2; + lim2.has_velocity_limits = false; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- +// ACCELERATION +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- + +/** + * @brief Check acceleration equality + */ +TEST_F(JointLimitsValidatorTest, CheckAccelerationEquality) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_acceleration_limits = true; + lim1.max_acceleration = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim1); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check max_acceleration inequality + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxAcceleration1) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_acceleration_limits = true; + lim1.max_acceleration = 1; + + JointLimit lim2; + lim2.has_acceleration_limits = true; + lim2.max_acceleration = 2; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check max_acceleration inequality (using 3 Limits) + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxAcceleration2) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_acceleration_limits = true; + lim1.max_acceleration = 1; + + JointLimit lim2; + lim2.has_acceleration_limits = true; + lim2.max_acceleration = 2; + + JointLimit lim3; + lim3.has_acceleration_limits = true; + lim3.max_acceleration = 2; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + container.addLimit("joint3", lim3); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check acceleration inequality in has_acceleration_limits false + * detection + */ +TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityHasAccelerationLimits) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_acceleration_limits = true; + + JointLimit lim2; + lim2.has_acceleration_limits = false; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- +// DECELERATION +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- + +/** + * @brief Check deceleration equality + */ +TEST_F(JointLimitsValidatorTest, CheckDecelerationEquality) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_deceleration_limits = true; + lim1.max_deceleration = 1; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim1); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check max_deceleration inequality + */ +TEST_F(JointLimitsValidatorTest, CheckInEqualityMaxDeceleration1) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_deceleration_limits = true; + lim1.max_deceleration = -1; + + JointLimit lim2; + lim2.has_deceleration_limits = true; + lim2.max_deceleration = -2; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check max_deceleration inequality + */ +TEST_F(JointLimitsValidatorTest, CheckInEqualityMaxDeceleration2) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_deceleration_limits = true; + lim1.max_deceleration = -1; + + JointLimit lim2; + lim2.has_deceleration_limits = true; + lim2.max_deceleration = -2; + + JointLimit lim3; + lim3.has_deceleration_limits = true; + lim3.max_deceleration = -2; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + container.addLimit("joint3", lim3); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +/** + * @brief Check deceleration inequality in has_deceleration_limits false + * detection + */ +TEST_F(JointLimitsValidatorTest, CheckInEqualityHasDecelerationLimits) +{ + JointLimitsContainer container; + + JointLimit lim1; + lim1.has_deceleration_limits = true; + lim1.max_deceleration = -1; + + JointLimit lim2; + lim2.has_deceleration_limits = false; + + container.addLimit("joint1", lim1); + container.addLimit("joint2", lim2); + + ASSERT_EQ(2u, container.getCount()); + + EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container)); + EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container)); + EXPECT_EQ(false, JointLimitsValidator::validateAllDecelerationLimitsEqual(container)); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp new file mode 100644 index 0000000000..f413cb835a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp @@ -0,0 +1,232 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include + +#include +#include + +#include "pilz_industrial_motion_planner/pilz_industrial_motion_planner.h" +#include "test_utils.h" + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +class CommandPlannerTest : public testing::TestWithParam +{ +protected: + void SetUp() override + { + createPlannerInstance(); + } + + /** + * @brief initialize the planner plugin + * The planner is loaded using the pluginlib. Checks that the planner was + * loaded properly. + * Exceptions will cause test failure. + * + * This function should be called only once during initialization of the + * class. + */ + void createPlannerInstance() + { + // Load planner name from parameter server + if (!ph_.getParam("planning_plugin", planner_plugin_name_)) + { + ROS_FATAL_STREAM("Could not find planner plugin name"); + FAIL(); + } + + // Load the plugin + try + { + planner_plugin_loader_.reset(new pluginlib::ClassLoader( + "moveit_core", "planning_interface::PlannerManager")); + } + catch (pluginlib::PluginlibException& ex) + { + ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); + FAIL(); + } + + // Create planner + try + { + planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_name_)); + ASSERT_TRUE(planner_instance_->initialize(robot_model_, ph_.getNamespace())) + << "Initialzing the planner instance failed."; + } + catch (pluginlib::PluginlibException& ex) + { + FAIL() << "Could not create planner " << ex.what() << "\n"; + } + } + + void TearDown() override + { + planner_plugin_loader_->unloadLibraryForClass(planner_plugin_name_); + } + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + + std::string planner_plugin_name_; + + std::unique_ptr> planner_plugin_loader_; + + planning_interface::PlannerManagerPtr planner_instance_; +}; + +// Instantiate the test cases for robot model with and without gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, CommandPlannerTest, + ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); + +/** + * @brief Test that PTP can be loaded + * This needs to be extended with every new planning Algorithm + */ +TEST_P(CommandPlannerTest, ObtainLoadedPlanningAlgorithms) +{ + // Check for the algorithms + std::vector algs; + + planner_instance_->getPlanningAlgorithms(algs); + ASSERT_EQ(3u, algs.size()) << "Found more or less planning algorithms as expected! Found:" + << ::testing::PrintToString(algs); + + // Collect the algorithms, check for uniqueness + std::set algs_set; + for (const auto& alg : algs) + { + algs_set.insert(alg); + } + ASSERT_EQ(algs.size(), algs_set.size()) << "There are two or more algorithms with the same name!"; + ASSERT_TRUE(algs_set.find("LIN") != algs_set.end()); + ASSERT_TRUE(algs_set.find("PTP") != algs_set.end()); + ASSERT_TRUE(algs_set.find("CIRC") != algs_set.end()); +} + +/** + * @brief Check that all announced planning algorithms can perform the service + * request if the planner_id is set. + */ +TEST_P(CommandPlannerTest, CheckValidAlgorithmsForServiceRequest) +{ + // Check for the algorithms + std::vector algs; + planner_instance_->getPlanningAlgorithms(algs); + + for (const auto& alg : algs) + { + planning_interface::MotionPlanRequest req; + req.planner_id = alg; + + EXPECT_TRUE(planner_instance_->canServiceRequest(req)); + } +} + +/** + * @brief Check that canServiceRequest(req) returns false if planner_id is not + * supported + */ +TEST_P(CommandPlannerTest, CheckInvalidAlgorithmsForServiceRequest) +{ + planning_interface::MotionPlanRequest req; + req.planner_id = "NON_EXISTEND_ALGORITHM_HASH_da39a3ee5e6b4b0d3255bfef95601890afd80709"; + + EXPECT_FALSE(planner_instance_->canServiceRequest(req)); +} + +/** + * @brief Check that canServiceRequest(req) returns false if planner_id is empty + */ +TEST_P(CommandPlannerTest, CheckEmptyPlannerIdForServiceRequest) +{ + planning_interface::MotionPlanRequest req; + req.planner_id = ""; + + EXPECT_FALSE(planner_instance_->canServiceRequest(req)); +} + +/** + * @brief Check integrety against empty input + */ +TEST_P(CommandPlannerTest, CheckPlanningContextRequestNull) +{ + moveit_msgs::MotionPlanRequest req; + moveit_msgs::MoveItErrorCodes error_code; + EXPECT_EQ(nullptr, planner_instance_->getPlanningContext(nullptr, req, error_code)); +} + +/** + * @brief Check that for the announced algorithmns getPlanningContext does not + * return nullptr + */ +TEST_P(CommandPlannerTest, CheckPlanningContextRequest) +{ + moveit_msgs::MotionPlanRequest req; + moveit_msgs::MoveItErrorCodes error_code; + + // Check for the algorithms + std::vector algs; + planner_instance_->getPlanningAlgorithms(algs); + + for (const auto& alg : algs) + { + req.planner_id = alg; + + EXPECT_NE(nullptr, planner_instance_->getPlanningContext(nullptr, req, error_code)); + } +} + +/** + * @brief Check the description can be obtained and is not empty + */ +TEST_P(CommandPlannerTest, CheckPlanningContextDescriptionNotEmptyAndStable) +{ + std::string desc = planner_instance_->getDescription(); + EXPECT_GT(desc.length(), 0u); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_pilz_industrial_motion_planner"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.test new file mode 100644 index 0000000000..574f005626 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.test @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner_direct.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner_direct.cpp new file mode 100644 index 0000000000..35bb528d27 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner_direct.cpp @@ -0,0 +1,121 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include + +#include "pilz_industrial_motion_planner/pilz_industrial_motion_planner.h" +#include "pilz_industrial_motion_planner/planning_context_loader_ptp.h" +#include "pilz_industrial_motion_planner/planning_exceptions.h" + +using namespace pilz_industrial_motion_planner; + +TEST(CommandPlannerTestDirect, ExceptionCoverage) +{ + std::shared_ptr p_ex{ new PlanningException("") }; + std::shared_ptr clr_ex{ new ContextLoaderRegistrationException("") }; +} + +/** + * This test uses pilz_industrial_motion_planner::CommandPlanner directly and + * is thus seperated from + * unittest_pilz_industrial_motion_planner.cpp since plugin loading via + * pluginlib does not allow loading of classes + * already + * defined. + */ + +/** + * @brief Check that a exception is thrown if a already loaded + * PlanningContextLoader is loaded + * + * It this point the planning_instance_ has loaded ptp, lin, circ. + * A additional ptp is loaded which should throw the respective exception. + */ +TEST(CommandPlannerTestDirect, CheckDoubleLoadingException) +{ + /// Registed a found loader + pilz_industrial_motion_planner::CommandPlanner planner; + pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader( + new pilz_industrial_motion_planner::PlanningContextLoaderPTP()); + + EXPECT_NO_THROW(planner.registerContextLoader(planning_context_loader)); + + EXPECT_THROW(planner.registerContextLoader(planning_context_loader), + pilz_industrial_motion_planner::ContextLoaderRegistrationException); +} + +/** + * @brief Check that getPlanningContext() fails if the underlying ContextLoader + * fails to load the context. + */ +TEST(CommandPlannerTestDirect, FailOnLoadContext) +{ + pilz_industrial_motion_planner::CommandPlanner planner; + + // Mock of failing PlanningContextLoader + class TestPlanningContextLoader : public pilz_industrial_motion_planner::PlanningContextLoader + { + public: + std::string getAlgorithm() const override + { + return "Test_Algorithm"; + } + + bool loadContext(planning_interface::PlanningContextPtr& /* planning_context */, const std::string& /* name */, + const std::string& /* group */) const override + { + // Mock behaviour: Cannot load planning context. + return false; + } + }; + + /// Registed a found loader + pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader(new TestPlanningContextLoader()); + planner.registerContextLoader(planning_context_loader); + + moveit_msgs::MotionPlanRequest req; + req.planner_id = "Test_Algorithm"; + + moveit_msgs::MoveItErrorCodes error_code; + EXPECT_FALSE(planner.getPlanningContext(nullptr, req, error_code)); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::PLANNING_FAILED, error_code.val); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp new file mode 100644 index 0000000000..016c612e0a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp @@ -0,0 +1,282 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include + +#include + +#include + +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/joint_limits_container.h" +#include "pilz_industrial_motion_planner/planning_context_circ.h" +#include "pilz_industrial_motion_planner/planning_context_lin.h" +#include "pilz_industrial_motion_planner/planning_context_ptp.h" + +#include "test_utils.h" + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string PARAM_TARGET_LINK_NAME("target_link"); + +/** + * A value type container to combine type and value + * In the tests types are trajectory generators. + * value = 0 refers to robot model without gripper + * value = 1 refers to robot model with gripper + */ +template +class ValueTypeContainer +{ +public: + typedef T Type_; + static const int VALUE = N; +}; +template +const int ValueTypeContainer::VALUE; + +typedef ValueTypeContainer PTP_NO_GRIPPER; +typedef ValueTypeContainer PTP_WITH_GRIPPER; +typedef ValueTypeContainer LIN_NO_GRIPPER; +typedef ValueTypeContainer LIN_WITH_GRIPPER; +typedef ValueTypeContainer CIRC_NO_GRIPPER; +typedef ValueTypeContainer CIRC_WITH_GRIPPER; + +typedef ::testing::Types + PlanningContextTestTypes; + +/** + * type parameterized test fixture + */ +template +class PlanningContextTest : public ::testing::Test +{ +protected: + void SetUp() override + { + ASSERT_FALSE(robot_model_ == nullptr) << "There is no robot model!"; + + // get parameters + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + testutils::createFakeLimits(robot_model_->getVariableNames()); + pilz_industrial_motion_planner::CartesianLimit cartesian_limit; + cartesian_limit.setMaxRotationalVelocity(1.0 * M_PI); + cartesian_limit.setMaxTranslationalAcceleration(1.0 * M_PI); + cartesian_limit.setMaxTranslationalDeceleration(1.0 * M_PI); + cartesian_limit.setMaxTranslationalVelocity(1.0 * M_PI); + + pilz_industrial_motion_planner::LimitsContainer limits; + limits.setJointLimits(joint_limits); + limits.setCartesianLimits(cartesian_limit); + + planning_context_ = std::unique_ptr( + new typename T::Type_("TestPlanningContext", "TestGroup", robot_model_, limits)); + + // Define and set the current scene + planning_scene::PlanningScenePtr scene(new planning_scene::PlanningScene(robot_model_)); + robot_state::RobotState current_state(robot_model_); + current_state.setToDefaultValues(); + current_state.setJointGroupPositions(planning_group_, { 0, 1.57, 1.57, 0, 0.2, 0 }); + scene->setCurrentState(current_state); + planning_context_->setPlanningScene(scene); // TODO Check what happens if this is missing + } + + /** + * @brief Generate a valid fully defined request + */ + planning_interface::MotionPlanRequest getValidRequest(const std::string& context_name) const + { + planning_interface::MotionPlanRequest req; + + req.planner_id = + std::string(context_name).erase(0, std::string("pilz_industrial_motion_planner::PlanningContext").length()); + req.group_name = this->planning_group_; + req.max_velocity_scaling_factor = 0.01; + req.max_acceleration_scaling_factor = 0.01; + + // start state + robot_state::RobotState rstate(this->robot_model_); + rstate.setToDefaultValues(); + // state state in joint space, used as initial positions, since IK does not + // work at zero positions + rstate.setJointGroupPositions(this->planning_group_, + { 4.430233957464225e-12, 0.007881892504574495, -1.8157263253868452, + 1.1801525390026025e-11, 1.8236082178909834, 8.591793942969161e-12 }); + Eigen::Isometry3d start_pose(Eigen::Isometry3d::Identity()); + start_pose.translation() = Eigen::Vector3d(0.3, 0, 0.65); + rstate.setFromIK(this->robot_model_->getJointModelGroup(this->planning_group_), start_pose); + moveit::core::robotStateToRobotStateMsg(rstate, req.start_state, false); + + // goal constraint + Eigen::Isometry3d goal_pose(Eigen::Isometry3d::Identity()); + goal_pose.translation() = Eigen::Vector3d(0, 0.3, 0.65); + Eigen::Matrix3d goal_rotation; + goal_rotation = Eigen::AngleAxisd(0 * M_PI, Eigen::Vector3d::UnitZ()); + goal_pose.linear() = goal_rotation; + rstate.setFromIK(this->robot_model_->getJointModelGroup(this->planning_group_), goal_pose); + req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( + rstate, this->robot_model_->getJointModelGroup(this->planning_group_))); + + // path constraint + req.path_constraints.name = "center"; + moveit_msgs::PositionConstraint center_point; + center_point.link_name = this->target_link_; + geometry_msgs::Pose center_position; + center_position.position.x = 0.0; + center_position.position.y = 0.0; + center_position.position.z = 0.65; + center_point.constraint_region.primitive_poses.push_back(center_position); + req.path_constraints.position_constraints.push_back(center_point); + + return req; + } + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ + robot_model_loader::RobotModelLoader(!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME) + .getModel() + }; + + std::unique_ptr planning_context_; + + std::string planning_group_, target_link_; +}; + +// Define the types we need to test +TYPED_TEST_SUITE(PlanningContextTest, PlanningContextTestTypes); + +/** + * @brief No request is set. Check the output of solve. Using robot model + * without gripper. + */ +TYPED_TEST(PlanningContextTest, NoRequest) +{ + planning_interface::MotionPlanResponse res; + bool result = this->planning_context_->solve(res); + + EXPECT_FALSE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN, res.error_code_.val) + << testutils::demangel(typeid(TypeParam).name()); +} + +/** + * @brief Solve a valid request. + */ +TYPED_TEST(PlanningContextTest, SolveValidRequest) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name())); + + this->planning_context_->setMotionPlanRequest(req); + + // TODO Formulate valid request + bool result = this->planning_context_->solve(res); + + EXPECT_TRUE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val) + << testutils::demangel(typeid(TypeParam).name()); + + planning_interface::MotionPlanDetailedResponse res_detailed; + bool result_detailed = this->planning_context_->solve(res_detailed); + + EXPECT_TRUE(result_detailed) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val) + << testutils::demangel(typeid(TypeParam).name()); +} + +/** + * @brief Solve a valid request. Expect a detailed response. + */ +TYPED_TEST(PlanningContextTest, SolveValidRequestDetailedResponse) +{ + planning_interface::MotionPlanDetailedResponse res; //<-- Detailed! + planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name())); + + this->planning_context_->setMotionPlanRequest(req); + bool result = this->planning_context_->solve(res); + + EXPECT_TRUE(result) << testutils::demangel(typeid(TypeParam).name()); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val) + << testutils::demangel(typeid(TypeParam).name()); +} + +/** + * @brief Call solve on a terminated context. + */ +TYPED_TEST(PlanningContextTest, SolveOnTerminated) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name())); + + this->planning_context_->setMotionPlanRequest(req); + + bool result_termination = this->planning_context_->terminate(); + EXPECT_TRUE(result_termination) << testutils::demangel(typeid(TypeParam).name()); + + bool result = this->planning_context_->solve(res); + EXPECT_FALSE(result) << testutils::demangel(typeid(TypeParam).name()); + + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::PLANNING_FAILED, res.error_code_.val) + << testutils::demangel(typeid(TypeParam).name()); +} + +/** + * @brief Check if clear can be called. So far only stability is expected. + */ +TYPED_TEST(PlanningContextTest, Clear) +{ + EXPECT_NO_THROW(this->planning_context_->clear()) << testutils::demangel(typeid(TypeParam).name()); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_planning_context"); + // ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.test new file mode 100644 index 0000000000..048e67c804 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.test @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp new file mode 100644 index 0000000000..a8926ed17e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp @@ -0,0 +1,179 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +// Boost includes +#include + +#include + +#include +#include + +#include "pilz_industrial_motion_planner/planning_context_loader.h" + +#include "test_utils.h" + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +class PlanningContextLoadersTest : public ::testing::TestWithParam> +{ +protected: + /** + * @brief To initialize the planning context loader + * The planning context loader is loaded using the pluginlib. + * Checks if planning context loader was loaded properly are performed. + * Exceptions will cause test failure. + */ + void SetUp() override + { + ASSERT_FALSE(robot_model_ == nullptr) << "There is no robot model!"; + + // Load the plugin + try + { + planning_context_loader_class_loader_.reset( + new pluginlib::ClassLoader( + "pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader")); + } + catch (pluginlib::PluginlibException& ex) + { + ROS_FATAL_STREAM("Exception while creating planning context loader " << ex.what()); + FAIL(); + } + + // Create planning context loader ptp + try + { + planning_context_loader_.reset(planning_context_loader_class_loader_->createUnmanagedInstance(GetParam().front())); + } + catch (pluginlib::PluginlibException& ex) + { + FAIL() << ex.what(); + } + return; + } + + void TearDown() override + { + if (planning_context_loader_class_loader_) + { + planning_context_loader_class_loader_->unloadLibraryForClass(GetParam().front()); + } + } + +protected: + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam().back()).getModel() }; + + // Load the plugin + boost::scoped_ptr> + planning_context_loader_class_loader_; + + pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader_; +}; + +// Instantiate the test cases for all loaders, extend here if you added a new +// ContextLoader you want to test +INSTANTIATE_TEST_SUITE_P( + InstantiationName, PlanningContextLoadersTest, + ::testing::Values(std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderPTP", "PTP", + PARAM_MODEL_NO_GRIPPER_NAME }, // Test for PTP + std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderPTP", "PTP", + PARAM_MODEL_WITH_GRIPPER_NAME }, // Test for PTP + std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderLIN", "LIN", + PARAM_MODEL_NO_GRIPPER_NAME }, // Test for LIN + std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderLIN", "LIN", + PARAM_MODEL_WITH_GRIPPER_NAME }, // Test for LIN + std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderCIRC", "CIRC", + PARAM_MODEL_NO_GRIPPER_NAME }, // Test for CIRC + std::vector{ "pilz_industrial_motion_planner::PlanningContextLoaderCIRC", "CIRC", + PARAM_MODEL_WITH_GRIPPER_NAME } // Test for CIRC + )); + +/** + * @brief Test getAlgorithm returns PTP + */ +TEST_P(PlanningContextLoadersTest, GetAlgorithm) +{ + std::string alg = planning_context_loader_->getAlgorithm(); + EXPECT_EQ(alg, GetParam().at(1)); +} + +/** + * @brief Check that load Context returns initialized shared pointer + */ +TEST_P(PlanningContextLoadersTest, LoadContext) +{ + planning_interface::PlanningContextPtr planning_context; + + // Without limits should return false + bool res = planning_context_loader_->loadContext(planning_context, "test", "test"); + EXPECT_EQ(false, res) << "Context returned even when no limits where set"; + + // After setting the limits this should work + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + testutils::createFakeLimits(robot_model_->getVariableNames()); + pilz_industrial_motion_planner::LimitsContainer limits; + limits.setJointLimits(joint_limits); + pilz_industrial_motion_planner::CartesianLimit cart_limits; + cart_limits.setMaxRotationalVelocity(1 * M_PI); + cart_limits.setMaxTranslationalAcceleration(2); + cart_limits.setMaxTranslationalDeceleration(2); + cart_limits.setMaxTranslationalVelocity(1); + limits.setCartesianLimits(cart_limits); + + planning_context_loader_->setLimits(limits); + planning_context_loader_->setModel(robot_model_); + + try + { + res = planning_context_loader_->loadContext(planning_context, "test", "test"); + } + catch (std::exception& ex) + { + FAIL() << "Exception!" << ex.what() << " " << typeid(ex).name(); + } + + EXPECT_EQ(true, res) << "Context could not be loaded!"; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_planning_context_loaders"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.test new file mode 100644 index 0000000000..766bc1af98 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.test @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp new file mode 100644 index 0000000000..0b34d00991 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp @@ -0,0 +1,727 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/trajectory_blend_request.h" +#include "pilz_industrial_motion_planner/trajectory_blend_response.h" +#include "pilz_industrial_motion_planner/trajectory_blender_transition_window.h" +#include "pilz_industrial_motion_planner/trajectory_generator_lin.h" +#include "test_utils.h" + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string PARAM_TARGET_LINK_NAME("target_link"); +const std::string CARTESIAN_VELOCITY_TOLERANCE("cartesian_velocity_tolerance"); +const std::string CARTESIAN_ANGULAR_VELOCITY_TOLERANCE("cartesian_angular_velocity_tolerance"); +const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance"); +const std::string JOINT_ACCELERATION_TOLERANCE("joint_acceleration_tolerance"); +const std::string OTHER_TOLERANCE("other_tolerance"); +const std::string SAMPLING_TIME("sampling_time"); +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); + +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner_testutils; + +class TrajectoryBlenderTransitionWindowTest : public testing::TestWithParam +{ +protected: + /** + * @brief Create test scenario for trajectory blender + * + */ + void SetUp() override; + + /** + * @brief Generate lin trajectories for blend sequences + */ + std::vector generateLinTrajs(const Sequence& seq, size_t num_cmds); + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + + std::unique_ptr lin_generator_; + std::unique_ptr blender_; + + // test parameters from parameter server + std::string planning_group_, target_link_; + double cartesian_velocity_tolerance_, cartesian_angular_velocity_tolerance_, joint_velocity_tolerance_, + joint_acceleration_tolerance_, sampling_time_; + LimitsContainer planner_limits_; + + std::string test_data_file_name_; + XmlTestDataLoaderUPtr data_loader_; +}; + +void TrajectoryBlenderTransitionWindowTest::SetUp() +{ + // get parameters + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + ASSERT_TRUE(ph_.getParam(CARTESIAN_VELOCITY_TOLERANCE, cartesian_velocity_tolerance_)); + ASSERT_TRUE(ph_.getParam(CARTESIAN_ANGULAR_VELOCITY_TOLERANCE, cartesian_angular_velocity_tolerance_)); + ASSERT_TRUE(ph_.getParam(JOINT_VELOCITY_TOLERANCE, joint_velocity_tolerance_)); + ASSERT_TRUE(ph_.getParam(JOINT_ACCELERATION_TOLERANCE, joint_acceleration_tolerance_)); + ASSERT_TRUE(ph_.getParam(SAMPLING_TIME, sampling_time_)); + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + + // load the test data provider + data_loader_.reset(new XmlTestdataLoader(test_data_file_name_, robot_model_)); + ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider."; + + // check robot model + testutils::checkRobotModel(robot_model_, planning_group_, target_link_); + + // create the limits container + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(ph_, + robot_model_->getActiveJointModels()); + CartesianLimit cart_limits; + cart_limits.setMaxRotationalVelocity(1 * M_PI); + cart_limits.setMaxTranslationalAcceleration(2); + cart_limits.setMaxTranslationalDeceleration(2); + cart_limits.setMaxTranslationalVelocity(1); + planner_limits_.setJointLimits(joint_limits); + planner_limits_.setCartesianLimits(cart_limits); + + // initialize trajectory generators and blender + lin_generator_.reset(new TrajectoryGeneratorLIN(robot_model_, planner_limits_)); + ASSERT_NE(nullptr, lin_generator_) << "failed to create LIN trajectory generator"; + blender_.reset(new TrajectoryBlenderTransitionWindow(planner_limits_)); + ASSERT_NE(nullptr, blender_) << "failed to create trajectory blender"; +} + +std::vector +TrajectoryBlenderTransitionWindowTest::generateLinTrajs(const Sequence& seq, size_t num_cmds) +{ + std::vector responses(num_cmds); + + for (size_t index = 0; index < num_cmds; ++index) + { + planning_interface::MotionPlanRequest req{ seq.getCmd(index).toRequest() }; + // Set start state of request to end state of previous trajectory (except + // for first) + if (index > 0) + { + moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory_->getLastWayPoint(), req.start_state); + } + // generate trajectory + planning_interface::MotionPlanResponse resp; + if (!lin_generator_->generate(req, resp, sampling_time_)) + { + std::runtime_error("Failed to generate trajectory."); + } + responses.at(index) = resp; + } + return responses; +} + +// Instantiate the test cases for robot model with and without gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryBlenderTransitionWindowTest, + ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); + +/** + * @brief Tests the blending of two trajectories with an invalid group name. + * + * Test Sequence: + * 1. Generate two linear trajectories. + * 2. Try to generate blending trajectory with invalid group name. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidGroupName) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = "invalid_group_name"; + blend_req.link_name = target_link_; + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + blend_req.blend_radius = seq.getBlendRadius(0); + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two trajectories with an invalid target link. + * + * Test Sequence: + * 1. Generate two linear trajectories. + * 2. Try to generate blending trajectory with invalid target link. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidTargetLink) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = "invalid_target_link"; + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + blend_req.blend_radius = seq.getBlendRadius(0); + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two trajectories with a negative blending + * radius. + * + * Test Sequence: + * 1. Generate two linear trajectories. + * 2. Try to generate blending trajectory with negative blending radius. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testNegativeRadius) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + blend_req.blend_radius = -0.1; + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two trajectories with zero blending radius. + * + * Test Sequence: + * 1. Generate two linear trajectories. + * 2. Try to generate blending trajectory with zero blending radius. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testZeroRadius) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + blend_req.blend_radius = 0.; + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two trajectories with differenent sampling + * times. + * + * Test Sequence: + * 1. Generate two linear trajectories with different sampling times. + * 2. Try to generate blending trajectory. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + // perform lin trajectory generation and modify sampling time + std::size_t num_cmds{ 2 }; + std::vector responses(num_cmds); + + for (size_t index = 0; index < num_cmds; ++index) + { + planning_interface::MotionPlanRequest req{ seq.getCmd(index).toRequest() }; + // Set start state of request to end state of previous trajectory (except + // for first) + if (index > 0) + { + moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory_->getLastWayPoint(), req.start_state); + sampling_time_ *= 2; + } + // generate trajectory + planning_interface::MotionPlanResponse resp; + if (!lin_generator_->generate(req, resp, sampling_time_)) + { + std::runtime_error("Failed to generate trajectory."); + } + responses.at(index) = resp; + } + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.first_trajectory = responses[0].trajectory_; + blend_req.second_trajectory = responses[1].trajectory_; + blend_req.blend_radius = seq.getBlendRadius(0); + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two trajectories with one trajectory + * having non-uniform sampling time (apart from the last sample, + * which is ignored). + * + * Test Sequence: + * 1. Generate two linear trajectories and corrupt uniformity of sampling + * time. + * 2. Try to generate blending trajectory. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + // Modify first time interval + EXPECT_GT(res[0].trajectory_->getWayPointCount(), 2u); + res[0].trajectory_->setWayPointDurationFromPrevious(1, 2 * sampling_time_); + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.blend_radius = seq.getBlendRadius(0); + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two trajectories which do not intersect. + * + * Test Sequence: + * 1. Generate two trajectories from valid test data set. + * 2. Replace the second trajectory by the first one. + * 2. Try to generate blending trajectory. + * + * Expected Results: + * 1. Two trajectories generated. + * 2. Two trajectories that do not intersect. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testNotIntersectingTrajectories) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 1) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.first_trajectory = res.at(0).trajectory_; + // replace the second trajectory to make the two trajectories timely not + // intersect + blend_req.second_trajectory = res.at(0).trajectory_; + blend_req.blend_radius = seq.getBlendRadius(0); + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two cartesian trajectories with the + * shared point (last point of first, first point of second trajectory) + * having a non-zero velocity + * + * Test Sequence: + * 1. Generate two trajectories from the test data set. + * 2. Generate blending trajectory modify the shared point to have velocity. + * Expected Results: + * 1. Two trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testNonStationaryPoint) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.blend_radius = seq.getBlendRadius(0); + + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + // Modify last waypoint of first trajectory and first point of second + // trajectory + blend_req.first_trajectory->getLastWayPointPtr()->setVariableVelocity(0, 1.0); + blend_req.second_trajectory->getFirstWayPointPtr()->setVariableVelocity(0, 1.0); + + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two cartesian trajectories where the first + * trajectory is completely within the sphere defined by the blend radius + * + * Test Sequence: + * 1. Generate two trajectories from the test data set. + * 2. Generate blending trajectory with a blend_radius larger + * than the smaller trajectory. + * + * Expected Results: + * 1. Two trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + double lin1_distance; + lin1_distance = (res[0].trajectory_->getFirstWayPoint().getFrameTransform(target_link_).translation() - + res[0].trajectory_->getLastWayPoint().getFrameTransform(target_link_).translation()) + .norm(); + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.blend_radius = 1.1 * lin1_distance; + + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two cartesian trajectories where the second + * trajectory is completely within the sphere defined by the blend radius + * + * Test Sequence: + * 1. Generate two trajectories from the test data set. + * 2. Generate blending trajectory with a blend_radius larger + * than the smaller trajectory. + * + * Expected Results: + * 1. Two trajectories generated. + * 2. Blending trajectory cannot be generated. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj2InsideBlendRadius) +{ + Sequence seq{ data_loader_->getSequence("NoIntersectionTraj2") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.blend_radius = seq.getBlendRadius(0); + + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + EXPECT_FALSE(blender_->blend(blend_req, blend_res)); +} + +/** + * @brief Tests the blending of two cartesian linear trajectories using robot + * model + * + * Test Sequence: + * 1. Generate two linear trajectories from the test data set. + * 2. Generate blending trajectory. + * 3. Check blending trajectory: + * - for position, velocity, and acceleration bounds, + * - for continuity in joint space, + * - for continuity in cartesian space. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory generated. + * 3. No bound is violated, the trajectories are continuous + * in joint and cartesian space. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testLinLinBlending) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.blend_radius = seq.getBlendRadius(0); + + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + + EXPECT_TRUE(blender_->blend(blend_req, blend_res)); + + EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_, + joint_acceleration_tolerance_, cartesian_velocity_tolerance_, + cartesian_angular_velocity_tolerance_)); +} + +/** + * @brief Tests the blending of two cartesian linear trajectories which have + * an overlap in the blending sphere using robot model. To be precise, + * the trajectories exactly lie on top of each other. + * + * Test Sequence: + * 1. Generate two linear trajectories from the test data set. Set goal of + * second traj to start of first traj. + * 2. Generate blending trajectory. + * 3. Check blending trajectory: + * - for position, velocity, and acceleration bounds, + * - for continuity in joint space, + * - for continuity in cartesian space. + * + * Expected Results: + * 1. Two linear trajectories generated. + * 2. Blending trajectory generated. + * 3. No bound is violated, the trajectories are continuous + * in joint and cartesian space. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testOverlappingBlendTrajectories) +{ + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + // Set goal of second traj to start of first traj. + seq.getCmd(1).setGoalConfiguration(seq.getCmd(0).getStartConfiguration()); + + std::vector res{ generateLinTrajs(seq, 2) }; + + pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; + pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.first_trajectory = res.at(0).trajectory_; + blend_req.second_trajectory = res.at(1).trajectory_; + blend_req.blend_radius = seq.getBlendRadius(0); + EXPECT_TRUE(blender_->blend(blend_req, blend_res)); + + EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_, + joint_acceleration_tolerance_, cartesian_velocity_tolerance_, + cartesian_angular_velocity_tolerance_)); +} + +/** + * @brief Tests the blending of two cartesian trajectories which differ + * from a straight line. + * + * Test Sequence: + * 1. Generate two trajectories from the test data set. + * 2. Add scaled sine function to cartesian trajectories, such that + * start and end state remain unchanged; generate resulting + * joint trajectories using a time scaling in order to preserve + * joint velocity limits. + * 3. Generate blending trajectory. + * 4. Check blending trajectory: + * - for position, velocity, and acceleration bounds, + * - for continuity in joint space, + * - for continuity in cartesian space. + * + * Expected Results: + * 1. Two trajectories generated. + * 2. Modified joint trajectories generated. + * 3. Blending trajectory generated. + * 4. No bound is violated, the trajectories are continuous + * in joint and cartesian space. + */ +TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) +{ + const double sine_scaling_factor{ 0.01 }; + const double time_scaling_factor{ 10 }; + + Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + + std::vector res{ generateLinTrajs(seq, 2) }; + + // prepare looping over trajectories + std::vector sine_trajs(2); + + for (size_t traj_index = 0; traj_index < 2; ++traj_index) + { + auto lin_traj{ res.at(traj_index).trajectory_ }; + + CartesianTrajectory cart_traj; + trajectory_msgs::JointTrajectory joint_traj; + const double duration{ lin_traj->getWayPointDurationFromStart(lin_traj->getWayPointCount()) }; + // time from start zero does not work + const double time_from_start_offset{ time_scaling_factor * lin_traj->getWayPointDurations().back() }; + + // generate modified cartesian trajectory + for (size_t i = 0; i < lin_traj->getWayPointCount(); ++i) + { + // transform time to interval [0, 4*pi] + const double sine_arg{ 4 * M_PI * lin_traj->getWayPointDurationFromStart(i) / duration }; + + // get pose + CartesianTrajectoryPoint waypoint; + geometry_msgs::Pose waypoint_pose; + Eigen::Isometry3d eigen_pose{ lin_traj->getWayPointPtr(i)->getFrameTransform(target_link_) }; + tf2::convert(eigen_pose, waypoint_pose); + + // add scaled sine function + waypoint_pose.position.x += sine_scaling_factor * sin(sine_arg); + waypoint_pose.position.y += sine_scaling_factor * sin(sine_arg); + waypoint_pose.position.z += sine_scaling_factor * sin(sine_arg); + + // add to trajectory + waypoint.pose = waypoint_pose; + waypoint.time_from_start = + ros::Duration(time_from_start_offset + time_scaling_factor * lin_traj->getWayPointDurationFromStart(i)); + cart_traj.points.push_back(waypoint); + } + + // prepare ik + std::map initial_joint_position, initial_joint_velocity; + for (const std::string& joint_name : + lin_traj->getFirstWayPointPtr()->getJointModelGroup(planning_group_)->getActiveJointModelNames()) + { + if (traj_index == 0) + { + initial_joint_position[joint_name] = lin_traj->getFirstWayPoint().getVariablePosition(joint_name); + initial_joint_velocity[joint_name] = lin_traj->getFirstWayPoint().getVariableVelocity(joint_name); + } + else + { + initial_joint_position[joint_name] = + sine_trajs[traj_index - 1]->getLastWayPoint().getVariablePosition(joint_name); + initial_joint_velocity[joint_name] = + sine_trajs[traj_index - 1]->getLastWayPoint().getVariableVelocity(joint_name); + } + } + + moveit_msgs::MoveItErrorCodes error_code; + if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_traj, planning_group_, + target_link_, initial_joint_position, initial_joint_velocity, joint_traj, error_code, + true)) + { + std::runtime_error("Failed to generate trajectory."); + } + + joint_traj.points.back().velocities.assign(joint_traj.points.back().velocities.size(), 0.0); + joint_traj.points.back().accelerations.assign(joint_traj.points.back().accelerations.size(), 0.0); + + // convert trajectory_msgs::JointTrajectory to + // robot_trajectory::RobotTrajectory + sine_trajs[traj_index] = std::make_shared(robot_model_, planning_group_); + sine_trajs.at(traj_index)->setRobotTrajectoryMsg(lin_traj->getFirstWayPoint(), joint_traj); + } + + TrajectoryBlendRequest blend_req; + TrajectoryBlendResponse blend_res; + + blend_req.group_name = planning_group_; + blend_req.link_name = target_link_; + blend_req.blend_radius = seq.getBlendRadius(0); + + blend_req.first_trajectory = sine_trajs.at(0); + blend_req.second_trajectory = sine_trajs.at(1); + + EXPECT_TRUE(blender_->blend(blend_req, blend_res)); + + EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_, + joint_acceleration_tolerance_, cartesian_velocity_tolerance_, + cartesian_angular_velocity_tolerance_)); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_trajectory_blender_transition_window"); + ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.test new file mode 100644 index 0000000000..2d0bf6c3d5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.test @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp new file mode 100644 index 0000000000..a70575361f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp @@ -0,0 +1,971 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/cartesian_trajectory.h" +#include "pilz_industrial_motion_planner/cartesian_trajectory_point.h" +#include "pilz_industrial_motion_planner/limits_container.h" +#include "pilz_industrial_motion_planner/trajectory_functions.h" +#include "test_utils.h" + +#define _USE_MATH_DEFINES + +static constexpr double EPSILON{ 1.0e-6 }; +static constexpr double IK_SEED_OFFSET{ 0.1 }; +static constexpr double L0{ 0.2604 }; // Height of foot +static constexpr double L1{ 0.3500 }; // Height of first connector +static constexpr double L2{ 0.3070 }; // Height of second connector +static constexpr double L3{ 0.0840 }; // Distance last joint to flange + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string GROUP_TIP_LINK_NAME("group_tip_link"); +const std::string ROBOT_TCP_LINK_NAME("tcp_link"); +const std::string IK_FAST_LINK_NAME("ik_fast_link"); +const std::string RANDOM_TEST_NUMBER("random_test_number"); + +/** + * @brief test fixtures base class + */ +class TrajectoryFunctionsTestBase : public testing::TestWithParam +{ +protected: + /** + * @brief Create test scenario for trajectory functions + * + */ + void SetUp() override; + + /** + * @brief check if two transformations are close + * @param pose1 + * @param pose2 + * @param epsilon + * @return + */ + bool tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, const double& epsilon); + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + + // test parameters from parameter server + std::string planning_group_, group_tip_link_, tcp_link_, ik_fast_link_; + int random_test_number_; + std::vector joint_names_; + std::map zero_state_; + + // random seed + boost::uint32_t random_seed_{ 100 }; + random_numbers::RandomNumberGenerator rng_{ random_seed_ }; +}; + +void TrajectoryFunctionsTestBase::SetUp() +{ + // parameters + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(GROUP_TIP_LINK_NAME, group_tip_link_)); + ASSERT_TRUE(ph_.getParam(ROBOT_TCP_LINK_NAME, tcp_link_)); + ASSERT_TRUE(ph_.getParam(IK_FAST_LINK_NAME, ik_fast_link_)); + ASSERT_TRUE(ph_.getParam(RANDOM_TEST_NUMBER, random_test_number_)); + + // check robot model + testutils::checkRobotModel(robot_model_, planning_group_, tcp_link_); + + // initialize the zero state configurationg and test joint state + joint_names_ = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames(); + for (const auto& joint_name : joint_names_) + { + zero_state_[joint_name] = 0.0; + } +} + +bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, + const double& epsilon) +{ + for (std::size_t i = 0; i < 3; ++i) + for (std::size_t j = 0; j < 4; ++j) + { + if (fabs(pose1(i, j) - pose2(i, j)) > fabs(epsilon)) + return false; + } + return true; +} + +/** + * @brief Parametrized class for tests with and without gripper. + */ +class TrajectoryFunctionsTestFlangeAndGripper : public TrajectoryFunctionsTestBase +{ +}; + +/** + * @brief Parametrized class for tests, that only run with a gripper + */ +class TrajectoryFunctionsTestOnlyGripper : public TrajectoryFunctionsTestBase +{ +}; + +// Instantiate the test cases for robot model with and without gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryFunctionsTestFlangeAndGripper, + ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); + +// Instantiate the test cases for robot model with a gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryFunctionsTestOnlyGripper, + ::testing::Values(PARAM_MODEL_WITH_GRIPPER_NAME)); + +/** + * @brief Test the forward kinematics function with simple robot poses for robot + * tip link + * using robot model without gripper. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK) +{ + Eigen::Isometry3d tip_pose; + std::map test_state = zero_state_; + EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, group_tip_link_, test_state, tip_pose)); + EXPECT_NEAR(tip_pose(0, 3), 0, EPSILON); + EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON); + EXPECT_NEAR(tip_pose(2, 3), L0 + L1 + L2 + L3, EPSILON); + + test_state[joint_names_.at(1)] = M_PI_2; + EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, group_tip_link_, test_state, tip_pose)); + EXPECT_NEAR(tip_pose(0, 3), L1 + L2 + L3, EPSILON); + EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON); + EXPECT_NEAR(tip_pose(2, 3), L0, EPSILON); + + test_state[joint_names_.at(1)] = -M_PI_2; + test_state[joint_names_.at(2)] = M_PI_2; + EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, group_tip_link_, test_state, tip_pose)); + EXPECT_NEAR(tip_pose(0, 3), -L1, EPSILON); + EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON); + EXPECT_NEAR(tip_pose(2, 3), L0 - L2 - L3, EPSILON); + + // wrong link name + std::string link_name = "wrong_link_name"; + EXPECT_FALSE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, link_name, test_state, tip_pose)); +} + +/** + * @brief Test the inverse kinematics directly through ikfast solver + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver) +{ + // Load solver + const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); + + // robot state + robot_state::RobotState rstate(robot_model_); + + while (random_test_number_ > 0) + { + // sample random robot state + rstate.setToRandomPositions(jmg, rng_); + rstate.update(); + geometry_msgs::Pose pose_expect; + tf2::convert(rstate.getFrameTransform(ik_fast_link_), pose_expect); + + // prepare inverse kinematics + std::vector ik_poses; + ik_poses.push_back(pose_expect); + std::vector ik_seed, ik_expect, ik_actual; + for (const auto& joint_name : jmg->getActiveJointModelNames()) + { + ik_expect.push_back(rstate.getVariablePosition(joint_name)); + if (rstate.getVariablePosition(joint_name) > 0) + ik_seed.push_back(rstate.getVariablePosition(joint_name) - IK_SEED_OFFSET); + else + ik_seed.push_back(rstate.getVariablePosition(joint_name) + IK_SEED_OFFSET); + } + + std::vector> ik_solutions; + kinematics::KinematicsResult ik_result; + moveit_msgs::MoveItErrorCodes err_code; + kinematics::KinematicsQueryOptions options = kinematics::KinematicsQueryOptions(); + + // compute all ik solutions + EXPECT_TRUE(solver->getPositionIK(ik_poses, ik_seed, ik_solutions, ik_result, options)); + + // compute one ik solution + EXPECT_TRUE(solver->getPositionIK(pose_expect, ik_seed, ik_actual, err_code)); + + ASSERT_EQ(ik_expect.size(), ik_actual.size()); + + for (std::size_t i = 0; i < ik_expect.size(); ++i) + { + EXPECT_NEAR(ik_actual.at(i), ik_expect.at(i), 4 * IK_SEED_OFFSET); + } + + --random_test_number_; + } +} + +/** + * @brief Test the inverse kinematics using RobotState class (setFromIK) using + * robot model + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState) +{ + // robot state + robot_state::RobotState rstate(robot_model_); + const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + while (random_test_number_ > 0) + { + // sample random robot state + rstate.setToRandomPositions(jmg, rng_); + + Eigen::Isometry3d pose_expect = rstate.getFrameTransform(tcp_link_); + + // copy the random state and set ik seed + std::map ik_seed, ik_expect; + for (const auto& joint_name : joint_names_) + { + ik_expect[joint_name] = rstate.getVariablePosition(joint_name); + if (rstate.getVariablePosition(joint_name) > 0) + ik_seed[joint_name] = rstate.getVariablePosition(joint_name) - IK_SEED_OFFSET; + else + ik_seed[joint_name] = rstate.getVariablePosition(joint_name) + IK_SEED_OFFSET; + } + + rstate.setVariablePositions(ik_seed); + rstate.update(); + + // compute the ik + std::map ik_actual; + + EXPECT_TRUE(rstate.setFromIK(robot_model_->getJointModelGroup(planning_group_), pose_expect, tcp_link_)); + + for (const auto& joint_name : joint_names_) + { + ik_actual[joint_name] = rstate.getVariablePosition(joint_name); + } + + // compare ik solution and expected value + for (const auto& joint_pair : ik_actual) + { + EXPECT_NEAR(joint_pair.second, ik_expect.at(joint_pair.first), 4 * IK_SEED_OFFSET); + } + + // compute the pose from ik_solution + rstate.setVariablePositions(ik_actual); + rstate.update(); + Eigen::Isometry3d pose_actual = rstate.getFrameTransform(tcp_link_); + + EXPECT_TRUE(tfNear(pose_expect, pose_actual, EPSILON)); + + --random_test_number_; + } +} + +/** + * @brief Test the wrapper function to compute inverse kinematics using robot + * model + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK) +{ + // robot state + robot_state::RobotState rstate(robot_model_); + + const std::string frame_id = robot_model_->getModelFrame(); + const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + while (random_test_number_ > 0) + { + // sample random robot state + rstate.setToRandomPositions(jmg, rng_); + + Eigen::Isometry3d pose_expect = rstate.getFrameTransform(tcp_link_); + + // copy the random state and set ik seed + std::map ik_seed, ik_expect; + for (const auto& joint_name : robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames()) + { + ik_expect[joint_name] = rstate.getVariablePosition(joint_name); + if (rstate.getVariablePosition(joint_name) > 0) + { + ik_seed[joint_name] = rstate.getVariablePosition(joint_name) - IK_SEED_OFFSET; + } + else + { + ik_seed[joint_name] = rstate.getVariablePosition(joint_name) + IK_SEED_OFFSET; + } + } + + // compute the ik + std::map ik_actual; + EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + frame_id, ik_seed, ik_actual, false)); + + // compare ik solution and expected value + for (const auto& joint_pair : ik_actual) + { + EXPECT_NEAR(joint_pair.second, ik_expect.at(joint_pair.first), 4 * IK_SEED_OFFSET); + } + + --random_test_number_; + } +} + +/** + * @brief Test computePoseIK for invalid group_name + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupName) +{ + const std::string frame_id = robot_model_->getModelFrame(); + Eigen::Isometry3d pose_expect; + + std::map ik_seed; + + // compute the ik + std::map ik_actual; + EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, "InvalidGroupName", tcp_link_, pose_expect, + frame_id, ik_seed, ik_actual, false)); +} + +/** + * @brief Test computePoseIK for invalid link_name + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName) +{ + const std::string frame_id = robot_model_->getModelFrame(); + Eigen::Isometry3d pose_expect; + + std::map ik_seed; + + // compute the ik + std::map ik_actual; + EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, "WrongLink", pose_expect, + frame_id, ik_seed, ik_actual, false)); +} + +/** + * @brief Test computePoseIK for invalid frame_id + * + * Currently only robot_model_->getModelFrame() == frame_id + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId) +{ + Eigen::Isometry3d pose_expect; + + std::map ik_seed; + + // compute the ik + std::map ik_actual; + EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + "InvalidFrameId", ik_seed, ik_actual, false)); +} + +/** + * @brief Test if activated self collision for a pose that would be in self + * collision without the check results in a + * valid ik solution. + */ +TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForValidPosition) +{ + const std::string frame_id = robot_model_->getModelFrame(); + const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + // create seed + std::vector ik_seed_states = { -0.553, 0.956, 1.758, 0.146, -1.059, 1.247 }; + auto joint_names = jmg->getActiveJointModelNames(); + + std::map ik_seed; + for (unsigned int i = 0; i < ik_seed_states.size(); ++i) + { + ik_seed[joint_names[i]] = ik_seed_states[i]; + } + + // create expected pose + geometry_msgs::Pose pose; + pose.position.x = -0.454; + pose.position.y = -0.15; + pose.position.z = 0.431; + pose.orientation.y = 0.991562; + pose.orientation.w = -0.1296328; + Eigen::Isometry3d pose_expect; + normalizeQuaternion(pose.orientation); + tf2::convert(pose, pose_expect); + + // compute the ik without self collision check and expect the resulting pose + // to be in self collission. + std::map ik_actual1; + EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + frame_id, ik_seed, ik_actual1, false)); + + robot_state::RobotState rstate(robot_model_); + planning_scene::PlanningScene rscene(robot_model_); + + std::vector ik_state; + std::transform(ik_actual1.begin(), ik_actual1.end(), std::back_inserter(ik_state), + boost::bind(&std::map::value_type::second, _1)); + + rstate.setJointGroupPositions(jmg, ik_state); + rstate.update(); + + collision_detection::CollisionRequest collision_req; + collision_req.group_name = jmg->getName(); + collision_detection::CollisionResult collision_res; + + rscene.checkSelfCollision(collision_req, collision_res, rstate); + + EXPECT_TRUE(collision_res.collision); + + // compute the ik with collision detection activated and expect the resulting + // pose to be without self collision. + std::map ik_actual2; + EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + frame_id, ik_seed, ik_actual2, true)); + + std::vector ik_state2; + std::transform(ik_actual2.begin(), ik_actual2.end(), std::back_inserter(ik_state2), + boost::bind(&std::map::value_type::second, _1)); + rstate.setJointGroupPositions(jmg, ik_state2); + rstate.update(); + + collision_detection::CollisionResult collision_res2; + rscene.checkSelfCollision(collision_req, collision_res2, rstate); + + EXPECT_FALSE(collision_res2.collision); +} + +/** + * @brief Test if self collision is considered by using a pose that always has + * self collision. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionForInvalidPose) +{ + // robot state + robot_state::RobotState rstate(robot_model_); + + const std::string frame_id = robot_model_->getModelFrame(); + const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); + + // create seed + std::map ik_seed; + for (const auto& joint_name : jmg->getActiveJointModelNames()) + { + ik_seed[joint_name] = 0; + } + + // create goal + std::vector ik_goal = { 0, 2.3, -2.3, 0, 0, 0 }; + + rstate.setJointGroupPositions(jmg, ik_goal); + + Eigen::Isometry3d pose_expect = rstate.getFrameTransform(tcp_link_); + + // compute the ik with disabled collision check + std::map ik_actual; + EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + frame_id, ik_seed, ik_actual, false)); + + // compute the ik with enabled collision check + EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect, + frame_id, ik_seed, ik_actual, true)); +} + +/** + * @brief Check that function VerifySampleJointLimits() returns 'false' in case + * of very small sample duration. + * + * Test Sequence: + * 1. Call function with very small sample duration. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithSmallDuration) +{ + const std::map position_last, velocity_last, position_current; + double duration_last{ 0.0 }; + const pilz_industrial_motion_planner::JointLimitsContainer joint_limits; + + double duration_current = 10e-7; + + EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current, + duration_last, duration_current, joint_limits)); +} + +/** + * @brief Check that function VerifySampleJointLimits() returns 'false' in case + * of a velocity violation. + * + * Test Sequence: + * 1. Call function with a velocity violation. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVelocityViolation) +{ + const std::string test_joint_name{ "joint" }; + + std::map position_last{ { test_joint_name, 2.0 } }; + std::map position_current{ { test_joint_name, 10.0 } }; + std::map velocity_last; + double duration_current{ 1.0 }; + double duration_last{ 0.0 }; + pilz_industrial_motion_planner::JointLimitsContainer joint_limits; + + JointLimit test_joint_limits; + // Calculate the max allowed velocity in such a way that it is always smaller + // than the current velocity. + test_joint_limits.max_velocity = + ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current) - 1.0; + test_joint_limits.has_velocity_limits = true; + joint_limits.addLimit(test_joint_name, test_joint_limits); + + EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current, + duration_last, duration_current, joint_limits)); +} + +/** + * @brief Check that function VerifySampleJointLimits() returns 'false' in case + * of a acceleration violation. + * + * Test Sequence: + * 1. Call function with a acceleration violation. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccelerationViolation) +{ + const std::string test_joint_name{ "joint" }; + + double duration_current = 1.0; + double duration_last = 1.0; + + std::map position_last{ { test_joint_name, 2.0 } }; + std::map position_current{ { test_joint_name, 20.0 } }; + double velocity_current = + ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current); + std::map velocity_last{ { test_joint_name, 9.0 } }; + pilz_industrial_motion_planner::JointLimitsContainer joint_limits; + + JointLimit test_joint_limits; + // Calculate the max allowed velocity in such a way that it is always bigger + // than the current velocity. + test_joint_limits.max_velocity = velocity_current + 1.0; + test_joint_limits.has_velocity_limits = true; + + double acceleration_current = + (velocity_current - velocity_last.at(test_joint_name)) / (duration_last + duration_current) * 2; + // Calculate the max allowed acceleration in such a way that it is always + // smaller than the current acceleration. + test_joint_limits.max_acceleration = acceleration_current - 1.0; + test_joint_limits.has_acceleration_limits = true; + + joint_limits.addLimit(test_joint_name, test_joint_limits); + + EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current, + duration_last, duration_current, joint_limits)); +} + +/** + * @brief Check that function VerifySampleJointLimits() returns 'false' in case + * of a deceleration violation. + * + * Test Sequence: + * 1. Call function with a deceleration violation. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecelerationViolation) +{ + const std::string test_joint_name{ "joint" }; + + double duration_current = 1.0; + double duration_last = 1.0; + + std::map position_last{ { test_joint_name, 20.0 } }; + std::map position_current{ { test_joint_name, 2.0 } }; + double velocity_current = + ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current); + std::map velocity_last{ { test_joint_name, 19.0 } }; + pilz_industrial_motion_planner::JointLimitsContainer joint_limits; + + JointLimit test_joint_limits; + // Calculate the max allowed velocity in such a way that it is always bigger + // than the current velocity. + test_joint_limits.max_velocity = fabs(velocity_current) + 1.0; + test_joint_limits.has_velocity_limits = true; + + double acceleration_current = + (velocity_current - velocity_last.at(test_joint_name)) / (duration_last + duration_current) * 2; + // Calculate the max allowed deceleration in such a way that it is always + // bigger than the current acceleration. + test_joint_limits.max_deceleration = acceleration_current + 1.0; + test_joint_limits.has_deceleration_limits = true; + + joint_limits.addLimit(test_joint_name, test_joint_limits); + + EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current, + duration_last, duration_current, joint_limits)); +} + +/** + * @brief Check that function generateJointTrajectory() returns 'false' if + * a joint trajectory cannot be computed from a cartesian trajectory. + * + * Please note: Both function variants are tested in this test. + * + * Test Sequence: + * 1. Call function with a cartesian trajectory which cannot be transformed + * into a joint trajectory by using + * an invalid group_name. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithInvalidCartesianTrajectory) +{ + // Create random test trajectory + // Note: 'path' is deleted by KDL::Trajectory_Segment + KDL::Path_RoundedComposite* path = + new KDL::Path_RoundedComposite(0.2, 0.01, new KDL::RotationalInterpolation_SingleAxis()); + path->Add(KDL::Frame(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(-1, 0, 0))); + path->Finish(); + // Note: 'velprof' is deleted by KDL::Trajectory_Segment + KDL::VelocityProfile* vel_prof = new KDL::VelocityProfile_Trap(0.5, 0.1); + vel_prof->SetProfile(0, path->PathLength()); + KDL::Trajectory_Segment kdl_trajectory(path, vel_prof); + + pilz_industrial_motion_planner::JointLimitsContainer joint_limits; + std::string group_name{ "invalid_group_name" }; + std::map initial_joint_position; + double sampling_time{ 0.1 }; + trajectory_msgs::JointTrajectory joint_trajectory; + moveit_msgs::MoveItErrorCodes error_code; + bool check_self_collision{ false }; + + EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory( + robot_model_, joint_limits, kdl_trajectory, group_name, tcp_link_, initial_joint_position, sampling_time, + joint_trajectory, error_code, check_self_collision)); + + std::map initial_joint_velocity; + + pilz_industrial_motion_planner::CartesianTrajectory cart_traj; + cart_traj.group_name = group_name; + cart_traj.link_name = tcp_link_; + pilz_industrial_motion_planner::CartesianTrajectoryPoint cart_traj_point; + cart_traj.points.push_back(cart_traj_point); + + EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory( + robot_model_, joint_limits, cart_traj, group_name, tcp_link_, initial_joint_position, initial_joint_velocity, + joint_trajectory, error_code, check_self_collision)); +} + +/** + * @brief Check that function determineAndCheckSamplingTime() returns 'false' if + * both of the needed vectors have an incorrect vector size. + * + * + * Test Sequence: + * 1. Call function with vectors of incorrect size. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeInvalidVectorSize) +{ + robot_trajectory::RobotTrajectoryPtr first_trajectory = + std::make_shared(robot_model_, planning_group_); + robot_trajectory::RobotTrajectoryPtr second_trajectory = + std::make_shared(robot_model_, planning_group_); + double epsilon{ 0.0 }; + double sampling_time{ 0.0 }; + + robot_state::RobotState rstate(robot_model_); + first_trajectory->insertWayPoint(0, rstate, 0.1); + second_trajectory->insertWayPoint(0, rstate, 0.1); + + EXPECT_FALSE(pilz_industrial_motion_planner::determineAndCheckSamplingTime(first_trajectory, second_trajectory, + epsilon, sampling_time)); +} + +/** + * @brief Check that function determineAndCheckSamplingTime() returns 'true' if + * sampling time is correct. + * + * + * Test Sequence: + * 1. Call function with trajectories which do NOT violate sampling time. + * + * Expected Results: + * 1. Function returns 'true'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeCorrectSamplingTime) +{ + robot_trajectory::RobotTrajectoryPtr first_trajectory = + std::make_shared(robot_model_, planning_group_); + robot_trajectory::RobotTrajectoryPtr second_trajectory = + std::make_shared(robot_model_, planning_group_); + double epsilon{ 0.0001 }; + double sampling_time{ 0.0 }; + double expected_sampling_time{ 0.1 }; + + robot_state::RobotState rstate(robot_model_); + first_trajectory->insertWayPoint(0, rstate, expected_sampling_time); + first_trajectory->insertWayPoint(1, rstate, expected_sampling_time); + + second_trajectory->insertWayPoint(0, rstate, expected_sampling_time); + second_trajectory->insertWayPoint(1, rstate, expected_sampling_time); + second_trajectory->insertWayPoint(2, rstate, expected_sampling_time); + + EXPECT_TRUE(pilz_industrial_motion_planner::determineAndCheckSamplingTime(first_trajectory, second_trajectory, + epsilon, sampling_time)); + EXPECT_EQ(expected_sampling_time, sampling_time); +} + +/** + * @brief Check that function determineAndCheckSamplingTime() returns 'false' if + * sampling time is violated. + * + * + * Test Sequence: + * 1. Call function with trajectories which violate sampling time. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeViolateSamplingTime) +{ + robot_trajectory::RobotTrajectoryPtr first_trajectory = + std::make_shared(robot_model_, planning_group_); + robot_trajectory::RobotTrajectoryPtr second_trajectory = + std::make_shared(robot_model_, planning_group_); + double epsilon{ 0.0001 }; + double sampling_time{ 0.0 }; + double expected_sampling_time{ 0.1 }; + + robot_state::RobotState rstate(robot_model_); + first_trajectory->insertWayPoint(0, rstate, expected_sampling_time); + first_trajectory->insertWayPoint(1, rstate, expected_sampling_time); + first_trajectory->insertWayPoint(2, rstate, expected_sampling_time); + // Violate sampling time + first_trajectory->insertWayPoint(2, rstate, expected_sampling_time + 1.0); + first_trajectory->insertWayPoint(3, rstate, expected_sampling_time); + + second_trajectory->insertWayPoint(0, rstate, expected_sampling_time); + second_trajectory->insertWayPoint(1, rstate, expected_sampling_time); + second_trajectory->insertWayPoint(2, rstate, expected_sampling_time); + second_trajectory->insertWayPoint(3, rstate, expected_sampling_time); + + EXPECT_FALSE(pilz_industrial_motion_planner::determineAndCheckSamplingTime(first_trajectory, second_trajectory, + epsilon, sampling_time)); + EXPECT_EQ(expected_sampling_time, sampling_time); +} + +/** + * @brief Check that function isRobotStateEqual() returns 'false' if + * the positions of the robot states are not equal. + * + * + * Test Sequence: + * 1. Call function with robot states with different positions. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUnequal) +{ + robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); + robot_state::RobotState rstate_2 = robot_state::RobotState(robot_model_); + + double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupPositions(planning_group_, default_joint_position); + // Ensure that the joint positions of both robot states are different + default_joint_position[0] = default_joint_position[0] + 70.0; + rstate_2.setJointGroupPositions(planning_group_, default_joint_position); + + double epsilon{ 0.0001 }; + EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateEqual(rstate_1, rstate_2, planning_group_, epsilon)); +} + +/** + * @brief Check that function isRobotStateEqual() returns 'false' if + * the velocity of the robot states are not equal. + * + * + * Test Sequence: + * 1. Call function with robot states with different velocities. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUnequal) +{ + robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); + robot_state::RobotState rstate_2 = robot_state::RobotState(robot_model_); + + // Ensure that the joint positions of both robot state are equal + double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupPositions(planning_group_, default_joint_position); + rstate_2.setJointGroupPositions(planning_group_, default_joint_position); + + double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity); + // Ensure that the joint velocites of both robot states are different + default_joint_velocity[1] = default_joint_velocity[1] + 10.0; + rstate_2.setJointGroupVelocities(planning_group_, default_joint_velocity); + + double epsilon{ 0.0001 }; + EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateEqual(rstate_1, rstate_2, planning_group_, epsilon)); +} + +/** + * @brief Check that function isRobotStateEqual() returns 'false' if + * the acceleration of the robot states are not equal. + * + * + * Test Sequence: + * 1. Call function with robot states with different acceleration. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAccelerationUnequal) +{ + robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); + robot_state::RobotState rstate_2 = robot_state::RobotState(robot_model_); + + // Ensure that the joint positions of both robot state are equal + double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupPositions(planning_group_, default_joint_position); + rstate_2.setJointGroupPositions(planning_group_, default_joint_position); + + // Ensure that the joint velocities of both robot state are equal + double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity); + rstate_2.setJointGroupVelocities(planning_group_, default_joint_velocity); + + double default_joint_acceleration[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupAccelerations(planning_group_, default_joint_acceleration); + // Ensure that the joint accelerations of both robot states are different + default_joint_acceleration[1] = default_joint_acceleration[1] + 10.0; + rstate_2.setJointGroupAccelerations(planning_group_, default_joint_acceleration); + + double epsilon{ 0.0001 }; + EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateEqual(rstate_1, rstate_2, planning_group_, epsilon)); +} + +/** + * @brief Check that function isRobotStateStationary() returns 'false' if + * the joint velocities are not equal to zero. + * + * + * Test Sequence: + * 1. Call function with robot state with joint velocities != 0. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVelocityUnequal) +{ + robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); + + // Ensure that the joint velocities are NOT zero + double default_joint_velocity[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity); + + double epsilon{ 0.0001 }; + EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateStationary(rstate_1, planning_group_, epsilon)); +} + +/** + * @brief Check that function isRobotStateStationary() returns 'false' if + * the joint acceleration are not equal to zero. + * + * + * Test Sequence: + * 1. Call function with robot state with joint acceleration != 0. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal) +{ + robot_state::RobotState rstate_1 = robot_state::RobotState(robot_model_); + + // Ensure that the joint velocities are zero + double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity); + + // Ensure that the joint acceleration are NOT zero + double default_joint_acceleration[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + rstate_1.setJointGroupAccelerations(planning_group_, default_joint_acceleration); + + double epsilon{ 0.0001 }; + EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateStationary(rstate_1, planning_group_, epsilon)); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_trajectory_functions"); + ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.test new file mode 100644 index 0000000000..ff1c41bb0f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.test @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp new file mode 100644 index 0000000000..c1a24e7285 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator.cpp @@ -0,0 +1,146 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include + +#include "pilz_industrial_motion_planner/trajectory_generator.h" + +using namespace pilz_industrial_motion_planner; + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + */ +TEST(TrajectoryGeneratorTest, TestExceptionErrorCodeMapping) +{ + { + std::shared_ptr tgil_ex{ new TrajectoryGeneratorInvalidLimitsException( + "") }; + EXPECT_EQ(tgil_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + } + + { + std::shared_ptr vsi_ex{ new VelocityScalingIncorrect("") }; + EXPECT_EQ(vsi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr asi_ex{ new AccelerationScalingIncorrect("") }; + EXPECT_EQ(asi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr upg_ex{ new UnknownPlanningGroup("") }; + EXPECT_EQ(upg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); + } + + { + std::shared_ptr njniss_ex{ new NoJointNamesInStartState("") }; + EXPECT_EQ(njniss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + } + + { + std::shared_ptr smiss_ex{ new SizeMismatchInStartState("") }; + EXPECT_EQ(smiss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + } + + { + std::shared_ptr jofssoor_ex{ new JointsOfStartStateOutOfRange("") }; + EXPECT_EQ(jofssoor_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + } + + { + std::shared_ptr nzviss_ex{ new NonZeroVelocityInStartState("") }; + EXPECT_EQ(nzviss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + } + + { + std::shared_ptr neogcg_ex{ new NotExactlyOneGoalConstraintGiven("") }; + EXPECT_EQ(neogcg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr oogta_ex{ new OnlyOneGoalTypeAllowed("") }; + EXPECT_EQ(oogta_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr ssgsm_ex{ new StartStateGoalStateMismatch("") }; + EXPECT_EQ(ssgsm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr jcdnbtg_ex{ new JointConstraintDoesNotBelongToGroup("") }; + EXPECT_EQ(jcdnbtg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr jogoor_ex{ new JointsOfGoalOutOfRange("") }; + EXPECT_EQ(jogoor_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr pcnm_ex{ new PositionConstraintNameMissing("") }; + EXPECT_EQ(pcnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr ocnm_ex{ new OrientationConstraintNameMissing("") }; + EXPECT_EQ(ocnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr pocnm_ex{ new PositionOrientationConstraintNameMismatch( + "") }; + EXPECT_EQ(pocnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr nisa_ex{ new NoIKSolverAvailable("") }; + EXPECT_EQ(nisa_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + } + + { + std::shared_ptr nppg_ex{ new NoPrimitivePoseGiven("") }; + EXPECT_EQ(nppg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp new file mode 100644 index 0000000000..1c1024cb4e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp @@ -0,0 +1,785 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/trajectory_generator_circ.h" +#include "pilz_industrial_motion_planner_testutils/command_types_typedef.h" +#include "pilz_industrial_motion_planner_testutils/xml_testdata_loader.h" +#include "test_utils.h" + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string PARAM_TARGET_LINK_NAME("target_link"); +const std::string CARTESIAN_POSITION_TOLERANCE("cartesian_position_tolerance"); +const std::string ANGULAR_ACC_TOLERANCE("angular_acc_tolerance"); +const std::string ROTATION_AXIS_NORM_TOLERANCE("rot_axis_norm_tolerance"); +const std::string ACCELERATION_TOLERANCE("acceleration_tolerance"); +const std::string OTHER_TOLERANCE("other_tolerance"); + +#define SKIP_IF_GRIPPER \ + if (GetParam() == PARAM_MODEL_WITH_GRIPPER_NAME) \ + { \ + SUCCEED(); \ + return; \ + }; + +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner_testutils; + +class TrajectoryGeneratorCIRCTest : public testing::TestWithParam +{ +protected: + /** + * @brief Create test scenario for circ trajectory generator + * + */ + void SetUp() override; + + void checkCircResult(const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanResponse& res); + + void getCircCenter(const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanResponse& res, Eigen::Vector3d& circ_center); + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + std::unique_ptr circ_; + // test data provider + std::unique_ptr tdp_; + + // test parameters from parameter server + std::string planning_group_, target_link_, test_data_file_name_; + int random_trial_num_; + double cartesian_position_tolerance_, angular_acc_tolerance_, rot_axis_norm_tolerance_, acceleration_tolerance_, + other_tolerance_; + LimitsContainer planner_limits_; +}; + +void TrajectoryGeneratorCIRCTest::SetUp() +{ + // get parameters + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + ASSERT_TRUE(ph_.getParam(CARTESIAN_POSITION_TOLERANCE, cartesian_position_tolerance_)); + ASSERT_TRUE(ph_.getParam(ANGULAR_ACC_TOLERANCE, angular_acc_tolerance_)); + ASSERT_TRUE(ph_.getParam(ROTATION_AXIS_NORM_TOLERANCE, rot_axis_norm_tolerance_)); + ASSERT_TRUE(ph_.getParam(ACCELERATION_TOLERANCE, acceleration_tolerance_)); + ASSERT_TRUE(ph_.getParam(OTHER_TOLERANCE, other_tolerance_)); + + // check robot model + testutils::checkRobotModel(robot_model_, planning_group_, target_link_); + + // load the test data provider + tdp_.reset(new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_ }); + ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider."; + + tdp_->setRobotModel(robot_model_); + + // create the limits container + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(ph_, + robot_model_->getActiveJointModels()); + CartesianLimit cart_limits; + // Cartesian limits are chose as such values to ease the manually compute the + // trajectory + + cart_limits.setMaxRotationalVelocity(1 * M_PI); + cart_limits.setMaxTranslationalAcceleration(1 * M_PI); + cart_limits.setMaxTranslationalDeceleration(1 * M_PI); + cart_limits.setMaxTranslationalVelocity(1 * M_PI); + planner_limits_.setJointLimits(joint_limits); + planner_limits_.setCartesianLimits(cart_limits); + + // initialize the LIN trajectory generator + circ_.reset(new TrajectoryGeneratorCIRC(robot_model_, planner_limits_)); + ASSERT_NE(nullptr, circ_) << "failed to create CIRC trajectory generator"; +} + +void TrajectoryGeneratorCIRCTest::checkCircResult(const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanResponse& res) +{ + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + EXPECT_TRUE(testutils::isGoalReached(res.trajectory_->getFirstWayPointPtr()->getRobotModel(), + res_msg.trajectory.joint_trajectory, req, other_tolerance_)); + + EXPECT_TRUE( + testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer())); + + EXPECT_EQ(req.path_constraints.position_constraints.size(), 1u); + EXPECT_EQ(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.size(), 1u); + + // Check that all point have the equal distance to the center + Eigen::Vector3d circ_center; + getCircCenter(req, res, circ_center); + + for (std::size_t i = 0; i < res.trajectory_->getWayPointCount(); ++i) + { + Eigen::Affine3d waypoint_pose = res.trajectory_->getWayPointPtr(i)->getFrameTransform(target_link_); + EXPECT_NEAR( + (res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation() - circ_center).norm(), + (circ_center - waypoint_pose.translation()).norm(), cartesian_position_tolerance_); + } + + // check translational and rotational paths + ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_, acceleration_tolerance_)); + ASSERT_TRUE(testutils::checkCartesianRotationalPath(res.trajectory_, target_link_, angular_acc_tolerance_, + rot_axis_norm_tolerance_)); + + for (size_t idx = 0; idx < res.trajectory_->getLastWayPointPtr()->getVariableCount(); ++idx) + { + EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_); + EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_); + } +} + +void TrajectoryGeneratorCIRCTest::getCircCenter(const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanResponse& res, + Eigen::Vector3d& circ_center) +{ + if (req.path_constraints.name == "center") + { + tf2::convert( + req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position, circ_center); + } + else if (req.path_constraints.name == "interim") + { + Eigen::Vector3d interim; + tf2::convert( + req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position, interim); + Eigen::Vector3d start = res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation(); + Eigen::Vector3d goal = res.trajectory_->getLastWayPointPtr()->getFrameTransform(target_link_).translation(); + + const Eigen::Vector3d t = interim - start; + const Eigen::Vector3d u = goal - start; + const Eigen::Vector3d v = goal - interim; + + const Eigen::Vector3d w = t.cross(u); + + ASSERT_GT(w.norm(), 1e-8) << "Circle center not well defined for given start, interim and goal."; + + circ_center = start + (u * t.dot(t) * u.dot(v) - t * u.dot(u) * t.dot(v)) * 0.5 / pow(w.norm(), 2); + } +} + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + */ +TEST(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping) +{ + { + std::shared_ptr cnp_ex{ new CircleNoPlane("") }; + EXPECT_EQ(cnp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr cts_ex{ new CircleToSmall("") }; + EXPECT_EQ(cts_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr cpdr_ex{ new CenterPointDifferentRadius("") }; + EXPECT_EQ(cpdr_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr ctcf_ex{ new CircTrajectoryConversionFailure("") }; + EXPECT_EQ(ctcf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr upcn_ex{ new UnknownPathConstraintName("") }; + EXPECT_EQ(upcn_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr npc_ex{ new NoPositionConstraints("") }; + EXPECT_EQ(npc_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr npp_ex{ new NoPrimitivePose("") }; + EXPECT_EQ(npp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + } + + { + std::shared_ptr ulnoap_ex{ new UnknownLinkNameOfAuxiliaryPoint("") }; + EXPECT_EQ(ulnoap_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME); + } + + { + std::shared_ptr nocm_ex{ new NumberOfConstraintsMismatch("") }; + EXPECT_EQ(nocm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr cjmiss_ex{ new CircJointMissingInStartState("") }; + EXPECT_EQ(cjmiss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + } + + { + std::shared_ptr cifgi_ex{ new CircInverseForGoalIncalculable("") }; + EXPECT_EQ(cifgi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + } +} + +// Instantiate the test cases for robot model with and without gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorCIRCTest, + ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); + +/** + * @brief Construct a TrajectoryGeneratorCirc with no limits given + */ +TEST_P(TrajectoryGeneratorCIRCTest, noLimits) +{ + LimitsContainer planner_limits; + EXPECT_THROW(TrajectoryGeneratorCIRC(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); +} + +/** + * @brief test invalid motion plan request with incomplete start state and + * cartesian goal + */ +TEST_P(TrajectoryGeneratorCIRCTest, incompleteStartState) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + planning_interface::MotionPlanRequest req{ circ.toRequest() }; + EXPECT_GT(req.start_state.joint_state.name.size(), 1u); + req.start_state.joint_state.name.resize(1); + req.start_state.joint_state.position.resize(1); // prevent failing check for equal sizes + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief test invalid motion plan request with non zero start velocity + */ +TEST_P(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity) +{ + moveit_msgs::MotionPlanRequest req{ tdp_->getCircJointCenterCart("circ1_center_2").toRequest() }; + + // start state has non-zero velocity + req.start_state.joint_state.velocity.push_back(1.0); + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + req.start_state.joint_state.velocity.clear(); +} + +TEST_P(TrajectoryGeneratorCIRCTest, ValidCommand) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + planning_interface::MotionPlanResponse res; + EXPECT_TRUE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); +} + +/** + * @brief Generate invalid circ with to high vel scaling + */ +TEST_P(TrajectoryGeneratorCIRCTest, velScaleToHigh) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + circ.setVelocityScale(1.0); + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::PLANNING_FAILED); +} + +/** + * @brief Generate invalid circ with to high acc scaling + */ +TEST_P(TrajectoryGeneratorCIRCTest, accScaleToHigh) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + circ.setAccelerationScale(1.0); + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::PLANNING_FAILED); +} + +/** + * @brief Use three points (with center) with a really small distance between to + * trigger a internal throw from KDL + */ +TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithCenter) +{ + // Define auxiliary point and goal to be the same as the start + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 1e-8; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1e-8; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.z += 1e-8; + circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getGoalConfiguration().getPose().position.x -= 1e-8; + circ.getGoalConfiguration().getPose().position.y -= 1e-8; + circ.getGoalConfiguration().getPose().position.z -= 1e-8; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief Use three points (with interim) with a really small distance between + * + * Expected: Planning should fail. + */ +TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithInterim) +{ + // Define auxiliary point and goal to be the same as the start + auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 1e-8; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1e-8; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.z += 1e-8; + circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getGoalConfiguration().getPose().position.x -= 1e-8; + circ.getGoalConfiguration().getPose().position.y -= 1e-8; + circ.getGoalConfiguration().getPose().position.z -= 1e-8; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief test invalid motion plan request with no aux point defined + */ +TEST_P(TrajectoryGeneratorCIRCTest, emptyAux) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + planning_interface::MotionPlanRequest req = circ.toRequest(); + + req.path_constraints.position_constraints.clear(); + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief test invalid motion plan request with no aux name defined + */ +TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxName) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + planning_interface::MotionPlanRequest req = circ.toRequest(); + + req.path_constraints.name = ""; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief test invalid motion plan request with invalid link name in the + * auxiliary point + */ +TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxLinkName) +{ + auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; + + planning_interface::MotionPlanRequest req = circ.toRequest(); + + req.path_constraints.position_constraints.front().link_name = "INVALID"; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME); +} + +/** + * @brief test the circ planner with invalid center point + */ +TEST_P(TrajectoryGeneratorCIRCTest, invalidCenter) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief test the circ planner with colinear start/goal/center position + * + * Expected: Planning should fail since the path is not uniquely defined. + */ +TEST_P(TrajectoryGeneratorCIRCTest, colinearCenter) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose()); + + // Stretch start and goal pose along line + circ.getStartConfiguration().getPose().position.x -= 0.1; + circ.getGoalConfiguration().getPose().position.x += 0.1; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief test the circ planner with colinear start/goal/interim position + * + * Expected: Planning should fail. These positions do not even represent a + * circle. + */ +TEST_P(TrajectoryGeneratorCIRCTest, colinearInterim) +{ + auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose()); + + // Stretch start and goal pose along line + circ.getStartConfiguration().getPose().position.x -= 0.1; + circ.getGoalConfiguration().getPose().position.x += 0.1; + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief test the circ planner with half circle with interim point + * + * The request contains start/interim/goal such that + * start, center (not explicitly given) and goal are colinear + * + * Expected: Planning should successfully return. + */ +TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim) +{ + // get the test data from xml + auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(circ.toRequest(), res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); +} + +/** + * @brief test the circ planner with colinear start/center/interim positions + * + * The request contains start/interim/goal such that + * start, center (not explicitly given) and interim are colinear. + * In case the interim is used as auxiliary point for KDL::Path_Circle this + * should fail. + * + * Expected: Planning should successfully return. + */ +TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim) +{ + auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; + + // alter start, interim and goal such that start/center and interim are + // colinear + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose()); + + circ.getStartConfiguration().getPose().position.x -= 0.2; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 0.2; + circ.getGoalConfiguration().getPose().position.y -= 0.2; + + circ.setAccelerationScale(0.05); + circ.setVelocityScale(0.05); + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + planning_interface::MotionPlanResponse res; + EXPECT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief test the circ planner with a circ path where the angle between goal + * and interim is larger than 180 degree + * + * The request contains start/interim/goal such that 180 degree < interim angle + * < goal angle. + * + * Expected: Planning should successfully return. + */ +TEST_P(TrajectoryGeneratorCIRCTest, interimLarger180Degree) +{ + auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; + + // alter start, interim and goal such that start/center and interim are + // colinear + circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose()); + circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose()); + + circ.getStartConfiguration().getPose().position.x -= 0.2; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 0.14142136; + circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y -= 0.14142136; + circ.getGoalConfiguration().getPose().position.y -= 0.2; + + circ.setAccelerationScale(0.05); + circ.setVelocityScale(0.05); + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + planning_interface::MotionPlanResponse res; + EXPECT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief test the circ planner with center point and joint goal + */ +TEST_P(TrajectoryGeneratorCIRCTest, centerPointJointGoal) +{ + SKIP_IF_GRIPPER + + auto circ{ tdp_->getCircJointCenterCart("circ1_center_2") }; + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief A valid circ request contains a helping point (interim or center), in + * this test a additional + * point is defined as an invalid test case + */ +TEST_P(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + // Contains one pose (interim / center) + ASSERT_EQ(req.path_constraints.position_constraints.back().constraint_region.primitive_poses.size(), 1u); + + // Define a additional pose here + geometry_msgs::Pose center_position; + center_position.position.x = 0.0; + center_position.position.y = 0.0; + center_position.position.z = 0.65; + req.path_constraints.position_constraints.back().constraint_region.primitive_poses.push_back(center_position); + + planning_interface::MotionPlanResponse res; + ASSERT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief Joint Goals are expected to match the start state in number and + * joint_names + * Here an additional joint constraints is "falsely" defined to check for the + * error. + */ +TEST_P(TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint) +{ + auto circ{ tdp_->getCircJointCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + // Define the additional joint constraint + moveit_msgs::JointConstraint joint_constraint; + joint_constraint.joint_name = req.goal_constraints.front().joint_constraints.front().joint_name; + req.goal_constraints.front().joint_constraints.push_back(joint_constraint); //<-- Additional constraint + + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief test the circ planner with center point and pose goal + */ +TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoal) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief Set a frame id only on the position constrainst + */ +TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraints) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief Set a frame id only on the orientation constrainst + */ +TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstraints) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief Set a frame_id on both position and orientation constraints + */ +TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints) +{ + auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + // Both set + req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame(); + req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief test the circ planner with interim point with joint goal + */ +TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoal) +{ + SKIP_IF_GRIPPER + + auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief test the circ planner with interim point with joint goal and a close + * to zero velocity of the start state + * + * The generator is expected to be robust against a velocity beeing almost zero. + */ +TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero) +{ + SKIP_IF_GRIPPER + + auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; + + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + // Set velocity near zero + req.start_state.joint_state.velocity = std::vector(req.start_state.joint_state.position.size(), 1e-16); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +/** + * @brief test the circ planner with interim point with pose goal + */ +TEST_P(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal) +{ + auto circ{ tdp_->getCircJointInterimCart("circ3_interim") }; + moveit_msgs::MotionPlanRequest req = circ.toRequest(); + + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(circ_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + checkCircResult(req, res); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_trajectory_generator_circ"); + ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.test new file mode 100644 index 0000000000..6d84d42105 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.test @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp new file mode 100644 index 0000000000..aaba86c0f1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp @@ -0,0 +1,434 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/joint_limits_container.h" +#include "pilz_industrial_motion_planner/trajectory_generator_circ.h" +#include "pilz_industrial_motion_planner/trajectory_generator_lin.h" +#include "pilz_industrial_motion_planner/trajectory_generator_ptp.h" + +#include "test_utils.h" + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string PARAM_TARGET_LINK_NAME("target_link"); + +/** + * A value type container to combine type and value + * In the tests types are trajectory generators. + * value = 0 refers to robot model without gripper + * value = 1 refers to robot model with gripper + */ +template +class ValueTypeContainer +{ +public: + typedef T Type_; + static const int VALUE = N; +}; +template +const int ValueTypeContainer::VALUE; + +typedef ValueTypeContainer PTP_NO_GRIPPER; +typedef ValueTypeContainer PTP_WITH_GRIPPER; +typedef ValueTypeContainer LIN_NO_GRIPPER; +typedef ValueTypeContainer LIN_WITH_GRIPPER; +typedef ValueTypeContainer CIRC_NO_GRIPPER; +typedef ValueTypeContainer CIRC_WITH_GRIPPER; + +typedef ::testing::Types + TrajectoryGeneratorCommonTestTypes; + +typedef ::testing::Types TrajectoryGeneratorCommonTestTypesNoGripper; + +typedef ::testing::Types + TrajectoryGeneratorCommonTestTypesWithGripper; + +/** + * type parameterized test fixture + */ +template +class TrajectoryGeneratorCommonTest : public ::testing::Test +{ +protected: + void SetUp() override + { + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + + testutils::checkRobotModel(robot_model_, planning_group_, target_link_); + + // create the limits container + std::string robot_description_param = (!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME); + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( + ros::NodeHandle(robot_description_param + "_planning"), robot_model_->getActiveJointModels()); + pilz_industrial_motion_planner::CartesianLimit cart_limits; + cart_limits.setMaxRotationalVelocity(0.5 * M_PI); + cart_limits.setMaxTranslationalAcceleration(2); + cart_limits.setMaxTranslationalDeceleration(2); + cart_limits.setMaxTranslationalVelocity(1); + pilz_industrial_motion_planner::LimitsContainer planner_limits; + planner_limits.setJointLimits(joint_limits); + planner_limits.setCartesianLimits(cart_limits); + + // create planner instance + trajectory_generator_ = std::unique_ptr(new typename T::Type_(robot_model_, planner_limits)); + ASSERT_NE(nullptr, trajectory_generator_) << "failed to create trajectory generator"; + + // create a valid motion plan request with goal in joint space as basis for + // tests + req_.group_name = planning_group_; + req_.max_velocity_scaling_factor = 1.0; + req_.max_acceleration_scaling_factor = 1.0; + robot_state::RobotState rstate(robot_model_); + rstate.setToDefaultValues(); + rstate.setJointGroupPositions(planning_group_, { 0, M_PI / 2, 0, M_PI / 2, 0, 0 }); + rstate.setVariableVelocities(std::vector(rstate.getVariableCount(), 0.0)); + moveit::core::robotStateToRobotStateMsg(rstate, req_.start_state, false); + moveit_msgs::Constraints goal_constraint; + moveit_msgs::JointConstraint joint_constraint; + joint_constraint.joint_name = this->robot_model_->getActiveJointModels().front()->getName(); + joint_constraint.position = 0.5; + goal_constraint.joint_constraints.push_back(joint_constraint); + req_.goal_constraints.push_back(goal_constraint); + } + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ + robot_model_loader::RobotModelLoader(!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME) + .getModel() + }; + + // trajectory generator + std::unique_ptr trajectory_generator_; + planning_interface::MotionPlanResponse res_; + planning_interface::MotionPlanRequest req_; + // test parameters from parameter server + std::string planning_group_, target_link_; +}; +// Define the types we need to test +TYPED_TEST_SUITE(TrajectoryGeneratorCommonTest, TrajectoryGeneratorCommonTestTypes); + +template +class TrajectoryGeneratorCommonTestNoGripper : public TrajectoryGeneratorCommonTest +{ +}; +TYPED_TEST_SUITE(TrajectoryGeneratorCommonTestNoGripper, TrajectoryGeneratorCommonTestTypesNoGripper); + +template +class TrajectoryGeneratorCommonTestWithGripper : public TrajectoryGeneratorCommonTest +{ +}; +TYPED_TEST_SUITE(TrajectoryGeneratorCommonTestWithGripper, TrajectoryGeneratorCommonTestTypesWithGripper); + +/** + * @brief test invalid scaling factor. The scaling factor must be in the range + * of [0.0001, 1] + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideScalingFactor) +{ + this->req_.max_velocity_scaling_factor = 2.0; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + + this->req_.max_velocity_scaling_factor = 1.0; + this->req_.max_acceleration_scaling_factor = 0; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + + this->req_.max_velocity_scaling_factor = 0.00001; + this->req_.max_acceleration_scaling_factor = 1; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); + + this->req_.max_velocity_scaling_factor = 1; + this->req_.max_acceleration_scaling_factor = -1; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN); +} + +/** + * @brief Test invalid motion plan request for all trajectory generators + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidGroupName) +{ + this->req_.group_name = "foot"; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val); +} + +/** + * @brief Test invalid motion plan request for all trajectory generators + */ +TYPED_TEST(TrajectoryGeneratorCommonTestNoGripper, GripperGroup) +{ + this->req_.group_name = "gripper"; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val); +} + +/** + * @brief Test invalid motion plan request for all trajectory generators + */ +TYPED_TEST(TrajectoryGeneratorCommonTestWithGripper, GripperGroup) +{ + this->req_.group_name = "gripper"; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS, this->res_.error_code_.val); +} + +/** + * @brief Test if there is a valid inverse kinematics solver for this planning + * group + * You can only test this case by commenting the planning_context.launch in the + * .test file + * //TODO create a separate robot model without ik solver and use it to create a + * trajectory generator + */ +// TYPED_TEST(TrajectoryGeneratorCommonTest, NoIKSolver) +//{ +// EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); +// EXPECT_EQ(this->res_.error_code_.val, +// moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); +//} + +/** + * @brief test the case of empty joint names in start state + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyJointNamesInStartState) +{ + this->req_.start_state.joint_state.name.clear(); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief size of joint name and joint position does not match in start state + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, InconsistentStartState) +{ + this->req_.start_state.joint_state.name.push_back("joint_7"); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief joint position out of limit in start state + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, StartPostionOutOfLimit) +{ + this->req_.start_state.joint_state.position[0] = 100; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief Check that no trajectory is generated if a start velocity is given + * + * @note This test is here for regression, however in general generators that + * can work with a given + * start velocity are highly desired. + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, StartPositionVelocityNoneZero) +{ + this->req_.start_state.joint_state.velocity[0] = 100; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief goal constraints is empty + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyGoalConstraints) +{ + this->req_.goal_constraints.clear(); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief multiple goals + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) +{ + moveit_msgs::JointConstraint joint_constraint; + moveit_msgs::PositionConstraint position_constraint; + moveit_msgs::OrientationConstraint orientation_constraint; + moveit_msgs::Constraints goal_constraint; + + // two goal constraints + this->req_.goal_constraints.push_back(goal_constraint); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + + // one joint constraint and one orientation constraint + goal_constraint.joint_constraints.push_back(joint_constraint); + goal_constraint.orientation_constraints.push_back(orientation_constraint); + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + + // one joint constraint and one Cartesian constraint + goal_constraint.position_constraints.push_back(position_constraint); + goal_constraint.orientation_constraints.push_back(orientation_constraint); + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + + // two Cartesian constraints + goal_constraint.joint_constraints.clear(); + goal_constraint.position_constraints.push_back(position_constraint); + goal_constraint.orientation_constraints.push_back(orientation_constraint); + goal_constraint.position_constraints.push_back(position_constraint); + goal_constraint.orientation_constraints.push_back(orientation_constraint); + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief invalid joint name in joint constraint + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointNameInGoal) +{ + moveit_msgs::JointConstraint joint_constraint; + joint_constraint.joint_name = "test_joint_2"; + this->req_.goal_constraints.front().joint_constraints[0] = joint_constraint; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief MissingJointConstraint + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, MissingJointConstraint) +{ + moveit_msgs::JointConstraint joint_constraint; + joint_constraint.joint_name = "test_joint_2"; + this->req_.goal_constraints.front().joint_constraints.pop_back(); //<-- Missing joint constraint + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief invalid joint position in joint constraint + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointPositionInGoal) +{ + this->req_.goal_constraints.front().joint_constraints[0].position = 100; + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief invalid link name in Cartesian goal constraint + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal) +{ + moveit_msgs::PositionConstraint position_constraint; + moveit_msgs::OrientationConstraint orientation_constraint; + moveit_msgs::Constraints goal_constraint; + // link name not set + goal_constraint.position_constraints.push_back(position_constraint); + goal_constraint.orientation_constraints.push_back(orientation_constraint); + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + + // different link names in position and orientation goals + goal_constraint.position_constraints.front().link_name = "test_link_1"; + goal_constraint.orientation_constraints.front().link_name = "test_link_2"; + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + + // no solver for the link + goal_constraint.orientation_constraints.front().link_name = "test_link_1"; + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); +} + +/** + * @brief no pose set in position constraint + */ +TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyPrimitivePoses) +{ + moveit_msgs::PositionConstraint position_constraint; + moveit_msgs::OrientationConstraint orientation_constraint; + moveit_msgs::Constraints goal_constraint; + position_constraint.link_name = + this->robot_model_->getJointModelGroup(this->planning_group_)->getLinkModelNames().back(); + orientation_constraint.link_name = position_constraint.link_name; + + goal_constraint.position_constraints.push_back(position_constraint); + goal_constraint.orientation_constraints.push_back(orientation_constraint); + this->req_.goal_constraints.clear(); + this->req_.goal_constraints.push_back(goal_constraint); + EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_)); + EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_trajectory_generator_common"); + // ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.test new file mode 100644 index 0000000000..702c2dc5fc --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.test @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp new file mode 100644 index 0000000000..f5724b7af5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp @@ -0,0 +1,470 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include + +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/trajectory_generator_lin.h" +#include "pilz_industrial_motion_planner_testutils/command_types_typedef.h" +#include "pilz_industrial_motion_planner_testutils/xml_testdata_loader.h" +#include "test_utils.h" + +#include +#include +#include +#include +#include +#include + +#include + +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string TEST_DATA_FILE_NAME("testdata_file_name"); +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string TARGET_LINK_HCD("target_link_hand_computed_data"); +const std::string RANDOM_TEST_TRIAL_NUM("random_trial_number"); +const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance"); +const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance"); +const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance"); +const std::string ROTATION_AXIS_NORM_TOLERANCE("rot_axis_norm_tolerance"); +const std::string VELOCITY_SCALING_FACTOR("velocity_scaling_factor"); +const std::string OTHER_TOLERANCE("other_tolerance"); + +using namespace pilz_industrial_motion_planner; +using namespace pilz_industrial_motion_planner_testutils; + +/** + * @brief Parameterized unittest of trajectory generator LIN to enable tests + * against + * different robot models.The parameter is the name of robot model parameter on + * the + * ros parameter server. + */ +class TrajectoryGeneratorLINTest : public testing::TestWithParam +{ +protected: + /** + * @brief Create test scenario for lin trajectory generator + * + */ + void SetUp() override; + + bool checkLinResponse(const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanResponse& res); + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + + // lin trajectory generator using model without gripper + std::unique_ptr lin_; + // test data provider + std::unique_ptr tdp_; + + // test parameters from parameter server + std::string planning_group_, target_link_hcd_, test_data_file_name_; + int random_trial_num_; + double joint_position_tolerance_, joint_velocity_tolerance_, pose_norm_tolerance_, rot_axis_norm_tolerance_, + velocity_scaling_factor_, other_tolerance_; + LimitsContainer planner_limits_; +}; + +void TrajectoryGeneratorLINTest::SetUp() +{ + // get the parameters + ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_)); + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(TARGET_LINK_HCD, target_link_hcd_)); + ASSERT_TRUE(ph_.getParam(RANDOM_TEST_TRIAL_NUM, random_trial_num_)); + ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_)); + ASSERT_TRUE(ph_.getParam(JOINT_VELOCITY_TOLERANCE, joint_velocity_tolerance_)); + ASSERT_TRUE(ph_.getParam(POSE_TRANSFORM_MATRIX_NORM_TOLERANCE, pose_norm_tolerance_)); + ASSERT_TRUE(ph_.getParam(ROTATION_AXIS_NORM_TOLERANCE, rot_axis_norm_tolerance_)); + ASSERT_TRUE(ph_.getParam(VELOCITY_SCALING_FACTOR, velocity_scaling_factor_)); + ASSERT_TRUE(ph_.getParam(OTHER_TOLERANCE, other_tolerance_)); + + testutils::checkRobotModel(robot_model_, planning_group_, target_link_hcd_); + + // load the test data provider + tdp_.reset(new pilz_industrial_motion_planner_testutils::XmlTestdataLoader{ test_data_file_name_ }); + ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider."; + + tdp_->setRobotModel(robot_model_); + + // create the limits container + // TODO, move this also into test data set + pilz_industrial_motion_planner::JointLimitsContainer joint_limits = + pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(ph_, + robot_model_->getActiveJointModels()); + CartesianLimit cart_limits; + cart_limits.setMaxRotationalVelocity(0.5 * M_PI); + cart_limits.setMaxTranslationalAcceleration(2); + cart_limits.setMaxTranslationalDeceleration(2); + cart_limits.setMaxTranslationalVelocity(1); + planner_limits_.setJointLimits(joint_limits); + planner_limits_.setCartesianLimits(cart_limits); + + // initialize the LIN trajectory generator + lin_.reset(new TrajectoryGeneratorLIN(robot_model_, planner_limits_)); + ASSERT_NE(nullptr, lin_) << "Failed to create LIN trajectory generator."; +} + +bool TrajectoryGeneratorLINTest::checkLinResponse(const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanResponse& res) +{ + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + if (!testutils::isGoalReached(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_)) + { + return false; + } + + if (!testutils::checkCartesianLinearity(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_, + rot_axis_norm_tolerance_)) + { + return false; + } + + if (!testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer())) + { + return false; + } + + return true; +} + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + */ +TEST(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping) +{ + { + std::shared_ptr ltcf_ex{ new LinTrajectoryConversionFailure("") }; + EXPECT_EQ(ltcf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + } + + { + std::shared_ptr jnm_ex{ new JointNumberMismatch("") }; + EXPECT_EQ(jnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + } + + { + std::shared_ptr ljmiss_ex{ new LinJointMissingInStartState("") }; + EXPECT_EQ(ljmiss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); + } + + { + std::shared_ptr lifgi_ex{ new LinInverseForGoalIncalculable("") }; + EXPECT_EQ(lifgi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + } +} + +// Instantiate the test cases for robot model with and without gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorLINTest, + ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); + +/** + * @brief test the lin planner with invalid motion plan request which has non + * zero start velocity + */ +TEST_P(TrajectoryGeneratorLINTest, nonZeroStartVelocity) +{ + planning_interface::MotionPlanRequest req{ tdp_->getLinJoint("lin2").toRequest() }; + + // add non-zero velocity in the start state + req.start_state.joint_state.velocity.push_back(1.0); + + // try to generate the result + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(lin_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief test the lin planner with joint space goal + */ +TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoal) +{ + planning_interface::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + + // generate the LIN trajectory + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(lin_->generate(lin_joint_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + + // check the resulted trajectory + EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); +} + +/** + * @brief test the lin planner with joint space goal with start velocity almost + * zero + */ +TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity) +{ + planning_interface::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + + // Set velocity near zero + lin_joint_req.start_state.joint_state.velocity = + std::vector(lin_joint_req.start_state.joint_state.position.size(), 1e-16); + + // generate the LIN trajectory + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(lin_->generate(lin_joint_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + + // check the resulted trajectory + EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); +} + +/** + * @brief test the lin planner with Cartesian goal + */ +TEST_P(TrajectoryGeneratorLINTest, cartesianSpaceGoal) +{ + // construct motion plan request + moveit_msgs::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; + + // generate lin trajectory + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(lin_->generate(lin_cart_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + + // check the resulted trajectory + EXPECT_TRUE(checkLinResponse(lin_cart_req, res)); +} + +/** + * @brief test the trapezoid shape of the planning trajectory in Cartesian space + * + * The test checks translational path for a trapezoid velocity profile. + * Due to the way the acceleration is calculated 1 or 2 intermediate points + * occur that are neither + * acceleration, constant or deceleration. + */ +TEST_P(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile) +{ + // construct motion plan request + moveit_msgs::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + + /// +++++++++++++++++++++++ + /// + plan LIN trajectory + + /// +++++++++++++++++++++++ + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(lin_->generate(lin_joint_req, res, 0.01)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + + ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_hcd_)); + + // check last point for vel=acc=0 + for (size_t idx = 0; idx < res.trajectory_->getLastWayPointPtr()->getVariableCount(); ++idx) + { + EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_); + EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_); + } +} + +/** + * @brief Check that lin planner returns 'false' if + * calculated lin trajectory violates velocity/acceleration or deceleration + * limits. + * + * + * Test Sequence: + * 1. Call function with lin request violating velocity/acceleration or + * deceleration limits. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryGeneratorLINTest, LinPlannerLimitViolation) +{ + LinJoint lin{ tdp_->getLinJoint("lin2") }; + lin.setAccelerationScale(1.01); + + planning_interface::MotionPlanResponse res; + ASSERT_FALSE(lin_->generate(lin.toRequest(), res)); +} + +/** + * @brief test joint linear movement with discontinuities in joint space + * + * This will violate joint velocity/acceleration limits. + * + * Test Sequence: + * 1. Generate lin trajectory which is discontinuous in joint space. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryGeneratorLINTest, LinPlannerDiscontinuousJointTraj) +{ + LinJoint lin{ tdp_->getLinJoint("lin2") }; + // Alter goal joint configuration (represents the same cartesian pose, but + // does not fit together with start config) + lin.getGoalConfiguration().setJoint(1, 1.63); + lin.getGoalConfiguration().setJoint(2, 0.96); + lin.getGoalConfiguration().setJoint(4, -2.48); + lin.setVelocityScale(1.0); + lin.setAccelerationScale(1.0); + + planning_interface::MotionPlanResponse res; + ASSERT_FALSE(lin_->generate(lin.toRequest(), res)); +} + +/** + * @brief test joint linear movement with equal goal and start + * + * Test Sequence: + * 1. Call function with lin request start = goal + * + * Expected Results: + * 1. trajectory generation is successful. + */ +TEST_P(TrajectoryGeneratorLINTest, LinStartEqualsGoal) +{ + // construct motion plan request + moveit_msgs::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + + moveit::core::RobotState start_state(robot_model_); + jointStateToRobotState(lin_joint_req.start_state.joint_state, start_state); + + for (auto& joint_constraint : lin_joint_req.goal_constraints.at(0).joint_constraints) + { + joint_constraint.position = start_state.getVariablePosition(joint_constraint.joint_name); + } + + // generate the LIN trajectory + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(lin_->generate(lin_joint_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + + // check the resulted trajectory + EXPECT_TRUE(checkLinResponse(lin_joint_req, res)); +} + +/** + * @brief Checks that constructor throws an exception if no limits are given. + * + * Test Sequence: + * 1. Call Ctor without set limits. + * + * Expected Results: + * 1. Ctor throws exception. + */ +TEST_P(TrajectoryGeneratorLINTest, CtorNoLimits) +{ + pilz_industrial_motion_planner::LimitsContainer planner_limits; + + EXPECT_THROW(pilz_industrial_motion_planner::TrajectoryGeneratorLIN(robot_model_, planner_limits), + pilz_industrial_motion_planner::TrajectoryGeneratorInvalidLimitsException); +} + +/** + * @brief Checks that generate() function returns 'false' if called with an + * incorrect number of joints. + * + * Test Sequence: + * 1. Call functions with incorrect number of joints. + * + * Expected Results: + * 1. Function returns 'false'. + */ +TEST_P(TrajectoryGeneratorLINTest, IncorrectJointNumber) +{ + // construct motion plan request + moveit_msgs::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + + // Ensure that request consists of an incorrect number of joints. + lin_joint_req.goal_constraints.front().joint_constraints.pop_back(); + + // generate the LIN trajectory + planning_interface::MotionPlanResponse res; + ASSERT_FALSE(lin_->generate(lin_joint_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief test invalid motion plan request with incomplete start state and + * cartesian goal + */ +TEST_P(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState) +{ + // construct motion plan request + moveit_msgs::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; + EXPECT_GT(lin_cart_req.start_state.joint_state.name.size(), 1u); + lin_cart_req.start_state.joint_state.name.resize(1); + lin_cart_req.start_state.joint_state.position.resize(1); // prevent failing check for equal sizes + + // generate lin trajectory + planning_interface::MotionPlanResponse res; + EXPECT_FALSE(lin_->generate(lin_cart_req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE); +} + +/** + * @brief Set a frame id in goal constraint with cartesian goal on both position + * and orientation constraints + */ +TEST_P(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints) +{ + // construct motion plan request + moveit_msgs::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; + + lin_cart_req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame(); + lin_cart_req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame(); + + // generate lin trajectory + planning_interface::MotionPlanResponse res; + ASSERT_TRUE(lin_->generate(lin_cart_req, res)); + EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS); + + // check the resulted trajectory + EXPECT_TRUE(checkLinResponse(lin_cart_req, res)); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_trajectory_generator_lin"); + ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.test new file mode 100644 index 0000000000..9cec117a1d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.test @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp new file mode 100644 index 0000000000..94889460d3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp @@ -0,0 +1,971 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include + +#include "pilz_industrial_motion_planner/joint_limits_aggregator.h" +#include "pilz_industrial_motion_planner/trajectory_generator_ptp.h" +#include "test_utils.h" + +#include +#include +#include +#include + +// parameters for parameterized tests +const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" }; +const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" }; + +// parameters from parameter server +const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); +const std::string PARAM_TARGET_LINK_NAME("target_link"); +const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance"); +const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance"); +const std::string JOINT_ACCELERATION_TOLERANCE("joint_acceleration_tolerance"); +const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance"); + +using namespace pilz_industrial_motion_planner; + +class TrajectoryGeneratorPTPTest : public testing::TestWithParam +{ +protected: + /** + * @brief Create test fixture for ptp trajectory generator + * + */ + void SetUp() override; + + /** + * @brief check the resulted joint trajectory + * @param trajectory + * @param req + * @param joint_limits + * @return + */ + bool checkTrajectory(const trajectory_msgs::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, const JointLimitsContainer& joint_limits); + +protected: + // ros stuff + ros::NodeHandle ph_{ "~" }; + robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() }; + + // trajectory generator + std::unique_ptr ptp_; + + // test parameters from parameter server + LimitsContainer planner_limits_; + std::string planning_group_, target_link_; + double joint_position_tolerance_, joint_velocity_tolerance_, joint_acceleration_tolerance_, pose_norm_tolerance_; +}; + +void TrajectoryGeneratorPTPTest::SetUp() +{ + // get parameters from parameter server + ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_)); + ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_)); + ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_)); + ASSERT_TRUE(ph_.getParam(JOINT_VELOCITY_TOLERANCE, joint_velocity_tolerance_)); + ASSERT_TRUE(ph_.getParam(JOINT_ACCELERATION_TOLERANCE, joint_acceleration_tolerance_)); + ASSERT_TRUE(ph_.getParam(POSE_TRANSFORM_MATRIX_NORM_TOLERANCE, pose_norm_tolerance_)); + + testutils::checkRobotModel(robot_model_, planning_group_, target_link_); + + // create the limits container + JointLimitsContainer joint_limits; + for (const auto& jmg : robot_model_->getJointModelGroups()) + { + std::vector joint_names = jmg->getActiveJointModelNames(); + JointLimit joint_limit; + joint_limit.max_position = 3.124; + joint_limit.min_position = -3.124; + joint_limit.has_velocity_limits = true; + joint_limit.max_velocity = 1; + joint_limit.has_acceleration_limits = true; + joint_limit.max_acceleration = 0.5; + joint_limit.has_deceleration_limits = true; + joint_limit.max_deceleration = -1; + for (const auto& joint_name : joint_names) + { + joint_limits.addLimit(joint_name, joint_limit); + } + } + + // create the trajectory generator + planner_limits_.setJointLimits(joint_limits); + ptp_.reset(new TrajectoryGeneratorPTP(robot_model_, planner_limits_)); + ASSERT_NE(nullptr, ptp_); +} + +bool TrajectoryGeneratorPTPTest::checkTrajectory(const trajectory_msgs::JointTrajectory& trajectory, + const planning_interface::MotionPlanRequest& req, + const JointLimitsContainer& joint_limits) +{ + return (testutils::isTrajectoryConsistent(trajectory) && + testutils::isGoalReached(trajectory, req.goal_constraints.front().joint_constraints, + joint_position_tolerance_, joint_velocity_tolerance_) && + testutils::isPositionBounded(trajectory, joint_limits) && + testutils::isVelocityBounded(trajectory, joint_limits) && + testutils::isAccelerationBounded(trajectory, joint_limits)); +} + +/** + * @brief Checks that each derived MoveItErrorCodeException contains the correct + * error code. + */ +TEST(TrajectoryGeneratorPTPTest, TestExceptionErrorCodeMapping) +{ + { + std::shared_ptr pvpsf_ex{ new PtpVelocityProfileSyncFailed("") }; + EXPECT_EQ(pvpsf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE); + } + + { + std::shared_ptr pnisfgp_ex{ new PtpNoIkSolutionForGoalPose("") }; + EXPECT_EQ(pnisfgp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + } +} + +// Instantiate the test cases for robot model with and without gripper +INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorPTPTest, + ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME)); +/** + * @brief Construct a TrajectoryGeneratorPTP with no limits given + */ +TEST_P(TrajectoryGeneratorPTPTest, noLimits) +{ + LimitsContainer planner_limits; + EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); +} + +/** + * @brief Send an empty request, define res.trajectory_ + * + * - Test Sequence: + * 1. Create request, define a trajectory in the result + * 2. assign at least one joint limit will all required limits + * + * - Expected Results: + * 1. the res.trajectory_ should be cleared (contain no waypoints) + */ +TEST_P(TrajectoryGeneratorPTPTest, emptyRequest) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + + robot_trajectory::RobotTrajectoryPtr trajectory( + new robot_trajectory::RobotTrajectory(this->robot_model_, planning_group_)); + robot_state::RobotState state(this->robot_model_); + trajectory->addPrefixWayPoint(state, 0); + res.trajectory_ = trajectory; + + EXPECT_FALSE(res.trajectory_->empty()); + + EXPECT_FALSE(ptp_->generate(req, res)); + + EXPECT_TRUE(res.trajectory_->empty()); +} + +/** + * @brief Construct a TrajectoryGeneratorPTP with missing velocity limits + */ +TEST_P(TrajectoryGeneratorPTPTest, missingVelocityLimits) +{ + LimitsContainer planner_limits; + + JointLimitsContainer joint_limits; + auto joint_models = robot_model_->getActiveJointModels(); + JointLimit joint_limit; + joint_limit.has_velocity_limits = false; + joint_limit.has_acceleration_limits = true; + joint_limit.max_deceleration = -1; + joint_limit.has_deceleration_limits = true; + for (const auto& joint_model : joint_models) + { + ASSERT_TRUE(joint_limits.addLimit(joint_model->getName(), joint_limit)) + << "Failed to add the limits for joint " << joint_model->getName(); + } + + planner_limits.setJointLimits(joint_limits); + EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); +} + +/** + * @brief Construct a TrajectoryGeneratorPTP missing deceleration limits + */ +TEST_P(TrajectoryGeneratorPTPTest, missingDecelerationimits) +{ + LimitsContainer planner_limits; + + JointLimitsContainer joint_limits; + const auto& joint_models = robot_model_->getActiveJointModels(); + JointLimit joint_limit; + joint_limit.has_velocity_limits = true; + joint_limit.has_acceleration_limits = true; + joint_limit.has_deceleration_limits = false; + for (const auto& joint_model : joint_models) + { + ASSERT_TRUE(joint_limits.addLimit(joint_model->getName(), joint_limit)) + << "Failed to add the limits for joint " << joint_model->getName(); + } + + planner_limits.setJointLimits(joint_limits); + EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException); +} + +/** + * @brief test the constructor when insufficient limits are given + * - Test Sequence: + * 1. assign joint limits without acc and dec + * 2. assign at least one joint limit per group with all required limits + * + * - Expected Results: + * 1. the constructor throws an exception of type + * TrajectoryGeneratorInvalidLimitsException + * 2. the constructor throws no exception + */ +TEST_P(TrajectoryGeneratorPTPTest, testInsufficientLimit) +{ + /**********/ + /* Step 1 */ + /**********/ + const auto& joint_models = robot_model_->getActiveJointModels(); + ASSERT_TRUE(joint_models.size()); + + // joint limit with insufficient limits (no acc/dec limits) + JointLimit insufficient_limit; + insufficient_limit.has_position_limits = true; + insufficient_limit.max_position = 2.5; + insufficient_limit.min_position = -2.5; + insufficient_limit.has_velocity_limits = true; + insufficient_limit.max_velocity = 1.256; + insufficient_limit.has_acceleration_limits = false; + insufficient_limit.has_deceleration_limits = false; + JointLimitsContainer insufficient_joint_limits; + for (const auto& joint_model : joint_models) + { + ASSERT_TRUE(insufficient_joint_limits.addLimit(joint_model->getName(), insufficient_limit)) + << "Failed to add the limits for joint " << joint_model->getName(); + } + LimitsContainer insufficient_planner_limits; + insufficient_planner_limits.setJointLimits(insufficient_joint_limits); + + EXPECT_THROW( + { + std::unique_ptr ptp_error( + new TrajectoryGeneratorPTP(robot_model_, insufficient_planner_limits)); + }, + TrajectoryGeneratorInvalidLimitsException); + + /**********/ + /* Step 2 */ + /**********/ + // joint limit with sufficient limits + JointLimit sufficient_limit; + sufficient_limit.has_position_limits = true; + sufficient_limit.max_position = 2.356; + sufficient_limit.min_position = -2.356; + sufficient_limit.has_velocity_limits = true; + sufficient_limit.max_velocity = 1; + sufficient_limit.has_acceleration_limits = true; + sufficient_limit.max_acceleration = 0.5; + sufficient_limit.has_deceleration_limits = true; + sufficient_limit.max_deceleration = -1; + JointLimitsContainer sufficient_joint_limits; + // fill joint limits container, such that it contains one sufficient limit and + // all others are insufficient + for (const auto& jmg : robot_model_->getJointModelGroups()) + { + const auto& joint_names{ jmg->getActiveJointModelNames() }; + ASSERT_FALSE(joint_names.empty()); + ASSERT_TRUE(sufficient_joint_limits.addLimit(joint_names.front(), sufficient_limit)) + << "Failed to add the limits for joint " << joint_names.front(); + + for (auto it = std::next(joint_names.begin()); it != joint_names.end(); ++it) + { + ASSERT_TRUE(sufficient_joint_limits.addLimit((*it), insufficient_limit)) + << "Failed to add the limits for joint " << (*it); + } + } + LimitsContainer sufficient_planner_limits; + sufficient_planner_limits.setJointLimits(sufficient_joint_limits); + + EXPECT_NO_THROW({ + std::unique_ptr ptp_no_error( + new TrajectoryGeneratorPTP(robot_model_, sufficient_planner_limits)); + }); +} + +/** + * @brief test the ptp trajectory generator of Cartesian space goal + */ +TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoal) +{ + //*************************************** + //*** prepare the motion plan request *** + //*************************************** + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + + // cartesian goal pose + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0.1; + pose.pose.position.y = 0.2; + pose.pose.position.z = 0.65; + pose.pose.orientation.w = 1.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + std::vector tolerance_pose(3, 0.01); + std::vector tolerance_angle(3, 0.01); + moveit_msgs::Constraints pose_goal = + kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle); + req.goal_constraints.push_back(pose_goal); + + //**************************************** + //*** test robot model without gripper *** + //**************************************** + ASSERT_TRUE(ptp_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + if (!res_msg.trajectory.joint_trajectory.points.empty()) + { + EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); + } + else + { + FAIL() << "Received empty trajectory."; + } + + // check goal pose + EXPECT_TRUE(testutils::isGoalReached(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_)); +} + +/** + * @brief Check that missing a link_name in position or orientation constraints + * is detected + */ +TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints) +{ + //*************************************** + //*** prepare the motion plan request *** + //*************************************** + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + + // cartesian goal pose + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0.1; + pose.pose.position.y = 0.2; + pose.pose.position.z = 0.65; + pose.pose.orientation.w = 1.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + std::vector tolerance_pose(3, 0.01); + std::vector tolerance_angle(3, 0.01); + moveit_msgs::Constraints pose_goal = + kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle); + req.goal_constraints.push_back(pose_goal); + + planning_interface::MotionPlanRequest req_no_position_constaint_link_name = req; + req_no_position_constaint_link_name.goal_constraints.front().position_constraints.front().link_name = ""; + ASSERT_FALSE(ptp_->generate(req_no_position_constaint_link_name, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + + planning_interface::MotionPlanRequest req_no_orientation_constaint_link_name = req; + req_no_orientation_constaint_link_name.goal_constraints.front().orientation_constraints.front().link_name = ""; + ASSERT_FALSE(ptp_->generate(req_no_orientation_constaint_link_name, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +} + +/** + * @brief test the ptp trajectory generator of invalid Cartesian space goal + */ +TEST_P(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0.1; + pose.pose.position.y = 0.2; + pose.pose.position.z = 2.5; + pose.pose.orientation.w = 1.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + std::vector tolerance_pose(3, 0.01); + std::vector tolerance_angle(3, 0.01); + moveit_msgs::Constraints pose_goal = + kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle); + req.goal_constraints.push_back(pose_goal); + + ASSERT_FALSE(ptp_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION); + EXPECT_EQ(res.trajectory_, nullptr); +} + +/** + * @brief test the ptp trajectory generator of joint space goal which is close + * enough to the start which does not need + * to plan the trajectory + */ +TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + ASSERT_TRUE(robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().size()) + << "No link exists in the planning group."; + + moveit_msgs::Constraints gc; + moveit_msgs::JointConstraint jc; + jc.joint_name = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().front(); + jc.position = 0.0; + gc.joint_constraints.push_back(jc); + req.goal_constraints.push_back(gc); + + // TODO lin and circ has different settings + ASSERT_TRUE(ptp_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + EXPECT_EQ(1u, res_msg.trajectory.joint_trajectory.points.size()); +} + +/** + * @brief test scaling factor + * with zero start velocity + */ +TEST_P(TrajectoryGeneratorPTPTest, testScalingFactor) +{ + // create ptp generator with different limits + JointLimit joint_limit; + JointLimitsContainer joint_limits; + + // set the joint limits + joint_limit.has_position_limits = true; + joint_limit.max_position = 2.967; + joint_limit.min_position = -2.967; + joint_limit.has_velocity_limits = true; + joint_limit.max_velocity = 2; + joint_limit.has_acceleration_limits = true; + joint_limit.max_acceleration = 1.5; + joint_limit.has_deceleration_limits = true; + joint_limit.max_deceleration = -3; + joint_limits.addLimit("prbt_joint_1", joint_limit); + joint_limit.max_position = 2.530; + joint_limit.min_position = -2.530; + joint_limits.addLimit("prbt_joint_2", joint_limit); + joint_limit.max_position = 2.356; + joint_limit.min_position = -2.356; + joint_limits.addLimit("prbt_joint_3", joint_limit); + joint_limit.max_position = 2.967; + joint_limit.min_position = -2.967; + joint_limits.addLimit("prbt_joint_4", joint_limit); + joint_limit.max_position = 2.967; + joint_limit.min_position = -2.967; + joint_limits.addLimit("prbt_joint_5", joint_limit); + joint_limit.max_position = 3.132; + joint_limit.min_position = -3.132; + joint_limits.addLimit("prbt_joint_6", joint_limit); + // add gripper limit such that generator does not complain about missing limit + joint_limits.addLimit("prbt_gripper_finger_left_joint", joint_limit); + + pilz_industrial_motion_planner::LimitsContainer planner_limits; + planner_limits.setJointLimits(joint_limits); + + // create the generator with new limits + ptp_.reset(new TrajectoryGeneratorPTP(robot_model_, planner_limits)); + + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + req.start_state.joint_state.position[2] = 0.1; + moveit_msgs::Constraints gc; + moveit_msgs::JointConstraint jc; + jc.joint_name = "prbt_joint_1"; + jc.position = 1.5; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_3"; + jc.position = 2.1; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_6"; + jc.position = 3.0; + gc.joint_constraints.push_back(jc); + req.goal_constraints.push_back(gc); + req.max_velocity_scaling_factor = 0.5; + req.max_acceleration_scaling_factor = 1.0 / 3.0; + + ASSERT_TRUE(ptp_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); + + // trajectory duration + EXPECT_NEAR(4.5, res.trajectory_->getWayPointDurationFromStart(res.trajectory_->getWayPointCount()), + joint_acceleration_tolerance_); + + // way point at 1s + int index; + index = testutils::getWayPointIndex(res.trajectory_, 1.0); + // joint_1 + EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(1.0 / 6.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], + joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + // other joints + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[4], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[4], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[4], joint_acceleration_tolerance_); + + // way point at 2s + index = testutils::getWayPointIndex(res.trajectory_, 2.0); + // joint_1 + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(2.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + // other joints + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_); + + // way point at 3s + index = testutils::getWayPointIndex(res.trajectory_, 3.0); + // joint_1 + EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(4.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(2.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + // other joints + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_); + + // way point at 4s + index = testutils::getWayPointIndex(res.trajectory_, 4.0); + // joint_1 + EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(5.75 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], + joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(2.875, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + + // way point at 4.5s + index = testutils::getWayPointIndex(res.trajectory_, 4.5); + // joint_1 + EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(2.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); +} + +/** + * @brief test the ptp trajectory generator of joint space goal + * with (almost) zero start velocity + */ +TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + req.start_state.joint_state.position[2] = 0.1; + + // Set velocity to all 1e-16 + req.start_state.joint_state.velocity = std::vector(req.start_state.joint_state.position.size(), 1e-16); + + moveit_msgs::Constraints gc; + moveit_msgs::JointConstraint jc; + jc.joint_name = "prbt_joint_1"; + jc.position = 1.5; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_3"; + jc.position = 2.1; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_6"; + jc.position = 3.0; + gc.joint_constraints.push_back(jc); + req.goal_constraints.push_back(gc); + + ASSERT_TRUE(ptp_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); + + // trajectory duration + EXPECT_NEAR(4.5, res.trajectory_->getWayPointDurationFromStart(res.trajectory_->getWayPointCount()), + joint_acceleration_tolerance_); + + // way point at 1s + int index; + index = testutils::getWayPointIndex(res.trajectory_, 1.0); + // joint_1 + EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(1.0 / 6.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], + joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + // other joints + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[4], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[4], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[4], joint_acceleration_tolerance_); + + // way point at 2s + index = testutils::getWayPointIndex(res.trajectory_, 2.0); + // joint_1 + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(2.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + // other joints + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_); + + // way point at 3s + index = testutils::getWayPointIndex(res.trajectory_, 3.0); + // joint_1 + EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(4.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(2.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + // other joints + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_); + + // way point at 4s + index = testutils::getWayPointIndex(res.trajectory_, 4.0); + // joint_1 + EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(5.75 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], + joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(2.875, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + + // way point at 4.5s + index = testutils::getWayPointIndex(res.trajectory_, 4.5); + // joint_1 + EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(2.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + + // Check that velocity at the end is all zero + EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().velocities.cbegin(), + res_msg.trajectory.joint_trajectory.points.back().velocities.cend(), + [this](double v) { return std::fabs(v) < this->joint_velocity_tolerance_; })); + + // Check that acceleration at the end is all zero + EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().accelerations.cbegin(), + res_msg.trajectory.joint_trajectory.points.back().accelerations.cend(), + [this](double v) { return std::fabs(v) < this->joint_acceleration_tolerance_; })); +} + +/** + * @brief test the ptp_ trajectory generator of joint space goal + * with zero start velocity + */ +TEST_P(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) +{ + planning_interface::MotionPlanResponse res; + planning_interface::MotionPlanRequest req; + testutils::createDummyRequest(robot_model_, planning_group_, req); + req.start_state.joint_state.position[4] = 0.3; + req.start_state.joint_state.position[2] = 0.11; + + moveit_msgs::Constraints gc; + moveit_msgs::JointConstraint jc; + + jc.joint_name = "prbt_joint_1"; + jc.position = 1.5; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_2"; + jc.position = -1.5; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_3"; + jc.position = 2.11; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_4"; + jc.position = -2.0; + gc.joint_constraints.push_back(jc); + jc.joint_name = "prbt_joint_6"; + jc.position = 3.0; + gc.joint_constraints.push_back(jc); + req.goal_constraints.push_back(gc); + + ASSERT_TRUE(ptp_->generate(req, res)); + EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS); + + moveit_msgs::MotionPlanResponse res_msg; + res.getMessage(res_msg); + EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer())); + + // trajectory duration + EXPECT_NEAR(4.5, res.trajectory_->getWayPointDurationFromStart(res.trajectory_->getWayPointCount()), + joint_position_tolerance_); + + // way point at 0s + // joint_1 + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[0], joint_velocity_tolerance_); + // joint_2 + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[1], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[1], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(0.11, res_msg.trajectory.joint_trajectory.points[0].positions[2], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[2], joint_velocity_tolerance_); + // joint_4 + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[3], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[3], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[5], joint_velocity_tolerance_); + + // way point at 1s + int index; + index = testutils::getWayPointIndex(res.trajectory_, 1.0); + // joint_1 + EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_2 + EXPECT_NEAR(-0.125, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(-0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + EXPECT_NEAR(-0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(1.0 / 6.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], + joint_acceleration_tolerance_); + // joint_4 + EXPECT_NEAR(-1.0 / 6.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(-1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + EXPECT_NEAR(-1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], + joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + + // way point at 2s + index = testutils::getWayPointIndex(res.trajectory_, 2.0); + // joint_1 + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + // joint_2 + EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(2.0 / 3.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + // joint_4 + EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + + // way point at 3s + index = testutils::getWayPointIndex(res.trajectory_, 3.0); + // joint_1 + EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_2 + EXPECT_NEAR(-1, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(4.0 / 3.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], joint_acceleration_tolerance_); + // joint_4 + EXPECT_NEAR(-4.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(2.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + + // way point at 4s + index = testutils::getWayPointIndex(res.trajectory_, 4.0); + // joint_1 + EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_); + // joint_2 + EXPECT_NEAR(-2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(-0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_); + // joint_3 + EXPECT_NEAR(5.75 / 3.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2], + joint_position_tolerance_); + EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], + joint_acceleration_tolerance_); + // joint_4 + EXPECT_NEAR(-5.75 / 3.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(-1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], + joint_acceleration_tolerance_); + // joint_6 + EXPECT_NEAR(2.875, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_); + + // way point at 4.5s + index = testutils::getWayPointIndex(res.trajectory_, 4.5); + // joint_1 + EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_); + // joint_2 + EXPECT_NEAR(-1.5, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_); + // joint_3 + EXPECT_NEAR(2.11, res_msg.trajectory.joint_trajectory.points[index].positions[2], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_); + // joint_4 + EXPECT_NEAR(-2.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_); + // joint_6 + EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_); + + // Check last point + EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_); + + // Check that velocity at the end is all zero + EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().velocities.cbegin(), + res_msg.trajectory.joint_trajectory.points.back().velocities.cend(), + [this](double v) { return std::fabs(v) < this->joint_velocity_tolerance_; })); + + // Check that acceleration at the end is all zero + EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().accelerations.cbegin(), + res_msg.trajectory.joint_trajectory.points.back().accelerations.cend(), + [this](double v) { return std::fabs(v) < this->joint_acceleration_tolerance_; })); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unittest_trajectory_generator_ptp"); + ros::NodeHandle nh; + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.test b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.test new file mode 100644 index 0000000000..86dbff3599 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.test @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_velocity_profile_atrap.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_velocity_profile_atrap.cpp new file mode 100644 index 0000000000..4962ab5301 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_velocity_profile_atrap.cpp @@ -0,0 +1,631 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include "pilz_industrial_motion_planner/velocity_profile_atrap.h" + +// Modultest Level1 of Class VelocityProfileATrap +#define EPSILON 1.0e-10 + +TEST(ATrapTest, Test_SetProfile1) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // can reach the maximal velocity + vp.SetProfile(3, 35); + + EXPECT_NEAR(vp.Duration(), 11.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 4.0, EPSILON); + EXPECT_NEAR(vp.Vel(1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 7.0, EPSILON); + EXPECT_NEAR(vp.Vel(2), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(4.5), 17.0, EPSILON); + EXPECT_NEAR(vp.Vel(4.5), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(4.5), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(7), 27.0, EPSILON); + EXPECT_NEAR(vp.Vel(7), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(7), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(9), 33.0, EPSILON); + EXPECT_NEAR(vp.Vel(9), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(9), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(11), 35.0, EPSILON); + EXPECT_NEAR(vp.Vel(11), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(11), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(12), 35.0, EPSILON); + EXPECT_NEAR(vp.Vel(12), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(12), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_SetProfile2) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(6, 2, 1.5); + + // just arrive the maximal velocity + vp.SetProfile(5, 26); + + EXPECT_NEAR(vp.Duration(), 7.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1.5), 7.25, EPSILON); + EXPECT_NEAR(vp.Vel(1.5), 3.0, EPSILON); + EXPECT_NEAR(vp.Acc(1.5), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 14.0, EPSILON); + EXPECT_NEAR(vp.Vel(3), 6.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(5), 23.0, EPSILON); + EXPECT_NEAR(vp.Vel(5), 3.0, EPSILON); + EXPECT_NEAR(vp.Acc(5), -1.5, EPSILON); + + EXPECT_NEAR(vp.Pos(7), 26.0, EPSILON); + EXPECT_NEAR(vp.Vel(7), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(7), -1.5, EPSILON); + + EXPECT_NEAR(vp.Pos(8), 26.0, EPSILON); + EXPECT_NEAR(vp.Vel(8), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(8), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_SetProfile3) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(6, 2, 1); + + // cannot reach the maximal velocity + vp.SetProfile(5, 17); + + EXPECT_NEAR(vp.Duration(), 6.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 6.0, EPSILON); + EXPECT_NEAR(vp.Vel(1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 9.0, EPSILON); + EXPECT_NEAR(vp.Vel(2), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(4), 15.0, EPSILON); + EXPECT_NEAR(vp.Vel(4), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(4), -1, EPSILON); + + EXPECT_NEAR(vp.Pos(6), 17.0, EPSILON); + EXPECT_NEAR(vp.Vel(6), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(6), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(7), 17.0, EPSILON); + EXPECT_NEAR(vp.Vel(7), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(7), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_SetProfile4) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(6, 2, 1); + + // empty profile + vp.SetProfile(5, 5); + + EXPECT_NEAR(vp.Duration(), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); +} + +/** + * @brief Test Description + * + * Test Sequence: + * 1. Generate two profiles with same specifica + * 2. Set double EPSILON as duration of the first + * 3. Set the resulting duration as the duration of the second trajectory + * + * Expected Results: + * 1. - + * 2. - + * 3. Both profiles should be the same (checked with testpoints in the middle + * of each phase + */ +TEST(ATrapTest, Test_SetProfileToLowDuration) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp1 = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + pilz_industrial_motion_planner::VelocityProfileATrap vp2 = vp1; + + vp1.SetProfileDuration(3, 35, std::numeric_limits::epsilon()); + double fastest_duration = vp1.Duration(); + + vp2.SetProfileDuration(3, 35, fastest_duration); + + EXPECT_TRUE(vp1 == vp2) << "Not equal! Profile 1: \n" << vp1 << "\n Profile 2: " << vp2; +} + +/** + * @brief Define Profile with setProfileAllDurations with to low duration + * + * Test Sequence: + * 1. Define a profile with SetProfile(double, double), this will yield the + * fastest duration + * 2. Try to define a profile with setProfileAllDurations with a faster + * combination of durations + * + * Expected Results: + * 1. + * 2. Both trajectories should be equal + */ +TEST(ATrapTest, Test_setProfileAllDurationsToLowDuration) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp1 = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + pilz_industrial_motion_planner::VelocityProfileATrap vp2 = vp1; + + vp1.SetProfile(3, 35); + double fastest_duration = vp1.Duration(); + + // Trigger Duration()>(3*fastest_duration/4) + vp2.setProfileAllDurations(3, 35, fastest_duration / 4, fastest_duration / 4, fastest_duration / 4); + + EXPECT_TRUE(vp1 == vp2) << "Not equal! Profile 1: \n" << vp1 << "\n Profile 2: " << vp2; +} + +/** + * @brief Define Profile with setProfileStartVelocity with zero velocity + * + * Test Sequence: + * 1. Define a profile with SetProfile(double, double) + * 2. Try to define a profile with setProfileStartVelocity with zero velocity + * + * Expected Results: + * 1. + * 2. Both trajectories should be equal + */ +TEST(ATrapTest, Test_SetProfileZeroStartVelocity) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp1 = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + pilz_industrial_motion_planner::VelocityProfileATrap vp2 = vp1; + + vp1.SetProfile(1, 2); + + vp2.setProfileStartVelocity(1, 2, 0); // <-- Set zero velocity + EXPECT_TRUE(vp1 == vp2) << "Not equal! Profile 1: \n" << vp1 << "\n Profile 2: " << vp2; +} + +TEST(ATrapTest, Test_SetProfileDuration) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // set the duration as twice as the fastest profile + vp.SetProfileDuration(3, 35, 22.0); + + EXPECT_NEAR(vp.Duration(), 22.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 4.0, EPSILON); + EXPECT_NEAR(vp.Vel(2), 1.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), 0.5, EPSILON); + + EXPECT_NEAR(vp.Pos(4), 7.0, EPSILON); + EXPECT_NEAR(vp.Vel(4), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(4), 0.5, EPSILON); + + EXPECT_NEAR(vp.Pos(9), 17.0, EPSILON); + EXPECT_NEAR(vp.Vel(9), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(9), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(14), 27.0, EPSILON); + EXPECT_NEAR(vp.Vel(14), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(14), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(18), 33.0, EPSILON); + EXPECT_NEAR(vp.Vel(18), 1.0, EPSILON); + EXPECT_NEAR(vp.Acc(18), -0.25, EPSILON); + + EXPECT_NEAR(vp.Pos(22), 35.0, EPSILON); + EXPECT_NEAR(vp.Vel(22), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(22), -0.25, EPSILON); + + EXPECT_NEAR(vp.Pos(23), 35.0, EPSILON); + EXPECT_NEAR(vp.Vel(23), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(23), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileAllDurations1) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // set durations + EXPECT_TRUE(vp.setProfileAllDurations(3, 35, 3.0, 4.0, 5.0)); + + EXPECT_NEAR(vp.Duration(), 12.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 3.0 + 8.0 / 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(2), 8.0 / 3.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), 4.0 / 3.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 9.0, EPSILON); + EXPECT_NEAR(vp.Vel(3), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), 4.0 / 3.0, EPSILON); + + EXPECT_NEAR(vp.Pos(5), 17.0, EPSILON); + EXPECT_NEAR(vp.Vel(5), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(5), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(7), 25.0, EPSILON); + EXPECT_NEAR(vp.Vel(7), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(7), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(9), 31.4, EPSILON); + EXPECT_NEAR(vp.Vel(9), 2.4, EPSILON); + EXPECT_NEAR(vp.Acc(9), -0.8, EPSILON); + + EXPECT_NEAR(vp.Pos(12), 35.0, EPSILON); + EXPECT_NEAR(vp.Vel(12), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(12), -0.8, EPSILON); + + EXPECT_NEAR(vp.Pos(13), 35.0, EPSILON); + EXPECT_NEAR(vp.Vel(13), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(13), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileAllDurations2) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // invalid maximal velocity + EXPECT_FALSE(vp.setProfileAllDurations(3, 35, 3.0, 3.0, 5.0)); + // invalid acceleration + EXPECT_FALSE(vp.setProfileAllDurations(3, 35, 1.0, 4.0, 7.0)); + // invalid deceleration + EXPECT_FALSE(vp.setProfileAllDurations(3, 35, 7.0, 4.0, 1.0)); +} + +TEST(ATrapTest, Test_setProfileStartVelocity1) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // invalide cases + EXPECT_FALSE(vp.setProfileStartVelocity(3.0, 5.0, -1.0)); + + // only deceleration + vp.setProfileStartVelocity(3.0, 5.0, 2.0); + + EXPECT_NEAR(vp.Duration(), 2.0, EPSILON); + EXPECT_NEAR(vp.firstPhaseDuration(), 2.0, EPSILON); + EXPECT_NEAR(vp.secondPhaseDuration(), 0.0, EPSILON); + EXPECT_NEAR(vp.thirdPhaseDuration(), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 4.5, EPSILON); + EXPECT_NEAR(vp.Vel(1), 1.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(2), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(3), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileStartVelocity2) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // deceleration, acceleration and deceleration + vp.setProfileStartVelocity(3.0, 4.0, 2.0); + EXPECT_NEAR(vp.Duration(), 2.0 + 3 * sqrt(1.0 / 3.0), EPSILON); + EXPECT_NEAR(vp.firstPhaseDuration(), 2.0, EPSILON); + EXPECT_NEAR(vp.secondPhaseDuration(), sqrt(1.0 / 3.0), EPSILON); + EXPECT_NEAR(vp.thirdPhaseDuration(), 2 * sqrt(1.0 / 3.0), EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 4.5, EPSILON); + EXPECT_NEAR(vp.Vel(1), 1.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 5.0, EPSILON); + EXPECT_NEAR(vp.Vel(2), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2.1), 4.99, EPSILON); + EXPECT_NEAR(vp.Vel(2.1), -0.2, EPSILON); + EXPECT_NEAR(vp.Acc(2.1), -2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2 + sqrt(1.0 / 3.0)), 5.0 - 1.0 / 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(2 + sqrt(1.0 / 3.0)), -2 * sqrt(1.0 / 3.0), EPSILON); + EXPECT_NEAR(vp.Acc(2 + sqrt(1.0 / 3.0)), -2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2 + 3 * sqrt(1.0 / 3.0) - 0.2), 4.02, EPSILON); + EXPECT_NEAR(vp.Vel(2 + 3 * sqrt(1.0 / 3.0) - 0.2), -0.2, EPSILON); + EXPECT_NEAR(vp.Acc(2 + 3 * sqrt(1.0 / 3.0) - 0.2), 1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2 + 3 * sqrt(1.0 / 3.0)), 4.0, EPSILON); + EXPECT_NEAR(vp.Vel(2 + 3 * sqrt(1.0 / 3.0)), 0, EPSILON); + EXPECT_NEAR(vp.Acc(2 + 3 * sqrt(1.0 / 3.0)), 1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(5), 4.0, EPSILON); + EXPECT_NEAR(vp.Vel(5), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(5), 0.0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileStartVelocity3) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // acceleration, deceleration + vp.setProfileStartVelocity(3, 14, 2); + EXPECT_NEAR(vp.Duration(), 5.0, EPSILON); + EXPECT_NEAR(vp.firstPhaseDuration(), 1.0, EPSILON); + EXPECT_NEAR(vp.secondPhaseDuration(), 0.0, EPSILON); + EXPECT_NEAR(vp.thirdPhaseDuration(), 4.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 6.0, EPSILON); + EXPECT_NEAR(vp.Vel(1), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 9.5, EPSILON); + EXPECT_NEAR(vp.Vel(2), 3.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 12.0, EPSILON); + EXPECT_NEAR(vp.Vel(3), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(5), 14.0, EPSILON); + EXPECT_NEAR(vp.Vel(5), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(5), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(6), 14, EPSILON); + EXPECT_NEAR(vp.Vel(6), 0, EPSILON); + EXPECT_NEAR(vp.Acc(6), 0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileStartVelocity4) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // acceleration, constant, deceleration + vp.setProfileStartVelocity(3, 14, 2); + EXPECT_NEAR(vp.Duration(), 5.0, EPSILON); + EXPECT_NEAR(vp.firstPhaseDuration(), 1.0, EPSILON); + EXPECT_NEAR(vp.secondPhaseDuration(), 0.0, EPSILON); + EXPECT_NEAR(vp.thirdPhaseDuration(), 4.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 6.0, EPSILON); + EXPECT_NEAR(vp.Vel(1), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 9.5, EPSILON); + EXPECT_NEAR(vp.Vel(2), 3.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 12.0, EPSILON); + EXPECT_NEAR(vp.Vel(3), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(5), 14.0, EPSILON); + EXPECT_NEAR(vp.Vel(5), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(5), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(6), 14, EPSILON); + EXPECT_NEAR(vp.Vel(6), 0, EPSILON); + EXPECT_NEAR(vp.Acc(6), 0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileStartVelocity5) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // acceleration, constant, deceleration + vp.setProfileStartVelocity(3, 18, 2); + EXPECT_NEAR(vp.Duration(), 6.0, EPSILON); + EXPECT_NEAR(vp.firstPhaseDuration(), 1.0, EPSILON); + EXPECT_NEAR(vp.secondPhaseDuration(), 1.0, EPSILON); + EXPECT_NEAR(vp.thirdPhaseDuration(), 4.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 6.0, EPSILON); + EXPECT_NEAR(vp.Vel(1), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON); + + EXPECT_NEAR(vp.Pos(2), 10, EPSILON); + EXPECT_NEAR(vp.Vel(2), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(2), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 13.5, EPSILON); + EXPECT_NEAR(vp.Vel(3), 3.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(4), 16.0, EPSILON); + EXPECT_NEAR(vp.Vel(4), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(4), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(6), 18.0, EPSILON); + EXPECT_NEAR(vp.Vel(6), 0.0, EPSILON); + EXPECT_NEAR(vp.Acc(6), -1.0, EPSILON); + + EXPECT_NEAR(vp.Pos(7), 18, EPSILON); + EXPECT_NEAR(vp.Vel(7), 0, EPSILON); + EXPECT_NEAR(vp.Acc(7), 0, EPSILON); +} + +TEST(ATrapTest, Test_setProfileStartVelocity6) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1); + + // acceleration, constant, deceleration + vp.setProfileStartVelocity(3, 15, 4); + EXPECT_NEAR(vp.Duration(), 5.0, EPSILON); + EXPECT_NEAR(vp.firstPhaseDuration(), 0.0, EPSILON); + EXPECT_NEAR(vp.secondPhaseDuration(), 1.0, EPSILON); + EXPECT_NEAR(vp.thirdPhaseDuration(), 4.0, EPSILON); + + EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(-1), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON); + EXPECT_NEAR(vp.Vel(0), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(1), 7.0, EPSILON); + EXPECT_NEAR(vp.Vel(1), 4.0, EPSILON); + EXPECT_NEAR(vp.Acc(1), 0.0, EPSILON); + + EXPECT_NEAR(vp.Pos(3), 13, EPSILON); + EXPECT_NEAR(vp.Vel(3), 2.0, EPSILON); + EXPECT_NEAR(vp.Acc(3), -1.0, EPSILON); +} + +/** + * @brief Check that the clone function returns a equal profile + * + * Note: Definitions other than setProfileAllDurations could fail due to numeric + * noise + */ +TEST(ATrapTest, Test_Clone) +{ + pilz_industrial_motion_planner::VelocityProfileATrap vp = + pilz_industrial_motion_planner::VelocityProfileATrap(4, 1, 1); + vp.setProfileAllDurations(0, 10, 10, 10, 10); + pilz_industrial_motion_planner::VelocityProfileATrap* vp_clone = + static_cast(vp.Clone()); + EXPECT_EQ(vp, *vp_clone); + delete vp_clone; +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst new file mode 100644 index 0000000000..69e8a98417 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pilz_industrial_motion_planner_testutils +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* [feature] Add Pilz industrial motion planner (`#1893 `_) +* Contributors: Pilz GmbH and Co. KG, Christian Henkel, Immanuel Martini, Joachim Schleicher, rfeistenauer + diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt new file mode 100644 index 0000000000..3e8b72f629 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 3.1.3) +project(pilz_industrial_motion_planner_testutils) + +## Add support for C++11, supported in ROS Kinetic and newer +add_definitions(-std=c++11) +add_definitions(-Wall) +add_definitions(-Wextra) +add_definitions(-Wno-unused-parameter) +add_definitions(-Werror) + +find_package(catkin REQUIRED COMPONENTS + eigen_conversions + moveit_core + moveit_msgs +) + +find_package(Boost REQUIRED COMPONENTS) + +################ +## Clang tidy ## +################ +if(CATKIN_ENABLE_CLANG_TIDY) + find_program( + CLANG_TIDY_EXE + NAMES "clang-tidy" + DOC "Path to clang-tidy executable" + ) + if(NOT CLANG_TIDY_EXE) + message(FATAL_ERROR "clang-tidy not found.") + else() + message(STATUS "clang-tidy found: ${CLANG_TIDY_EXE}") + set(CMAKE_CXX_CLANG_TIDY "${CLANG_TIDY_EXE}") + endif() +endif() + +################################### +## catkin specific configuration ## +################################### +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS moveit_core moveit_msgs + DEPENDS Boost +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/cartesianconfiguration.cpp + src/jointconfiguration.cpp + src/robotconfiguration.cpp + src/sequence.cpp + src/xml_testdata_loader.cpp +) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS}) + +############# +## Install ## +############# + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/README.md b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/README.md new file mode 100644 index 0000000000..d24acad427 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/README.md @@ -0,0 +1,34 @@ +# Test data provider/loader + +## General information +- Use as little as possible test points (Reason: Reduces maintenance overhead). +- Test points should be defined following the concept shown below. +![TestDataConcept](../../pilz_trajectory_generation/test/test_robots/concept_testdata.png) +- Test points can be defined in joint space or Cartesian space. However, one +test point should not be defined in both spaces (data redundancy) +- If a test point is defined in Cartesian space, then also state the +corresponding seed. +- Store preferably only valid test points and test commands. You can use the +valid test points and test commands to create invalid test points or commands if +needed (Reason: Reduces maintenance overhead). + +## Diagrams/ Data types +- The following diagrams show the main classes which can be loaded from the +test data provider/loader, and the relationship between them. + +### Robot configurations +![RobotConfigurations](diagrams/diag_class_robot_configurations.png) + +### Command types +![Commands](diagrams/diag_class_commands.png) + +### Circ auxiliary types +![AuxiliaryTypes](diagrams/diag_class_circ_auxiliary.png) + +## Usage +The usage of the TestDataLoader is as shown below. +![RobotConfigurations](diagrams/diag_seq_testdataloader_usage.png) + +The idea is that the TestdataLoader returns high level data abstraction classes +which can then directly be used to generate/build the ROS messages needed +for testing. diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.png b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.png new file mode 100644 index 0000000000..a8e633c8a2 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.uxf b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.uxf new file mode 100644 index 0000000000..a1f7b6b415 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.uxf @@ -0,0 +1,133 @@ + + 10 + + UMLClass + + 40 + 100 + 380 + 70 + + CartesianPathConstraintsBuilder +-- ++ to PathConstraints() : moveit_msgs::Constraints + + + + UMLClass + + 410 + 430 + 210 + 40 + + CartesianCenter + + + + UMLClass + + 460 + 90 + 470 + 80 + + template=ConfigType> +CircAuxiliary +{abstract} +-- ++ toPathConstraints(): moveit_msgs::Constraints + + + + UMLClass + + 470 + 280 + 230 + 50 + + template=ConfigType, Builder> +Center + + + + UMLClass + + 730 + 280 + 220 + 50 + + template=ConfigType, Builder> +Interim + + + + Relation + + 730 + 160 + 30 + 150 + + lt=<<. + 10.0;10.0;10.0;130.0 + + + Relation + + 470 + 160 + 40 + 150 + + lt=<<. + 20.0;10.0;17.0;130.0 + + + Relation + + 490 + 320 + 340 + 130 + + lt=<<. +<<bind>> +<ConfigType->CartesianConfiguration> +<Builder->CartesianPathConstraintsBuilder> + 10.0;10.0;10.0;110.0 + + + UMLClass + + 710 + 510 + 210 + 40 + + CartesianInterim + + + + Relation + + 800 + 320 + 340 + 210 + + lt=<<. + + + + + + +<<bind>> +<ConfigType->CartesianConfiguration> +<Builder->CartesianPathConstraintsBuilder> + 10.0;10.0;10.0;190.0 + + diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.png b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.png new file mode 100644 index 0000000000..74900cf338 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.uxf b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.uxf new file mode 100644 index 0000000000..1c57939fb2 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.uxf @@ -0,0 +1,175 @@ + + 10 + + UMLClass + + 530 + 370 + 560 + 150 + + template=StartType, GoalType +BaseCommand +{abstract} +-- ++ setStartConfiguration(...) ++ getStartConfiguration(...) : StartType + ++ setGoalConfiguration(...) ++ getGoalConfiguration(...) : GoalType + + + + + UMLClass + + 910 + 560 + 230 + 70 + + template=StartType, GoalType +Ptp + + + + Relation + + 970 + 510 + 30 + 80 + + lt=<<- + 10.0;10.0;10.0;60.0 + + + Relation + + 710 + 510 + 30 + 80 + + lt=<<- + 10.0;10.0;10.0;60.0 + + + UMLClass + + 650 + 560 + 230 + 70 + + template=StartType, GoalType +Lin + + + + UMLClass + + 530 + 670 + 300 + 70 + + template=StartType, AuxiliaryType, GoalType +Circ + + + + Relation + + 540 + 510 + 30 + 190 + + lt=<<- + 10.0;10.0;10.0;170.0 + + + UMLClass + + 50 + 200 + 400 + 150 + + Sequence +-- ++ add(cmd : MotionCmdUPtr) ++ setAllBlendRadiiToZero() ++ getBlendRadius(index: int) ++ getCmd() : MotionCmd& + ++ toRequest() : moveit_msgs::MotionSequenceRequest + + + + UMLClass + + 590 + 200 + 390 + 120 + + MotionCmd +{abstract} +-- ++ setPlanningGroup(...) ++ setTargetLink(...) ++ setVelocityScale(...) ++ setAccelerationScale(...) + + + + Relation + + 750 + 310 + 30 + 90 + + lt=<<- + 10.0;10.0;10.0;70.0 + + + UMLClass + + 640 + 80 + 260 + 60 + + MPReqConvertible +{abstract} +-- +/+ toRequest(): MotionPlanRequest/ + + + + Relation + + 750 + 130 + 30 + 90 + + lt=<<- + 10.0;10.0;10.0;70.0 + + + Relation + + 440 + 280 + 170 + 40 + + lt=<<<<- +m2=* + 10.0;10.0;150.0;10.0 + + diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.png b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.png new file mode 100644 index 0000000000..4ad0c01e35 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.uxf b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.uxf new file mode 100644 index 0000000000..a6d1c2b94b --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_robot_configurations.uxf @@ -0,0 +1,133 @@ + + 10 + + UMLClass + + 330 + 210 + 280 + 70 + + RobotConfiguration +-- ++ setRobotModel() ++ setGroupName() + + + + UMLClass + + 220 + 340 + 170 + 30 + + JointConfiguration + + + + UMLClass + + 530 + 340 + 170 + 30 + + CartesianConfiguration + + + + Relation + + 310 + 270 + 110 + 90 + + lt=<<- + 90.0;10.0;10.0;70.0 + + + Relation + + 480 + 270 + 140 + 90 + + lt=<<- + 10.0;10.0;120.0;70.0 + + + UMLNote + + 700 + 200 + 210 + 40 + + Defines a configuration of the robot in space + + + + Relation + + 600 + 210 + 120 + 30 + + lt=.. + 10.0;10.0;100.0;10.0 + + + UMLClass + + 20 + 20 + 450 + 80 + + RobotStateMsgConvertible +{abstract} +-- +/+ toMoveitMsgRobotState() : moveit_msgs::RobotState / + + + + UMLClass + + 490 + 20 + 400 + 80 + + GoalConstraintMsgConvertible +{abstract} +-- +/+ toGoalConstraints() : moveit_msgs::Constraints / + + + + Relation + + 470 + 90 + 150 + 140 + + lt=<<- + 130.0;10.0;10.0;120.0 + + + Relation + + 270 + 90 + 160 + 140 + + lt=<<- + 10.0;10.0;140.0;120.0 + + diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.png b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.png new file mode 100644 index 0000000000..4659b8be23 Binary files /dev/null and b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.png differ diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.uxf b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.uxf new file mode 100644 index 0000000000..9707b26ea5 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.uxf @@ -0,0 +1,215 @@ + + // Uncomment the following line to change the fontsize and font: +fontsize=10 +// fontfamily=SansSerif //possible: SansSerif,Serif,Monospaced + + +////////////////////////////////////////////////////////////////////////////////////////////// +// Welcome to UMLet! +// +// Double-click on elements to add them to the diagram, or to copy them +// Edit elements by modifying the text in this panel +// Hold Ctrl to select multiple elements +// Use Ctrl+mouse to select via lasso +// +// Use +/- or Ctrl+mouse wheel to zoom +// Drag a whole relation at its central square icon +// +// Press Ctrl+C to copy the whole diagram to the system clipboard (then just paste it to, eg, Word) +// Edit the files in the "palettes" directory to create your own element palettes +// +// Select "Custom Elements > New..." to create new element types +////////////////////////////////////////////////////////////////////////////////////////////// + + +// This text will be stored with each diagram; use it for notes. + 12 + + UMLGeneric + + 252 + 96 + 168 + 36 + + _:ExampleTestClass_ + + + + Relation + + 324 + 120 + 36 + 408 + + lt=. +layer=-10 + 10.0;10.0;10.0;320.0 + + + UMLGeneric + + 708 + 96 + 144 + 36 + + _:TestDataLoader_ + + + + Relation + + 768 + 120 + 36 + 396 + + lt=. +layer=-10 + 10.0;10.0;10.0;310.0 + + + UMLGeneric + + 324 + 156 + 24 + 312 + + layer=10 +transparency=0 + + + + UMLSpecialState + + 132 + 144 + 24 + 24 + + type=initial + + + + Relation + + 144 + 132 + 204 + 48 + + lt=<- +myAwesomeTest() + 150.0;20.0;10.0;20.0 + + + UMLGeneric + + 768 + 204 + 24 + 84 + + layer=10 +transparency=0 + + + + Relation + + 336 + 180 + 456 + 48 + + lt=<- +getPtpCart() : PtpCart + 360.0;20.0;10.0;20.0 + + + UMLGeneric + + 612 + 360 + 24 + 24 + + layer=10 +transparency=0 + + + + Relation + + 348 + 336 + 288 + 48 + + lt=<- +toRequest() : +moveit_msgs::MotionPlanRequest + 220.0;20.0;10.0;20.0 + + + UMLGeneric + + 576 + 252 + 96 + 36 + + _:PtpCart_ + + + + Relation + + 612 + 276 + 36 + 216 + + lt=. + 10.0;10.0;10.0;160.0 + + + Relation + + 660 + 252 + 132 + 36 + + lt=<- + 10.0;10.0;90.0;10.0 + + + UMLGeneric + + 336 + 324 + 24 + 96 + + layer=100 +transparency=0 + + + + + Relation + + 336 + 276 + 204 + 72 + + lt=<- +doAwesomeThings() + 20.0;40.0;50.0;40.0;50.0;10.0;10.0;10.0 + + diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h new file mode 100644 index 0000000000..4b408e2bd8 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h @@ -0,0 +1,176 @@ +/* + * Copyright (c) 2018 Pilz GmbH & Co. KG + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ASYNC_TEST_H +#define ASYNC_TEST_H + +#include +#include +#include +#include +#include + +#include + +#include + +namespace testing +{ +/** + * @brief Test class that allows the handling of asynchronous test objects + * + * The class provides the two basic functions AsyncTest::barricade and AsyncTest::triggerClearEvent. + * During the test setup gates between the steps with one or more clear events. Allow passing on by calling + * triggerClearEvent after a test. + * + * \e Usage:
+ * Suppose you want to test a function that calls another function asynchronously, like the following example: + * + * \code + * void asyncCall(std::function fun) + * { + * std::thread t(fun); + * t.detach(); + * } + * \endcode + * + * You expect that fun gets called, so your test thread has to wait for the completion, else it would fail. This can be + * achieved via: + * + * \code + * class MyTest : public testing::Test, public testing::AsyncTest + * { + * public: + * MOCK_METHOD0(myMethod, void()); + * }; + * + * TEST_F(MyTest, testCase) + * { + * EXPECT_CALL(*this, myMethod()).Times(1).WillOnce(ACTION_OPEN_BARRIER_VOID("myMethod")); + * const int timeout_ms {100}; + * asyncCall(std::bind(&MyTest::myMethod, this)); + * BARRIER("myMethod", timeout_ms) << "Timed-out waiting for myMethod call."; + * } + * \endcode + */ +class AsyncTest +{ +public: + /** + * @brief Triggeres a clear event. If a call to barricade is currently pending it will unblock as soon as all clear + * events are triggered. Else the event is put on the waitlist. This waitlist is emptied upon a call to barricade. + * + * @param event The event that is triggered + */ + void triggerClearEvent(const std::string& event); + + /** + * @brief Will block until the event given by clear_event is triggered or a timeout is reached. + * Unblocks immediately, if the event was on the waitlist. + * + * @param clear_event Event that allows the test to pass on + * @param timeout_ms Timeout [ms] (optional). + * @return True if the event was triggered, false otherwise. + */ + bool barricade(const std::string& clear_event, const int timeout_ms = -1); + + /** + * @brief Will block until all events given by clear_events are triggered or a timeout is reached. + * Events on the waitlist are taken into account, too. + * + * @param clear_events List of events that allow the test to pass on + * @param timeout_ms Timeout [ms] (optional). + * @return True if all events were triggered, false otherwise. + */ + bool barricade(std::initializer_list clear_events, const int timeout_ms = -1); + +protected: + std::mutex m_; + std::condition_variable cv_; + std::set clear_events_{}; + std::set waitlist_{}; +}; + +// for better readability in tests +#define BARRIER(...) EXPECT_TRUE(barricade(__VA_ARGS__)) +#define BARRIER_FATAL(...) ASSERT_TRUE(barricade(__VA_ARGS__)) + +#define ACTION_OPEN_BARRIER(str) \ + ::testing::InvokeWithoutArgs([this](void) { \ + this->triggerClearEvent(str); \ + return true; \ + }) +#define ACTION_OPEN_BARRIER_VOID(str) ::testing::InvokeWithoutArgs([this](void) { this->triggerClearEvent(str); }) + +inline void AsyncTest::triggerClearEvent(const std::string& event) +{ + std::lock_guard lk(m_); + + if (clear_events_.empty()) + { + ROS_DEBUG_NAMED("Test", "Clearing Barricade[%s]", event.c_str()); + waitlist_.insert(event); + } + else if (clear_events_.erase(event) < 1) + { + ROS_WARN_STREAM("Triggered event " << event << " despite not waiting for it."); + } + cv_.notify_one(); +} + +inline bool AsyncTest::barricade(const std::string& clear_event, const int timeout_ms) +{ + return barricade({ clear_event }, timeout_ms); +} + +inline bool AsyncTest::barricade(std::initializer_list clear_events, const int timeout_ms) +{ + std::unique_lock lk(m_); + + std::stringstream events_stringstream; + for (const auto& event : clear_events) + { + events_stringstream << event << ", "; + } + ROS_DEBUG_NAMED("Test", "Adding Barricade[%s]", events_stringstream.str().c_str()); + + std::copy_if(clear_events.begin(), clear_events.end(), std::inserter(clear_events_, clear_events_.end()), + [this](std::string event) { return this->waitlist_.count(event) == 0; }); + waitlist_.clear(); + + auto end_time_point = std::chrono::system_clock::now() + std::chrono::milliseconds(timeout_ms); + + while (!clear_events_.empty()) + { + if (timeout_ms < 0) + { + cv_.wait(lk); + } + else + { + std::cv_status status = cv_.wait_for(lk, end_time_point - std::chrono::system_clock::now()); + if (status == std::cv_status::timeout) + { + return clear_events_.empty(); + } + } + } + return true; +} + +} // namespace testing + +#endif // ASYNC_TEST_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h new file mode 100644 index 0000000000..8b1e0382ea --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h @@ -0,0 +1,128 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef BASECMD_H +#define BASECMD_H + +#include + +#include "motioncmd.h" + +namespace pilz_industrial_motion_planner_testutils +{ +template +class BaseCmd : public MotionCmd +{ +public: + BaseCmd() : MotionCmd() + { + } + + virtual ~BaseCmd() = default; + +public: + planning_interface::MotionPlanRequest toRequest() const override; + + void setStartConfiguration(StartType start); + void setGoalConfiguration(GoalType goal); + + StartType& getStartConfiguration(); + const StartType& getStartConfiguration() const; + + GoalType& getGoalConfiguration(); + const GoalType& getGoalConfiguration() const; + +private: + virtual std::string getPlannerId() const = 0; + +protected: + GoalType goal_; + StartType start_; +}; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +template +inline void BaseCmd::setStartConfiguration(StartType start) +{ + start_ = start; +} + +template +inline void BaseCmd::setGoalConfiguration(GoalType goal) +{ + goal_ = goal; +} + +template +inline StartType& BaseCmd::getStartConfiguration() +{ + return start_; +} + +template +inline const StartType& BaseCmd::getStartConfiguration() const +{ + return start_; +} + +template +inline GoalType& BaseCmd::getGoalConfiguration() +{ + return goal_; +} + +template +inline const GoalType& BaseCmd::getGoalConfiguration() const +{ + return goal_; +} + +template +planning_interface::MotionPlanRequest BaseCmd::toRequest() const +{ + planning_interface::MotionPlanRequest req; + req.planner_id = getPlannerId(); + req.group_name = this->planning_group_; + + req.max_velocity_scaling_factor = this->vel_scale_; + req.max_acceleration_scaling_factor = this->acc_scale_; + + req.start_state = this->start_.toMoveitMsgsRobotState(); + req.goal_constraints.push_back(this->goal_.toGoalConstraints()); + + return req; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // BASECMD_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h new file mode 100644 index 0000000000..b1c9dbeb53 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h @@ -0,0 +1,188 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef CARTESIANCONFIGURATION_H +#define CARTESIANCONFIGURATION_H + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "robotconfiguration.h" +#include "jointconfiguration.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Class to define a robot configuration in space + * with the help of cartesian coordinates. + */ +class CartesianConfiguration : public RobotConfiguration +{ +public: + CartesianConfiguration(); + + CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector& config); + + CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector& config, + const moveit::core::RobotModelConstPtr& robot_model); + +public: + moveit_msgs::Constraints toGoalConstraints() const override; + moveit_msgs::RobotState toMoveitMsgsRobotState() const override; + + void setLinkName(const std::string& link_name); + const std::string& getLinkName() const; + + void setPose(const geometry_msgs::Pose& pose); + const geometry_msgs::Pose& getPose() const; + geometry_msgs::Pose& getPose(); + + void setSeed(const JointConfiguration& config); + const JointConfiguration& getSeed() const; + //! @brief States if a seed for the cartesian configuration is set. + bool hasSeed() const; + + void setPoseTolerance(const double tol); + const boost::optional getPoseTolerance() const; + + void setAngleTolerance(const double tol); + const boost::optional getAngleTolerance() const; + +private: + static geometry_msgs::Pose toPose(const std::vector& pose); + static geometry_msgs::PoseStamped toStampedPose(const geometry_msgs::Pose& pose); + +private: + std::string link_name_; + geometry_msgs::Pose pose_; + + //! @brief The dimensions of the sphere associated with the target region + //! of the position constraint. + boost::optional tolerance_pose_{ boost::none }; + + //! @brief The value to assign to the absolute tolerances of the + //! orientation constraint. + boost::optional tolerance_angle_{ boost::none }; + + //! @brief The seed for computing the IK solution of the cartesian configuration. + boost::optional seed_{ boost::none }; +}; + +std::ostream& operator<<(std::ostream& /*os*/, const CartesianConfiguration& /*obj*/); + +inline void CartesianConfiguration::setLinkName(const std::string& link_name) +{ + link_name_ = link_name; +} + +inline const std::string& CartesianConfiguration::getLinkName() const +{ + return link_name_; +} + +inline void CartesianConfiguration::setPose(const geometry_msgs::Pose& pose) +{ + pose_ = pose; +} + +inline const geometry_msgs::Pose& CartesianConfiguration::getPose() const +{ + return pose_; +} + +inline geometry_msgs::Pose& CartesianConfiguration::getPose() +{ + return pose_; +} + +inline moveit_msgs::Constraints CartesianConfiguration::toGoalConstraints() const +{ + if (!tolerance_pose_ || !tolerance_angle_) + { + return kinematic_constraints::constructGoalConstraints(link_name_, toStampedPose(pose_)); + } + else + { + return kinematic_constraints::constructGoalConstraints(link_name_, toStampedPose(pose_), tolerance_pose_.value(), + tolerance_angle_.value()); + } +} + +inline void CartesianConfiguration::setSeed(const JointConfiguration& config) +{ + seed_ = config; +} + +inline const JointConfiguration& CartesianConfiguration::getSeed() const +{ + return seed_.value(); +} + +inline bool CartesianConfiguration::hasSeed() const +{ + return seed_.is_initialized(); +} + +inline void CartesianConfiguration::setPoseTolerance(const double tol) +{ + tolerance_pose_ = tol; +} + +inline const boost::optional CartesianConfiguration::getPoseTolerance() const +{ + return tolerance_pose_; +} + +inline void CartesianConfiguration::setAngleTolerance(const double tol) +{ + tolerance_angle_ = tol; +} + +inline const boost::optional CartesianConfiguration::getAngleTolerance() const +{ + return tolerance_angle_; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CARTESIANCONFIGURATION_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h new file mode 100644 index 0000000000..c30ce98360 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef CARTESIANPATHCONSTRAINTSBUILDER_H +#define CARTESIANPATHCONSTRAINTSBUILDER_H + +#include + +#include + +#include "cartesianconfiguration.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Helper class to build moveit_msgs::Constraints from a + * given configuration. + */ +class CartesianPathConstraintsBuilder +{ +public: + CartesianPathConstraintsBuilder& setConstraintName(const std::string& constraint_name); + CartesianPathConstraintsBuilder& setConfiguration(const CartesianConfiguration& configuration); + + moveit_msgs::Constraints toPathConstraints() const; + +private: + std::string constraint_name_; + CartesianConfiguration configuration_; +}; + +inline CartesianPathConstraintsBuilder& +CartesianPathConstraintsBuilder::setConstraintName(const std::string& constraint_name) +{ + constraint_name_ = constraint_name; + return *this; +} + +inline CartesianPathConstraintsBuilder& +CartesianPathConstraintsBuilder::setConfiguration(const CartesianConfiguration& configuration) +{ + configuration_ = configuration; + return *this; +} + +inline moveit_msgs::Constraints CartesianPathConstraintsBuilder::toPathConstraints() const +{ + moveit_msgs::PositionConstraint pos_constraint; + pos_constraint.link_name = configuration_.getLinkName(); + pos_constraint.constraint_region.primitive_poses.push_back(configuration_.getPose()); + + moveit_msgs::Constraints path_constraints; + path_constraints.name = constraint_name_; + path_constraints.position_constraints.push_back(pos_constraint); + return path_constraints; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CARTESIANPATHCONSTRAINTSBUILDER_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.h new file mode 100644 index 0000000000..3e7d7f8a55 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.h @@ -0,0 +1,60 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef CENTERAUXILIARY_H +#define CENTERAUXILIARY_H + +#include "circauxiliary.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Class to define the center point of the circle + * on which the robot is supposed to move via circ command. + */ +template +class Center : public CircAuxiliary +{ +private: + std::string getConstraintName() const override; +}; + +template +std::string Center::getConstraintName() const +{ + return "center"; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CENTERAUXILIARY_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h new file mode 100644 index 0000000000..e9519f2071 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h @@ -0,0 +1,62 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef CHECKS_H +#define CHECKS_H + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +::testing::AssertionResult isAtExpectedPosition(const robot_state::RobotState& expected, + const robot_state::RobotState& actual, const double epsilon, + const std::string& group_name = "") +{ + if (group_name.empty() && expected.distance(actual) <= epsilon) + { + return ::testing::AssertionSuccess(); + } + else if (!group_name.empty() && expected.distance(actual, actual.getJointModelGroup(group_name)) <= epsilon) + { + return ::testing::AssertionSuccess(); + } + std::stringstream msg; + msg << " expected: " << expected << " actual: " << actual; + + return ::testing::AssertionFailure() << msg.str(); +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CENTERAUXILIARY_H \ No newline at end of file diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.h new file mode 100644 index 0000000000..1888cfbffb --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.h @@ -0,0 +1,106 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef CIRC_H +#define CIRC_H + +#include + +#include "basecmd.h" +#include "circauxiliary.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Data class storing all information regarding a Circ command. + */ +template +class Circ : public BaseCmd +{ +public: + Circ() : BaseCmd() + { + } + +public: + void setAuxiliaryConfiguration(AuxiliaryType auxiliary); + AuxiliaryType& getAuxiliaryConfiguration(); + const AuxiliaryType& getAuxiliaryConfiguration() const; + +public: + planning_interface::MotionPlanRequest toRequest() const override; + +private: + std::string getPlannerId() const override; + +private: + AuxiliaryType auxiliary_; +}; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +template +inline void Circ::setAuxiliaryConfiguration(AuxiliaryType auxiliary) +{ + auxiliary_ = auxiliary; +} + +template +inline std::string Circ::getPlannerId() const +{ + return "CIRC"; +} + +template +inline planning_interface::MotionPlanRequest Circ::toRequest() const +{ + planning_interface::MotionPlanRequest req{ BaseCmd::toRequest() }; + req.path_constraints = auxiliary_.toPathConstraints(); + + return req; +} + +template +inline AuxiliaryType& Circ::getAuxiliaryConfiguration() +{ + return auxiliary_; +} + +template +inline const AuxiliaryType& Circ::getAuxiliaryConfiguration() const +{ + return auxiliary_; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CIRC_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.h new file mode 100644 index 0000000000..3fbe7b7b7d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.h @@ -0,0 +1,49 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef CIRC_AUXILIARY_TYPES_H +#define CIRC_AUXILIARY_TYPES_H + +#include "center.h" +#include "interim.h" +#include "cartesianconfiguration.h" +#include "cartesianpathconstraintsbuilder.h" + +namespace pilz_industrial_motion_planner_testutils +{ +using CartesianCenter = Center; +using CartesianInterim = Interim; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CIRC_AUXILIARY_TYPES_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h new file mode 100644 index 0000000000..81958f9b67 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h @@ -0,0 +1,91 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef CIRCAUXILIARY_H +#define CIRCAUXILIARY_H + +#include + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Base class to define an auxiliary point needed to specify + * circ commands. + */ +template +class CircAuxiliary +{ +public: + void setConfiguration(const ConfigType& auxiliary_config); + ConfigType& getConfiguration(); + const ConfigType& getConfiguration() const; + +public: + moveit_msgs::Constraints toPathConstraints() const; + +private: + virtual std::string getConstraintName() const = 0; + +protected: + ConfigType auxiliary_config_; +}; + +template +void CircAuxiliary::setConfiguration(const ConfigType& auxiliary_config) +{ + auxiliary_config_ = auxiliary_config; +} + +template +inline ConfigType& CircAuxiliary::getConfiguration() +{ + return auxiliary_config_; +} + +template +inline const ConfigType& CircAuxiliary::getConfiguration() const +{ + return auxiliary_config_; +} + +template +inline moveit_msgs::Constraints CircAuxiliary::toPathConstraints() const +{ + return BuilderType().setConstraintName(getConstraintName()).setConfiguration(getConfiguration()).toPathConstraints(); +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // CIRCAUXILIARY_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h new file mode 100644 index 0000000000..6bd4356637 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h @@ -0,0 +1,69 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef COMMAND_TYPES_TYPEDEF_H +#define COMMAND_TYPES_TYPEDEF_H + +#include + +#include "ptp.h" +#include "lin.h" +#include "circ.h" +#include "gripper.h" +#include "jointconfiguration.h" +#include "cartesianconfiguration.h" +#include "circ_auxiliary_types.h" + +namespace pilz_industrial_motion_planner_testutils +{ +typedef Ptp PtpJoint; +typedef Ptp PtpJointCart; +typedef Ptp PtpCart; + +typedef Lin LinJoint; +typedef Lin LinJointCart; +typedef Lin LinCart; + +typedef Circ CircCenterCart; +typedef Circ CircInterimCart; + +typedef Circ CircJointCenterCart; +typedef Circ CircJointInterimCart; + +typedef boost::variant + CmdVariant; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // COMMAND_TYPES_TYPEDEF_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.h new file mode 100644 index 0000000000..ddded20bf3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.h @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef DEFAULT_VALUES_H +#define DEFAULT_VALUES_H + +/* + * @brief This file contains all default values needed for testing. + */ +namespace pilz_industrial_motion_planner_testutils +{ +static constexpr double DEFAULT_VEL{ 0.01 }; +static constexpr double DEFAULT_ACC{ 0.01 }; +static constexpr double DEFAULT_BLEND_RADIUS{ 0.01 }; + +static constexpr double DEFAULT_VEL_GRIPPER{ 0.5 }; +static constexpr double DEFAULT_ACC_GRIPPER{ 0.8 }; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // DEFAULT_VALUES_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.h new file mode 100644 index 0000000000..dec173bbc1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.h @@ -0,0 +1,52 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef EXCEPTION_TYPES_H +#define EXCEPTION_TYPES_H + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +class TestDataLoaderReadingException : public std::runtime_error +{ +public: + TestDataLoaderReadingException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // EXCEPTION_TYPES_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h new file mode 100644 index 0000000000..1595570449 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h @@ -0,0 +1,56 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef GOALCONSTRAINTSMSGCONVERTIBLE_H +#define GOALCONSTRAINTSMSGCONVERTIBLE_H + +#include + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Interface class to express that a derived class can be converted + * into a moveit_msgs::Constaints. + */ +class GoalConstraintMsgConvertible +{ +public: + virtual moveit_msgs::Constraints toGoalConstraints() const = 0; +}; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // GOALCONSTRAINTSMSGCONVERTIBLE_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.h new file mode 100644 index 0000000000..b3d0f2fd5a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.h @@ -0,0 +1,48 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef GRIPPER_H +#define GRIPPER_H + +#include "ptp.h" +#include "jointconfiguration.h" + +namespace pilz_industrial_motion_planner_testutils +{ +class Gripper : public Ptp +{ +}; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // GRIPPER_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.h new file mode 100644 index 0000000000..d6822ab43a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.h @@ -0,0 +1,60 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef INTERIMAXILIARY_H +#define INTERIMAXILIARY_H + +#include "circauxiliary.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Class to define a point on the circle on which the robot is supposed + * to move via circ command. + */ +template +class Interim : public CircAuxiliary +{ +private: + std::string getConstraintName() const override; +}; + +template +std::string Interim::getConstraintName() const +{ + return "interim"; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // INTERIMAXILIARY_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h new file mode 100644 index 0000000000..b133d568c2 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h @@ -0,0 +1,145 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef JOINTCONFIGURATION_H +#define JOINTCONFIGURATION_H + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "robotconfiguration.h" + +namespace pilz_industrial_motion_planner_testutils +{ +class JointConfigurationException : public std::runtime_error +{ +public: + JointConfigurationException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; + +using CreateJointNameFunc = std::function; + +/** + * @brief Class to define a robot configuration in space with the help + * of joint values. + */ +class JointConfiguration : public RobotConfiguration +{ +public: + JointConfiguration(); + + JointConfiguration(const std::string& group_name, const std::vector& config, + CreateJointNameFunc&& create_joint_name_func); + + JointConfiguration(const std::string& group_name, const std::vector& config, + const moveit::core::RobotModelConstPtr& robot_model); + +public: + void setJoint(const size_t index, const double value); + double getJoint(const size_t index) const; + const std::vector getJoints() const; + + size_t size() const; + + moveit_msgs::Constraints toGoalConstraints() const override; + moveit_msgs::RobotState toMoveitMsgsRobotState() const override; + + sensor_msgs::JointState toSensorMsg() const; + + robot_state::RobotState toRobotState() const; + + void setCreateJointNameFunc(CreateJointNameFunc create_joint_name_func); + +private: + moveit_msgs::RobotState toMoveitMsgsRobotStateWithoutModel() const; + moveit_msgs::RobotState toMoveitMsgsRobotStateWithModel() const; + + moveit_msgs::Constraints toGoalConstraintsWithoutModel() const; + moveit_msgs::Constraints toGoalConstraintsWithModel() const; + +private: + //! Joint positions + std::vector joints_; + + CreateJointNameFunc create_joint_name_func_; +}; + +std::ostream& operator<<(std::ostream& /*os*/, const JointConfiguration& /*obj*/); + +inline moveit_msgs::Constraints JointConfiguration::toGoalConstraints() const +{ + return robot_model_ ? toGoalConstraintsWithModel() : toGoalConstraintsWithoutModel(); +} + +inline moveit_msgs::RobotState JointConfiguration::toMoveitMsgsRobotState() const +{ + return robot_model_ ? toMoveitMsgsRobotStateWithModel() : toMoveitMsgsRobotStateWithoutModel(); +} + +inline void JointConfiguration::setJoint(const size_t index, const double value) +{ + joints_.at(index) = value; +} + +inline double JointConfiguration::getJoint(const size_t index) const +{ + return joints_.at(index); +} + +inline const std::vector JointConfiguration::getJoints() const +{ + return joints_; +} + +inline size_t JointConfiguration::size() const +{ + return joints_.size(); +} + +inline void JointConfiguration::setCreateJointNameFunc(CreateJointNameFunc create_joint_name_func) +{ + create_joint_name_func_ = std::move(create_joint_name_func); +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // JOINTCONFIGURATION_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.h new file mode 100644 index 0000000000..56e74a51d3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.h @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef LIN_H +#define LIN_H + +#include + +#include "basecmd.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Data class storing all information regarding a linear command. + */ +template +class Lin : public BaseCmd +{ +public: + Lin() : BaseCmd() + { + } + +private: + std::string getPlannerId() const override; +}; + +// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +template +inline std::string Lin::getPlannerId() const +{ + return "LIN"; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // LIN_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.h new file mode 100644 index 0000000000..f54beb0c56 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.h @@ -0,0 +1,93 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef MOTIONCMD_H +#define MOTIONCMD_H + +#include +#include + +#include "motionplanrequestconvertible.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Base class for commands storing all general information of a command. + */ +class MotionCmd : public MotionPlanRequestConvertible +{ +public: + MotionCmd() : MotionPlanRequestConvertible() + { + } + +public: + void setPlanningGroup(const std::string& planning_group); + const std::string& getPlanningGroup() const; + + void setVelocityScale(double velocity_scale); + void setAccelerationScale(double acceleration_scale); + +protected: + std::string planning_group_; + //! Link to which all cartesian poses refer to. + std::string target_link_; + double vel_scale_{ 1.0 }; + double acc_scale_{ 1.0 }; +}; + +inline void MotionCmd::setPlanningGroup(const std::string& planning_group) +{ + planning_group_ = planning_group; +} + +inline const std::string& MotionCmd::getPlanningGroup() const +{ + return planning_group_; +} + +inline void MotionCmd::setVelocityScale(double velocity_scale) +{ + vel_scale_ = velocity_scale; +} + +inline void MotionCmd::setAccelerationScale(double acceleration_scale) +{ + acc_scale_ = acceleration_scale; +} + +using MotionCmdUPtr = std::unique_ptr; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // MOTIONCMD_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.h new file mode 100644 index 0000000000..803fc499fa --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.h @@ -0,0 +1,54 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef MOTIONPLANREQUESTCONVERTIBLE_H +#define MOTIONPLANREQUESTCONVERTIBLE_H + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Interface class to express that a derived class can be converted + * into a planning_interface::MotionPlanRequest. + */ +class MotionPlanRequestConvertible +{ +public: + virtual planning_interface::MotionPlanRequest toRequest() const = 0; +}; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // MOTIONPLANREQUESTCONVERTIBLE_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.h new file mode 100644 index 0000000000..cc7c2ce228 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.h @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef PTP_H +#define PTP_H + +#include + +#include "basecmd.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Data class storing all information regarding a Ptp command. + */ +template +class Ptp : public BaseCmd +{ +public: + Ptp() : BaseCmd() + { + } + +private: + std::string getPlannerId() const override; +}; + +// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +template +inline std::string Ptp::getPlannerId() const +{ + return "PTP"; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // PTP_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.h new file mode 100644 index 0000000000..433146dcfb --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.h @@ -0,0 +1,92 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef ROBOTCONFIGURATION_H +#define ROBOTCONFIGURATION_H + +#include +#include + +#include + +#include "goalconstraintsmsgconvertible.h" +#include "robotstatemsgconvertible.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Class to define robot configuration in space. + */ +class RobotConfiguration : public RobotStateMsgConvertible, public GoalConstraintMsgConvertible +{ +public: + RobotConfiguration(); + + RobotConfiguration(const std::string& group_name); + + RobotConfiguration(const std::string& group_name, const moveit::core::RobotModelConstPtr& robot_model); + +public: + void setRobotModel(moveit::core::RobotModelConstPtr robot_model); + void setGroupName(const std::string& group_name); + std::string getGroupName() const; + void clearModel(); + +protected: + std::string group_name_; + moveit::core::RobotModelConstPtr robot_model_; +}; + +inline void RobotConfiguration::setRobotModel(moveit::core::RobotModelConstPtr robot_model) +{ + robot_model_ = std::move(robot_model); +} + +inline void RobotConfiguration::setGroupName(const std::string& group_name) +{ + group_name_ = group_name; +} + +inline std::string RobotConfiguration::getGroupName() const +{ + return group_name_; +} + +inline void RobotConfiguration::clearModel() +{ + robot_model_ = nullptr; +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // ROBOTCONFIGURATION_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h new file mode 100644 index 0000000000..4b9ba87df4 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h @@ -0,0 +1,54 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef ROBOTSTATEMSGCONVERTIBLE_H +#define ROBOTSTATEMSGCONVERTIBLE_H + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Interface class to express that a derived class can be converted + * into a moveit_msgs::RobotState. + */ +class RobotStateMsgConvertible +{ +public: + virtual moveit_msgs::RobotState toMoveitMsgsRobotState() const = 0; +}; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // ROBOTSTATEMSGCONVERTIBLE_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h new file mode 100644 index 0000000000..e555b8405d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h @@ -0,0 +1,147 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef SEQUENCE_H +#define SEQUENCE_H + +#include +#include +#include +#include + +#include + +#include "command_types_typedef.h" +#include "motioncmd.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Data class storing all information regarding a Sequence command. + */ +class Sequence +{ +public: + /** + * @brief Adds a command to the end of the sequence. + * @param cmd The command which has to be added. + */ + void add(const CmdVariant& cmd, const double blend_radius = 0.); + + /** + * @brief Returns the number of commands. + */ + size_t size() const; + + template + T& getCmd(const size_t index_cmd); + + template + const T& getCmd(const size_t index_cmd) const; + + /** + * @return TRUE if the specified command is of the specified type, + * otherwise FALSE. + */ + template + bool cmdIsOfType(const size_t index_cmd) const; + + /** + * @brief Returns the specific command as base class reference. + * This function allows the user to operate on the sequence without + * having knowledge of the underlying specific command type. + */ + MotionCmd& getCmd(const size_t index_cmd); + + void setAllBlendRadiiToZero(); + void setBlendRadius(const size_t index_cmd, const double blend_radius); + double getBlendRadius(const size_t index_cmd) const; + + /** + * @brief Deletes all commands from index 'start' to index 'end'. + */ + void erase(const size_t start, const size_t end); + + moveit_msgs::MotionSequenceRequest toRequest() const; + +private: + using TCmdRadiiPair = std::pair; + std::vector cmds_; +}; + +inline void Sequence::add(const CmdVariant& cmd, const double blend_radius) +{ + cmds_.emplace_back(cmd, blend_radius); +} + +inline size_t Sequence::size() const +{ + return cmds_.size(); +} + +template +inline T& Sequence::getCmd(const size_t index_cmd) +{ + return boost::get(cmds_.at(index_cmd).first); +} + +template +inline const T& Sequence::getCmd(const size_t index_cmd) const +{ + return boost::get(cmds_.at(index_cmd).first); +} + +inline double Sequence::getBlendRadius(const size_t index_cmd) const +{ + return cmds_.at(index_cmd).second; +} + +inline void Sequence::setBlendRadius(const size_t index_cmd, const double blend_radius) +{ + cmds_.at(index_cmd).second = blend_radius; +} + +inline void Sequence::setAllBlendRadiiToZero() +{ + std::for_each(cmds_.begin(), cmds_.end(), [](TCmdRadiiPair& cmd) { cmd.second = 0.; }); +} + +template +inline bool Sequence::cmdIsOfType(const size_t index_cmd) const +{ + return cmds_.at(index_cmd).first.type() == typeid(T); +} +} // namespace pilz_industrial_motion_planner_testutils + +#endif // SEQUENCE_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h new file mode 100644 index 0000000000..5b2f550bfb --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h @@ -0,0 +1,117 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef TESTDATA_LOADER_H +#define TESTDATA_LOADER_H + +#include +#include + +#include + +#include "jointconfiguration.h" +#include "cartesianconfiguration.h" +#include "command_types_typedef.h" +#include "sequence.h" +#include "gripper.h" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Abstract base class describing the interface to access + * test data like robot poses and robot commands. + */ +class TestdataLoader +{ +public: + TestdataLoader() = default; + + TestdataLoader(moveit::core::RobotModelConstPtr robot_model) : robot_model_(std::move(robot_model)) + { + } + + virtual ~TestdataLoader() = default; + +public: + void setRobotModel(moveit::core::RobotModelConstPtr robot_model); + + virtual JointConfiguration getJoints(const std::string& pos_name, const std::string& group_name) const = 0; + + virtual CartesianConfiguration getPose(const std::string& pos_name, const std::string& group_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual PtpJoint getPtpJoint(const std::string& cmd_name) const = 0; + virtual PtpCart getPtpCart(const std::string& cmd_name) const = 0; + virtual PtpJointCart getPtpJointCart(const std::string& cmd_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual LinJoint getLinJoint(const std::string& cmd_name) const = 0; + virtual LinCart getLinCart(const std::string& cmd_name) const = 0; + virtual LinJointCart getLinJointCart(const std::string& cmd_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual CircCenterCart getCircCartCenterCart(const std::string& cmd_name) const = 0; + virtual CircJointCenterCart getCircJointCenterCart(const std::string& cmd_name) const = 0; + virtual CircInterimCart getCircCartInterimCart(const std::string& cmd_name) const = 0; + virtual CircJointInterimCart getCircJointInterimCart(const std::string& cmd_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual Sequence getSequence(const std::string& cmd_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual Gripper getGripper(const std::string& cmd_name) const = 0; + +protected: + moveit::core::RobotModelConstPtr robot_model_; +}; + +inline void TestdataLoader::setRobotModel(moveit::core::RobotModelConstPtr robot_model) +{ + robot_model_ = std::move(robot_model); +} + +using TestdataLoaderUPtr = std::unique_ptr; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // TESTDATA_LOADER_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.h new file mode 100644 index 0000000000..8c6c4e6914 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.h @@ -0,0 +1,80 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef XML_CONSTANTS_H +#define XML_CONSTANTS_H + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +const std::string EMPTY_STR{}; + +const std::string XML_ATTR_STR{ "" }; +const std::string JOINT_STR{ "joints" }; +const std::string POSE_STR{ "pos" }; +const std::string XYZ_QUAT_STR{ "xyzQuat" }; +const std::string XYZ_EULER_STR{ "xyzEuler" }; +const std::string SEED_STR{ "seed" }; + +const std::string PTP_STR{ "ptp" }; +const std::string LIN_STR{ "lin" }; +const std::string CIRC_STR{ "circ" }; +const std::string BLEND_STR{ "blend" }; +const std::string GRIPPER_STR{ "gripper" }; + +const std::string PLANNING_GROUP_STR{ "planningGroup" }; +const std::string TARGET_LINK_STR{ "targetLink" }; +const std::string START_POS_STR{ "startPos" }; +const std::string END_POS_STR{ "endPos" }; +const std::string INTERMEDIATE_POS_STR{ "intermediatePos" }; +const std::string CENTER_POS_STR{ "centerPos" }; +const std::string VEL_STR{ "vel" }; +const std::string ACC_STR{ "acc" }; + +const std::string POSES_PATH_STR{ "testdata.poses" }; +const std::string PTPS_PATH_STR{ "testdata." + PTP_STR + "s" }; +const std::string LINS_PATH_STR{ "testdata." + LIN_STR + "s" }; +const std::string CIRCS_PATH_STR{ "testdata." + CIRC_STR + "s" }; +const std::string SEQUENCE_PATH_STR{ "testdata.sequences" }; +const std::string GRIPPERS_PATH_STR{ "testdata." + GRIPPER_STR + "s" }; + +const std::string NAME_PATH_STR{ XML_ATTR_STR + ".name" }; +const std::string CMD_TYPE_PATH_STR{ XML_ATTR_STR + ".type" }; +const std::string BLEND_RADIUS_PATH_STR{ XML_ATTR_STR + ".blend_radius" }; +const std::string LINK_NAME_PATH_STR{ XML_ATTR_STR + ".link_name" }; +const std::string GROUP_NAME_PATH_STR{ XML_ATTR_STR + ".group_name" }; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // XML_CONSTANTS_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h new file mode 100644 index 0000000000..c07da6ffb7 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h @@ -0,0 +1,223 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef XML_TESTDATA_LOADER_H +#define XML_TESTDATA_LOADER_H + +#include +#include +#include +#include +#include + +#include + +#include "pilz_industrial_motion_planner_testutils/testdata_loader.h" + +namespace pt = boost::property_tree; +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Implements a test data loader which uses a xml file + * to store the test data. + * + * The Xml-file has the following structure: + * + * + * + * + * + * j1 j2 j3 j4 j5 j6 + * + * x y z wx wy wz w + * s1 s2 s3 s4 s5 s6 + * + * j_gripper + * + * + * + * j1 j2 j3 j4 j5 j6 + * x y z wx wy wz w + * j_gripper + * + * + * + * + * + * MyTestPos1 + * MyTestPos2 + * 0.1 + * 0.2 + * + * + * + * + * + * manipulator + * prbt_tcp + * MyTestPos1 + * MyTestPos2 + * 0.3 + * 0.4 + * + * + * + * + * + * manipulator + * prbt_tcp + * MyTestPos1 + * MyTestPos1 + * MyTestPos2 + * MyTestPos1 + * 0.2 + * 0.5 + * + * + * + * + * + * + * + * + * + * + * + * + */ + +class XmlTestdataLoader : public TestdataLoader +{ +public: + XmlTestdataLoader(const std::string& path_filename); + XmlTestdataLoader(const std::string& path_filename, const moveit::core::RobotModelConstPtr& robot_model); + ~XmlTestdataLoader() override; + +public: + JointConfiguration getJoints(const std::string& pos_name, const std::string& group_name) const override; + + CartesianConfiguration getPose(const std::string& pos_name, const std::string& group_name) const override; + + PtpJoint getPtpJoint(const std::string& cmd_name) const override; + PtpCart getPtpCart(const std::string& cmd_name) const override; + PtpJointCart getPtpJointCart(const std::string& cmd_name) const override; + + LinJoint getLinJoint(const std::string& cmd_name) const override; + LinCart getLinCart(const std::string& cmd_name) const override; + LinJointCart getLinJointCart(const std::string& cmd_name) const override; + + CircCenterCart getCircCartCenterCart(const std::string& cmd_name) const override; + CircInterimCart getCircCartInterimCart(const std::string& cmd_name) const override; + CircJointCenterCart getCircJointCenterCart(const std::string& cmd_name) const override; + CircJointInterimCart getCircJointInterimCart(const std::string& cmd_name) const override; + + Sequence getSequence(const std::string& cmd_name) const override; + + Gripper getGripper(const std::string& cmd_name) const override; + +private: + /** + * @brief Use this function to search for a node (like an pos or cmd) + * with a given name. + * + * @param tree Tree containing the node. + * @param name Name of node to look for. + */ + const pt::ptree::value_type& findNodeWithName(const boost::property_tree::ptree& tree, const std::string& name, + const std::string& key, const std::string& path = "") const; + + /** + * @brief Use this function to search for a cmd-node with a given name. + */ + const pt::ptree::value_type& findCmd(const std::string& cmd_name, const std::string& cmd_path, + const std::string& cmd_key) const; + + CartesianCenter getCartesianCenter(const std::string& cmd_name, const std::string& planning_group) const; + + CartesianInterim getCartesianInterim(const std::string& cmd_name, const std::string& planning_group) const; + +private: + JointConfiguration getJoints(const boost::property_tree::ptree& joints_tree, const std::string& group_name) const; + +private: + /** + * @brief Converts string vector to double vector. + */ + inline static std::vector strVec2doubleVec(std::vector& strVec); + +public: + /** + * @brief Abstract base class providing a GENERIC getter-function signature + * which can be used to load DIFFERENT command types (like Ptp, Lin, etc.) + * from the test data file. + */ + class AbstractCmdGetterAdapter + { + public: + virtual CmdVariant getCmd(const std::string& /*cmd_name*/) const = 0; + virtual ~AbstractCmdGetterAdapter() = default; + }; + +private: + std::string path_filename_; + pt::ptree tree_{}; + + using AbstractCmdGetterUPtr = std::unique_ptr; + + //! Stores the mapping between command type and the getter function + //! which have to be called. + //! + //! Please note: + //! This mapping is only relevant for sequence commands. + std::map cmd_getter_funcs_; + +private: + const pt::ptree::value_type empty_value_type_{}; + const pt::ptree empty_tree_{}; +}; + +std::vector XmlTestdataLoader::strVec2doubleVec(std::vector& strVec) +{ + std::vector vec; + + vec.resize(strVec.size()); + std::transform(strVec.begin(), strVec.end(), vec.begin(), [](const std::string& val) { return std::stod(val); }); + + return vec; +} + +using XmlTestDataLoaderUPtr = std::unique_ptr; +} // namespace pilz_industrial_motion_planner_testutils + +#endif // XML_TESTDATA_LOADER_H diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml new file mode 100644 index 0000000000..e2b1718a89 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml @@ -0,0 +1,29 @@ + + + pilz_industrial_motion_planner_testutils + 1.1.1 + Helper scripts and functionality to test industrial motion generation + + Alexander Gutenkunst + Christian Henkel + Immanuel Martini + Joachim Schleicher + Hagen Slusarek + + BSD + + http://moveit.ros.org + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + eigen_conversions + moveit_core + moveit_msgs + + moveit_core + moveit_msgs + moveit_commander + + catkin + + diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp new file mode 100644 index 0000000000..104d762567 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp @@ -0,0 +1,132 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner_testutils/cartesianconfiguration.h" + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +CartesianConfiguration::CartesianConfiguration() : RobotConfiguration() +{ +} + +CartesianConfiguration::CartesianConfiguration(const std::string& group_name, const std::string& link_name, + const std::vector& config) + : RobotConfiguration(group_name), link_name_(link_name), pose_(toPose(config)) +{ +} + +CartesianConfiguration::CartesianConfiguration(const std::string& group_name, const std::string& link_name, + const std::vector& config, + const moveit::core::RobotModelConstPtr& robot_model) + : RobotConfiguration(group_name, robot_model), link_name_(link_name), pose_(toPose(config)) +{ + if (robot_model && (!robot_model_->hasLinkModel(link_name_))) + { + std::string msg{ "Link \"" }; + msg.append(link_name).append("\" not known to robot model"); + throw std::invalid_argument(msg); + } + + if (robot_model && (!robot_state::RobotState(robot_model_).knowsFrameTransform(link_name_))) + { + std::string msg{ "Tranform of \"" }; + msg.append(link_name).append("\" is unknown"); + throw std::invalid_argument(msg); + } +} + +geometry_msgs::Pose CartesianConfiguration::toPose(const std::vector& pose) +{ + geometry_msgs::Pose pose_msg; + pose_msg.position.x = pose.at(0); + pose_msg.position.y = pose.at(1); + pose_msg.position.z = pose.at(2); + pose_msg.orientation.x = pose.at(3); + pose_msg.orientation.y = pose.at(4); + pose_msg.orientation.z = pose.at(5); + pose_msg.orientation.w = pose.at(6); + + return pose_msg; +} + +geometry_msgs::PoseStamped CartesianConfiguration::toStampedPose(const geometry_msgs::Pose& pose) +{ + geometry_msgs::PoseStamped pose_stamped_msg; + pose_stamped_msg.pose = pose; + return pose_stamped_msg; +} + +moveit_msgs::RobotState CartesianConfiguration::toMoveitMsgsRobotState() const +{ + if (!robot_model_) + { + throw std::runtime_error("No robot model set"); + } + + robot_state::RobotState rstate(robot_model_); + rstate.setToDefaultValues(); + if (hasSeed()) + { + rstate.setJointGroupPositions(group_name_, getSeed().getJoints()); + } + + rstate.update(); + + // set to Cartesian pose + Eigen::Isometry3d start_pose; + tf::poseMsgToEigen(pose_, start_pose); + if (!rstate.setFromIK(rstate.getRobotModel()->getJointModelGroup(group_name_), start_pose, link_name_)) + { + std::ostringstream os; + os << "No solution for ik \n" << start_pose.translation() << "\n" << start_pose.linear(); + throw std::runtime_error(os.str()); + } + + // Conversion to RobotState msg type + moveit_msgs::RobotState robot_state_msg; + moveit::core::robotStateToRobotStateMsg(rstate, robot_state_msg, true); + return robot_state_msg; +} + +std::ostream& operator<<(std::ostream& os, const CartesianConfiguration& obj) +{ + os << "Group name: \"" << obj.getGroupName() << "\""; + os << " | link name: \"" << obj.getLinkName() << "\""; + os << "\n" << obj.getPose(); + return os; +} + +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp new file mode 100644 index 0000000000..4ab2cbb5d9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp @@ -0,0 +1,161 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner_testutils/jointconfiguration.h" + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +JointConfiguration::JointConfiguration() : RobotConfiguration() +{ +} + +JointConfiguration::JointConfiguration(const std::string& group_name, const std::vector& config, + CreateJointNameFunc&& create_joint_name_func) + : RobotConfiguration(group_name), joints_(config), create_joint_name_func_(create_joint_name_func) +{ +} + +JointConfiguration::JointConfiguration(const std::string& group_name, const std::vector& config, + const moveit::core::RobotModelConstPtr& robot_model) + : RobotConfiguration(group_name, robot_model), joints_(config) +{ +} + +moveit_msgs::Constraints JointConfiguration::toGoalConstraintsWithoutModel() const +{ + if (!create_joint_name_func_) + { + throw JointConfigurationException("Create-Joint-Name function not set"); + } + + moveit_msgs::Constraints gc; + + for (size_t i = 0; i < joints_.size(); ++i) + { + moveit_msgs::JointConstraint jc; + jc.joint_name = create_joint_name_func_(i); + jc.position = joints_.at(i); + gc.joint_constraints.push_back(jc); + } + + return gc; +} + +moveit_msgs::Constraints JointConfiguration::toGoalConstraintsWithModel() const +{ + if (!robot_model_) + { + throw JointConfigurationException("No robot model set"); + } + + robot_state::RobotState state(robot_model_); + state.setToDefaultValues(); + state.setJointGroupPositions(group_name_, joints_); + + return kinematic_constraints::constructGoalConstraints(state, state.getRobotModel()->getJointModelGroup(group_name_)); +} + +moveit_msgs::RobotState JointConfiguration::toMoveitMsgsRobotStateWithoutModel() const +{ + if (!create_joint_name_func_) + { + throw JointConfigurationException("Create-Joint-Name function not set"); + } + + moveit_msgs::RobotState robot_state; + for (size_t i = 0; i < joints_.size(); ++i) + { + robot_state.joint_state.name.emplace_back(create_joint_name_func_(i)); + robot_state.joint_state.position.push_back(joints_.at(i)); + } + return robot_state; +} + +robot_state::RobotState JointConfiguration::toRobotState() const +{ + if (!robot_model_) + { + throw JointConfigurationException("No robot model set"); + } + + robot_state::RobotState robot_state(robot_model_); + robot_state.setToDefaultValues(); + robot_state.setJointGroupPositions(group_name_, joints_); + return robot_state; +} + +moveit_msgs::RobotState JointConfiguration::toMoveitMsgsRobotStateWithModel() const +{ + robot_state::RobotState start_state(toRobotState()); + moveit_msgs::RobotState rob_state_msg; + moveit::core::robotStateToRobotStateMsg(start_state, rob_state_msg, false); + return rob_state_msg; +} + +sensor_msgs::JointState JointConfiguration::toSensorMsg() const +{ + if (!create_joint_name_func_) + { + throw JointConfigurationException("Create-Joint-Name function not set"); + } + + sensor_msgs::JointState state; + for (size_t i = 0; i < joints_.size(); ++i) + { + state.name.emplace_back(create_joint_name_func_(i)); + state.position.push_back(joints_.at(i)); + } + return state; +} + +std::ostream& operator<<(std::ostream& os, const JointConfiguration& obj) +{ + const size_t n{ obj.size() }; + os << "JointConfiguration: ["; + for (size_t i = 0; i < n; ++i) + { + os << obj.getJoint(i); + if (i != n - 1) + { + os << ", "; + } + } + os << "]"; + + return os; +} + +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/robotconfiguration.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/robotconfiguration.cpp new file mode 100644 index 0000000000..7de626c3f1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/robotconfiguration.cpp @@ -0,0 +1,62 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner_testutils/robotconfiguration.h" + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +RobotConfiguration::RobotConfiguration() : RobotStateMsgConvertible(), GoalConstraintMsgConvertible() +{ +} + +RobotConfiguration::RobotConfiguration(const std::string& group_name) + : RobotStateMsgConvertible(), GoalConstraintMsgConvertible(), group_name_(group_name) +{ +} + +RobotConfiguration::RobotConfiguration(const std::string& group_name, + const moveit::core::RobotModelConstPtr& robot_model) + : RobotStateMsgConvertible(), GoalConstraintMsgConvertible(), group_name_(group_name), robot_model_(robot_model) +{ + if (robot_model && (!robot_model_->hasJointModelGroup(group_name_))) + { + std::string msg{ "Specified robot model does not contain specified group \"" }; + msg.append(group_name).append("\""); + throw std::invalid_argument(msg); + } +} + +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp new file mode 100644 index 0000000000..b606668385 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp @@ -0,0 +1,120 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner_testutils/sequence.h" + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Visitor transforming the stored command into a MotionPlanRequest. + */ +class ToReqVisitor : public boost::static_visitor +{ +public: + template + planning_interface::MotionPlanRequest operator()(T& cmd) const + { + return cmd.toRequest(); + } +}; + +/** + * @brief Visitor returning not the specific command type but the base type. + */ +class ToBaseVisitor : public boost::static_visitor +{ +public: + template + MotionCmd& operator()(T& cmd) const + { + return cmd; + } +}; + +moveit_msgs::MotionSequenceRequest Sequence::toRequest() const +{ + moveit_msgs::MotionSequenceRequest req; + + std::vector group_names; + for (const auto& cmd : cmds_) + { + moveit_msgs::MotionSequenceItem item; + item.req = boost::apply_visitor(ToReqVisitor(), cmd.first); + + if (std::find(group_names.begin(), group_names.end(), item.req.group_name) != group_names.end()) + { + // Remove start state because only the first request of a group + // is allowed to have a start state in a sequence. + item.req.start_state = moveit_msgs::RobotState(); + } + else + { + group_names.emplace_back(item.req.group_name); + } + + item.blend_radius = cmd.second; + req.items.push_back(item); + } + return req; +} + +void Sequence::erase(const size_t start, const size_t end) +{ + const size_t orig_n{ size() }; + if (start > orig_n || end > orig_n) + { + std::string msg; + msg.append("Parameter start=").append(std::to_string(start)); + msg.append(" and end=").append(std::to_string(end)); + msg.append(" must not be greater then the number of #commands="); + msg.append(std::to_string(size())); + throw std::invalid_argument(msg); + } + cmds_.erase(cmds_.begin() + start, cmds_.begin() + end); + if (end == orig_n) + { + // Make sure last radius is set zero + cmds_.at(size() - 1).second = 0.; + } +} + +MotionCmd& Sequence::getCmd(const size_t index_cmd) +{ + return boost::apply_visitor(ToBaseVisitor(), cmds_.at(index_cmd).first); +} + +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp new file mode 100644 index 0000000000..69054bc45d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp @@ -0,0 +1,555 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "pilz_industrial_motion_planner_testutils/xml_testdata_loader.h" + +#include + +#include +#include +#include + +#include "pilz_industrial_motion_planner_testutils/default_values.h" +#include "pilz_industrial_motion_planner_testutils/exception_types.h" +#include "pilz_industrial_motion_planner_testutils/xml_constants.h" + +namespace pt = boost::property_tree; +namespace pilz_industrial_motion_planner_testutils +{ +class CmdReader +{ +public: + CmdReader(const pt::ptree::value_type& node) : cmd_node_(node) + { + } + +public: + std::string getPlanningGroup() const; + std::string getTargetLink() const; + std::string getStartPoseName() const; + std::string getEndPoseName() const; + + double getVelocityScale() const; + double getAccelerationScale() const; + + CmdReader& setDefaultVelocityScale(double scale); + CmdReader& setDefaultAccelerationScale(double scale); + +private: + const pt::ptree::value_type& cmd_node_; + + double default_velocity_scale_{ DEFAULT_VEL }; + double default_acceleration_scale_{ DEFAULT_ACC }; +}; + +inline std::string CmdReader::getPlanningGroup() const +{ + return cmd_node_.second.get(PLANNING_GROUP_STR); +} + +inline std::string CmdReader::getTargetLink() const +{ + return cmd_node_.second.get(TARGET_LINK_STR); +} + +inline std::string CmdReader::getStartPoseName() const +{ + return cmd_node_.second.get(START_POS_STR); +} + +inline std::string CmdReader::getEndPoseName() const +{ + return cmd_node_.second.get(END_POS_STR); +} + +inline double CmdReader::getVelocityScale() const +{ + return cmd_node_.second.get(VEL_STR, default_velocity_scale_); +} + +inline double CmdReader::getAccelerationScale() const +{ + return cmd_node_.second.get(ACC_STR, default_acceleration_scale_); +} + +inline CmdReader& CmdReader::setDefaultVelocityScale(double scale) +{ + default_velocity_scale_ = scale; + return *this; +} + +inline CmdReader& CmdReader::setDefaultAccelerationScale(double scale) +{ + default_acceleration_scale_ = scale; + return *this; +} + +template +class CmdGetterAdapter : public XmlTestdataLoader::AbstractCmdGetterAdapter +{ +public: + using FuncType = std::function; + + CmdGetterAdapter(FuncType func) : AbstractCmdGetterAdapter(), func_(func) + { + } + +public: + CmdVariant getCmd(const std::string& cmd_name) const override + { + return CmdVariant(func_(cmd_name)); + } + +private: + FuncType func_; +}; + +XmlTestdataLoader::XmlTestdataLoader(const std::string& path_filename) : TestdataLoader(), path_filename_(path_filename) +{ + // Parse the XML into the property tree. + pt::read_xml(path_filename_, tree_, pt::xml_parser::no_comments); + + using std::placeholders::_1; + cmd_getter_funcs_["ptp"] = + AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpJoint, this, _1))); + cmd_getter_funcs_["ptp_joint_cart"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpJointCart, this, _1))); + cmd_getter_funcs_["ptp_cart_cart"] = + AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getPtpCart, this, _1))); + + cmd_getter_funcs_["lin"] = + AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getLinJoint, this, _1))); + cmd_getter_funcs_["lin_cart"] = + AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getLinCart, this, _1))); + + cmd_getter_funcs_["circ_center_cart"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getCircCartCenterCart, this, _1))); + cmd_getter_funcs_["circ_interim_cart"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getCircCartInterimCart, this, _1))); + cmd_getter_funcs_["circ_joint_interim_cart"] = AbstractCmdGetterUPtr( + new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getCircJointInterimCart, this, _1))); + + cmd_getter_funcs_["gripper"] = + AbstractCmdGetterUPtr(new CmdGetterAdapter(std::bind(&XmlTestdataLoader::getGripper, this, _1))); +} + +XmlTestdataLoader::XmlTestdataLoader(const std::string& path_filename, + const moveit::core::RobotModelConstPtr& robot_model) + : XmlTestdataLoader(path_filename) +{ + setRobotModel(robot_model); +} + +XmlTestdataLoader::~XmlTestdataLoader() +{ +} + +const pt::ptree::value_type& XmlTestdataLoader::findNodeWithName(const boost::property_tree::ptree& tree, + const std::string& name, const std::string& key, + const std::string& path) const +{ + std::string path_str{ (path.empty() ? NAME_PATH_STR : path) }; + + // Search for node with given name. + for (const pt::ptree::value_type& val : tree) + { + // Ignore attributes which are always the first element of a tree. + if (val.first == XML_ATTR_STR) + { + continue; + } + + if (val.first != key) + { + continue; + } + + const auto& node{ val.second.get_child(path_str, empty_tree_) }; + if (node == empty_tree_) + { + break; + } + if (node.data() == name) + { + return val; + } + } + + std::string msg; + msg.append("Node of type \"") + .append(key) + .append("\" with ") + .append(path_str) + .append("=\"") + .append(name) + .append("\" " + "not " + "foun" + "d."); + throw TestDataLoaderReadingException(msg); +} + +JointConfiguration XmlTestdataLoader::getJoints(const std::string& pos_name, const std::string& group_name) const +{ + // Search for node with given name. + const auto& poses_tree{ tree_.get_child(POSES_PATH_STR, empty_tree_) }; + if (poses_tree == empty_tree_) + { + throw TestDataLoaderReadingException("No poses found."); + } + return getJoints(findNodeWithName(poses_tree, pos_name, POSE_STR).second, group_name); +} + +JointConfiguration XmlTestdataLoader::getJoints(const boost::property_tree::ptree& joints_tree, + const std::string& group_name) const +{ + // Search joints node with given group_name. + if (joints_tree == empty_tree_) + { + throw TestDataLoaderReadingException("No joints found."); + } + const auto& joint_node{ findNodeWithName(joints_tree, group_name, JOINT_STR, GROUP_NAME_PATH_STR) }; + + std::vector strs; + boost::split(strs, joint_node.second.data(), boost::is_any_of(" ")); + return JointConfiguration(group_name, strVec2doubleVec(strs), robot_model_); +} + +CartesianConfiguration XmlTestdataLoader::getPose(const std::string& pos_name, const std::string& group_name) const +{ + const auto& all_poses_tree{ tree_.get_child(POSES_PATH_STR, empty_tree_) }; + if (all_poses_tree == empty_tree_) + { + throw TestDataLoaderReadingException("No poses found."); + } + const auto& pose_tree{ findNodeWithName(all_poses_tree, pos_name, POSE_STR).second }; + const auto& xyz_quat_tree{ findNodeWithName(pose_tree, group_name, XYZ_QUAT_STR, GROUP_NAME_PATH_STR).second }; + const boost::property_tree::ptree& link_name_attr{ xyz_quat_tree.get_child(LINK_NAME_PATH_STR, empty_tree_) }; + if (link_name_attr == empty_tree_) + { + throw TestDataLoaderReadingException("No link name found."); + } + + // Get rid of things like "\n", etc. + std::string data{ xyz_quat_tree.data() }; + boost::trim(data); + + std::vector pos_ori_str; + boost::split(pos_ori_str, data, boost::is_any_of(" ")); + CartesianConfiguration cart_config{ CartesianConfiguration(group_name, link_name_attr.data(), + strVec2doubleVec(pos_ori_str), robot_model_) }; + + const auto& seeds_tree{ xyz_quat_tree.get_child(SEED_STR, empty_tree_) }; + if (seeds_tree != empty_tree_) + { + cart_config.setSeed(getJoints(seeds_tree, group_name)); + } + return cart_config; +} + +PtpJoint XmlTestdataLoader::getPtpJoint(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, PTPS_PATH_STR, PTP_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + PtpJoint cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +PtpJointCart XmlTestdataLoader::getPtpJointCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, PTPS_PATH_STR, PTP_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + PtpJointCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +PtpCart XmlTestdataLoader::getPtpCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, PTPS_PATH_STR, PTP_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + PtpCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getPose(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +LinJoint XmlTestdataLoader::getLinJoint(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, LINS_PATH_STR, LIN_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + LinJoint cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +LinCart XmlTestdataLoader::getLinCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, LINS_PATH_STR, LIN_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + LinCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getPose(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +LinJointCart XmlTestdataLoader::getLinJointCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, LINS_PATH_STR, LIN_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + LinJointCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +const pt::ptree::value_type& XmlTestdataLoader::findCmd(const std::string& cmd_name, const std::string& cmd_path, + const std::string& cmd_key) const +{ + // Search for node with given name. + const boost::property_tree::ptree& cmds_tree{ tree_.get_child(cmd_path, empty_tree_) }; + if (cmds_tree == empty_tree_) + { + throw TestDataLoaderReadingException("No list of commands of type \"" + cmd_key + "\" found"); + } + + return findNodeWithName(cmds_tree, cmd_name, cmd_key); +} + +CartesianCenter XmlTestdataLoader::getCartesianCenter(const std::string& cmd_name, + const std::string& planning_group) const +{ + const pt::ptree::value_type& cmd_node{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) }; + std::string aux_pos_name; + try + { + aux_pos_name = cmd_node.second.get(CENTER_POS_STR); + } + catch (...) + { + throw TestDataLoaderReadingException("Did not find center of circ"); + } + + CartesianCenter aux; + aux.setConfiguration(getPose(aux_pos_name, planning_group)); + return aux; +} + +CartesianInterim XmlTestdataLoader::getCartesianInterim(const std::string& cmd_name, + const std::string& planning_group) const +{ + const pt::ptree::value_type& cmd_node{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) }; + std::string aux_pos_name; + try + { + aux_pos_name = cmd_node.second.get(INTERMEDIATE_POS_STR); + } + catch (...) + { + throw TestDataLoaderReadingException("Did not find interim of circ"); + } + + CartesianInterim aux; + aux.setConfiguration(getPose(aux_pos_name, planning_group)); + return aux; +} + +CircCenterCart XmlTestdataLoader::getCircCartCenterCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + CircCenterCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getPose(cmd_reader.getStartPoseName(), planning_group)); + cmd.setAuxiliaryConfiguration(getCartesianCenter(cmd_name, planning_group)); + cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +CircInterimCart XmlTestdataLoader::getCircCartInterimCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + CircInterimCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getPose(cmd_reader.getStartPoseName(), planning_group)); + cmd.setAuxiliaryConfiguration(getCartesianInterim(cmd_name, planning_group)); + cmd.setGoalConfiguration(getPose(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +CircJointInterimCart XmlTestdataLoader::getCircJointInterimCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + CircJointInterimCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setAuxiliaryConfiguration(getCartesianInterim(cmd_name, planning_group)); + cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +CircJointCenterCart XmlTestdataLoader::getCircJointCenterCart(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, CIRCS_PATH_STR, CIRC_STR) }; + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + CircJointCenterCart cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setAuxiliaryConfiguration(getCartesianCenter(cmd_name, planning_group)); + cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +Sequence XmlTestdataLoader::getSequence(const std::string& cmd_name) const +{ + Sequence seq; + + // Find sequence with given name and loop over all its cmds + const auto& sequence_cmd_tree{ findCmd(cmd_name, SEQUENCE_PATH_STR, BLEND_STR).second }; + for (const pt::ptree::value_type& seq_cmd : sequence_cmd_tree) + { + // Ignore attributes which are always the first element of a tree. + if (seq_cmd.first == XML_ATTR_STR) + { + continue; + } + + // Get name of blend cmd. + const boost::property_tree::ptree& cmd_name_attr = seq_cmd.second.get_child(NAME_PATH_STR, empty_tree_); + if (cmd_name_attr == empty_tree_) + { + throw TestDataLoaderReadingException("Did not find name of sequence cmd"); + } + + const std::string& cmd_name{ cmd_name_attr.data() }; + + // Get type of blend cmd + const boost::property_tree::ptree& type_name_attr{ seq_cmd.second.get_child(CMD_TYPE_PATH_STR, empty_tree_) }; + if (type_name_attr == empty_tree_) + { + throw TestDataLoaderReadingException("Did not find type of sequence cmd \"" + cmd_name + "\""); + } + const std::string& cmd_type{ type_name_attr.data() }; + + // Get blend radius of blend cmd. + double blend_radius{ seq_cmd.second.get(BLEND_RADIUS_PATH_STR, DEFAULT_BLEND_RADIUS) }; + + // Read current command from test data + Add command to sequence + seq.add(cmd_getter_funcs_.at(cmd_type)->getCmd(cmd_name), blend_radius); + } + + return seq; +} + +Gripper XmlTestdataLoader::getGripper(const std::string& cmd_name) const +{ + CmdReader cmd_reader{ findCmd(cmd_name, GRIPPERS_PATH_STR, GRIPPER_STR) }; + cmd_reader.setDefaultVelocityScale(DEFAULT_VEL_GRIPPER); + cmd_reader.setDefaultAccelerationScale(DEFAULT_ACC_GRIPPER); + std::string planning_group{ cmd_reader.getPlanningGroup() }; + + Gripper cmd; + cmd.setPlanningGroup(planning_group); + cmd.setVelocityScale(cmd_reader.getVelocityScale()); + cmd.setAccelerationScale(cmd_reader.getAccelerationScale()); + + cmd.setStartConfiguration(getJoints(cmd_reader.getStartPoseName(), planning_group)); + cmd.setGoalConfiguration(getJoints(cmd_reader.getEndPoseName(), planning_group)); + + return cmd; +} + +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/sbpl/core/sbpl_interface/CMakeLists.txt b/moveit_planners/sbpl/core/sbpl_interface/CMakeLists.txt index 2b0568a402..1b436f0143 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/CMakeLists.txt +++ b/moveit_planners/sbpl/core/sbpl_interface/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.4.6) +cmake_minimum_required(VERSION 3.1.3) include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) # Set the build type. Options are: @@ -13,7 +13,7 @@ rosbuild_init() find_package(PkgConfig REQUIRED) pkg_check_modules(SBPL REQUIRED sbpl) -include_directories(${SBPL_INCLUDE_DIRS}) +include_directories(SYSTEM ${SBPL_INCLUDE_DIRS}) link_directories(${SBPL_LIBRARY_DIRS}) #set the default path for built executables to the "bin" directory diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h index 8b60176b72..d37da370c1 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bfs3d/BFS_3D.h @@ -34,8 +34,7 @@ /** \Author: Benjamin Cohen /bcohen@willowgarage.com, E. Gil Jones **/ -#ifndef _SBPL_BFS_3D_H_ -#define _SBPL_BFS_3D_H_ +#pragma once #include @@ -76,6 +75,4 @@ class BFS_3D int getDistance(int, int, int); }; -} - -#endif +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bresenham.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bresenham.h index 748bddf830..4f8f7847b9 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bresenham.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/bresenham.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef _BRESENHAM3D_ -#define _BRESENHAM3D_ +#pragma once #include #include @@ -56,5 +55,3 @@ void get_bresenham3d_parameters(int p1x, int p1y, int p1z, int p2x, int p2y, int void get_current_point3d(bresenham3d_param_t* params, int* x, int* y, int* z); int get_next_point3d(bresenham3d_param_t* params); - -#endif diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/dummy_environment.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/dummy_environment.h index 3f9c43ba34..8dda73802e 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/dummy_environment.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/dummy_environment.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef _DUMMY_ENVIRONMENT_H_ -#define _DUMMY_ENVIRONMENT_H_ +#pragma once class DummyEnvironment : public DiscreteSpaceInformation { @@ -100,5 +99,3 @@ class DummyEnvironment : public DiscreteSpaceInformation { } }; - -#endif diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h index e33fdec39e..b27e3a7617 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/environment_chain3d.h @@ -34,8 +34,7 @@ /** \Author: Benjamin Cohen /bcohen@willowgarage.com, E. Gil Jones **/ -#ifndef _ENVIRONMENT_CHAIN3D_H_ -#define _ENVIRONMENT_CHAIN3D_H_ +#pragma once #include #include @@ -51,8 +50,7 @@ #include #include #include -#include -#include +#include #include #include @@ -249,8 +247,7 @@ class EnvironmentChain3D : public DiscreteSpaceInformation std::vector > joint_motion_wrappers_; std::vector > possible_actions_; planning_models::RobotState* state_; - const collision_detection::CollisionWorldHybrid* hy_world_; - const collision_detection::CollisionRobotHybrid* hy_robot_; + const collision_detection::CollisionEnvHybrid* hy_env_; planning_models::RobotState* ::JointStateGroup* joint_state_group_; boost::shared_ptr gsr_; // boost::shared_ptr kinematics_solver_; @@ -333,6 +330,4 @@ inline void EnvironmentChain3D::convertJointAnglesToCoord(const std::vector #include @@ -317,6 +316,4 @@ class SingleJointMotionPrimitive : public JointMotionPrimitive unsigned int ind_; double delta_; }; -} - -#endif +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_interface.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_interface.h index 753211ccdb..7c07d7fbd4 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_interface.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_interface.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_SBPL_INTERFACE_H_ -#define MOVEIT_SBPL_INTERFACE_H_ +#pragma once #include #include @@ -67,6 +66,4 @@ class SBPLInterface // DummyEnvironment* dummy_env_; // SBPLPlanner *planner_; }; -} - -#endif +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_meta_interface.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_meta_interface.h index 654d41b11d..66a4a4c36c 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_meta_interface.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_meta_interface.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_SBPL_META_INTERFACE_H_ -#define MOVEIT_SBPL_META_INTERFACE_H_ +#pragma once #include #include @@ -76,6 +75,4 @@ class SBPLMetaInterface PlanningStatistics last_planning_statistics_; }; -} - -#endif +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_params.h b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_params.h index 17e309740f..74b0acb25d 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_params.h +++ b/moveit_planners/sbpl/core/sbpl_interface/include/sbpl_interface/sbpl_params.h @@ -32,15 +32,13 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef _SBPL_PARAMS_H_ -#define _SBPL_PARAMS_H_ +#pragma once #include #include #include #include #include -#include #include #include @@ -203,6 +201,4 @@ class SBPLParams std::vector motion_primitive_type_names_; }; -} - -#endif +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp index 9fc8f84eb6..e8b7ab9113 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/BFS_3D.cpp @@ -1,3 +1,37 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Maxim Likhachev + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Maxim Likhachev nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + #include namespace sbpl_interface @@ -111,4 +145,4 @@ int BFS_3D::getDistance(int x, int y, int z) ; return distance_grid[node]; } -} +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/Search.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/Search.cpp index b446977e8f..e485e5bf13 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/Search.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/bfs3d/Search.cpp @@ -1,3 +1,37 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Maxim Likhachev + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Maxim Likhachev nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + #include #include #include @@ -49,4 +83,4 @@ void BFS_3D::search(int width, int planeSize, int volatile* distance_grid, int* // std::cerr << "Search thread done" << std::endl; running = false; } -} +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/bresenham.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/bresenham.cpp index 7d67420b2f..37712556ed 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/bresenham.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/bresenham.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Maxim Likhachev -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Maxim Likhachev nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Maxim Likhachev + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Maxim Likhachev nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Benjamin Cohen */ diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp index 6e84c9ac11..0db1d39420 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Maxim Likhachev -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Maxim Likhachev nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Maxim Likhachev + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Maxim Likhachev nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Benjamin Cohen, E. Gil Jones */ @@ -247,8 +247,7 @@ void EnvironmentChain3D::GetSuccs(int source_state_ID, std::vector* succ_id req.group_name = planning_group_; if (!planning_parameters_.use_standard_collision_checking_) { - hy_world_->checkCollisionDistanceField(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), state_, - gsr_); + hy_env_->checkCollisionDistanceField(req, res, *hy_env_->getCollisionRobotDistanceField().get(), state_, gsr_); } else { @@ -436,18 +435,16 @@ bool EnvironmentChain3D::setupForMotionPlan(const planning_scene::PlanningSceneC if (!planning_parameters_.use_standard_collision_checking_) { - hy_world_ = - dynamic_cast(planning_scene->getCollisionWorld().get()); - if (!hy_world_) + hy_env_ = dynamic_cast(planning_scene->getCollisionEnv().get()); + if (!hy_env_) { ROS_WARN_STREAM("Could not initialize hybrid collision world from planning scene"); mres.error_code.val = moveit_msgs::MoveItErrorCodes::COLLISION_CHECKING_UNAVAILABLE; return false; } - hy_robot_ = - dynamic_cast(planning_scene->getCollisionRobot().get()); - if (!hy_robot_) + hy_env_ = dynamic_cast(planning_scene->getCollisionEnv().get()); + if (!hy_env_) { ROS_WARN_STREAM("Could not initialize hybrid collision robot from planning scene"); mres.error_code.val = moveit_msgs::MoveItErrorCodes::COLLISION_CHECKING_UNAVAILABLE; @@ -473,8 +470,8 @@ bool EnvironmentChain3D::setupForMotionPlan(const planning_scene::PlanningSceneC req.group_name = planning_group_; if (!planning_parameters_.use_standard_collision_checking_) { - hy_world_->checkCollisionDistanceField(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), state_, - planning_scene_->getAllowedCollisionMatrix(), gsr_); + hy_env_->checkCollisionDistanceField(req, res, *hy_env_->getCollisionRobotDistanceField().get(), state_, + planning_scene_->getAllowedCollisionMatrix(), gsr_); } else { @@ -507,7 +504,7 @@ bool EnvironmentChain3D::setupForMotionPlan(const planning_scene::PlanningSceneC gsr_->dfce_->distance_field_->getZNumCells()); boost::shared_ptr world_distance_field = - hy_world_->getCollisionWorldDistanceField()->getDistanceField(); + hy_env_->getCollisionWorldDistanceField()->getDistanceField(); if (world_distance_field->getXNumCells() != gsr_->dfce_->distance_field_->getXNumCells() || world_distance_field->getYNumCells() != gsr_->dfce_->distance_field_->getYNumCells() || world_distance_field->getZNumCells() != gsr_->dfce_->distance_field_->getZNumCells()) @@ -579,8 +576,8 @@ bool EnvironmentChain3D::setupForMotionPlan(const planning_scene::PlanningSceneC goal_state.setStateValues(goal_vals); if (!planning_parameters_.use_standard_collision_checking_) { - hy_world_->checkCollisionDistanceField(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), goal_state, - planning_scene_->getAllowedCollisionMatrix(), gsr_); + hy_env_->checkCollisionDistanceField(req, res, *hy_env_->getCollisionRobotDistanceField().get(), goal_state, + planning_scene_->getAllowedCollisionMatrix(), gsr_); } else { @@ -1101,8 +1098,8 @@ bool EnvironmentChain3D::interpolateAndCollisionCheck(const std::vector collision_detection::CollisionResult res; if (!planning_parameters_.use_standard_collision_checking_) { - hy_world_->checkCollisionDistanceField(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), - interpolation_state_temp_, gsr_); + hy_env_->checkCollisionDistanceField(req, res, *hy_env_->getCollisionRobotDistanceField().get(), + interpolation_state_temp_, gsr_); } else { @@ -1217,4 +1214,4 @@ void EnvironmentChain3D::attemptShortcut(const trajectory_msgs::JointTrajectory& } } } -} +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp index 134e816f99..e82bad8793 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp @@ -117,4 +117,4 @@ bool SBPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_ (const_cast(this))->last_planning_statistics_ = stats; return true; } -} +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp index 31cc629039..e4411ea367 100644 --- a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp +++ b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp @@ -170,4 +170,4 @@ void SBPLMetaInterface::runSolver(bool use_first, const planning_scene::Planning std::cerr << "Interruption requested\n"; } } -} +} // namespace sbpl_interface diff --git a/moveit_planners/sbpl/ros/sbpl_interface_ros/CMakeLists.txt b/moveit_planners/sbpl/ros/sbpl_interface_ros/CMakeLists.txt index 36c26c49b3..edc0cf34bc 100644 --- a/moveit_planners/sbpl/ros/sbpl_interface_ros/CMakeLists.txt +++ b/moveit_planners/sbpl/ros/sbpl_interface_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.4.6) +cmake_minimum_required(VERSION 3.1.3) include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) # Set the build type. Options are: @@ -12,7 +12,7 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) rosbuild_init() find_package(Eigen REQUIRED) -include_directories(${EIGEN_INCLUDE_DIRS}) +include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS}) #set the default path for built executables to the "bin" directory set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) diff --git a/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_meta_plugin.cpp b/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_meta_plugin.cpp index d522429d71..3cd3444134 100644 --- a/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_meta_plugin.cpp +++ b/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_meta_plugin.cpp @@ -106,6 +106,6 @@ class SBPLMetaPlanner : public planning_interface::Planner boost::shared_ptr sbpl_meta_interface_; }; -} // ompl_interface_ros +} // namespace sbpl_interface_ros PLUGINLIB_EXPORT_CLASS(sbpl_interface_ros::SBPLMetaPlanner, planning_interface::Planner); diff --git a/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_plugin.cpp b/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_plugin.cpp index e74d4fcdb4..d7e8de4de3 100644 --- a/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_plugin.cpp +++ b/moveit_planners/sbpl/ros/sbpl_interface_ros/src/sbpl_plugin.cpp @@ -111,6 +111,6 @@ class SBPLPlanner : public planning_interface::Planner boost::shared_ptr sbpl_interface_; }; -} // ompl_interface_ros +} // namespace sbpl_interface_ros PLUGINLIB_EXPORT_CLASS(sbpl_interface_ros::SBPLPlanner, planning_interface::Planner); diff --git a/moveit_planners/trajopt/trajopt/CMakeLists.txt b/moveit_planners/trajopt/trajopt/CMakeLists.txt new file mode 100644 index 0000000000..478a71b07f --- /dev/null +++ b/moveit_planners/trajopt/trajopt/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 2.8.3) +project(trajopt) + +add_compile_options(-std=c++11 -Wall -Wextra) + +find_package(catkin REQUIRED COMPONENTS + moveit_core + trajopt_sco + trajopt_utils + roscpp +) + +find_package(Eigen3 REQUIRED) +find_package(Boost COMPONENTS system python thread program_options REQUIRED) + +find_package(PkgConfig REQUIRED) +pkg_check_modules(JSONCPP jsoncpp) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + moveit_core + trajopt_sco + trajopt_utils + roscpp + DEPENDS + EIGEN3 + Boost +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + SYSTEM ${EIGEN3_INCLUDE_DIRS} + SYSTEM ${Boost_INCLUDE_DIRS} + SYSTEM ${JSONCPP_INCLUDE_DIRS} +) + +set(TRAJOPT_SOURCE_FILES + src/trajectory_costs.cpp + src/kinematic_terms.cpp + src/problem_description.cpp + src/utils.cpp + src/collision_terms.cpp +) + +add_library(${PROJECT_NAME} ${TRAJOPT_SOURCE_FILES}) +target_link_libraries(${PROJECT_NAME} ${Boost_SYSTEM_LIBRARY} ${JSONCPP_LIBRARIES} ${catkin_LIBRARIES}) +target_compile_options(${PROJECT_NAME} PRIVATE -Wsuggest-override -Wconversion -Wsign-conversion) + +# Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" PATTERN "*.hxx" + ) diff --git a/moveit_planners/trajopt/trajopt/LICENSE b/moveit_planners/trajopt/trajopt/LICENSE new file mode 100644 index 0000000000..9103917b0f --- /dev/null +++ b/moveit_planners/trajopt/trajopt/LICENSE @@ -0,0 +1,11 @@ +Copyright (c) 2013, John Schulman +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +http://opensource.org/licenses/BSD-2-Clause \ No newline at end of file diff --git a/moveit_planners/trajopt/trajopt/README.md b/moveit_planners/trajopt/trajopt/README.md new file mode 100644 index 0000000000..1c18708b1f --- /dev/null +++ b/moveit_planners/trajopt/trajopt/README.md @@ -0,0 +1,3 @@ +TODO(ommmid): Remove code duplication. +This package temporarily copied into moveit until the repo trajopt_ros does not depend on Tesseract +See ros-industrial-consortium/trajopt_ros#122 \ No newline at end of file diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/basic_types.h b/moveit_planners/trajopt/trajopt/include/trajopt/basic_types.h new file mode 100644 index 0000000000..bede4010c9 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/basic_types.h @@ -0,0 +1,379 @@ +/** + * @file basic_types.h + * @brief The tesseract_core package types. + * + * @author Levi Armstrong + * @date April 15, 2018 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2013, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +//#include + +namespace trajopt +{ +template +using AlignedVector = std::vector>; +// aligned_allocator provides at least 16 bytes alignment and more + + +template +using AlignedMap = std::map, Eigen::aligned_allocator>>; + +template +using AlignedUnorderedMap = std::unordered_map, + std::equal_to, + Eigen::aligned_allocator>>; + +using VectorIsometry3d = AlignedVector; +using VectorVector4d = AlignedVector; +using VectorVector3d = std::vector; +using TransformMap = AlignedMap; + +typedef Eigen::Matrix TrajArray; + +struct AllowedCollisionMatrix +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + virtual ~AllowedCollisionMatrix() = default; + + /** + * @brief Disable collision between two collision objects + * @param obj1 Collision object name + * @param obj2 Collision object name + * @param reason The reason for disabling collison + */ + virtual void addAllowedCollision(const std::string& link_name1, + const std::string& link_name2, + const std::string& reason) + { + auto link_pair = makeOrderedLinkPair(link_name1, link_name2); + lookup_table_[link_pair] = reason; + } + + /** + * @brief Remove disabled collision pair from allowed collision matrix + * @param obj1 Collision object name + * @param obj2 Collision object name + */ + virtual void removeAllowedCollision(const std::string& link_name1, const std::string& link_name2) + { + auto link_pair = makeOrderedLinkPair(link_name1, link_name2); + lookup_table_.erase(link_pair); + } + + /** + * @brief This checks if two links are allowed to be in collision + * @param link_name1 First link name + * @param link_name2 Second link anme + * @return True if allowed to be in collision, otherwise false + */ + virtual bool isCollisionAllowed(const std::string& link_name1, const std::string& link_name2) const + { + auto link_pair = makeOrderedLinkPair(link_name1, link_name2); + return (lookup_table_.find(link_pair) != lookup_table_.end()); + } + + /** + * @brief Clears the list of allowed collisions, so that no collision will be + * allowed. + */ + void clearAllowedCollisions() { lookup_table_.clear(); } +private: + typedef std::pair LinkNamesPair; + struct PairHash + { + std::size_t operator()(const LinkNamesPair& pair) const + { + return std::hash()(pair.first + pair.second); + } + }; + typedef std::unordered_map AllowedCollisionEntries; + AllowedCollisionEntries lookup_table_; + + /** + * @brief Create a pair of strings, where the pair.first is always <= pair.second + * @param link_name1 First link name + * @param link_name2 Second link anme + * @return LinkNamesPair a lexicographically sorted pair of strings + */ + static inline LinkNamesPair makeOrderedLinkPair(const std::string& link_name1, const std::string& link_name2) + { + if (link_name1 <= link_name2) + return std::make_pair(link_name1, link_name2); + else + return std::make_pair(link_name2, link_name1); + } + +public: + /** + * @brief Clears the list of allowed collisions + * @return AllowedCollisionEntries an unordered map containing all allowed + * collision entries. The keys of the unordered map are a std::pair + * of the link names in the allowed collision pair. + */ + const AllowedCollisionEntries& getAllAllowedCollisions() const { return lookup_table_; } +}; +typedef std::shared_ptr AllowedCollisionMatrixPtr; +typedef std::shared_ptr AllowedCollisionMatrixConstPtr; + +/** + * @brief Should return true if contact allowed, otherwise false. + * + * Also the order of strings should not matter, the function should handled by the function. + */ +typedef std::function IsContactAllowedFn; + +namespace CollisionObjectTypes +{ +enum CollisionObjectType +{ + UseShapeType = 0, /**< @brief Infer the type from the type specified in the shapes::Shape class */ + + // These convert the meshes to custom collision objects + ConvexHull = + 1, /**< @brief Use the mesh in shapes::Shape but make it a convex hulls collision object. (if not convex it will + be converted) */ + MultiSphere = 2, /**< @brief Use the mesh and represent it by multiple spheres collision object */ + SDF = 3 /**< @brief Use the mesh and rpresent it by a signed distance fields collision object */ +}; +} +typedef CollisionObjectTypes::CollisionObjectType CollisionObjectType; +typedef std::vector CollisionObjectTypeVector; + +namespace BodyTypes +{ +enum BodyType +{ + ROBOT_LINK = 0, /**< @brief These are links at the creation of the environment */ + ROBOT_ATTACHED = 1 /**< @brief These are links that are added after initial creation */ +}; +} +typedef BodyTypes::BodyType BodyType; + +namespace ContinouseCollisionTypes +{ +enum ContinouseCollisionType +{ + CCType_None, + CCType_Time0, + CCType_Time1, + CCType_Between +}; +} +typedef ContinouseCollisionTypes::ContinouseCollisionType ContinouseCollisionType; + +namespace ContactTestTypes +{ +enum ContactTestType +{ + FIRST = 0, /**< Return at first contact for any pair of objects */ + CLOSEST = 1, /**< Return the global minimum for a pair of objects */ + ALL = 2, /**< Return all contacts for a pair of objects */ + LIMITED = 3 /**< Return limited set of contacts for a pair of objects */ +}; +} +typedef ContactTestTypes::ContactTestType ContactTestType; + +struct ContactResult +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + double distance; + int type_id[2]; + std::string link_names[2]; + Eigen::Vector3d nearest_points[2]; // this is an array of two elements of type Vector3d + Eigen::Vector3d normal; + + Eigen::Vector3d cc_nearest_points[2]; + // this is used when performing continous collision checking. so it should be the nearest point on the + // swept volume of a robot's link and an obstacle (obstacle coul be fixed in space or moving. in the case + // of moving, we should calculate the swpet volume of the obstacle too, I guess?) + + double cc_time; + ContinouseCollisionType cc_type; + + ContactResult() { clear(); } + /// Clear structure data + void clear() + { + distance = std::numeric_limits::max(); + nearest_points[0].setZero(); + nearest_points[1].setZero(); + link_names[0] = ""; + link_names[1] = ""; + type_id[0] = 0; + type_id[1] = 0; + normal.setZero(); + cc_nearest_points[0].setZero(); + cc_nearest_points[1].setZero(); + cc_time = -1; + cc_type = ContinouseCollisionType::CCType_None; + } + + // convert collision_detection::Contact from MoveIt to trajopt::ContactResult (from tesseract) + ContactResult(collision_detection::Contact moveit_contact, + Eigen::Vector3d moveit_cc_nearest_points[2], + double moveit_cc_time, + ContinouseCollisionType moveit_cc_type) + { + distance = moveit_contact.depth; + nearest_points[0] = moveit_contact.nearest_points[0]; + nearest_points[1] = moveit_contact.nearest_points[1]; + link_names[0] = moveit_contact.body_name_1; + link_names[1] = moveit_contact.body_name_2; + // in tesseaerct_message: type_id # ROBOT_LINK = 0, ROBOT_ATTACHED = 1 + type_id[0] = (moveit_contact.body_type_1 == collision_detection::BodyTypes::Type::ROBOT_LINK) ? 0 : 1; + type_id[1] = (moveit_contact.body_type_2 == collision_detection::BodyTypes::Type::ROBOT_LINK) ? 0 : 1; + normal = moveit_contact.normal; + // to find out the correct information for the following, I probably need to look at + // tesseract_collision/bullet_utils.h + cc_nearest_points[0] = moveit_cc_nearest_points[0]; + cc_nearest_points[1] = moveit_cc_nearest_points[1]; + cc_time = moveit_cc_time; + cc_type = moveit_cc_type; + } +}; + +// ContactResultVector: is an aligned vector which works with memory. It is a vector containing ContactResult +typedef AlignedVector ContactResultVector; +typedef AlignedMap, ContactResultVector> ContactResultMap; + +/// Contact test data and query results information +struct ContactTestData +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ContactTestData(const std::vector& active, + const double& contact_distance, + const IsContactAllowedFn& fn, + const ContactTestType& type, + ContactResultMap& res) + : active(active), contact_distance(contact_distance), fn(fn), type(type), res(res), done(false) + { + } + + const std::vector& active; + const double& contact_distance; + const IsContactAllowedFn& fn; + const ContactTestType& type; + + /// Destance query results information + ContactResultMap& res; + + /// Indicate if search is finished + bool done; +}; + +static inline void moveContactResultsMapToContactResultsVector(ContactResultMap& contact_map, + ContactResultVector& contact_vector) +{ + std::size_t size = 0; + for (const auto& contact : contact_map) + size += contact.second.size(); + + contact_vector.reserve(size); + for (auto& contact : contact_map) + std::move(contact.second.begin(), contact.second.end(), std::back_inserter(contact_vector)); +} + +/** @brief This holds a state of the environment */ +struct EnvState +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + std::unordered_map joints; + TransformMap transforms; +}; +typedef std::shared_ptr EnvStatePtr; +typedef std::shared_ptr EnvStateConstPtr; + +/**< @brief Information on how the object is attached to the environment */ +struct AttachedBodyInfo +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + AttachedBodyInfo() : transform(Eigen::Isometry3d::Identity()) {} + std::string object_name; /**< @brief The name of the AttachableObject being used */ + std::string parent_link_name; /**< @brief The name of the link to attach the body */ + Eigen::Isometry3d transform; /**< @brief The transform between parent link and object */ + std::vector touch_links; /**< @brief The names of links which the attached body is allowed to be in + contact with */ +}; + +/** @brief Contains visual geometry data */ +struct VisualObjectGeometry +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + std::vector shapes; /**< @brief The shape */ + VectorIsometry3d shape_poses; /**< @brief The pose of the shape */ + VectorVector4d shape_colors; /**< @brief (Optional) The shape color (R, G, B, A) */ +}; + +/** @brief Contains visual geometry data */ +struct CollisionObjectGeometry : public VisualObjectGeometry +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + CollisionObjectTypeVector + collision_object_types; /**< @brief The collision object type. This is used by the collision libraries */ +}; + +/** @brief Contains data about an attachable object */ +struct AttachableObject +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + std::string name; /**< @brief The name of the attachable object (aka. link name and must be unique) */ + VisualObjectGeometry visual; /**< @brief The objects visual geometry */ + CollisionObjectGeometry collision; /**< @brief The objects collision geometry */ +}; +typedef std::shared_ptr AttachableObjectPtr; +typedef std::shared_ptr AttachableObjectConstPtr; + +/** @brief ObjectColorMap Stores Object color in a 4d vector as RGBA*/ +struct ObjectColor +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + VectorVector4d visual; + VectorVector4d collision; +}; +typedef AlignedUnorderedMap ObjectColorMap; +typedef std::shared_ptr ObjectColorMapPtr; +typedef std::shared_ptr ObjectColorMapConstPtr; +typedef AlignedUnorderedMap AttachedBodyInfoMap; +typedef std::unordered_map AttachableObjectConstPtrMap; +} + diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/cache.hxx b/moveit_planners/trajopt/trajopt/include/trajopt/cache.hxx new file mode 100644 index 0000000000..c527ace0e0 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/cache.hxx @@ -0,0 +1,27 @@ +#pragma once +#include + +template +class Cache +{ +public: + //EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + KeyT keybuf[bufsize]; // circular buffer + ValueT valbuf[bufsize]; // circular buffer + int m_i; + Cache() : m_i(0) { memset(keybuf, 666, sizeof(keybuf)); } + void put(const KeyT& key, const ValueT& value) + { + keybuf[m_i] = key; + valbuf[m_i] = value; + ++m_i; + if (m_i == bufsize) + m_i = 0; + } + ValueT* get(const KeyT& key) + { + KeyT* it = std::find(&keybuf[0], &keybuf[0] + bufsize, key); + return (it == &keybuf[0] + bufsize) ? nullptr : &valbuf[it - &keybuf[0]]; + } +}; diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/collision_terms.h b/moveit_planners/trajopt/trajopt/include/trajopt/collision_terms.h new file mode 100644 index 0000000000..b8c4bf553a --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/collision_terms.h @@ -0,0 +1,172 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + + +namespace trajopt +{ +// template +// using AlignedVector = std::vector>; + +struct CollisionEvaluator +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + CollisionEvaluator(planning_scene::PlanningSceneConstPtr& planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data) + :safety_margin_data_(safety_margin_data), planning_group_(planning_group) + { + // we need a child of planning scene so I can do the changing robot states and do collsiiong detenction + planning_scene_ = planning_scene->diff(); + // remove/add magic objects in planning scene + std::vector objectIds = planning_scene_->getWorld()->getObjectIds(); + for (auto objectId : objectIds) + { + collision_detection::World::ObjectConstPtr objPtr = planning_scene_->getWorld()->getObject(objectId); + planning_scene_->getWorldNonConst()->removeObject(objectId); + planning_scene_->getWorldNonConst()->addToObject(objPtr->id_, objPtr->shapes_, objPtr->shape_poses_); + } + + + // OR ? + //planning_scene_ = planning_scene::PlanningScene::clone(planning_scene); + } + virtual ~CollisionEvaluator() = default; + virtual void CalcDistExpressions(const DblVec& x, sco::AffExprVector& exprs) = 0; + virtual void CalcDists(const DblVec& x, DblVec& exprs) = 0; + // calculate the collision in planning scene + virtual void CalcCollisions(const DblVec& x, ContactResultVector& dist_results) = 0; + void GetCollisionsCached(const DblVec& x, ContactResultVector& dist_results); + //virtual void Plot(const tesseract::BasicPlottingPtr plotter, const DblVec& x) = 0; + virtual sco::VarVector GetVars() = 0; + + const SafetyMarginDataConstPtr getSafetyMarginData() const { return safety_margin_data_; } + + // CaclCollisions() : calculates the collisions in the scene + // CollisionsToDistance(): converts collisions contacts in the scenes to distances + // CollisionToDistanceExpressions(): is where acttual linearized math expressions is created + // CalcDistExpressions(): + + // CalcDists(): + // Value(): calls CalcDists() + + Cache m_cache; + // this calss is not dependent to any tesseract stuff, I just need to figure out ContactResulVector that is passed to it + // I do not understand what it is doing exactly though + // it is creating a buffer of ContactResultVector + // Cache, 10> m_cache; + + // what is ContactResultVector? + // is an aligned vector which works with memory. Basically, it is a vector containing ContactResult + // now what is ContactResult? it has all the informatin related to contact between two bodies + +protected: + //from MoveIt macros: + // typedef std::shared_ptr PlanningSceneConstPtr; + // the object the pointer is pointing to is const, i.e. the planning scene is constant + // Also, PlanningScen is noncopyable + // setActiveCollisionDetector is a non-const member of PlanningScene which is not accisible from + // a PlanningSceneConstPtr. So I have to deal with the const ptr of planning scene here and + // let the user set the collision detector to bullet + + planning_scene::PlanningScenePtr planning_scene_; + std::string planning_group_; + SafetyMarginDataConstPtr safety_margin_data_; + +private: + CollisionEvaluator() {} +}; + +typedef std::shared_ptr CollisionEvaluatorPtr; + +struct SingleTimestepCollisionEvaluator : public CollisionEvaluator +{ +public: + SingleTimestepCollisionEvaluator(planning_scene::PlanningSceneConstPtr& planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data, + const sco::VarVector& vars); + /** + @brief linearize all contact distances in terms of robot dofs; + Do a collision check between robot and environment. + For each contact generated, return a linearization of the signed distance + function. + */ + void CalcDistExpressions(const DblVec& x, sco::AffExprVector& exprs) override; + /** + * Same as CalcDistExpressions, but just the distances--not the expressions + */ + void CalcDists(const DblVec& x, DblVec& exprs) override; + void CalcCollisions(const DblVec& x, ContactResultVector& dist_results) override; + sco::VarVector GetVars() override { return m_vars; } +private: + sco::VarVector m_vars; +}; + +struct CastCollisionEvaluator : public CollisionEvaluator +{ +public: + CastCollisionEvaluator(planning_scene::PlanningSceneConstPtr planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data, + const sco::VarVector& vars0, + const sco::VarVector& vars1); + void CalcDistExpressions(const DblVec& x, sco::AffExprVector& exprs) override; + void CalcDists(const DblVec& x, DblVec& exprs) override; + void CalcCollisions(const DblVec& x, ContactResultVector& dist_results) override; + sco::VarVector GetVars() override { return concat(m_vars0, m_vars1); } +private: +// for castcollision, swpt volume, we need two states + sco::VarVector m_vars0; // contains joint values for state 0 + sco::VarVector m_vars1; // contains joint values for state 1 + +}; + +// sco::Cost does not depend on tesseract. So whatever dependency is there should be here +class TRAJOPT_API CollisionCost : public sco::Cost +{ +public: + /* constructor for single timestep. + This constructor initializes m_calc which is type of CollisionEvaluator */ + CollisionCost(planning_scene::PlanningSceneConstPtr& planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data, + const sco::VarVector& vars); + /* constructor for cast cost */ + CollisionCost(planning_scene::PlanningSceneConstPtr& planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data, + const sco::VarVector& vars0, + const sco::VarVector& vars1); + virtual sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + virtual double value(const DblVec&) override; + sco::VarVector getVars() override { return m_calc->GetVars(); } +private: + CollisionEvaluatorPtr m_calc; +}; + +class TRAJOPT_API CollisionConstraint : public sco::IneqConstraint +{ +public: + /* constructor for single timestep */ + CollisionConstraint(planning_scene::PlanningSceneConstPtr planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data, + const sco::VarVector& vars); + /* constructor for cast cost */ + CollisionConstraint(planning_scene::PlanningSceneConstPtr planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data, + const sco::VarVector& vars0, + const sco::VarVector& vars1); + virtual sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + virtual DblVec value(const DblVec&) override; + void Plot(const DblVec& x); + sco::VarVector getVars() override { return m_calc->GetVars(); } +private: + CollisionEvaluatorPtr m_calc; +}; +} diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/common.hpp b/moveit_planners/trajopt/trajopt/include/trajopt/common.hpp new file mode 100644 index 0000000000..41b82a6046 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/common.hpp @@ -0,0 +1,3 @@ +#pragma once +#include +#include diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/kinematic_terms.h b/moveit_planners/trajopt/trajopt/include/trajopt/kinematic_terms.h new file mode 100644 index 0000000000..93c305fc95 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/kinematic_terms.h @@ -0,0 +1,129 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik, LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Omid Heidari */ + +#pragma once + +#include + +#include + +namespace trajopt +{ +/** + * @brief Extracts the vector part of quaternion + */ +inline Eigen::Vector3d quaternionRotationVector(const Eigen::Matrix3d& matrix) +{ + Eigen::Quaterniond quaternion; + quaternion = matrix; + return Eigen::Vector3d(quaternion.x(), quaternion.y(), quaternion.z()); +} + +/** + * @brief Appends b to a of type VectorXd + */ +inline Eigen::VectorXd concatVector(const Eigen::VectorXd& vector_a, const Eigen::VectorXd& vector_b) +{ + Eigen::VectorXd out(vector_a.size() + vector_b.size()); + out.topRows(vector_a.size()) = vector_a; + out.middleRows(vector_a.size(), vector_b.size()) = vector_b; + return out; +} + +/** + * @brief Appends b to a of type T + */ +template +inline std::vector concatVector(const std::vector& vector_a, const std::vector& vector_b) +{ + std::vector out; + out.insert(out.end(), vector_a.begin(), vector_a.end()); + out.insert(out.end(), vector_b.begin(), vector_b.end()); + return out; +} + +/** + * @brief Used to calculate the error for StaticCartPoseTermInfo + * This is converted to a cost or constraint using TrajOptCostFromErrFunc or TrajOptConstraintFromErrFunc + */ +struct CartPoseErrCalculator : public sco::VectorOfVector +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Isometry3d target_pose_inv_; + planning_scene::PlanningSceneConstPtr planning_scene_; + std::string link_; + Eigen::Isometry3d tcp_; + + CartPoseErrCalculator(const Eigen::Isometry3d& pose, const planning_scene::PlanningSceneConstPtr planning_scene, + const std::string& link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) + : target_pose_inv_(pose.inverse()), planning_scene_(planning_scene), link_(link), tcp_(tcp) + { + } + + Eigen::VectorXd operator()(const Eigen::VectorXd& dof_vals) const override; +}; + +// TODO(omid): The following should be added and adjusted from trajopt +// JointPosEqCost +// JointPosIneqCost +// JointPosEqConstraint +// JointPosIneqConstraint + +struct JointVelErrCalculator : sco::VectorOfVector +{ + /** @brief Velocity target */ + double target_; + /** @brief Upper tolerance */ + double upper_tol_; + /** @brief Lower tolerance */ + double lower_tol_; + JointVelErrCalculator() : target_(0.0), upper_tol_(0.0), lower_tol_(0.0) + { + } + JointVelErrCalculator(double target, double upper_tol, double lower_tol) + : target_(target), upper_tol_(upper_tol), lower_tol_(lower_tol) + { + } + + Eigen::VectorXd operator()(const Eigen::VectorXd& var_vals) const override; +}; + +struct JointVelJacobianCalculator : sco::MatrixOfVector +{ + Eigen::MatrixXd operator()(const Eigen::VectorXd& var_vals) const override; +}; + +} // namespace trajopt diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/problem_description.h b/moveit_planners/trajopt/trajopt/include/trajopt/problem_description.h new file mode 100644 index 0000000000..30b335272d --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/problem_description.h @@ -0,0 +1,438 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik, LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Omid Heidari */ + +#pragma once + +#include +#include + +#include +#include + +#include + +namespace trajopt +{ +/** +@brief Used to apply cost/constraint to joint-space velocity + +Term is applied to every step between first_step and last_step. It applies two limits, upper_limits/lower_limits, +to the joint velocity subject to the following cases. + +* term_type = TT_COST +** upper_limit = lower_limit = 0 - Cost is applied with a SQUARED error scaled for each joint by coeffs +** upper_limit != lower_limit - 2 hinge costs are applied scaled for each joint by coeffs. If velocity < upper_limit and +velocity > lower_limit, no penalty. + +* term_type = TT_CNT +** upper_limit = lower_limit = 0 - Equality constraint is applied +** upper_limit != lower_limit - 2 Inequality constraints are applied. These are both satisfied when velocity < +upper_limit and velocity > lower_limit + +Note: coeffs, upper_limits, and lower_limits are optional. If a term is not given it will default to 0 for all joints. +If one value is given, this will be broadcast to all joints. + +Note: Velocity is calculated numerically using forward finite difference + +\f{align*}{ + cost = \sum_{t=0}^{T-2} \sum_j c_j (x_{t+1,j} - x_{t,j})^2 +\f} +where j indexes over DOF, and \f$c_j\f$ are the coeffs. +*/ + +struct TermInfo; +MOVEIT_CLASS_FORWARD(TermInfo); // Defines TermInfoPtr, ConstPtr, WeakPtr... etc + +class TrajOptProblem; +MOVEIT_CLASS_FORWARD(TrajOptProblem); // Defines TrajOptProblemPtr, ConstPtr, WeakPtr... etc + +struct JointPoseTermInfo; +MOVEIT_CLASS_FORWARD(JointPoseTermInfo); // Defines JointPoseTermInfoPtr, ConstPtr, WeakPtr... etc + +struct CartPoseTermInfo; +MOVEIT_CLASS_FORWARD(CartPoseTermInfo); // Defines CartPoseTermInfoPtr, ConstPtr, WeakPtr... etc + +struct JointVelTermInfo; +MOVEIT_CLASS_FORWARD(JointVelTermInfo); // Defines JointVelTermInfoPtr, ConstPtr, WeakPtr... etc + +struct CollisionTermInfo; +MOVEIT_CLASS_FORWARD(CollisionTermInfo); // Defines JointVelTermInfoPtr, ConstPtr, WeakPtr... etc + +struct ProblemInfo; +TrajOptProblemPtr ConstructProblem(const ProblemInfo&); + +enum TermType +{ + TT_COST = 0x1, // 0000 0001 + TT_CNT = 0x2, // 0000 0010 + TT_USE_TIME = 0x4, // 0000 0100 +}; + +struct BasicInfo +{ + /** @brief If true, first time step is fixed with a joint level constraint + If this is false, the starting point of the trajectory will + not be the current position of the robot. The use case example is: if we are trying to execute a process like + sanding the critical part which is the actual process path not how we get to the start of the process path. So we + plan the + process path first leaving the start free to hopefully get the most optimal and then we plan from the current + location with + start fixed to the start of the process path. It depends on what we want the default behavior to be + */ + bool start_fixed; + /** @brief Number of time steps (rows) in the optimization matrix */ + int n_steps; + sco::IntVec dofs_fixed; // optional + sco::ModelType convex_solver; // which convex solver to use + + /** @brief If true, the last column in the optimization matrix will be 1/dt */ + bool use_time = false; + /** @brief The upper limit of 1/dt values allowed in the optimization*/ + double dt_upper_lim = 1.0; + /** @brief The lower limit of 1/dt values allowed in the optimization*/ + double dt_lower_lim = 1.0; +}; + +/** +Initialization info read from json +*/ +struct InitInfo +{ + /** @brief Methods of initializing the optimization matrix. This defines how robot moves from the current + state to the start state + + STATIONARY: Initializes all joint values to the initial value (the current value in the env) + JOINT_INTERPOLATED: Linearly interpolates between initial value and the joint position specified in InitInfo.data + GIVEN_TRAJ: Initializes the matrix to a given trajectory + + In all cases the dt column (if present) is appended the selected method is defined. + */ + enum Type + { + STATIONARY, + JOINT_INTERPOLATED, + GIVEN_TRAJ, + }; + /** @brief Specifies the type of initialization to use */ + Type type; + + /** @brief Data used during initialization. Use depends on the initialization selected. This data will be used + to create initialization matrix. We need to give the goal information to this init info + */ + trajopt::TrajArray data; + + /** @brief Default value the final column of the optimization is initialized too if time is being used */ + double dt = 1.0; +}; + +/** +When cost or constraint element of JSON doc is read, one of these guys gets +constructed to hold the parameters. +Then it later gets converted to a Cost object by the addObjectiveTerms method +*/ +struct TermInfo +{ + std::string name; + int term_type; + int getSupportedTypes() + { + return supported_term_types_; + } + // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; + virtual void addObjectiveTerms(TrajOptProblem& prob) = 0; + + static TermInfoPtr fromName(const std::string& type); + + /** + * Registers a user-defined TermInfo so you can use your own cost + * see function RegisterMakers.cpp + */ + using MakerFunc = TermInfoPtr (*)(void); + static void RegisterMaker(const std::string& type, MakerFunc); + + virtual ~TermInfo() = default; + +protected: + TermInfo(int supported_term_types) : supported_term_types_(supported_term_types) + { + } + +private: + static std::map name_to_maker_; + int supported_term_types_; +}; + +struct ProblemInfo +{ +public: + BasicInfo basic_info; + sco::BasicTrustRegionSQPParameters opt_info; + std::vector cost_infos; + std::vector cnt_infos; + InitInfo init_info; + + planning_scene::PlanningSceneConstPtr planning_scene; + std::string planning_group_name; + + ProblemInfo(planning_scene::PlanningSceneConstPtr ps, const std::string& pg) + : planning_scene(ps), planning_group_name(pg) + { + } +}; + +/** + * Holds all the data for a trajectory optimization problem + * so you can modify it programmatically, e.g. add your own costs + */ +class TrajOptProblem : public sco::OptProb +{ +public: + TrajOptProblem(); + TrajOptProblem(const ProblemInfo& problem_info); + virtual ~TrajOptProblem() = default; + /** @brief Returns the values of the specified joints (start_col to num_col) for the specified timestep i.*/ + sco::VarVector GetVarRow(int i, int start_col, int num_col) + { + return matrix_traj_vars.rblock(i, start_col, num_col); + } + /** @brief Returns the values of all joints for the specified timestep i.*/ + sco::VarVector GetVarRow(int i) + { + return matrix_traj_vars.row(i); + } + /** @brief Returns the value of the specified joint j for the specified timestep i.*/ + sco::Var& GetVar(int i, int j) + { + return matrix_traj_vars.at(i, j); + } + trajopt::VarArray& GetVars() + { + return matrix_traj_vars; + } + /** @brief Returns the number of steps in the problem. This is the number of rows in the optimization matrix.*/ + int GetNumSteps() + { + return matrix_traj_vars.rows(); + } + /** @brief Returns the problem DOF. This is the number of columns in the optization matrix. + * Note that this is not necessarily the same as the kinematic DOF.*/ + int GetNumDOF() + { + return matrix_traj_vars.cols(); + } + /** @brief Returns the kinematic DOF of the active joint model group + */ + int GetActiveGroupNumDOF() + { + return dof_; + } + planning_scene::PlanningSceneConstPtr& GetPlanningScene() + { + return planning_scene_; + } + std::string GetPlanningGroup() + { + return planning_group_; + } + void SetInitTraj(const trajopt::TrajArray& x) + { + matrix_init_traj = x; + } + trajopt::TrajArray GetInitTraj() + { + return matrix_init_traj; + } + // friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&); + /** @brief Returns TrajOptProb.has_time */ + bool GetHasTime() + { + return has_time; + } + /** @brief Sets TrajOptProb.has_time */ + void SetHasTime(bool tmp) + { + has_time = tmp; + } + +private: + /** @brief If true, the last column in the optimization matrix will be 1/dt */ + bool has_time; + /** @brief is the matrix holding the joint values of the trajectory for all timesteps */ + trajopt::VarArray matrix_traj_vars; + planning_scene::PlanningSceneConstPtr planning_scene_; + std::string planning_group_; + /** @brief Kinematic DOF of the active joint model group */ + int dof_; + trajopt::TrajArray matrix_init_traj; +}; + +/** @brief This term is used when the goal frame is fixed in cartesian space + + Set term_type == TT_COST or TT_CNT for cost or constraint. +*/ +struct CartPoseTermInfo : public TermInfo +{ + // EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** @brief Timestep at which to apply term */ + int timestep; + /** @brief Cartesian position */ + Eigen::Vector3d xyz; + /** @brief Rotation quaternion */ + Eigen::Vector4d wxyz; + /** @brief coefficients for position and rotation */ + Eigen::Vector3d pos_coeffs, rot_coeffs; + /** @brief Link which should reach desired pose */ + std::string link; + /** @brief Static transform applied to the link */ + Eigen::Isometry3d tcp; + + CartPoseTermInfo(); + + /** @brief Used to add term to pci from json */ + // void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; + /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ + void addObjectiveTerms(TrajOptProblem& prob) override; + + static TermInfoPtr create() + { + TermInfoPtr out(new CartPoseTermInfo()); + return out; + } +}; + +/** + \brief Joint space position cost + Position operates on a single point (unlike velocity, etc). This is b/c the primary usecase is joint-space + position waypoints + + \f{align*}{ + \sum_i c_i (x_i - xtarg_i)^2 + \f} + where \f$i\f$ indexes over dof and \f$c_i\f$ are coeffs + */ +struct JointPoseTermInfo : public TermInfo +{ + /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec coeffs; + /** @brief Vector of position targets. This is a required value. Size should be the DOF of the system */ + trajopt::DblVec targets; + /** @brief Vector of position upper limits. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec upper_tols; + /** @brief Vector of position lower limits. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec lower_tols; + /** @brief First time step to which the term is applied. Default: 0 */ + int first_step = 0; + /** @brief Last time step to which the term is applied. Default: prob.GetNumSteps() - 1*/ + int last_step = -1; + + /** @brief Initialize term with it's supported types */ + JointPoseTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) + { + } + + /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ + void addObjectiveTerms(TrajOptProblem& prob) override; + + static TermInfoPtr create() + { + TermInfoPtr out(new JointPoseTermInfo()); + return out; + } +}; + +struct JointVelTermInfo : public TermInfo +{ + /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec coeffs; + /** @brief Vector of velocity targets. This is a required value. Size should be the DOF of the system */ + trajopt::DblVec targets; + /** @brief Vector of velocity upper limits. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec upper_tols; + /** @brief Vector of velocity lower limits. Size should be the DOF of the system. Default: vector of 0's*/ + trajopt::DblVec lower_tols; + /** @brief First time step to which the term is applied. Default: 0*/ + int first_step = 0; + /** @brief Last time step to which the term is applied. Default: prob.GetNumSteps() - 1*/ + int last_step = -1; + + /** @brief Initialize term with it's supported types */ + JointVelTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME){} + + /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ + void addObjectiveTerms(TrajOptProblem& prob) override; + + static TermInfoPtr create() + { + TermInfoPtr out(new JointVelTermInfo()); + return out; + } +}; + +struct CollisionTermInfo : public TermInfo +{ + /** @brief first_step and last_step are inclusive */ + int first_step, last_step; + + /** @brief Indicate if continuous collision checking should be used. */ + bool continuous; + + /** @brief for continuous-time penalty, use swept-shape between timesteps t and t+gap */ + /** @brief (gap=1 by default) */ + int gap; + + /** @brief Contains distance penalization data: Safety Margin, Coeff used during */ + /** @brief optimization, etc. */ + std::vector info; + + /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ + void addObjectiveTerms(TrajOptProblem& prob) override; + + static TermInfoPtr create() + { + TermInfoPtr out(new CollisionTermInfo()); + return out; + } + + CollisionTermInfo() : TermInfo(TT_COST | TT_CNT) + { + } +}; + +void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, + trajopt::TrajArray& init_traj); + +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/trajectory_costs.hpp b/moveit_planners/trajopt/trajopt/include/trajopt/trajectory_costs.hpp new file mode 100644 index 0000000000..c96b150bd2 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/trajectory_costs.hpp @@ -0,0 +1,541 @@ +#pragma once + +/** +Simple quadratic costs on trajectory +*/ + +#include +#include + +#include "trajopt/common.hpp" + +namespace trajopt +{ +class TRAJOPT_API JointPosEqCost : public sco::Cost +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointPosEqCost(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the cost as an expression */ + sco::QuadExpr expr_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +/** + * @brief The JointVelIneqCost class + * Assumes that the target is ... + */ +class TRAJOPT_API JointPosIneqCost : public sco::Cost +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointPosIneqCost(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, const Eigen::VectorXd& lower_limits, int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*num_timesteps*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointPosEqConstraint : public sco::EqConstraint +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointPosEqConstraint(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targetss, + int& first_step, int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the costs as an expression. Will be length num_jnts*num_timesteps */ + std::vector expr_vec_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointPosIneqConstraint : public sco::IneqConstraint +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointPosIneqConstraint(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, const Eigen::VectorXd& lower_limits, int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*num_timesteps*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointVelEqCost : public sco::Cost +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointVelEqCost(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values using Eigen*/ + double value(const DblVec&) override; + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the cost as an expression */ + sco::QuadExpr expr_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointVelIneqCost : public sco::Cost +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointVelIneqCost(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, const Eigen::VectorXd& lower_limits, int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*num_timesteps*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointVelEqConstraint : public sco::EqConstraint +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointVelEqConstraint(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + int& first_step, int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-1) */ + std::vector expr_vec_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointVelIneqConstraint : public sco::IneqConstraint +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointVelIneqConstraint(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, const Eigen::VectorXd& lower_limits, int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-1)*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointAccEqCost : public sco::Cost +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointAccEqCost(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the cost as an expression */ + sco::QuadExpr expr_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointAccIneqCost : public sco::Cost +{ +public: + /** @brief Forms error in a vector of AffExpr - independent of penalty type */ + JointAccIneqCost(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, const Eigen::VectorXd& lower_limits, int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of acceleration targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-2)*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointAccEqConstraint : public sco::EqConstraint +{ +public: + /** @brief Forms error in a vector of AffExpr - independent of penalty type */ + JointAccEqConstraint(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + int& first_step, int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-2) */ + std::vector expr_vec_; + /** @brief Vector of acceleration targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointAccIneqConstraint : public sco::IneqConstraint +{ +public: + /** @brief Forms error in a vector of AffExpr - independent of penalty type */ + JointAccIneqConstraint(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, const Eigen::VectorXd& lower_limits, int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of acceleration targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-2)*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointJerkEqCost : public sco::Cost +{ +public: + /** @brief Forms error in QuadExpr - independent of penalty type */ + JointJerkEqCost(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the cost as an expression */ + sco::QuadExpr expr_; + /** @brief Vector of jerk targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointJerkIneqCost : public sco::Cost +{ +public: + /** @brief Forms error in a vector of AffExpr - independent of penalty type */ + JointJerkIneqCost(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, const Eigen::VectorXd& lower_limits, int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexObjectivePtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + double value(const DblVec&) override; + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of jerk targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-4)*2 */ + std::vector expr_vec_; +}; + +class TRAJOPT_API JointJerkEqConstraint : public sco::EqConstraint +{ +public: + /** @brief Forms error in a vector of AffExpr - independent of penalty type */ + JointJerkEqConstraint(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + int& first_step, int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-4) */ + std::vector expr_vec_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; +}; + +class TRAJOPT_API JointJerkIneqConstraint : public sco::IneqConstraint +{ +public: + /** @brief Forms error in a vector of AffExpr - independent of penalty type */ + JointJerkIneqConstraint(const VarArray& traj, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_limits, const Eigen::VectorXd& lower_limits, int& first_step, + int& last_step); + /** @brief Convexifies cost expression - In this case, it is already quadratic so there's nothing to do */ + sco::ConvexConstraintsPtr convex(const DblVec& x, sco::Model* model) override; + /** @brief Numerically evaluate cost given the vector of values */ + DblVec value(const DblVec&) override; + sco::VarVector getVars() override + { + return vars_.flatten(); + } + +private: + /** @brief The variables being optimized. Used to properly index the vector being optimized */ + VarArray vars_; + /** @brief The coefficients used to weight the cost */ + Eigen::VectorXd coeffs_; + /** @brief Vector of upper tolerances */ + Eigen::VectorXd upper_tols_; + /** @brief Vector of lower tolerances */ + Eigen::VectorXd lower_tols_; + /** @brief Vector of velocity targets */ + Eigen::VectorXd targets_; + /** @brief First time step to which the term is applied */ + int first_step_; + /** @brief Last time step to which the term is applied */ + int last_step_; + /** @brief Stores the costs as an expression. Will be length num_jnts*(num_timesteps-4)*2 */ + std::vector expr_vec_; +}; +} // namespace trajopt diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/typedefs.hpp b/moveit_planners/trajopt/trajopt/include/trajopt/typedefs.hpp new file mode 100644 index 0000000000..d6ae14e9a9 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/typedefs.hpp @@ -0,0 +1,60 @@ +#pragma once +#include +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include + +namespace trajopt +{ +typedef util::BasicArray VarArray; +typedef util::BasicArray AffArray; +typedef util::BasicArray CntArray; +typedef Eigen::Matrix DblMatrix; +typedef Eigen::Matrix TrajArray; +typedef sco::DblVec DblVec; +typedef sco::IntVec IntVec; + +/** @brief Adds plotting to the CostFromErrFunc class in trajopt_sco */ +class TrajOptCostFromErrFunc : public sco::CostFromErrFunc +{ +public: + /// supply error function, obtain derivative numerically + TrajOptCostFromErrFunc(sco::VectorOfVectorPtr f, const sco::VarVector& vars, const Eigen::VectorXd& coeffs, + sco::PenaltyType pen_type, const std::string& name) + : CostFromErrFunc(f, vars, coeffs, pen_type, name) + { + } + + /// supply error function and gradient + TrajOptCostFromErrFunc(sco::VectorOfVectorPtr f, sco::MatrixOfVectorPtr dfdx, const sco::VarVector& vars, + const Eigen::VectorXd& coeffs, sco::PenaltyType pen_type, const std::string& name) + : CostFromErrFunc(f, dfdx, vars, coeffs, pen_type, name) + { + } +}; + +/** @brief Adds plotting to the ConstraintFromFunc class in trajopt_sco */ +class TrajOptConstraintFromErrFunc : public sco::ConstraintFromErrFunc +{ +public: + /// supply error function, obtain derivative numerically + TrajOptConstraintFromErrFunc(sco::VectorOfVectorPtr f, const sco::VarVector& vars, const Eigen::VectorXd& coeffs, + sco::ConstraintType type, const std::string& name) + : ConstraintFromErrFunc(f, vars, coeffs, type, name) + { + } + + /// supply error function and gradient + TrajOptConstraintFromErrFunc(sco::VectorOfVectorPtr f, sco::MatrixOfVectorPtr dfdx, const sco::VarVector& vars, + const Eigen::VectorXd& coeffs, sco::ConstraintType type, const std::string& name) + : ConstraintFromErrFunc(f, dfdx, vars, coeffs, type, name) + { + } +}; +} diff --git a/moveit_planners/trajopt/trajopt/include/trajopt/utils.hpp b/moveit_planners/trajopt/trajopt/include/trajopt/utils.hpp new file mode 100644 index 0000000000..c517dfe4bd --- /dev/null +++ b/moveit_planners/trajopt/trajopt/include/trajopt/utils.hpp @@ -0,0 +1,169 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include + +#include + +namespace trajopt +{ +template +using AlignedUnorderedMap = std::unordered_map, std::equal_to, + Eigen::aligned_allocator>>; + +/** +Extract trajectory array from solution vector x using indices in array vars +*/ +TrajArray TRAJOPT_API getTraj(const DblVec& x, const VarArray& vars); +TrajArray TRAJOPT_API getTraj(const DblVec& x, const AffArray& arr); + +inline DblVec trajToDblVec(const TrajArray& x) +{ + return DblVec(x.data(), x.data() + x.rows() * x.cols()); +} + +/** + * @brief Appends b to a of type VectorXd + */ +inline Eigen::VectorXd concat(const Eigen::VectorXd& a, const Eigen::VectorXd& b) +{ + Eigen::VectorXd out(a.size() + b.size()); + out.topRows(a.size()) = a; + out.middleRows(a.size(), b.size()) = b; + return out; +} + +/** + * @brief Appends b to a of type T + */ +template +std::vector concat(const std::vector& a, const std::vector& b) +{ + std::vector out; + std::vector x(a.size() + b.size()); + out.insert(out.end(), a.begin(), a.end()); + out.insert(out.end(), b.begin(), b.end()); + return out; +} + +template +std::vector singleton(const T& x) +{ + return std::vector(1, x); +} + +void TRAJOPT_API AddVarArrays(sco::OptProb& prob, int rows, const std::vector& cols, + const std::vector& name_prefix, const std::vector& newvars); + +void TRAJOPT_API AddVarArray(sco::OptProb& prob, int rows, int cols, const std::string& name_prefix, VarArray& newvars); + +/** @brief Store Safety Margin Data for a given timestep */ +struct SafetyMarginData +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + SafetyMarginData(const double& default_safety_margin, const double& default_safety_margin_coeff) + : default_safety_margin_data_(default_safety_margin, default_safety_margin_coeff) + , max_safety_margin_(default_safety_margin) + { + } + + /** + * @brief Set the safety margin for a given contact pair + * + * The order of the object names does not matter, that is handled internal to + * the class. + * + * @param obj1 The first object name + * @param obj2 The Second object name + * @param safety_margins contacts with distance < safety_margin are penalized + * @param safety_margin_coeffs A safety margin coefficient vector where each + * element corresponds to a given timestep. + */ + void SetPairSafetyMarginData(const std::string& obj1, const std::string& obj2, const double& safety_margin, + const double& safety_margin_coeff) + { + Eigen::Vector2d data(safety_margin, safety_margin_coeff); + + pair_lookup_table_[obj1 + obj2] = data; + pair_lookup_table_[obj2 + obj1] = data; + + if (safety_margin > max_safety_margin_) + { + max_safety_margin_ = safety_margin; + } + } + + const Eigen::Vector2d& getPairSafetyMarginData(const std::string& obj1, const std::string& obj2) const + { + const std::string& key = obj1 + obj2; + auto it = pair_lookup_table_.find(key); + + if (it != pair_lookup_table_.end()) + { + return it->second; + } + else + { + return default_safety_margin_data_; + } + } + + const double& getMaxSafetyMargin() const + { + return max_safety_margin_; + } + +private: + /// The coeff used during optimization + /// safety margin: contacts with distance < dist_pen are penalized + /// Stores [dist_pen, coeff] + Eigen::Vector2d default_safety_margin_data_; + + /// This use when requesting collision data because you can only provide a + /// single contact distance threshold. + double max_safety_margin_; + + /// A map of link pair to contact distance setting [dist_pen, coeff] + AlignedUnorderedMap pair_lookup_table_; +}; +typedef std::shared_ptr SafetyMarginDataPtr; +typedef std::shared_ptr SafetyMarginDataConstPtr; + +/** + * @brief This is a utility function for creating the Safety Margin data vector + * @param num_elements The number of objects to create + * @param default_safety_margin Default safety margin + * @param default_safety_margin_coeff Default safety margin coeff + * @return A vector of Safety Margin Data + */ +inline std::vector createSafetyMarginDataVector(int num_elements, + const double& default_safety_margin, + const double& default_safety_margin_coeff) +{ + std::vector info; + info.reserve(static_cast(num_elements)); + for (auto i = 0; i < num_elements; ++i) + { + info.push_back(SafetyMarginDataPtr(new SafetyMarginData(default_safety_margin, default_safety_margin_coeff))); + } + return info; +} + +/** + * Print a vector + * */ +template +void printVector(std::string str, std::vector v) +{ + std::stringstream ss; + ss << str << " "; + for (std::size_t i = 0; i < v.size(); ++i) + ss << v[i] << " "; + + std::cout << ss.str() << std::endl; +} + +} diff --git a/moveit_planners/trajopt/trajopt/package.xml b/moveit_planners/trajopt/trajopt/package.xml new file mode 100644 index 0000000000..f48c24f154 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/package.xml @@ -0,0 +1,28 @@ + + + trajopt + 0.1.0 + The trajopt package + Levi Armstrong + BSD + Apache 2.0 + Levi Armstrong + John Schulman + + catkin + trajopt_sco + trajopt_utils + roscpp + moveit_core + + gtest + rostest + roslib + libpcl-all-dev + pcl_conversions + pr2_description + + + + + diff --git a/moveit_planners/trajopt/trajopt/src/collision_terms.cpp b/moveit_planners/trajopt/trajopt/src/collision_terms.cpp new file mode 100644 index 0000000000..cb290304d6 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/src/collision_terms.cpp @@ -0,0 +1,633 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +//#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace trajopt +{ + +// converts a vector of contacts to a vector of distances +void CollisionsToDistances(const trajopt::ContactResultVector& dist_results, DblVec& dists) +{ + dists.clear(); + dists.reserve(dist_results.size()); + + // how to get distance from MoveIt to feed dists + for (auto i = 0u; i < dist_results.size(); ++i) + dists.push_back(dist_results[i].distance); + + // for (auto i = 0u; i < dist_results.size(); ++i) + // dists.push_back(dist_results[i].depth); +} + +// directly related to equation 16 in the TrajOpt paper (the version I have) +// this function goes over all the contact in dist_results and calcualte the jac of the first-link-in-contact +// at the nearest point. Also it calculates the jac of the second-link-in-contact at the nearest point +// However, if the collision type is set to CCType_Between, then the cc_nearest_point is used only for the +// second link's jacobian calculations. +void CollisionsToDistanceExpressions(const trajopt::ContactResultVector& dist_results, + planning_scene::PlanningSceneConstPtr planning_scene, + std::string planning_group, + const sco::VarVector& vars, + const DblVec& x, + sco::AffExprVector& exprs, + bool isTimestep1) +{ + // we are trying to fill up exprs which should be the signed distance expression ??? + // typedef std::vector AffExprVector + // AffExpr is struct in solver_interface.h: + // constant, coeffs, vars + + // typedef std::vector VarVector + // typedef std:vector DblVec + + Eigen::VectorXd dofvals = sco::getVec(x, vars); // joint values + // const std::vector& link_names = manip->getLinkNames(); + const std::vector& link_names = planning_scene->getRobotModel()->getLinkModelNames(); + + // All collision data is in world corrdinate system. This provides the + // transfrom for converting data between world frame and manipulator + // frame. + // tesseract::EnvStateConstPtr state = env->getState(); + // Eigen::Isometry3d change_base = state->transforms.at(manip->getBaseLinkName()); + // assert(change_base.isApprox(env->getState(manip->getJointNames(), dofvals)->transforms.at(manip->getBaseLinkName()))); + + const moveit::core::RobotState robot_state = planning_scene->getCurrentState(); + const robot_state::JointModelGroup* joint_model_group = robot_state.getJointModelGroup(planning_group); + std::vector group_joint_names = joint_model_group->getActiveJointModelNames(); + // trajopt::printVector("===>>> group joint names", group_joint_names); + int group_dof = (int)group_joint_names.size(); + + exprs.clear(); + exprs.reserve(dist_results.size()); + for (auto i = 0u; i < dist_results.size(); ++i) + { + // => for each ContactResult in the total ContactResultVector that contains all the contacts of each pair and all the pairs + // const tesseract::ContactResult& res = dist_results[i]; + const trajopt::ContactResult& res = dist_results[i]; + + // cc_type is a member of ContactResult in tesseract and by defualt (in clear() function) is set + // to cc_type = ContinouseCollisionType::CCType_None; I do the same here: + trajopt::ContinouseCollisionType cc_type = ContinouseCollisionType::CCType_None; + + // ContactResult in the original trajopt has two bodies and a distance + // In MoveIt, we have Contact (corresponding to ContactResult in tesseract) type + // which has depth (penetration between bodies). I am going to use this depth + sco::AffExpr dist(res.distance); + // sco::AffExpr dist(res.depth); // depth could be positive or negative, if I use bullet + // is that a problem here? + + Eigen::VectorXd dist_grad_a, dist_grad_b; + // => find the name of linkA in collision + std::vector::const_iterator itA = std::find(link_names.begin(), link_names.end(), res.link_names[0]); + if (itA != link_names.end()) + { + // => create a jacobian matrix + Eigen::MatrixXd jac; + jac.resize(6, group_dof); + + const moveit::core::LinkModel* link_0 = robot_state.getLinkModel(res.link_names[0]); + + + /* Compute the Jacobian with reference to a particular point on a given link, for a specified group. + * - group: The group to compute the Jacobian for + * - link_name: The name of the link + * - reference_point_position: The reference point position (with respect to the link specified in link_name) + * - jacobian: The resultant jacobian + * - use_quaternion_representation Flag indicating if the Jacobian should use a quaternion representation + * (default is false) + * - return True if jacobian was successfully computed, false otherwise + */ + bool succeed = robot_state.getJacobian( + joint_model_group, + link_0, + res.nearest_points[0], + jac); + // joint values are fixed, we are chaning links in contacts to calculate the jacobian + + // what does it meant to caluclate a jacobian at a given link and point ???????? + + /** tesseract: + * Calculated jacobian at a link given joint angles + * jac: jacobian Output jacobian for a given link + * change_base: The transform from the base frame of the manipulator to the desired frame. + * joint_angles: Input vector of joint angles + * link_name: Name of link to calculate jacobian + * state: The state of the environment + * link_point: Point in the base frame for which to calculate the jacobian about + * output is a bool: True if calculation successful, False if anything is wrong (including uninitialized BasicKin) + */ + // manip->calcJacobian(jac, + // change_base, + // dofvals, + // res.link_names[0], + // *state, + // res.nearest_points[0]); + + dist_grad_a = -res.normal.transpose() * jac.topRows(3); + // this means: [normal . jac(1:3,1), normal . jac(1:3,2), normal . jac(1:3,3)] + // in the paper: nT * J_PA + + sco::exprInc(dist, sco::varDot(dist_grad_a, vars)); + // varDot: creates an AffExpr with constant = 0, vars = second arg and coeffs = first arg + // exprInc: adds the second arg to the first one + + sco::exprInc(dist, -dist_grad_a.dot(dofvals)); + // the above means: -dist_grad_a (dot_product) dofvals + } + + std::vector::const_iterator itB = std::find(link_names.begin(), link_names.end(), res.link_names[1]); + if (itB != link_names.end()) + { + Eigen::MatrixXd jac; + jac.resize(6, group_dof); + + const moveit::core::LinkModel* link_1 = robot_state.getLinkModel(res.link_names[1]); + + + // how do I calculate cc_nearest_points, Contact in MoveIt does not have such a thing + bool succeed = robot_state.getJacobian( + joint_model_group, + link_1, + (isTimestep1 && (cc_type == trajopt::ContinouseCollisionType::CCType_Between)) ? + res.cc_nearest_points[1] : + res.nearest_points[1], + jac); + + + + + dist_grad_b = res.normal.transpose() * jac.topRows(3); + sco::exprInc(dist, sco::varDot(dist_grad_b, vars)); + sco::exprInc(dist, -dist_grad_b.dot(dofvals)); + } + // DebugPrintInfo(res, dist_grad_a, dist_grad_b, dofvals, i == 0); + + if (itA != link_names.end() || itB != link_names.end()) + { + exprs.push_back(dist); + } + } +} + +// This function creates affine expression +void CollisionsToDistanceExpressions(const trajopt::ContactResultVector& dist_results, + planning_scene::PlanningSceneConstPtr planning_scene, + std::string planning_group, + const sco::VarVector& vars0, + const sco::VarVector& vars1, + const DblVec& x, + sco::AffExprVector& exprs) +{ + sco::AffExprVector exprs0, exprs1; + CollisionsToDistanceExpressions(dist_results, planning_scene, planning_group, vars0, x, exprs0, false); + CollisionsToDistanceExpressions(dist_results, planning_scene, planning_group, vars1, x, exprs1, true); + + exprs.resize(exprs0.size()); + for (std::size_t i = 0; i < exprs0.size(); ++i) + { + // ??? dist_result: contact in MoveIt does not have cc_time + assert(dist_results[i].cc_time >= 0.0 && dist_results[i].cc_time <= 1.0); + sco::exprScale(exprs0[i], (1 - dist_results[i].cc_time)); + sco::exprScale(exprs1[i], dist_results[i].cc_time); + exprs[i] = sco::AffExpr(0); + sco::exprInc(exprs[i], exprs0[i]); + sco::exprInc(exprs[i], exprs1[i]); + sco::cleanupAff(exprs[i]); + } +} + +inline size_t hash(const DblVec& x) { return boost::hash_range(x.begin(), x.end()); } + +void CollisionEvaluator::GetCollisionsCached(const DblVec& x, trajopt::ContactResultVector& dist_results) +{ + size_t key = hash(sco::getDblVec(x, GetVars())); + trajopt::ContactResultVector* it = m_cache.get(key); + + if (it != nullptr) + { + LOG_DEBUG("using cached collision check\n"); + dist_results = *it; + } + else + { + LOG_DEBUG("not using cached collision check\n"); + CalcCollisions(x, dist_results); + m_cache.put(key, dist_results); + } +} + +SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator( + planning_scene::PlanningSceneConstPtr& planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data, + const sco::VarVector& vars) + : CollisionEvaluator(planning_scene, planning_group, safety_margin_data), m_vars(vars) +{ + // contact_manager_ = env_->getDiscreteContactManager(); + // contact_manager_->setActiveCollisionObjects(manip_->getLinkNames()); + // contact_manager_->setContactDistanceThreshold(safety_margin_data_->getMaxSafetyMargin() + + // 0.04); // The original implementation added a margin of 0.04; +} + +// So, each pair has a vector of contacts, and this functions puts all of these vectors of all pairs to one big vector called dist_results +void SingleTimestepCollisionEvaluator::CalcCollisions(const DblVec& x, trajopt::ContactResultVector& dist_results) +{ + // tesseract::ContactResultMap contacts; + // tesseract::EnvStatePtr state = env_->getState(manip_->getJointNames(), sco::getVec(x, m_vars)); + // ===> getVec is defined as: + // Eigen::VectorXd getVec(const DblVec& x, const VarVector& vars) and + // DblVec is vector + // VarVec is vector + // Var has VarRep (look in solver_interface) member which has properties like name and index + // getVec extracts the index of VarRep of each Var of a VarVector and finds the element in DblVec with the + // same index and creates the output as VectorXd. + // Finally, getState(joint_names, joint_values) + // So, input x of this function, CalcCollisions, is represting the joint values + // joint_values = sco::getVec(x, m_vars); + + // So, I guess I need to set the robot state based on these joint values + Eigen::VectorXd joint_values = sco::getVec(x, m_vars); + std::vector joint_values_DblVec = std::vector(joint_values.data(), joint_values.data() + joint_values.size()); + // the following robot state will be a copy not a pointer so it wont make any change in the planning scene + moveit::core::RobotState& rob_state = planning_scene_->getCurrentStateNonConst(); + const moveit::core::JointModelGroup* joint_model_group = rob_state.getJointModelGroup(planning_group_); + // rob_state.setToDefaultValues(); + rob_state.setJointGroupPositions(joint_model_group, joint_values_DblVec); + rob_state.update(); + // Here is the point: I created a copy of the robot_state from planning_scene_->getCurrentState() + // and updated that copy but that does not change anything in the planning_scene_ + // as it is a const pointer. const pointer planning scene comes from planning_context and + // I can not change that. + // planning_scene->setCurrentState(rob_state); + std::vector vv; + planning_scene_->getCurrentState().copyJointGroupPositions(joint_model_group, vv); + // trajopt::printVector("current state joint values: ", vv); + + // std::cout << "===>>> collision detection name: " << planning_scene_->getActiveCollisionDetectorName() << std::endl; + // std::cout << "planning scene's name: " << planning_scene_->getName() << std::endl; + + // does planning_scene include the collision box? + // std::vector collision_objs; + // planning_scene_->getCollisionObjectMsgs(collision_objs); + // for(std::size_t z = 0; z < collision_objs.size(); ++z) + // std::cout << "===>>> name of the collision object: " << collision_objs[z].id << " "; + // std::cout << std::endl; + + // std::vector obj_ids = planning_scene_->getWorld()->getObjectIds(); + // trajopt::printVector("world object ids: ", obj_ids); + + // std::cout << "===>>> robot model: " << std::endl; + // std::cout << "name: " << planning_scene_->getRobotModel()->getName() << std::endl; + // std::cout << "root joint: " << planning_scene_->getRobotModel()->getRootJointName() << std::endl; + // std::cout << "root link: " << planning_scene_->getRobotModel()->getRootLinkName() << std::endl; + // std::cout << "joint name: " << planning_scene_->getRobotModel()->getJointModelNames()[0] << std::endl; + + // std::vector lnames = planning_scene_->getRobotModel()->getLinkModelNames(); + // trajopt::printVector("link model names: ", lnames); + + + // for (const auto& link_name : manip_->getLinkNames()) + // contact_manager_->setCollisionObjectsTransform(link_name, state->transforms[link_name]); + // ====> + // setCollisionObjectTransform sets the transform of a collision object. + // contact manager has collision objects and has functions to remove/add them or + // set their transforms. This is basically adding all the robot links as collision objects + // We already have this in planning_scene, calling fcl or bullet would consider all the robot links as collision object? + + //contact_manager_->contactTest(contacts, tesseract::ContactTestTypes::ALL); + // ====> ContactTestTypes::ALL = > Return all contacts for a pair of objects + + // tesseract::moveContactResultsMapToContactResultsVector(contacts, dist_results); + // ====> moveContactResultsMapToContactResultsVector moves the elements of ContactResultVector + // of each ContactResultMap in contacts to one ContactResultVector called dist_results + + // ===> for MoveIt + collision_detection::CollisionRequest collision_request; + collision_detection::CollisionResult collision_result; + collision_request.group_name = planning_group_; + // collision_request.distance = true; // If true, compute proximity distance + collision_request.contacts = true; // If true, compute contacts. Otherwise only a binary collision yes/no is reported + collision_request.max_contacts = 100; + collision_request.max_contacts_per_pair = 5; + collision_request.verbose = false; + + // planning_scene_->printKnownObjects(); + + planning_scene_->checkCollision(collision_request, collision_result, rob_state); + // ====> checkCollision will check for both self-collisions and for collisions with the environment + LOG_DEBUG("single time step in collision? %s", collision_result.collision ? "true" : "false"); + LOG_DEBUG("contact count: %i", collision_result.contact_count); + + // tesseract: + // ContactResultMap => AlignedMap, ContactResultVector> + // ContactResultVector: is an array of ContactResult msg + // ContactResult: + // distance + // array of two (link_names ) + // MoveIt: + // CollisionResult: + // distance (min distance among all collision pairs) (double) + // contact_count (number of contacts) + // ContactMap + // + // ContactMap => std::map, std::vector > + // Contact: + // pos (contact position) + // normal (normal unit vector at contact) + // depth (penetration between bodies) + // body_name_1 + // body_name_2 + // + + + // Fill dist_result with all the Contacts of all the collision pairs. Each pair has a vector of contacts + // and we have many pairs. Here we are putting all of these contact to one big vector. Not sure why that is ??? + // This happens in moveContactResultsMapToContactResultsVector + for (auto it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it) + { + ROS_INFO("Contact between: %s and %s", it->first.first.c_str(), it->first.second.c_str()); + + std::vector contact_vector = it->second; + for(size_t k = 0; k < contact_vector.size(); ++k) + { + // convert Contact from MoveIt to ContactResult + Eigen::Vector3d moveit_cc_nearest_points[2]; + moveit_cc_nearest_points[0].setZero(); + moveit_cc_nearest_points[1].setZero(); + double moveit_cc_time = -1; + trajopt::ContinouseCollisionType moviet_cc_type = trajopt::ContinouseCollisionType::CCType_None; + + ContactResult contact_result(contact_vector[k], + moveit_cc_nearest_points, + moveit_cc_time, + moviet_cc_type); + dist_results.push_back(contact_result); + } + } +} + +// so, given a set of joint values called x, this function converts the vector of contacts to the vector of distances +void SingleTimestepCollisionEvaluator::CalcDists(const DblVec& x, DblVec& dists) +{ + trajopt::ContactResultVector dist_results; + GetCollisionsCached(x, dist_results); + CollisionsToDistances(dist_results, dists); +} + +void SingleTimestepCollisionEvaluator::CalcDistExpressions(const DblVec& x, sco::AffExprVector& exprs) +{ + trajopt::ContactResultVector dist_results; + GetCollisionsCached(x, dist_results); + CollisionsToDistanceExpressions(dist_results, planning_scene_, planning_group_, m_vars, x, exprs, false); + LOG_DEBUG("%ld distance expressions\n", exprs.size()); +} + + +//////////////////////////////////////// + +CastCollisionEvaluator::CastCollisionEvaluator(planning_scene::PlanningSceneConstPtr planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data, + const sco::VarVector& vars0, + const sco::VarVector& vars1) + : CollisionEvaluator(planning_scene, planning_group, safety_margin_data), m_vars0(vars0), m_vars1(vars1) +{ + // contact_manager_ = env_->getContinuousContactManager(); + // contact_manager_->setActiveCollisionObjects(manip_->getLinkNames()); + // contact_manager_->setContactDistanceThreshold(safety_margin_data_->getMaxSafetyMargin() + + // 0.04); // The original implementation added a margin of 0.04; + +} + +// similar to CalcCollisions from SingleTimestepCollisionEvaluator except we need two states because it is the +// swept volume? +void CastCollisionEvaluator::CalcCollisions(const DblVec& x, trajopt::ContactResultVector& dist_results) +{ + // tesseract::ContactResultMap contacts; + // tesseract::EnvStatePtr state0 = env_->getState(manip_->getJointNames(), sco::getVec(x, m_vars0)); + // tesseract::EnvStatePtr state1 = env_->getState(manip_->getJointNames(), sco::getVec(x, m_vars1)); + // ===> so far they create two states based on two sets of joint values + + // for (const auto& link_name : manip_->getLinkNames()) + // contact_manager_->setCollisionObjectsTransform(link_name, state0->transforms[link_name], state1->transforms[link_name]); + // ===>>> in here, they add two collision transforms for each link + + // ===>>> in the folloinwg, considering two collision transforms for each link, they calculate all the contacts among them + // contact_manager_->contactTest(contacts, tesseract::ContactTestTypes::ALL); + // tesseract::moveContactResultsMapToContactResultsVector(contacts, dist_results); + + // In MoveIt we do have a checkCollision function that gets the two states in continous manner + // when using Bullet that function will be overriden by a real continous collision checking process + // All I have to do here is use that function for two + + // =====>>> So, I need to do it for two states as well. + // x has joint valus for all time steps: [j1_0 j2_0 ... j7_0, j1_1 j2_1 ... j7_1 ... j1_n j2_n ... j7_n] + Eigen::VectorXd joint_values_state_0 = sco::getVec(x, m_vars0); + std::vector joint_values_DblVec_0 = std::vector(joint_values_state_0.data(), joint_values_state_0.data() + joint_values_state_0.size()); + + moveit::core::RobotState& rob_state = planning_scene_->getCurrentStateNonConst(); + const moveit::core::JointModelGroup* joint_model_group = rob_state.getJointModelGroup(planning_group_); + + collision_detection::CollisionRequest collision_request; + collision_detection::CollisionResult collision_result; + collision_request.group_name = planning_group_; + // collision_request.distance = true; + collision_request.contacts = true; + collision_request.max_contacts = 100; + collision_request.max_contacts_per_pair = 5; + collision_request.verbose = false; + + rob_state.setToDefaultValues(); + rob_state.setJointGroupPositions(joint_model_group, joint_values_DblVec_0); + rob_state.update(); + robot_state::RobotState rob_state_previous(rob_state); + + // Eigen::VectorXd first_values; + // rob_state.copyJointGroupPositions(joint_model_group, first_values); + // std::cout << "===>>> first state joint values: " << first_values.transpose() << std::endl; + + Eigen::VectorXd joint_values_state_1 = sco::getVec(x, m_vars1); + std::vector joint_values_DblVec_1 = std::vector(joint_values_state_1.data(), joint_values_state_1.data() + joint_values_state_1.size()); + rob_state.setJointGroupPositions(joint_model_group, joint_values_DblVec_1); + rob_state.update(); + + // Eigen::VectorXd second_values; + // rob_state.copyJointGroupPositions(joint_model_group, second_values); + // std::cout << "===>>> second state joint values: " << second_values.transpose() << std::endl; + + // check collision between two states + planning_scene_->getCollisionEnv()->checkRobotCollision(collision_request, collision_result, + rob_state, rob_state_previous); + + LOG_DEBUG("single time step in collision? %s", collision_result.collision ? "true" : "false"); + LOG_DEBUG("contact count: %i", collision_result.contact_count); + + // ????? I have to calculate: cc_time, cc_type and cc_nearest_points here. test + + // when we use bullet in MoveIt, CollisionResult returns all the contacts in the scene by ContactMap data type. + // ContactMap < , vector> + // when using bullet, vector has only one member while fcl might return multiple contact points for + // each pair in contact + + + + for (auto it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it) + { + ROS_INFO("Contact between: %s and %s", it->first.first.c_str(), it->first.second.c_str()); + + // using bullet returns one contact point per pair of contact objects but just in case, I traverse through + // all the element of the vector + std::vector contact_vector = it->second; + for(size_t k = 0; k < contact_vector.size(); ++k) + { + // convert Contact from MoveIt to ContactResult in tesseract: + Eigen::Vector3d moveit_cc_nearest_points[2] = contact_vector[k].nearest_points; + double moveit_cc_time = 0.5; + trajopt::ContinouseCollisionType moviet_cc_type = trajopt::ContinouseCollisionType::CCType_Between; + + ContactResult contact_result(contact_vector[k], + moveit_cc_nearest_points, + moveit_cc_time, + moviet_cc_type); + dist_results.push_back(contact_result); + } + } +} + +void CastCollisionEvaluator::CalcDistExpressions(const DblVec& x, sco::AffExprVector& exprs) +{ + trajopt::ContactResultVector dist_results; + GetCollisionsCached(x, dist_results); + CollisionsToDistanceExpressions(dist_results, planning_scene_, planning_group_, m_vars0, m_vars1, x, exprs); +} +void CastCollisionEvaluator::CalcDists(const DblVec& x, DblVec& dists) +{ + trajopt::ContactResultVector dist_results; + GetCollisionsCached(x, dist_results); + CollisionsToDistances(dist_results, dists); +} + +////////////////////////////////////////// + +CollisionCost::CollisionCost(planning_scene::PlanningSceneConstPtr& planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data, + const sco::VarVector& vars) + : Cost("collision"), m_calc(new SingleTimestepCollisionEvaluator(planning_scene, planning_group, safety_margin_data, vars)) +{ +} + +CollisionCost::CollisionCost(planning_scene::PlanningSceneConstPtr& planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data, + const sco::VarVector& vars0, + const sco::VarVector& vars1) + : Cost("cast_collision"), m_calc(new CastCollisionEvaluator(planning_scene, planning_group, safety_margin_data, vars0, vars1)) +{ +} + +sco::ConvexObjectivePtr CollisionCost::convex(const sco::DblVec& x, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + sco::AffExprVector exprs; + m_calc->CalcDistExpressions(x, exprs); + + trajopt::ContactResultVector dist_results; + m_calc->GetCollisionsCached(x, dist_results); + for (std::size_t i = 0; i < exprs.size(); ++i) + { + const Eigen::Vector2d& data = m_calc->getSafetyMarginData()->getPairSafetyMarginData(dist_results[i].link_names[0], + dist_results[i].link_names[1]); + + sco::AffExpr viol = sco::exprSub(sco::AffExpr(data[0]), exprs[i]); + out->addHinge(viol, data[1]); + } + return out; +} + +double CollisionCost::value(const sco::DblVec& x) +{ + DblVec dists; + m_calc->CalcDists(x, dists); + + trajopt::ContactResultVector dist_results; + m_calc->GetCollisionsCached(x, dist_results); + double out = 0; + for (std::size_t i = 0; i < dists.size(); ++i) + { + //??? link_name from dist_results in MoveIt + const Eigen::Vector2d& data = m_calc->getSafetyMarginData()->getPairSafetyMarginData(dist_results[i].link_names[0], + dist_results[i].link_names[1]); + out += sco::pospart(data[0] - dists[i]) * data[1]; + } + return out; +} + +// ALMOST EXACTLY COPIED FROM CollisionCost + +CollisionConstraint::CollisionConstraint(planning_scene::PlanningSceneConstPtr planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data, + const sco::VarVector& vars) + : m_calc(new SingleTimestepCollisionEvaluator(planning_scene, planning_group, safety_margin_data, vars)) +{ + name_ = "collision"; +} + +CollisionConstraint::CollisionConstraint(planning_scene::PlanningSceneConstPtr planning_scene, std::string planning_group, + SafetyMarginDataConstPtr safety_margin_data, + const sco::VarVector& vars0, + const sco::VarVector& vars1) + : m_calc(new CastCollisionEvaluator(planning_scene, planning_group, safety_margin_data, vars0, vars1)) +{ + name_ = "collision"; +} + +sco::ConvexConstraintsPtr CollisionConstraint::convex(const sco::DblVec& x, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + sco::AffExprVector exprs; + m_calc->CalcDistExpressions(x, exprs); + + trajopt::ContactResultVector dist_results; + m_calc->GetCollisionsCached(x, dist_results); + for (std::size_t i = 0; i < exprs.size(); ++i) + { + const Eigen::Vector2d& data = m_calc->getSafetyMarginData()->getPairSafetyMarginData(dist_results[i].link_names[0], + dist_results[i].link_names[1]); + + sco::AffExpr viol = sco::exprSub(sco::AffExpr(data[0]), exprs[i]); + out->addIneqCnt(sco::exprMult(viol, data[1])); + } + return out; +} + +DblVec CollisionConstraint::value(const sco::DblVec& x) +{ + DblVec dists; + m_calc->CalcDists(x, dists); + + trajopt::ContactResultVector dist_results; + m_calc->GetCollisionsCached(x, dist_results); + DblVec out(dists.size()); + for (std::size_t i = 0; i < dists.size(); ++i) + { + const Eigen::Vector2d& data = m_calc->getSafetyMarginData()->getPairSafetyMarginData(dist_results[i].link_names[0], + dist_results[i].link_names[1]); + + out[i] = sco::pospart(data[0] - dists[i]) * data[1]; + } + return out; +} +} diff --git a/moveit_planners/trajopt/trajopt/src/kinematic_terms.cpp b/moveit_planners/trajopt/trajopt/src/kinematic_terms.cpp new file mode 100644 index 0000000000..09e6daeadf --- /dev/null +++ b/moveit_planners/trajopt/trajopt/src/kinematic_terms.cpp @@ -0,0 +1,100 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik, LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Omid Heidari */ + +#include +#include + +#include +#include + +#include "trajopt/kinematic_terms.h" + +using namespace std; +using namespace sco; +using namespace Eigen; + +namespace trajopt +{ +VectorXd CartPoseErrCalculator::operator()(const VectorXd& dof_vals) const +{ + // TODO: create the actual error function from information in planning scene + VectorXd err; + return err; +} + +VectorXd JointVelErrCalculator::operator()(const VectorXd& var_vals) const +{ + assert(var_vals.rows() % 2 == 0); + // var_vals = (theta_t1, theta_t2, theta_t3 ... 1/dt_1, 1/dt_2, 1/dt_3 ...) + int half = static_cast(var_vals.rows() / 2); + int num_vels = half - 1; + // (x1-x0)*(1/dt) + VectorXd vel = (var_vals.segment(1, num_vels) - var_vals.segment(0, num_vels)).array() * + var_vals.segment(half + 1, num_vels).array(); + + // Note that for equality terms tols are 0, so error is effectively doubled + VectorXd result(vel.rows() * 2); + result.topRows(vel.rows()) = -(upper_tol_ - (vel.array() - target_)); + result.bottomRows(vel.rows()) = lower_tol_ - (vel.array() - target_); + return result; +} + +MatrixXd JointVelJacobianCalculator::operator()(const VectorXd& var_vals) const +{ + // var_vals = (theta_t1, theta_t2, theta_t3 ... 1/dt_1, 1/dt_2, 1/dt_3 ...) + int num_vals = static_cast(var_vals.rows()); + int half = num_vals / 2; + int num_vels = half - 1; + MatrixXd jac = MatrixXd::Zero(num_vels * 2, num_vals); + + for (int i = 0; i < num_vels; i++) + { + // v = (j_i+1 - j_i)*(1/dt) + // We calculate v with the dt from the second pt + int time_index = i + half + 1; + jac(i, i) = -1.0 * var_vals(time_index); + jac(i, i + 1) = 1.0 * var_vals(time_index); + jac(i, time_index) = var_vals(i + 1) - var_vals(i); + // All others are 0 + } + + // bottom half is negative velocities + jac.bottomRows(num_vels) = -jac.topRows(num_vels); + + return jac; +} + +} // namespace trajopt diff --git a/moveit_planners/trajopt/trajopt/src/problem_description.cpp b/moveit_planners/trajopt/trajopt/src/problem_description.cpp new file mode 100644 index 0000000000..a2eab2d8cd --- /dev/null +++ b/moveit_planners/trajopt/trajopt/src/problem_description.cpp @@ -0,0 +1,667 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, John Schulman + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * http://opensource.org/licenses/BSD-2-Clause + *********************************************************************/ + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include "trajopt/problem_description.h" +#include "trajopt/collision_terms.h" + +/** + * @brief Checks the size of the parameter given and throws if incorrect + * @param parameter The vector whose size is getting checked + * @param expected_size The expected size of the vector + * @param name The name to use when printing an error or warning + * @param apply_first If true and only one value is given, broadcast value to length of expected_size + */ +void checkParameterSize(trajopt::DblVec& parameter, const unsigned int& expected_size, const std::string& name, + const bool& apply_first = true) +{ + if (apply_first == true && parameter.size() == 1) + { + parameter = trajopt::DblVec(expected_size, parameter[0]); + ROS_INFO("1 %s given. Applying to all %i joints", name.c_str(), expected_size); + } + else if (parameter.size() != expected_size) + { + PRINT_AND_THROW(boost::format("wrong number of %s. expected %i got %i") % name % expected_size % parameter.size()); + } +} + +namespace trajopt +{ +TrajOptProblem::TrajOptProblem() +{ +} + +TrajOptProblem::TrajOptProblem(const ProblemInfo& problem_info) + : OptProb(problem_info.basic_info.convex_solver) + , planning_scene_(problem_info.planning_scene) + , planning_group_(problem_info.planning_group_name) +{ + moveit::core::RobotModelConstPtr robot_model = planning_scene_->getRobotModel(); + moveit::core::RobotState current_state = planning_scene_->getCurrentState(); + const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup(planning_group_); + + moveit::core::JointBoundsVector bounds = joint_model_group->getActiveJointModelsBounds(); + dof_ = static_cast(joint_model_group->getActiveJointModelNames().size()); // or bounds.size(); + + int n_steps = problem_info.basic_info.n_steps; + + ROS_INFO(" ======================================= problem_description: limits"); + Eigen::MatrixX2d limits(dof_, 2); + for (int k = 0; k < limits.size() / 2; ++k) + { + moveit::core::JointModel::Bounds bound = *bounds[k]; + // In MoveIt, joints are considered to have multiple dofs but we only have single dof joints: + moveit::core::VariableBounds joint_bound = bound.front(); + + limits(k, 0) = joint_bound.min_position_; + limits(k, 1) = joint_bound.max_position_; + + ROS_INFO("joint %i with lower bound: %f, upper bound: %f", k, joint_bound.min_position_, joint_bound.max_position_); + } + + Eigen::VectorXd lower, upper; + lower = limits.col(0); + upper = limits.col(1); + + trajopt::DblVec vlower, vupper; + std::vector names; + for (int i = 0; i < n_steps; ++i) + { + for (int j = 0; j < dof_; ++j) + { + names.push_back((boost::format("j_%i_%i") % i % j).str()); + } + vlower.insert(vlower.end(), lower.data(), lower.data() + lower.size()); + vupper.insert(vupper.end(), upper.data(), upper.data() + upper.size()); + + if (problem_info.basic_info.use_time == true) + { + vlower.insert(vlower.end(), problem_info.basic_info.dt_lower_lim); + vupper.insert(vupper.end(), problem_info.basic_info.dt_upper_lim); + names.push_back((boost::format("dt_%i") % i).str()); + } + } + + sco::VarVector trajvarvec = createVariables(names, vlower, vupper); + matrix_traj_vars = trajopt::VarArray(n_steps, dof_ + (problem_info.basic_info.use_time ? 1 : 0), trajvarvec.data()); + // matrix_traj_vars is essentialy a matrix of elements like: + // j_0_0, j_0_1 ... + // j_1_0, j_1_1 ... + // its size is n_steps by n_dof +} + +TrajOptProblemPtr ConstructProblem(const ProblemInfo& pci) +{ + const BasicInfo& bi = pci.basic_info; + int n_steps = bi.n_steps; + + bool use_time = false; + // Check that all costs and constraints support the types that are specified in pci + for (TermInfoPtr cost : pci.cost_infos) + { + if (cost->term_type & TT_CNT) + ROS_WARN("%s is listed as a type TT_CNT but was added to cost_infos", (cost->name).c_str()); + if (!(cost->getSupportedTypes() & TT_COST)) + PRINT_AND_THROW(boost::format("%s is only a constraint, but you listed it as a cost") % cost->name); + if (cost->term_type & TT_USE_TIME) + { + use_time = true; + if (!(cost->getSupportedTypes() & TT_USE_TIME)) + PRINT_AND_THROW(boost::format("%s does not support time, but you listed it as a using time") % cost->name); + } + } + for (TermInfoPtr cnt : pci.cnt_infos) + { + if (cnt->term_type & TT_COST) + ROS_WARN("%s is listed as a type TT_COST but was added to cnt_infos", (cnt->name).c_str()); + if (!(cnt->getSupportedTypes() & TT_CNT)) + PRINT_AND_THROW(boost::format("%s is only a cost, but you listed it as a constraint") % cnt->name); + if (cnt->term_type & TT_USE_TIME) + { + use_time = true; + if (!(cnt->getSupportedTypes() & TT_USE_TIME)) + PRINT_AND_THROW(boost::format("%s does not support time, but you listed it as a using time") % cnt->name); + } + } + + // Check that if a cost or constraint uses time, basic_info is set accordingly + if ((use_time == true) && (pci.basic_info.use_time == false)) + PRINT_AND_THROW("A term is using time and basic_info is not set correctly. Try basic_info.use_time = true"); + + // This could be removed in the future once we are sure that all costs are + if ((use_time == false) && (pci.basic_info.use_time == true)) + PRINT_AND_THROW("No terms use time and basic_info is not set correctly. Try basic_info.use_time = false"); + + TrajOptProblemPtr prob(new TrajOptProblem(pci)); + + // Generate initial trajectory and check its size + moveit::core::RobotModelConstPtr robot_model = pci.planning_scene->getRobotModel(); + moveit::core::RobotState current_state = pci.planning_scene->getCurrentState(); + + const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup(pci.planning_group_name); + int n_dof = prob->GetNumDOF(); + + std::vector current_joint_values; + current_state.copyJointGroupPositions(joint_model_group, current_joint_values); + // trajopt::printVector("===>> from problem_description: ", current_joint_values); + + trajopt::TrajArray init_traj; + generateInitialTrajectory(pci, current_joint_values, init_traj); + + if (pci.basic_info.use_time == true) + { + prob->SetHasTime(true); + if (init_traj.rows() != n_steps || init_traj.cols() != n_dof + 1) + { + PRINT_AND_THROW(boost::format("Initial trajectory is not the right size matrix\n" + "Expected %i rows (time steps) x %i columns (%i dof + 1 time column)\n" + "Got %i rows and %i columns") % + n_steps % (n_dof + 1) % n_dof % init_traj.rows() % init_traj.cols()); + } + } + else + { + prob->SetHasTime(false); + if (init_traj.rows() != n_steps || init_traj.cols() != n_dof) + { + PRINT_AND_THROW(boost::format("Initial trajectory is not the right size matrix\n" + "Expected %i rows (time steps) x %i columns\n" + "Got %i rows and %i columns") % + n_steps % n_dof % init_traj.rows() % init_traj.cols()); + } + } + prob->SetInitTraj(init_traj); + + trajopt::VarArray matrix_traj_vars_temp; + // If start_fixed, constrain the joint values for the first time step to be their initialized values + if (bi.start_fixed) + { + if (init_traj.rows() < 1) + { + PRINT_AND_THROW("Initial trajectory must contain at least the start state."); + } + + if (init_traj.cols() != (n_dof + (use_time ? 1 : 0))) + { + PRINT_AND_THROW("robot dof values don't match initialization. I don't " + "know what you want me to use for the dof values"); + } + + for (int j = 0; j < static_cast(n_dof); ++j) + { + matrix_traj_vars_temp = prob->GetVars(); + prob->addLinearConstraint(sco::exprSub(sco::AffExpr(matrix_traj_vars_temp(0, j)), init_traj(0, j)), sco::EQ); + } + } + + // Apply constraint to each fixed dof to its initial value for all timesteps (freeze that column) + if (!bi.dofs_fixed.empty()) + { + for (const int& dof_ind : bi.dofs_fixed) + { + for (int i = 1; i < prob->GetNumSteps(); ++i) + { + matrix_traj_vars_temp = prob->GetVars(); + prob->addLinearConstraint(sco::exprSub(sco::AffExpr(matrix_traj_vars_temp(i, dof_ind)), + sco::AffExpr(init_traj(0, dof_ind))), + sco::EQ); + } + } + } + + for (const TermInfoPtr& ci : pci.cost_infos) + { + ROS_INFO("problem description: the name of the cost: %s", ci->name.c_str()); + ci->addObjectiveTerms(*prob); + } + + for (const TermInfoPtr& ci : pci.cnt_infos) + { + ROS_INFO("problem description:: the name of the constraint: %s", ci->name.c_str()); + ci->addObjectiveTerms(*prob); + } + return prob; +} + +CartPoseTermInfo::CartPoseTermInfo() : TermInfo(TT_COST | TT_CNT) +{ + pos_coeffs = Eigen::Vector3d::Ones(); + rot_coeffs = Eigen::Vector3d::Ones(); + tcp.setIdentity(); +} + +void CartPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob) +{ + unsigned int n_dof = prob.GetActiveGroupNumDOF(); + + Eigen::Isometry3d input_pose; + Eigen::Quaterniond q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + input_pose.linear() = q.matrix(); + input_pose.translation() = xyz; + + if (term_type == (TT_COST | TT_USE_TIME)) + { + ROS_ERROR("Use time version of this term has not been defined."); + } + else if (term_type == (TT_CNT | TT_USE_TIME)) + { + ROS_ERROR("Use time version of this term has not been defined."); + } + else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) + { + sco::VectorOfVectorPtr f(new CartPoseErrCalculator(input_pose, prob.GetPlanningScene(), link, tcp)); + prob.addCost(sco::CostPtr(new sco::CostFromErrFunc(f, prob.GetVarRow(timestep, 0, n_dof), + concatVector(rot_coeffs, pos_coeffs), sco::ABS, name))); + } + else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) + { + sco::VectorOfVectorPtr f(new CartPoseErrCalculator(input_pose, prob.GetPlanningScene(), link, tcp)); + prob.addConstraint(sco::ConstraintPtr(new sco::ConstraintFromErrFunc( + f, prob.GetVarRow(timestep, 0, n_dof), concatVector(rot_coeffs, pos_coeffs), sco::EQ, name))); + } + else + { + ROS_WARN("CartPoseTermInfo does not have a valid term_type defined. No cost/constraint applied"); + } +} + +void JointPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob) +{ + unsigned int n_dof = prob.GetActiveGroupNumDOF(); + + // If optional parameter not given, set to default + if (coeffs.empty()) + coeffs = trajopt::DblVec(n_dof, 1); + if (upper_tols.empty()) + upper_tols = trajopt::DblVec(n_dof, 0); + if (lower_tols.empty()) + lower_tols = trajopt::DblVec(n_dof, 0); + if (last_step <= -1) + last_step = prob.GetNumSteps() - 1; + + // Check time step is valid + if ((prob.GetNumSteps() - 1) <= first_step) + first_step = prob.GetNumSteps() - 1; + if ((prob.GetNumSteps() - 1) <= last_step) + last_step = prob.GetNumSteps() - 1; + // if (last_step == first_step) + // last_step += 1; + if (last_step < first_step) + { + int tmp = first_step; + first_step = last_step; + last_step = tmp; + ROS_WARN("Last time step for JointPosTerm comes before first step. Reversing them."); + } + if (last_step == -1) // last_step not set + last_step = first_step; + + // Check if parameters are the correct size. + checkParameterSize(coeffs, n_dof, "JointPoseTermInfo coeffs", true); + checkParameterSize(targets, n_dof, "JointPoseTermInfo targets", true); + checkParameterSize(upper_tols, n_dof, "JointPoseTermInfo upper_tols", true); + checkParameterSize(lower_tols, n_dof, "JointPoseTermInfo lower_tols", true); + + // Check if tolerances are all zeros + bool is_upper_zeros = + std::all_of(upper_tols.begin(), upper_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); + bool is_lower_zeros = + std::all_of(lower_tols.begin(), lower_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); + + // Get vars associated with joints + trajopt::VarArray vars = prob.GetVars(); + trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); + if (prob.GetHasTime()) + ROS_INFO("JointPoseTermInfo does not differ based on setting of TT_USE_TIME"); + + if (term_type & TT_COST) + { + // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost + if (is_upper_zeros && is_lower_zeros) + { + prob.addCost(sco::CostPtr(new trajopt::JointPosEqCost(joint_vars, util::toVectorXd(coeffs), + util::toVectorXd(targets), first_step, last_step))); + prob.getCosts().back()->setName(name); + } + else + { + prob.addCost(sco::CostPtr(new trajopt::JointPosIneqCost(joint_vars, util::toVectorXd(coeffs), + util::toVectorXd(targets), util::toVectorXd(upper_tols), + util::toVectorXd(lower_tols), first_step, last_step))); + prob.getCosts().back()->setName(name); + } + } + else if (term_type & TT_CNT) + { + // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint + if (is_upper_zeros && is_lower_zeros) + { + prob.addConstraint(sco::ConstraintPtr(new trajopt::JointPosEqConstraint( + joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step))); + prob.getConstraints().back()->setName(name); + } + else + { + prob.addConstraint(sco::ConstraintPtr(new trajopt::JointPosIneqConstraint( + joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), util::toVectorXd(upper_tols), + util::toVectorXd(lower_tols), first_step, last_step))); + prob.getConstraints().back()->setName(name); + } + } + else + { + ROS_WARN("JointPosTermInfo does not have a valid term_type defined. No cost/constraint applied"); + } +} + +void JointVelTermInfo::addObjectiveTerms(TrajOptProblem& prob) +{ + unsigned int n_dof = prob.GetNumDOF(); + + // If optional parameter not given, set to default + if (coeffs.empty()) + coeffs = trajopt::DblVec(n_dof, 1); + if (upper_tols.empty()) + upper_tols = trajopt::DblVec(n_dof, 0); + if (lower_tols.empty()) + lower_tols = trajopt::DblVec(n_dof, 0); + if (last_step <= -1) + last_step = prob.GetNumSteps() - 1; + + // If only one time step is desired, calculate velocity with next step (2 steps are needed for 1 velocity calculation) + if ((prob.GetNumSteps() - 2) <= first_step) + first_step = prob.GetNumSteps() - 2; + if ((prob.GetNumSteps() - 1) <= last_step) + last_step = prob.GetNumSteps() - 1; + if (last_step == first_step) + last_step += 1; + if (last_step < first_step) + { + int tmp = first_step; + first_step = last_step; + last_step = tmp; + ROS_WARN("Last time step for JointVelTerm comes before first step. Reversing them."); + } + + // Check if parameters are the correct size. + checkParameterSize(coeffs, n_dof, "JointVelTermInfo coeffs", true); + checkParameterSize(targets, n_dof, "JointVelTermInfo targets", true); + checkParameterSize(upper_tols, n_dof, "JointVelTermInfo upper_tols", true); + checkParameterSize(lower_tols, n_dof, "JointVelTermInfo lower_tols", true); + assert(last_step > first_step); + assert(first_step >= 0); + + // Check if tolerances are all zeros + bool is_upper_zeros = + std::all_of(upper_tols.begin(), upper_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); + bool is_lower_zeros = + std::all_of(lower_tols.begin(), lower_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); + + // Get vars associated with joints + trajopt::VarArray vars = prob.GetVars(); + trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); + + if (term_type == (TT_COST | TT_USE_TIME)) + { + unsigned num_vels = last_step - first_step; + + // Apply seperate cost to each joint b/c that is how the error function is currently written + for (size_t j = 0; j < n_dof; j++) + { + // Get a vector of a single column of vars + sco::VarVector joint_vars_vec = joint_vars.cblock(first_step, j, last_step - first_step + 1); + sco::VarVector time_vars_vec = vars.cblock(first_step, vars.cols() - 1, last_step - first_step + 1); + + // If the tolerances are 0, an equality cost is set + if (is_upper_zeros && is_lower_zeros) + { + trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); + prob.addCost(sco::CostPtr(new sco::CostFromErrFunc( + sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])), + sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec), + util::toVectorXd(single_jnt_coeffs), sco::SQUARED, name + "_j" + std::to_string(j)))); + } + // Otherwise it's a hinged "inequality" cost + else + { + trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); + prob.addCost(sco::CostPtr(new sco::CostFromErrFunc( + sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])), + sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec), + util::toVectorXd(single_jnt_coeffs), sco::HINGE, name + "_j" + std::to_string(j)))); + } + } + } + else if (term_type == (TT_CNT | TT_USE_TIME)) + { + unsigned num_vels = last_step - first_step; + + // Apply seperate cnt to each joint b/c that is how the error function is currently written + for (size_t j = 0; j < n_dof; j++) + { + // Get a vector of a single column of vars + sco::VarVector joint_vars_vec = joint_vars.cblock(first_step, j, last_step - first_step + 1); + sco::VarVector time_vars_vec = vars.cblock(first_step, vars.cols() - 1, last_step - first_step + 1); + + // If the tolerances are 0, an equality cnt is set + if (is_upper_zeros && is_lower_zeros) + { + trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); + prob.addConstraint(sco::ConstraintPtr(new sco::ConstraintFromErrFunc( + sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])), + sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec), + util::toVectorXd(single_jnt_coeffs), sco::EQ, name + "_j" + std::to_string(j)))); + } + // Otherwise it's a hinged "inequality" constraint + else + { + trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); + prob.addConstraint(sco::ConstraintPtr(new sco::ConstraintFromErrFunc( + sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])), + sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec), + util::toVectorXd(single_jnt_coeffs), sco::INEQ, name + "_j" + std::to_string(j)))); + } + } + } + else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) + { + // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost + if (is_upper_zeros && is_lower_zeros) + { + prob.addCost(sco::CostPtr(new trajopt::JointVelEqCost(joint_vars, util::toVectorXd(coeffs), + util::toVectorXd(targets), first_step, last_step))); + prob.getCosts().back()->setName(name); + } + else + { + prob.addCost(sco::CostPtr(new trajopt::JointVelIneqCost(joint_vars, util::toVectorXd(coeffs), + util::toVectorXd(targets), util::toVectorXd(upper_tols), + util::toVectorXd(lower_tols), first_step, last_step))); + prob.getCosts().back()->setName(name); + } + } + else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) + { + // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint + if (is_upper_zeros && is_lower_zeros) + { + prob.addConstraint(sco::ConstraintPtr(new trajopt::JointVelEqConstraint( + joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step))); + prob.getConstraints().back()->setName(name); + } + else + { + prob.addConstraint(sco::ConstraintPtr(new trajopt::JointVelIneqConstraint( + joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), util::toVectorXd(upper_tols), + util::toVectorXd(lower_tols), first_step, last_step))); + prob.getConstraints().back()->setName(name); + } + } + else + { + ROS_WARN("JointVelTermInfo does not have a valid term_type defined. No cost/constraint applied"); + } +} + +void CollisionTermInfo::addObjectiveTerms(TrajOptProblem& prob) +{ + + //int n_dof = static_cast(prob.GetKin()->numJoints()); + unsigned int n_dof = prob.GetNumDOF(); + + if (term_type == TT_COST) + { + if (continuous) + { + for (int i = first_step; i <= last_step - gap; ++i) + { + prob.addCost(sco::CostPtr(new CollisionCost(prob.GetPlanningScene(), + prob.GetPlanningGroup(), + info[static_cast(i - first_step)], + prob.GetVarRow(i, 0, n_dof), + prob.GetVarRow(i + gap, 0, n_dof)))); + prob.getCosts().back()->setName((boost::format("%s_%i") % name.c_str() % i).str()); + } + } + else + { + for (int i = first_step; i <= last_step; ++i) + { + prob.addCost(sco::CostPtr(new CollisionCost( + prob.GetPlanningScene(), prob.GetPlanningGroup(), + info[static_cast(i - first_step)], prob.GetVarRow(i, 0, n_dof)))); + prob.getCosts().back()->setName((boost::format("%s_%i") % name.c_str() % i).str()); + } + } + } + else + { // ALMOST COPIED + if (continuous) + { + for (int i = first_step; i < last_step; ++i) + { + prob.addIneqConstraint(sco::ConstraintPtr(new CollisionConstraint( + prob.GetPlanningScene(), + prob.GetPlanningGroup(), + info[static_cast(i - first_step)], + prob.GetVarRow(i, 0, n_dof), + prob.GetVarRow(i + 1, 0, n_dof)))); + prob.getIneqConstraints().back()->setName((boost::format("%s_%i") % name.c_str() % i).str()); + } + } + else + { + for (int i = first_step; i <= last_step; ++i) + { + prob.addIneqConstraint(sco::ConstraintPtr(new CollisionConstraint( + prob.GetPlanningScene(), prob.GetPlanningGroup(), + info[static_cast(i - first_step)], prob.GetVarRow(i, 0, n_dof)))); + prob.getIneqConstraints().back()->setName((boost::format("%s_%i") % name.c_str() % i).str()); + } + } + } +} + +// ---------------- + + +void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, + trajopt::TrajArray& init_traj) +{ + Eigen::VectorXd current_pos(current_joint_values.size()); + for (std::size_t joint_index = 0; joint_index < current_joint_values.size(); ++joint_index) + { + current_pos(joint_index) = current_joint_values[joint_index]; + } + + InitInfo init_info = pci.init_info; + + // initialize based on type specified + if (init_info.type == InitInfo::STATIONARY) + { + // Initializes all joint values to the initial value (the current value in scene) + init_traj = current_pos.transpose().replicate(pci.basic_info.n_steps, 1); + } + else if (init_info.type == InitInfo::JOINT_INTERPOLATED) + { + // Linearly interpolates between initial value (current values in the scene) and the joint position specified in + // InitInfo.data + Eigen::VectorXd end_pos = init_info.data; + init_traj.resize(pci.basic_info.n_steps, end_pos.rows()); + for (int dof_index = 0; dof_index < current_pos.rows(); ++dof_index) + { + init_traj.col(dof_index) = + Eigen::VectorXd::LinSpaced(pci.basic_info.n_steps, current_pos(dof_index), end_pos(dof_index)); + } + } + else if (init_info.type == InitInfo::GIVEN_TRAJ) + { + // Initializes the matrix to a given trajectory + init_traj = init_info.data; + } + else + { + PRINT_AND_THROW("Init Info did not have a valid type. Valid types are " + "STATIONARY, JOINT_INTERPOLATED, or GIVEN_TRAJ"); + } + + // Currently all trajectories are generated without time then appended here + if (pci.basic_info.use_time) + { + std::cout << "heeeeeeeeeeeeeeeeeeeeeeeer " << std::endl; + // add on time (default to 1 sec) + init_traj.conservativeResize(Eigen::NoChange_t(), init_traj.cols() + 1); + + init_traj.block(0, init_traj.cols() - 1, init_traj.rows(), 1) = + Eigen::VectorXd::Constant(init_traj.rows(), init_info.dt); + } +} +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/trajopt/src/trajectory_costs.cpp b/moveit_planners/trajopt/trajopt/src/trajectory_costs.cpp new file mode 100644 index 0000000000..b12fcf925b --- /dev/null +++ b/moveit_planners/trajopt/trajopt/src/trajectory_costs.cpp @@ -0,0 +1,899 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include + +#include "trajopt/trajectory_costs.hpp" + +namespace +{ +/** @brief Returns the difference between each row of a matrixXd and the row before */ +static Eigen::MatrixXd diffAxis0(const Eigen::MatrixXd& in) +{ + return in.middleRows(1, in.rows() - 1) - in.middleRows(0, in.rows() - 1); +} +} // namespace + +namespace trajopt +{ +//////////// Joint cost functions ///////////////// + +//////////////////// Position ///////////////////// +JointPosEqCost::JointPosEqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + int& first_step, int& last_step) + : Cost("JointPosEq"), vars_(vars), coeffs_(coeffs), targets_(targets), first_step_(first_step), last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // pos = x1 - targ + sco::AffExpr pos; + sco::exprInc(pos, sco::exprMult(vars(i, j), 1)); + sco::exprDec(pos, targets_[j]); + // expr_ = coeff * vel^2 + sco::exprInc(expr_, sco::exprMult(sco::exprSquare(pos), coeffs_[j])); + } + } +} +double JointPosEqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = (getTraj(xvec, vars_)); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = + (traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())).rowwise() - targets_.transpose(); + // Element-wise square it, multiply it by a diagonal matrix of coefficients, and sums output + return (diff.array().square().matrix() * coeffs_.asDiagonal()).sum(); +} +sco::ConvexObjectivePtr JointPosEqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + out->addQuadExpr(expr_); + return out; +} + +JointPosIneqCost::JointPosIneqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_tols, const Eigen::VectorXd& lower_tols, + int& first_step, int& last_step) + : Cost("JointPosIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + sco::AffExpr expr; + sco::AffExpr expr_neg; + // pos = x1 - targ + sco::AffExpr pos; + sco::exprInc(pos, sco::exprMult(vars(i, j), 1)); + sco::exprDec(pos, targets_[j]); + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprDec(expr, pos); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprDec(expr_neg, pos); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +double JointPosIneqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd pos = traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()); + // Subtract targets to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (pos.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum(); +} + +sco::ConvexObjectivePtr JointPosIneqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addHinge(expr, 1); + } + return out; +} + +JointPosEqConstraint::JointPosEqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, int& first_step, int& last_step) + : EqConstraint("JointPosEq") + , vars_(vars) + , coeffs_(coeffs) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // pos = x1 - targ + sco::AffExpr pos; + sco::exprInc(pos, sco::exprMult(vars(i, j), 1)); + sco::exprDec(pos, targets_[j]); + // expr_ = coeff * vel - Not squared b/c QuadExpr cnt not yet supported (TODO) + expr_vec_.push_back(sco::exprMult(pos, coeffs_[j])); + } + } +} + +DblVec JointPosEqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = + (traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())).rowwise() - targets_.transpose(); + // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector + return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal()); +} +sco::ConvexConstraintsPtr JointPosEqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + for (sco::AffExpr& expr : expr_vec_) + { + out->addEqCnt(expr); + } + return out; +} + +JointPosIneqConstraint::JointPosIneqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, int& first_step, int& last_step) + : IneqConstraint("JointPosIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + sco::AffExpr expr; + sco::AffExpr expr_neg; + // pos = x1 - targ + sco::AffExpr pos; + sco::exprInc(pos, sco::exprMult(vars(i, j), 1)); + sco::exprDec(pos, targets_[j]); + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprDec(expr, pos); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form lower limit expr = (upper_tol-(vel-targ)) + sco::exprDec(expr_neg, pos); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +DblVec JointPosIneqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd pos = diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())); + // Subtract targets to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (pos.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols()); + out << diff1, diff2; + return util::toDblVec(out); +} + +sco::ConvexConstraintsPtr JointPosIneqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addIneqCnt(expr); + } + return out; +} + +//////////////////// Velocity ///////////////////// +JointVelEqCost::JointVelEqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + int& first_step, int& last_step) + : Cost("JointVelEq"), vars_(vars), coeffs_(coeffs), targets_(targets), first_step_(first_step), last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 1; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // vel = (x2 - x1) - targ + sco::AffExpr vel; + sco::exprInc(vel, sco::exprMult(vars(i, j), -1)); + sco::exprInc(vel, sco::exprMult(vars(i + 1, j), 1)); + exprDec(vel, targets_[j]); + // expr_ = coeff * vel^2 + exprInc(expr_, exprMult(exprSquare(vel), coeffs_[j])); + } + } +} +double JointVelEqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = (getTraj(xvec, vars_)); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = (diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))).rowwise() - + targets_.transpose(); + // Element-wise square it, multiply it by a diagonal matrix of coefficients, and sums output + return (diff.array().square().matrix() * coeffs_.asDiagonal()).sum(); +} +sco::ConvexObjectivePtr JointVelEqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + out->addQuadExpr(expr_); + return out; +} + +JointVelIneqCost::JointVelIneqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_tols, const Eigen::VectorXd& lower_tols, + int& first_step, int& last_step) + : Cost("JointVelIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 1; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // vel = (x2 - x1) - targ + sco::AffExpr vel; + sco::AffExpr expr; + sco::AffExpr expr_neg; + sco::exprInc(vel, sco::exprMult(vars(i, j), -1)); + sco::exprInc(vel, sco::exprMult(vars(i + 1, j), 1)); + sco::exprDec(vel, targets_[j]); // offset to center about 0 + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + sco::exprDec(expr, vel); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form lower limit expr = (upper_tol-(vel-targ)) + sco::exprInc(expr_neg, lower_tols_[j]); // expr_ = lower_tol_ + sco::exprDec(expr_neg, vel); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +double JointVelIneqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd vel = diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())); + // Subtract targets_ to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (vel.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum(); +} + +sco::ConvexObjectivePtr JointVelIneqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addHinge(expr, 1); + } + return out; +} + +JointVelEqConstraint::JointVelEqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, int& first_step, int& last_step) + : EqConstraint("JointVelEq") + , vars_(vars) + , coeffs_(coeffs) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 1; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // vel = (x2 - x1) - targ + sco::AffExpr vel; + sco::exprInc(vel, sco::exprMult(vars(i, j), -1)); + sco::exprInc(vel, sco::exprMult(vars(i + 1, j), 1)); + sco::exprDec(vel, targets_[j]); + // expr_ = coeff * vel - Not squared b/c QuadExpr cnt not yet supported (TODO) + expr_vec_.push_back(sco::exprMult(vel, coeffs_[j])); + } + } +} + +DblVec JointVelEqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = (diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))).rowwise() - + targets_.transpose(); + // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector + return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal()); +} +sco::ConvexConstraintsPtr JointVelEqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + for (sco::AffExpr& expr : expr_vec_) + { + out->addEqCnt(expr); + } + return out; +} + +JointVelIneqConstraint::JointVelIneqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, int& first_step, int& last_step) + : IneqConstraint("JointVelIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 1; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // vel = (x2 - x1) - targ + sco::AffExpr vel; + sco::AffExpr expr; + sco::AffExpr expr_neg; + sco::exprInc(vel, sco::exprMult(vars(i, j), -1)); + sco::exprInc(vel, sco::exprMult(vars(i + 1, j), 1)); + sco::exprDec(vel, targets_[j]); // offset to center about 0 + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + sco::exprDec(expr, vel); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form lower limit expr = (lower_tol-(vel-targ)) + sco::exprInc(expr_neg, lower_tols_[j]); // expr_ = lower_tol_ + sco::exprDec(expr_neg, vel); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +DblVec JointVelIneqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd vel = diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())); + // Subtract targets_ to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (vel.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols()); + out << diff1, diff2; + return util::toDblVec(out.cwiseMax(0)); +} + +sco::ConvexConstraintsPtr JointVelIneqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addIneqCnt(expr); + } + return out; +} + +//////////////////// Acceleration ///////////////////// +JointAccEqCost::JointAccEqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + int& first_step, int& last_step) + : Cost("JointAccEq"), vars_(vars), coeffs_(coeffs), targets_(targets), first_step_(first_step), last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 2; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // acc = (x3 - 2*x2 + x1) - targ + sco::AffExpr acc; + sco::exprInc(acc, sco::exprMult(vars(i, j), 1.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 1, j), -2.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 2, j), 1.0)); + + sco::exprDec(acc, targets_[j]); + // expr_ = coeff * acc^2 + sco::exprInc(expr_, sco::exprMult(sco::exprSquare(acc), coeffs_[j])); + } + } +} +double JointAccEqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = (getTraj(xvec, vars_)); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = + (diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())))).rowwise() - + targets_.transpose(); + // Element-wise square it, multiply it by a diagonal matrix of coefficients, and sums output + return (diff.array().square().matrix() * coeffs_.asDiagonal()).sum(); +} +sco::ConvexObjectivePtr JointAccEqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + out->addQuadExpr(expr_); + return out; +} + +JointAccIneqCost::JointAccIneqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + const Eigen::VectorXd& upper_tols, const Eigen::VectorXd& lower_tols, + int& first_step, int& last_step) + : Cost("JointAccIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 2; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // acc = (x3 - 2*x2 + x1) - targ + sco::AffExpr acc; + sco::AffExpr expr; + sco::AffExpr expr_neg; + sco::exprInc(acc, sco::exprMult(vars(i, j), 1.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 1, j), -2.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 2, j), 1.0)); + sco::exprDec(acc, targets_[j]); // offset to center about 0 + + // Form upper limit expr = - (upper_tol-(acc-targ)) + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + sco::exprDec(expr, acc); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form lower limit expr = (lower_tol-(acc-targ)) + sco::exprInc(expr_neg, lower_tols_[j]); // expr_ = lower_tol_ + sco::exprDec(expr_neg, acc); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +double JointAccIneqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd acc = diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))); + // Subtract targets_ to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (acc.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum(); +} + +sco::ConvexObjectivePtr JointAccIneqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addHinge(expr, 1); + } + return out; +} + +JointAccEqConstraint::JointAccEqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, int& first_step, int& last_step) + : EqConstraint("JointAccEq") + , vars_(vars) + , coeffs_(coeffs) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 2; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // acc = (x3 - 2*x2 + x1) - targ + sco::AffExpr acc; + sco::exprInc(acc, sco::exprMult(vars(i, j), 1.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 1, j), -2.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 2, j), 1.0)); + + sco::exprDec(acc, targets_[j]); // offset to center about 0 + // expr_ = coeff * vel - Not squared b/c QuadExpr cnt not yet supported (TODO) + expr_vec_.push_back(sco::exprMult(acc, coeffs_[j])); + } + } +} + +DblVec JointAccEqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = + (diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())))).rowwise() - + targets_.transpose(); + // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector + return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal()); +} +sco::ConvexConstraintsPtr JointAccEqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + for (sco::AffExpr& expr : expr_vec_) + { + out->addEqCnt(expr); + } + return out; +} + +JointAccIneqConstraint::JointAccIneqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, int& first_step, int& last_step) + : IneqConstraint("JointAccIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + // Form upper limit expr = - (upper_tol-(vel-targ)) + for (int i = first_step_; i <= last_step_ - 2; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + // acc = (x3 - 2*x2 + x1) - targ + sco::AffExpr acc; + sco::AffExpr expr; + sco::AffExpr expr_neg; + sco::exprInc(acc, sco::exprMult(vars(i, j), 1.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 1, j), -2.0)); + sco::exprInc(acc, sco::exprMult(vars(i + 2, j), 1.0)); + sco::exprDec(acc, targets_[j]); // offset to center about 0 + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + sco::exprDec(expr, acc); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprInc(expr_neg, lower_tols_[j]); // expr_ = lower_tol_ + sco::exprDec(expr_neg, acc); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +DblVec JointAccIneqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd acc = diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))); + // Subtract targets_ to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (acc.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols()); + out << diff1, diff2; + return util::toDblVec(out.cwiseMax(0)); +} + +sco::ConvexConstraintsPtr JointAccIneqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addIneqCnt(expr); + } + return out; +} + +//////////////////// Jerk ///////////////////// +JointJerkEqCost::JointJerkEqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets, + int& first_step, int& last_step) + : Cost("JointJerkEq"), vars_(vars), coeffs_(coeffs), targets_(targets), first_step_(first_step), last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 4; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + sco::AffExpr jerk; + sco::exprInc(jerk, sco::exprMult(vars(i, j), -1.0 / 2.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 1, j), 1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 2, j), 0.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 3, j), -1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 4, j), 1.0 / 2.0)); + + sco::exprDec(jerk, targets_[j]); + // expr_ = coeff * jerk^2 + sco::exprInc(expr_, sco::exprMult(sco::exprSquare(jerk), coeffs_[j])); + } + } +} +double JointJerkEqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = (getTraj(xvec, vars_)); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = + (diffAxis0(diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))))) + .rowwise() - + targets_.transpose(); + // Element-wise square it, multiply it by a diagonal matrix of coefficients, and sums output + return (diff.array().square().matrix() * coeffs_.asDiagonal()).sum(); +} +sco::ConvexObjectivePtr JointJerkEqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + out->addQuadExpr(expr_); + return out; +} + +JointJerkIneqCost::JointJerkIneqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, int& first_step, int& last_step) + : Cost("JointJerkIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 4; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + sco::AffExpr jerk; + sco::AffExpr expr; + sco::AffExpr expr_neg; + sco::exprInc(jerk, sco::exprMult(vars(i, j), -1.0 / 2.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 1, j), 1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 2, j), 0.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 3, j), -1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 4, j), 1.0 / 2.0)); + sco::exprDec(jerk, targets_[j]); + + // Form upper limit expr = - (upper_tol-(jerk-targ)) + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + sco::exprDec(expr, jerk); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form lower limit expr = (lower_tol-(acc-targ)) + sco::exprInc(expr_neg, lower_tols_[j]); // expr_ = lower_tol_ + sco::exprDec(expr_neg, jerk); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +double JointJerkIneqCost::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd jerk = + diffAxis0(diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())))); + // Subtract targets_ to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (jerk.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum(); +} + +sco::ConvexObjectivePtr JointJerkIneqCost::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexObjectivePtr out(new sco::ConvexObjective(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addHinge(expr, 1); + } + return out; +} + +JointJerkEqConstraint::JointJerkEqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, int& first_step, int& last_step) + : EqConstraint("JointJerkEq") + , vars_(vars) + , coeffs_(coeffs) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 4; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + sco::AffExpr jerk; + sco::exprInc(jerk, sco::exprMult(vars(i, j), -1.0 / 2.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 1, j), 1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 2, j), 0.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 3, j), -1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 4, j), 1.0 / 2.0)); + + sco::exprDec(jerk, targets_[j]); // offset to center about 0 + // expr_ = coeff * jerk - Not squared b/c QuadExpr cnt not yet supported (TODO) + expr_vec_.push_back(sco::exprMult(jerk, coeffs_[j])); + } + } +} + +DblVec JointJerkEqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector + Eigen::MatrixXd diff = + (diffAxis0(diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))))) + .rowwise() - + targets_.transpose(); + // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector + return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal()); +} +sco::ConvexConstraintsPtr JointJerkEqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + for (sco::AffExpr& expr : expr_vec_) + { + out->addEqCnt(expr); + } + return out; +} + +JointJerkIneqConstraint::JointJerkIneqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs, + const Eigen::VectorXd& targets, const Eigen::VectorXd& upper_tols, + const Eigen::VectorXd& lower_tols, int& first_step, int& last_step) + : IneqConstraint("JointJerkIneq") + , vars_(vars) + , coeffs_(coeffs) + , upper_tols_(upper_tols) + , lower_tols_(lower_tols) + , targets_(targets) + , first_step_(first_step) + , last_step_(last_step) +{ + for (int i = first_step_; i <= last_step_ - 4; ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + sco::AffExpr jerk; + sco::AffExpr expr; + sco::AffExpr expr_neg; + sco::exprInc(jerk, sco::exprMult(vars(i, j), -1.0 / 2.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 1, j), 1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 2, j), 0.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 3, j), -1.0)); + sco::exprInc(jerk, sco::exprMult(vars(i + 4, j), 1.0 / 2.0)); + sco::exprDec(jerk, targets_[j]); // offset to center about 0 + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprInc(expr, upper_tols_[j]); // expr_ = upper_tol + sco::exprDec(expr, jerk); // expr = upper_tol_- (vel - targets_) + sco::exprScale(expr, -coeffs[j]); // expr = - (upper_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr); + + // Form upper limit expr = - (upper_tol-(vel-targ)) + sco::exprInc(expr_neg, lower_tols_[j]); // expr_ = lower_tol_ + sco::exprDec(expr_neg, jerk); // expr = lower_tol_- (vel - targets_) + sco::exprScale(expr_neg, coeffs[j]); // expr = (lower_tol_- (vel - targets_)) * coeffs_ + expr_vec_.push_back(expr_neg); + } + } +} + +DblVec JointJerkIneqConstraint::value(const DblVec& xvec) +{ + // Convert vector from optimization to trajectory + Eigen::MatrixXd traj = getTraj(xvec, vars_); + // Takes diff b/n the subsequent rows to get velocity + Eigen::MatrixXd acc = diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))); + // Subtract targets_ to center about 0 and then subtract from tolerance + Eigen::MatrixXd diff0 = (acc.rowwise() - targets_.transpose()); + Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal(); + Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal(); + // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to + // vector + Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols()); + out << diff1, diff2; + return util::toDblVec(out.cwiseMax(0)); +} + +sco::ConvexConstraintsPtr JointJerkIneqConstraint::convex(const DblVec& /*x*/, sco::Model* model) +{ + sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model)); + // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already + // This is necessary since we want a seperate coefficient per joint + for (sco::AffExpr& expr : expr_vec_) + { + out->addIneqCnt(expr); + } + return out; +} + +} // namespace trajopt diff --git a/moveit_planners/trajopt/trajopt/src/utils.cpp b/moveit_planners/trajopt/trajopt/src/utils.cpp new file mode 100644 index 0000000000..dbd87f4fb6 --- /dev/null +++ b/moveit_planners/trajopt/trajopt/src/utils.cpp @@ -0,0 +1,87 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include + +namespace trajopt +{ +TrajArray getTraj(const DblVec& x, const VarArray& vars) +{ + TrajArray out(vars.rows(), vars.cols()); + for (int i = 0; i < vars.rows(); ++i) + { + for (int j = 0; j < vars.cols(); ++j) + { + out(i, j) = vars(i, j).value(x); + } + } + return out; +} + +TrajArray getTraj(const DblVec& x, const AffArray& arr) +{ + Eigen::MatrixXd out(arr.rows(), arr.cols()); + for (int i = 0; i < arr.rows(); ++i) + { + for (int j = 0; j < arr.cols(); ++j) + { + out(i, j) = arr(i, j).value(x); + } + } + return out; +} + +void AddVarArrays(sco::OptProb& prob, int rows, const IntVec& cols, const std::vector& name_prefix, + const std::vector& newvars) +{ + size_t n_arr = name_prefix.size(); + assert(static_cast(n_arr) == newvars.size()); + + std::vector index(n_arr); + for (size_t i = 0; i < n_arr; ++i) + { + newvars[i]->resize(rows, cols[i]); + index[i].resize(rows, cols[i]); + } + + std::vector names; + int var_idx = prob.getNumVars(); + for (int i = 0; i < rows; ++i) + { + for (size_t k = 0; k < n_arr; ++k) + { + for (int j = 0; j < cols[k]; ++j) + { + index[k](i, j) = var_idx; + names.push_back((boost::format("%s_%i_%i") % name_prefix[k] % i % j).str()); + ++var_idx; + } + } + } + prob.createVariables(names); // note that w,r, are both unbounded + + const std::vector& vars = prob.getVars(); + for (size_t k = 0; k < n_arr; ++k) + { + for (int i = 0; i < rows; ++i) + { + for (int j = 0; j < cols[k]; ++j) + { + (*newvars[k])(i, j) = vars[static_cast(index[k](i, j))]; + } + } + } +} + +void AddVarArray(sco::OptProb& prob, int rows, int cols, const std::string& name_prefix, VarArray& newvars) +{ + std::vector arrs(1, &newvars); + std::vector prefixes(1, name_prefix); + std::vector colss(1, cols); + AddVarArrays(prob, rows, colss, prefixes, arrs); +} +} diff --git a/moveit_planners/trajopt/trajopt_interface/CMakeLists.txt b/moveit_planners/trajopt/trajopt_interface/CMakeLists.txt new file mode 100644 index 0000000000..85247d2e5f --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/CMakeLists.txt @@ -0,0 +1,112 @@ +cmake_minimum_required(VERSION 3.1.3) +project(moveit_planners_trajopt) + +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +find_package(catkin REQUIRED + COMPONENTS + moveit_core + moveit_visual_tools + moveit_ros_planning + moveit_ros_planning_interface + pluginlib + roscpp + trajopt + trajopt_sco + trajopt_utils +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES + ${PROJECT_NAME} + INCLUDE_DIRS + CATKIN_DEPENDS + roscpp + pluginlib + moveit_core + moveit_visual_tools + moveit_ros_planning_interface +) + +# The following include_directory should have include folder of the new planner. +include_directories( + include + ${catkin_INCLUDE_DIRS} + SYSTEM + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +add_library( + ${PROJECT_NAME} + src/trajopt_interface.cpp + src/trajopt_planning_context.cpp +) + +target_link_libraries( + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) + +set_target_properties( + ${PROJECT_NAME} + PROPERTIES + VERSION + "${${PROJECT_NAME}_VERSION}" +) + +# TrajOpt planning plugin +add_library(moveit_trajopt_planner_plugin src/trajopt_planner_manager.cpp) +set_target_properties(moveit_trajopt_planner_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +target_link_libraries(moveit_trajopt_planner_plugin ${PROJECT_NAME} ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark executables and/or libraries for installation +install( + TARGETS ${PROJECT_NAME} moveit_trajopt_planner_plugin + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install( + DIRECTORY + include/trajopt_interface/ + DESTINATION + ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(FILES trajopt_interface_plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +############# +## Testing ## +############# + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + add_rostest_gtest(trajectory_test test/trajectory_test.test test/trajectory_test.cpp) + target_link_libraries( + trajectory_test + ${LIBRARY_NAME} + ${catkin_LIBRARIES} + ${PROJECT_NAME} + ) + +endif() diff --git a/moveit_planners/trajopt/trajopt_interface/README.md b/moveit_planners/trajopt/trajopt_interface/README.md new file mode 100644 index 0000000000..84e0ce1d44 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/README.md @@ -0,0 +1 @@ +As of August 2019, this is a work in progress towards adding trajopt motion planning algorithm to MoveIt as a planner plugin. diff --git a/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_interface.h b/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_interface.h new file mode 100644 index 0000000000..a534d01cff --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_interface.h @@ -0,0 +1,79 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik, LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Omid Heidari */ +#pragma once + +#include + +#include + +#include +#include + +namespace trajopt_interface +{ +MOVEIT_CLASS_FORWARD(TrajOptInterface); // Defines TrajOptInterfacePtr, ConstPtr, WeakPtr... etc + +class TrajOptInterface +{ +public: + TrajOptInterface(const ros::NodeHandle& nh = ros::NodeHandle("~")); + + const sco::BasicTrustRegionSQPParameters& getParams() const + { + return params_; + } + + bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, moveit_msgs::MotionPlanDetailedResponse& res); + +protected: + /** @brief Configure everything using the param server */ + void setTrajOptParams(sco::BasicTrustRegionSQPParameters& param); + void setDefaultTrajOPtParams(); + void setProblemInfoParam(trajopt::ProblemInfo& problem_info); + void setJointPoseTermInfoParams(trajopt::JointPoseTermInfoPtr& jp, std::string name); + trajopt::DblVec extractStartJointValues(const planning_interface::MotionPlanRequest& req, + const std::vector& group_joint_names); + + ros::NodeHandle nh_; /// The ROS node handle + sco::BasicTrustRegionSQPParameters params_; + std::vector optimizer_callbacks_; + trajopt::TrajOptProblemPtr trajopt_problem_; + std::string name_; +}; + +void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res); +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_planning_context.h b/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_planning_context.h new file mode 100644 index 0000000000..d028af67ef --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/include/trajopt_interface/trajopt_planning_context.h @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik, LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Omid Heidari */ + +#pragma once + +#include + +#include +#include + +namespace trajopt_interface +{ +MOVEIT_CLASS_FORWARD(TrajOptPlanningContext); // Defines TrajOptPlanningContextPtr, ConstPtr, WeakPtr... etc + +class TrajOptPlanningContext : public planning_interface::PlanningContext +{ +public: + TrajOptPlanningContext(const std::string& name, const std::string& group, + const moveit::core::RobotModelConstPtr& model); + ~TrajOptPlanningContext() override + { + } + + bool solve(planning_interface::MotionPlanResponse& res) override; + bool solve(planning_interface::MotionPlanDetailedResponse& res) override; + + bool terminate() override; + void clear() override; + +private: + moveit::core::RobotModelConstPtr robot_model_; + + TrajOptInterfacePtr trajopt_interface_; +}; +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/trajopt_interface/package.xml b/moveit_planners/trajopt/trajopt_interface/package.xml new file mode 100644 index 0000000000..6d35ebdba1 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/package.xml @@ -0,0 +1,36 @@ + + moveit_planners_trajopt + 1.1.0 + TrajOpt planning plugin, an optimization based motion planner + + Omid Heidari + + Omid Heidari + MoveIt Release Team + + BSD + + http://moveit.ros.org + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + pluginlib + moveit_core + moveit_ros_planning + moveit_ros_planning_interface + moveit_visual_tools + roscpp + trajopt + trajopt_sco + trajopt_utils + + rosunit + rostest + + + + + + diff --git a/moveit_planners/trajopt/trajopt_interface/src/trajopt_interface.cpp b/moveit_planners/trajopt/trajopt_interface/src/trajopt_interface.cpp new file mode 100644 index 0000000000..68999ab353 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/src/trajopt_interface.cpp @@ -0,0 +1,526 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik, LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Omid Heidari */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "trajopt_interface/trajopt_interface.h" + +namespace trajopt_interface +{ +TrajOptInterface::TrajOptInterface(const ros::NodeHandle& nh) : nh_(nh), name_("TrajOptInterface") +{ + ROS_INFO_STREAM_NAMED(name_, "TrajOptInterface is constructed"); + trajopt_problem_ = trajopt::TrajOptProblemPtr(); + setDefaultTrajOPtParams(); + + // TODO: callbacks should be defined by the user + optimizer_callbacks_.push_back(callBackFunc); +} + +bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::MotionPlanDetailedResponse& res) +{ + ROS_INFO_STREAM_NAMED(name_, "solve is called"); + setTrajOptParams(params_); + + if (!planning_scene) + { + ROS_ERROR_STREAM_NAMED(name_, "No planning scene initialized."); + res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; + return false; + } + + ROS_INFO_STREAM_NAMED(name_, "Extract current state information"); + ros::WallTime start_time = ros::WallTime::now(); + moveit::core::RobotModelConstPtr robot_model = planning_scene->getRobotModel(); + bool robot_model_ok = static_cast(robot_model); + if (!robot_model_ok) + ROS_ERROR_STREAM_NAMED(name_, "robot model is not loaded properly"); + + robot_state::RobotStatePtr current_state(new robot_state::RobotState(robot_model)); + *current_state = planning_scene->getCurrentState(); + const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(req.group_name); + if (joint_model_group == nullptr) + ROS_ERROR_STREAM_NAMED(name_, "joint model group is empty"); + + std::vector group_joint_names = joint_model_group->getActiveJointModelNames(); + int dof = group_joint_names.size(); + std::vector current_joint_values; + current_state->copyJointGroupPositions(joint_model_group, current_joint_values); + trajopt::printVector("===>>> from interface upper:): ", current_joint_values); + + // Current state is different from star state in general + ROS_INFO(" ======================================= Extract start state infromation"); + trajopt::DblVec start_joint_values = extractStartJointValues(req, group_joint_names); + + // check the start state for being empty or joint limit violiation + if (start_joint_values.empty()) + { + ROS_ERROR_STREAM_NAMED(name_, "Start_state is empty"); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; + return false; + } + + if (not joint_model_group->satisfiesPositionBounds(start_joint_values.data())) + { + ROS_ERROR_STREAM_NAMED(name_, "Start state violates joint limits"); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; + return false; + } + + ROS_INFO(" ======================================= Create ProblemInfo"); + trajopt::ProblemInfo problem_info(planning_scene, req.group_name); + setProblemInfoParam(problem_info); + + ROS_INFO(" ======================================= Populate init info"); + // Check if the reference trajectoriy is empty when init_info.type is not set to STATIONARY + if (problem_info.init_info.type != trajopt::InitInfo::STATIONARY) + { + if (req.reference_trajectories.empty()) + { + ROS_ERROR_STREAM_NAMED(name_, "reference_trajectoies is empty. If you are using Motion Planning Display in RViz, " + "set init_info.type in the yaml file to STATIONARY"); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; + return false; + } + else if (req.reference_trajectories[0].joint_trajectory.empty()) + { + ROS_ERROR_STREAM_NAMED(name_, "joint_trajectory is empty. If you are using Motion Planning Display in RViz, set " + "init_info.type in the yaml file to STATIONARY"); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; + return false; + } + } + + // For type JOINT_INTERPOLATED, we need the configuration of the robot at one state (which should be the goal), we do + // not need the robot configuration along the whole trajectory. That is why the last element of points, i.e. + // poinst.back() is considered. + if (problem_info.init_info.type == trajopt::InitInfo::JOINT_INTERPOLATED) + { + Eigen::VectorXd initial_joint_values_eigen(dof); + for (int joint_index = 0; joint_index < dof; ++joint_index) + { + initial_joint_values_eigen(joint_index) = + req.reference_trajectories[0].joint_trajectory[0].points.back().positions[joint_index]; + } + + problem_info.init_info.data = initial_joint_values_eigen; + } + else if (problem_info.init_info.type == trajopt::InitInfo::GIVEN_TRAJ) + { + int num_steps = problem_info.basic_info.n_steps; + trajopt::TrajArray init_traj; + init_traj.resize(num_steps, dof); + for (std::size_t step = 0; step < num_steps; ++step) + { + for (std::size_t joint_index = 0; joint_index < dof; ++joint_index) + { + init_traj(step, joint_index) = + req.reference_trajectories[0].joint_trajectory[0].points[step].positions[joint_index]; + } + } + + problem_info.init_info.data = init_traj; + } + + ROS_INFO(" ======================================= Create Constraints"); + if (req.goal_constraints.empty()) + { + ROS_ERROR_STREAM_NAMED("trajopt_planner", "No goal constraints specified!"); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; + return false; + } + + ROS_INFO(" ======================================= Cartesian Constraints"); + if (!req.goal_constraints[0].position_constraints.empty() && !req.goal_constraints[0].orientation_constraints.empty()) + { + trajopt::CartPoseTermInfoPtr cart_goal_pos(new trajopt::CartPoseTermInfo); + // TODO: Feed cart_goal_pos with request information and the needed param to the setup.yaml file + // TODO: Multiple Cartesian constraints + + // Add the constraint to problem_info + problem_info.cnt_infos.push_back(cart_goal_pos); + } + else if (req.goal_constraints[0].position_constraints.empty() && + !req.goal_constraints[0].orientation_constraints.empty()) + { + ROS_ERROR_STREAM_NAMED("trajopt_planner", "position constraint is not defined"); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; + return false; + } + else if (!req.goal_constraints[0].orientation_constraints.empty() && + req.goal_constraints[0].orientation_constraints.empty()) + { + ROS_ERROR_STREAM_NAMED("trajopt_planner", "orientation constraint is not defined"); + res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; + return false; + } + + ROS_INFO(" ======================================= Constraints from request goal_constraints"); + for (auto goal_cnt : req.goal_constraints) + { + trajopt::JointPoseTermInfoPtr joint_pos_term(new trajopt::JointPoseTermInfo); + // When using MotionPlanning Display in RViz, the created request has no name for the constriant + setJointPoseTermInfoParams(joint_pos_term, (goal_cnt.name != "") ? goal_cnt.name : "goal_tmp"); + + trajopt::DblVec joint_goal_constraints; + for (const moveit_msgs::JointConstraint& joint_goal_constraint : goal_cnt.joint_constraints) + { + joint_goal_constraints.push_back(joint_goal_constraint.position); + } + joint_pos_term->targets = joint_goal_constraints; + problem_info.cnt_infos.push_back(joint_pos_term); + } + + ROS_INFO(" ======================================= Constraints from request start_state"); + // add the start pos from request as a constraint + trajopt::JointPoseTermInfoPtr joint_start_pos(new trajopt::JointPoseTermInfo); + + joint_start_pos->targets = start_joint_values; + setJointPoseTermInfoParams(joint_start_pos, "start_pos"); + problem_info.cnt_infos.push_back(joint_start_pos); + + ROS_INFO(" ======================================= Velocity Cost, hard-coded"); + // TODO: should be defined by user, its parametes should be added to setup.yaml + trajopt::JointVelTermInfoPtr joint_vel(new trajopt::JointVelTermInfo); + + joint_vel->coeffs = std::vector(dof, 5.0); + joint_vel->targets = std::vector(dof, 0.0); + joint_vel->first_step = 0; + joint_vel->last_step = problem_info.basic_info.n_steps - 1; + joint_vel->name = "joint_velocity"; + joint_vel->term_type = trajopt::TT_COST; + problem_info.cost_infos.push_back(joint_vel); + + ROS_INFO(" ======================================= Velocity Constraint, hard-coded"); + // TODO: should be defined by user, its parametes should be added to setup.yaml + trajopt::JointVelTermInfoPtr joint_vel_cnt(new trajopt::JointVelTermInfo); + + // Add a velocity cnt with time to insure that robot dynamics are obeyed + std::vector vel_lower_lim{ 0, 0, 0, 0, 0, 0, 0 }; + std::vector vel_upper_lim{ 150 *3.14/180, 150 *3.14/180, 150 *3.14/180, 150 *3.14/180, 180 *3.14/180, 180 *3.14/180, 180 *3.14/180}; + + joint_vel_cnt->targets = std::vector(dof, 0.0); + joint_vel_cnt->coeffs = std::vector(dof, 50.0); + joint_vel_cnt->lower_tols = vel_lower_lim; + joint_vel_cnt->upper_tols = vel_upper_lim; + joint_vel_cnt->term_type = (trajopt::TT_CNT | trajopt::TT_USE_TIME); + joint_vel_cnt->first_step = 0; + joint_vel_cnt->last_step = problem_info.basic_info.n_steps - 1; + joint_vel_cnt->name = "joint_velocity_cnt"; + // problem_info.cnt_infos.push_back(joint_vel_cnt); + + ROS_INFO(" ======================================= Collision Cost, hard-coded"); + // TODO: should be defined by user, its parametes should be added to trajopt_planning.yaml + trajopt::CollisionTermInfoPtr collision(new trajopt::CollisionTermInfo); + collision->name = "collision"; + collision->term_type = trajopt::TT_COST; + collision->continuous = true; + collision->first_step = 0; + collision->last_step = problem_info.basic_info.n_steps - 1; + collision->gap = 1; + collision->info = trajopt::createSafetyMarginDataVector(problem_info.basic_info.n_steps, 0.025, 40); + problem_info.cost_infos.push_back(collision); + + ROS_INFO(" ======================================= Visibility Constraints"); + if (!req.goal_constraints[0].visibility_constraints.empty()) + { + // TODO: Add visibility constraint + } + + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.improve_ratio_threshold: " << params_.improve_ratio_threshold); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.min_trust_box_size: " << params_.min_trust_box_size); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.min_approx_improve: " << params_.min_approx_improve); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.params_.min_approx_improve_frac: " << params_.min_approx_improve_frac); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_iter: " << params_.max_iter); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_shrink_ratio: " << params_.trust_shrink_ratio); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_expand_ratio: " << params_.trust_expand_ratio); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.cnt_tolerance: " << params_.cnt_tolerance); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_merit_coeff_increases: " << params_.max_merit_coeff_increases); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.merit_coeff_increase_ratio: " << params_.merit_coeff_increase_ratio); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_time: " << params_.max_time); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.merit_error_coeff: " << params_.merit_error_coeff); + ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_box_size: " << params_.trust_box_size); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.n_steps: " << problem_info.basic_info.n_steps); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt_upper_lim: " << problem_info.basic_info.dt_upper_lim); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt_lower_lim: " << problem_info.basic_info.dt_lower_lim); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.start_fixed: " << problem_info.basic_info.start_fixed); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.use_time: " << problem_info.basic_info.use_time); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.convex_solver: " << problem_info.basic_info.convex_solver); + + std::string problem_info_type; + switch (problem_info.init_info.type) + { + case trajopt::InitInfo::STATIONARY: + problem_info_type = "STATIONARY"; + break; + case trajopt::InitInfo::JOINT_INTERPOLATED: + problem_info_type = "JOINT_INTERPOLATED"; + break; + case trajopt::InitInfo::GIVEN_TRAJ: + problem_info_type = "GIVEN_TRAJ"; + break; + } + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.type: " << problem_info_type); + ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt: " << problem_info.init_info.dt); + + ROS_INFO(" ======================================= Construct problem"); + trajopt_problem_ = ConstructProblem(problem_info); + + ROS_INFO_STREAM_NAMED("num_cost", trajopt_problem_->getNumCosts()); + ROS_INFO_STREAM_NAMED("num_constraints", trajopt_problem_->getNumConstraints()); + + ROS_INFO(" ======================================= TrajOpt Optimization"); + sco::BasicTrustRegionSQP opt(trajopt_problem_); + + opt.setParameters(params_); + opt.initialize(trajopt::trajToDblVec(trajopt_problem_->GetInitTraj())); + + // Add all callbacks + for (const sco::Optimizer::Callback& callback : optimizer_callbacks_) + { + opt.addCallback(callback); + } + + // Optimize + ros::WallTime create_time = ros::WallTime::now(); + opt.optimize(); + + ROS_INFO(" ======================================= TrajOpt Solution"); + trajopt::TrajArray opt_solution = trajopt::getTraj(opt.x(), trajopt_problem_->GetVars()); + + // Assume that the trajectory is now optimized, fill in the output structure: + ROS_INFO_STREAM_NAMED("num_rows", opt_solution.rows()); + ROS_INFO_STREAM_NAMED("num_cols", opt_solution.cols()); + + res.trajectory.resize(1); + res.trajectory[0].joint_trajectory.joint_names = group_joint_names; + res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header; + + // Fill in the entire trajectory + res.trajectory[0].joint_trajectory.points.resize(opt_solution.rows()); + for (size_t i = 0; i < opt_solution.rows(); i++) + { + res.trajectory[0].joint_trajectory.points[i].positions.resize(opt_solution.cols()); + for (size_t j = 0; j < opt_solution.cols(); j++) + { + res.trajectory[0].joint_trajectory.points[i].positions[j] = opt_solution(i, j); + } + // Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints. + res.trajectory[0].joint_trajectory.points[i].time_from_start = ros::Duration(0.0); + } + + res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + res.processing_time.push_back((ros::WallTime::now() - start_time).toSec()); + + ROS_INFO(" ======================================= check if final state is within goal tolerances"); + kinematic_constraints::JointConstraint joint_cnt(planning_scene->getRobotModel()); + moveit::core::RobotState last_state(*current_state); + last_state.setJointGroupPositions(req.group_name, res.trajectory[0].joint_trajectory.points.back().positions); + + for (int jn = 0; jn < res.trajectory[0].joint_trajectory.points.back().positions.size(); ++jn) + { + ROS_INFO_STREAM_NAMED("joint_value", + "response: " << res.trajectory[0].joint_trajectory.points.back().positions[jn] + << " goal: " << req.goal_constraints.back().joint_constraints[jn].position); + } + + bool constraints_are_ok = true; + int joint_cnt_index = 0; + for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints.back().joint_constraints) + { + ROS_INFO("index %i: jc.configure(constraint)=> %d, jc.decide(last_state).satisfied=> %d, tolerance %f", + joint_cnt_index, joint_cnt.configure(constraint), joint_cnt.decide(last_state).satisfied, + constraint.tolerance_above); + constraints_are_ok = constraints_are_ok and joint_cnt.configure(constraint); + constraints_are_ok = constraints_are_ok and joint_cnt.decide(last_state).satisfied; + // if (not constraints_are_ok) + // { + // ROS_ERROR_STREAM_NAMED("trajopt_planner", "Goal constraints are violated: " << constraint.joint_name); + // res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED; + // return false; + // } + joint_cnt_index = joint_cnt_index + 1; + } + + ROS_INFO(" ==================================== Response"); + res.trajectory_start = req.start_state; + + ROS_INFO(" ==================================== Debug Response"); + ROS_INFO_STREAM_NAMED("group_name", res.group_name); + ROS_INFO_STREAM_NAMED("start_traj_name_size", res.trajectory_start.joint_state.name.size()); + ROS_INFO_STREAM_NAMED("start_traj_position_size", res.trajectory_start.joint_state.position.size()); + ROS_INFO_STREAM_NAMED("traj_name_size", res.trajectory[0].joint_trajectory.joint_names.size()); + ROS_INFO_STREAM_NAMED("traj_point_size", res.trajectory[0].joint_trajectory.points.size()); + return true; +} + +void TrajOptInterface::setDefaultTrajOPtParams() +{ + sco::BasicTrustRegionSQPParameters params; + params_ = params; +} + +void TrajOptInterface::setTrajOptParams(sco::BasicTrustRegionSQPParameters& params) +{ + nh_.param("trajopt_param/improve_ratio_threshold", params.improve_ratio_threshold, 0.25); + nh_.param("trajopt_param/min_trust_box_size", params.min_trust_box_size, 1e-4); + nh_.param("trajopt_param/min_approx_improve", params.min_approx_improve, 1e-4); + nh_.param("trajopt_param/min_approx_improve_frac", params.min_approx_improve_frac, -static_cast(INFINITY)); + nh_.param("trajopt_param/max_iter", params.max_iter, 100.0); + nh_.param("trajopt_param/trust_shrink_ratio", params.trust_shrink_ratio, 0.1); + + nh_.param("trajopt_param/trust_expand_ratio", params.trust_expand_ratio, 1.5); + nh_.param("trajopt_param/cnt_tolerance", params.cnt_tolerance, 1e-4); + nh_.param("trajopt_param/max_merit_coeff_increases", params.max_merit_coeff_increases, 5.0); + nh_.param("trajopt_param/merit_coeff_increase_ratio", params.merit_coeff_increase_ratio, 10.0); + nh_.param("trajopt_param/max_time", params.max_time, static_cast(INFINITY)); + nh_.param("trajopt_param/merit_error_coeff", params.merit_error_coeff, 10.0); + nh_.param("trajopt_param/trust_box_size", params.trust_box_size, 1e-1); +} + +void TrajOptInterface::setProblemInfoParam(trajopt::ProblemInfo& problem_info) +{ + nh_.param("problem_info/basic_info/n_steps", problem_info.basic_info.n_steps, 20); + nh_.param("problem_info/basic_info/dt_upper_lim", problem_info.basic_info.dt_upper_lim, 2.0); + nh_.param("problem_info/basic_info/dt_lower_lim", problem_info.basic_info.dt_lower_lim, 100.0); + nh_.param("problem_info/basic_info/start_fixed", problem_info.basic_info.start_fixed, true); + nh_.param("problem_info/basic_info/use_time", problem_info.basic_info.use_time, false); + int convex_solver_index; + nh_.param("problem_info/basic_info/convex_solver", convex_solver_index, 1); + switch (convex_solver_index) + { + case 1: + problem_info.basic_info.convex_solver = sco::ModelType::AUTO_SOLVER; + break; + case 2: + problem_info.basic_info.convex_solver = sco::ModelType::BPMPD; + break; + case 3: + problem_info.basic_info.convex_solver = sco::ModelType::OSQP; + break; + case 4: + problem_info.basic_info.convex_solver = sco::ModelType::QPOASES; + break; + case 5: + problem_info.basic_info.convex_solver = sco::ModelType::GUROBI; + break; + } + + nh_.param("problem_info/init_info/dt", problem_info.init_info.dt, 0.5); + int type_index; + nh_.param("problem_info/init_info/type", type_index, 1); + switch (type_index) + { + case 1: + problem_info.init_info.type = trajopt::InitInfo::STATIONARY; + break; + case 2: + problem_info.init_info.type = trajopt::InitInfo::JOINT_INTERPOLATED; + break; + case 3: + problem_info.init_info.type = trajopt::InitInfo::GIVEN_TRAJ; + break; + } +} + +void TrajOptInterface::setJointPoseTermInfoParams(trajopt::JointPoseTermInfoPtr& jp, std::string name) +{ + int term_type_index; + std::string term_type_address = "joint_pos_term_info/" + name + "/term_type"; + nh_.param(term_type_address, term_type_index, 1); + + switch (term_type_index) + { + case 1: + jp->term_type = trajopt::TT_COST; + break; + case 2: + jp->term_type = trajopt::TT_CNT; + break; + case 3: + jp->term_type = trajopt::TT_USE_TIME; + break; + } + + nh_.getParam("joint_pos_term_info/" + name + "/first_timestep", jp->first_step); + nh_.getParam("joint_pos_term_info/" + name + "/last_timestep", jp->last_step); + nh_.getParam("joint_pos_term_info/" + name + "/name", jp->name); +} + +trajopt::DblVec TrajOptInterface::extractStartJointValues(const planning_interface::MotionPlanRequest& req, + const std::vector& group_joint_names) +{ + std::unordered_map all_joints; + trajopt::DblVec start_joint_vals; + + for (int joint_index = 0; joint_index < req.start_state.joint_state.position.size(); ++joint_index) + { + all_joints[req.start_state.joint_state.name[joint_index]] = req.start_state.joint_state.position[joint_index]; + } + + for (auto joint_name : group_joint_names) + { + ROS_INFO(" joint position from start state, name: %s, value: %f", joint_name.c_str(), all_joints[joint_name]); + start_joint_vals.push_back(all_joints[joint_name]); + } + + return start_joint_vals; +} + +void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res) +{ + // TODO: Create the actuall implementation +} + +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/trajopt_interface/src/trajopt_planner_manager.cpp b/moveit_planners/trajopt/trajopt_interface/src/trajopt_planner_manager.cpp new file mode 100644 index 0000000000..24bda1cb52 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/src/trajopt_planner_manager.cpp @@ -0,0 +1,146 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Omid Heidari + Desc: TrajOpt planning plugin +*/ + +#include + +#include "moveit/collision_detection_fcl/collision_detector_allocator_fcl.h" + +#include + +#include "trajopt_interface/trajopt_planning_context.h" + +namespace trajopt_interface +{ +class TrajOptPlannerManager : public planning_interface::PlannerManager +{ +public: + TrajOptPlannerManager() : planning_interface::PlannerManager() + { + } + + bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) override + { + ROS_INFO(" ======================================= initialize gets called"); + + if (!ns.empty()) + nh_ = ros::NodeHandle(ns); + std::string trajopt_ns = ns.empty() ? "trajopt" : ns + "/trajopt"; + + for (const std::string& gpName : model->getJointModelGroupNames()) + { + ROS_INFO(" ======================================= group name: %s, robot model: %s", gpName.c_str(), + model->getName().c_str()); + planning_contexts_[gpName] = + TrajOptPlanningContextPtr(new TrajOptPlanningContext("trajopt_planning_context", gpName, model)); + } + + return true; + } + + bool canServiceRequest(const moveit_msgs::MotionPlanRequest& req) const override + { + return req.trajectory_constraints.constraints.empty(); + } + + std::string getDescription() const override + { + return "TrajOpt"; + } + + void getPlanningAlgorithms(std::vector& algs) const override + { + algs.clear(); + algs.push_back("trajopt"); + } + + planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::MoveItErrorCodes& error_code) const override + { + ROS_INFO(" ======================================= getPlanningContext() is called "); + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + + std::string collision_detector = planning_scene->getActiveCollisionDetectorName(); + ROS_INFO(" ======================================= collision detector is set to %s", collision_detector.c_str()); + if( collision_detector != "Bullet") + { + ROS_WARN("Colllision detector used in trajopt must be Bullet"); // change this to ROS_ERROR afte I figure out how to use bullet instead of FCL + // TODO: sholuld I just force bullet using diff() and setActiveColisionDetector() + } + + if (req.group_name.empty()) + { + ROS_ERROR("No group specified to plan for"); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; + return planning_interface::PlanningContextPtr(); + } + + if (!planning_scene) + { + ROS_ERROR("No planning scene supplied as input"); + error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; + return planning_interface::PlanningContextPtr(); + } + + // create PlanningScene using hybrid collision detector + planning_scene::PlanningScenePtr ps = planning_scene->diff(); + + // retrieve and configure existing context + const TrajOptPlanningContextPtr& context = planning_contexts_.at(req.group_name); + + ROS_INFO(" ======================================= context is made "); + + context->setPlanningScene(ps); + context->setMotionPlanRequest(req); + + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + + return context; + } + +private: + ros::NodeHandle nh_; + +protected: + std::map planning_contexts_; +}; + +} // namespace trajopt_interface + +// register the TrajOptPlannerManager class as a plugin +CLASS_LOADER_REGISTER_CLASS(trajopt_interface::TrajOptPlannerManager, planning_interface::PlannerManager); diff --git a/moveit_planners/trajopt/trajopt_interface/src/trajopt_planning_context.cpp b/moveit_planners/trajopt/trajopt_interface/src/trajopt_planning_context.cpp new file mode 100644 index 0000000000..c12135c65b --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/src/trajopt_planning_context.cpp @@ -0,0 +1,113 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik, LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Omid Heidari */ + +#include +#include +#include +#include + +#include + +#include + +#include "trajopt_interface/trajopt_planning_context.h" +#include "trajopt_interface/trajopt_interface.h" + +namespace trajopt_interface +{ +TrajOptPlanningContext::TrajOptPlanningContext(const std::string& context_name, const std::string& group_name, + const moveit::core::RobotModelConstPtr& model) + : planning_interface::PlanningContext(context_name, group_name), robot_model_(model) +{ + ROS_INFO_STREAM_NAMED(name_, "TrajOptPlanningContext is constructed"); + trajopt_interface_ = TrajOptInterfacePtr(new TrajOptInterface()); +} + +bool TrajOptPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) +{ + moveit_msgs::MotionPlanDetailedResponse res_msg; + + bool trajopt_solved = trajopt_interface_->solve(planning_scene_, request_, res_msg); + + if (trajopt_solved) + { + res.trajectory_.resize(1); + res.trajectory_[0] = + robot_trajectory::RobotTrajectoryPtr(new robot_trajectory::RobotTrajectory(robot_model_, getGroupName())); + + moveit::core::RobotState start_state(robot_model_); + moveit::core::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); + res.trajectory_[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]); + + res.description_.push_back("plan"); + // TODO: Add the initial trajectory to res (MotionPlanDetailedResponse) + res.processing_time_ = res_msg.processing_time; + res.error_code_ = res_msg.error_code; + return true; + } + else + { + res.error_code_ = res_msg.error_code; + return false; + } +} + +bool TrajOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) +{ + planning_interface::MotionPlanDetailedResponse res_detailed; + bool planning_success = solve(res_detailed); + + res.error_code_ = res_detailed.error_code_; + + if (planning_success) + { + res.trajectory_ = res_detailed.trajectory_[0]; + res.planning_time_ = res_detailed.processing_time_[0]; + } + + return planning_success; +} + +bool TrajOptPlanningContext::terminate() +{ + ROS_ERROR_STREAM_NAMED("trajopt_planning_context", "TrajOpt is not interruptable yet"); + return false; +} +void TrajOptPlanningContext::clear() +{ +} + +} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/trajopt_interface/test/joint_limits.yaml b/moveit_planners/trajopt/trajopt_interface/test/joint_limits.yaml new file mode 100644 index 0000000000..a2f08edd3b --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/test/joint_limits.yaml @@ -0,0 +1,55 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] + +# As MoveIt! does not support jerk limits, the acceleration limits provided here are the highest values that guarantee +# that no jerk limits will be violated. More precisely, applying Euler differentiation in the worst case (from min accel +# to max accel in 1 ms) the acceleration limits are the ones that satisfy +# max_jerk = (max_acceleration - min_acceleration) / 0.001 + +joint_limits: + panda_joint1: + has_velocity_limits: true + max_velocity: 2.1750 + has_acceleration_limits: true + max_acceleration: 3.75 + panda_joint2: + has_velocity_limits: true + max_velocity: 2.1750 + has_acceleration_limits: true + max_acceleration: 1.875 + panda_joint3: + has_velocity_limits: true + max_velocity: 2.1750 + has_acceleration_limits: true + max_acceleration: 2.5 + panda_joint4: + has_velocity_limits: true + max_velocity: 2.1750 + has_acceleration_limits: true + max_acceleration: 3.125 + panda_joint5: + has_velocity_limits: true + max_velocity: 2.6100 + has_acceleration_limits: true + max_acceleration: 3.75 + panda_joint6: + has_velocity_limits: true + max_velocity: 2.6100 + has_acceleration_limits: true + max_acceleration: 5 + panda_joint7: + has_velocity_limits: true + max_velocity: 2.6100 + has_acceleration_limits: true + max_acceleration: 5 + panda_finger_joint1: + has_velocity_limits: true + max_velocity: 0.1 + has_acceleration_limits: false + max_acceleration: 0 + panda_finger_joint2: + has_velocity_limits: true + max_velocity: 0.1 + has_acceleration_limits: false + max_acceleration: 0 diff --git a/moveit_planners/trajopt/trajopt_interface/test/kinematics.yaml b/moveit_planners/trajopt/trajopt_interface/test/kinematics.yaml new file mode 100644 index 0000000000..e721385b75 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/test/kinematics.yaml @@ -0,0 +1,4 @@ +panda_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 diff --git a/moveit_planners/trajopt/trajopt_interface/test/panda.launch b/moveit_planners/trajopt/trajopt_interface/test/panda.launch new file mode 100644 index 0000000000..bba22d9f57 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/test/panda.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + diff --git a/moveit_planners/trajopt/trajopt_interface/test/semantic/panda_arm.srdf.xacro b/moveit_planners/trajopt/trajopt_interface/test/semantic/panda_arm.srdf.xacro new file mode 100644 index 0000000000..a833871996 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/test/semantic/panda_arm.srdf.xacro @@ -0,0 +1,9 @@ + + + + + + diff --git a/moveit_planners/trajopt/trajopt_interface/test/semantic/panda_arm.xacro b/moveit_planners/trajopt/trajopt_interface/test/semantic/panda_arm.xacro new file mode 100644 index 0000000000..092b68fa0f --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/test/semantic/panda_arm.xacro @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/trajopt/trajopt_interface/test/trajectory_test.cpp b/moveit_planners/trajopt/trajopt_interface/test/trajectory_test.cpp new file mode 100644 index 0000000000..3719a5e64c --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/test/trajectory_test.cpp @@ -0,0 +1,236 @@ +/********************************************************************* + * Software License Agreement + * + * Copyright (c) 2019, PickNik Consulting + * All rights reserved. + * + *********************************************************************/ + +/* Author: Omid Heidari + Desc: Test the trajectory planned by trajopt + */ + +// C++ +#include +#include +#include + +// ROS +#include + +// Testing +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +class TrajectoryTest : public ::testing::Test +{ +public: + TrajectoryTest() + { + } + +protected: + void SetUp() override + { + node_handle_ = ros::NodeHandle("~"); + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + bool robot_model_ok_ = static_cast(robot_model_); + if (!robot_model_ok_) + ROS_ERROR_STREAM_NAMED("trajectory_test", "robot model is not loaded correctly"); + } + +protected: + moveit::core::RobotModelPtr robot_model_; + std::vector group_joint_names_; + const std::string PLANNING_GROUP = "panda_arm"; + const double GOAL_TOLERANCE = 0.1; + ros::NodeHandle node_handle_; +}; // class TrajectoryTest + +TEST_F(TrajectoryTest, concatVectorValidation) +{ + std::vector keys; + node_handle_.getParamNames(keys); + for (std::string key : keys) + { + std::cerr << key << std::endl; + } + + std::vector vec_a = { 1, 2, 3, 4, 5 }; + std::vector vec_b = { 6, 7, 8, 9, 10 }; + std::vector vec_c = trajopt::concatVector(vec_a, vec_b); + EXPECT_EQ(vec_c.size(), vec_a.size() + vec_b.size()); + + // Check if the output of concatVector is correct. + // Loop over the output and the input vectors to see if they match + std::size_t length_ab = vec_a.size() + vec_b.size(); + for (std::size_t index = 0; index < length_ab; ++index) + { + if (index < vec_a.size()) + { + EXPECT_EQ(vec_c[index], vec_a[index]); + } + else + { + EXPECT_EQ(vec_c[index], vec_b[index - vec_a.size()]); + } + } +} + +TEST_F(TrajectoryTest, goalTolerance) +{ + const std::string NODE_NAME = "trajectory_test"; + + // Create a planing scene monitor + planning_scene_monitor::PlanningSceneMonitorPtr psm( + new planning_scene_monitor::PlanningSceneMonitor("robot_description")); + + psm->startSceneMonitor(); + psm->startWorldGeometryMonitor(); + psm->startStateMonitor(); + + // Create a RobotState to keep track of the current robot pose and planning group + robot_state::RobotStatePtr robot_state( + new robot_state::RobotState(planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentState())); + robot_state->setToDefaultValues(); + robot_state->update(); + + // Create JointModelGroup + const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); + const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); + const std::vector& link_model_names = joint_model_group->getLinkModelNames(); + ROS_INFO_NAMED(NODE_NAME, "end effector name %s\n", link_model_names.back().c_str()); + + planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model_)); + + // Set the planner + std::string planner_plugin_name = "trajopt_interface/TrajOptPlanner"; + node_handle_.setParam("planning_plugin", planner_plugin_name); + + // Create pipeline + planning_pipeline::PlanningPipelinePtr planning_pipeline( + new planning_pipeline::PlanningPipeline(robot_model_, node_handle_, "planning_plugin", "request_adapters")); + + // Current state + std::vector current_joint_values = { 0, 0, 0, -1.5, 0, 0.6, 0.9 }; + robot_state->setJointGroupPositions(joint_model_group, current_joint_values); + geometry_msgs::Pose pose_msg_current; + const Eigen::Isometry3d& end_effector_transform_current = + robot_state->getGlobalLinkTransform(link_model_names.back()); + pose_msg_current = tf2::toMsg(end_effector_transform_current); + + // Create response and request + planning_interface::MotionPlanRequest req; + planning_interface::MotionPlanResponse res; + + // Set start state + // ======================================================================================== + std::vector start_joint_values = { 0.4, 0.3, 0.5, -0.55, 0.88, 1.0, -0.075 }; + + moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(robot_model_)); + start_state->setJointGroupPositions(joint_model_group, start_joint_values); + start_state->update(); + + req.start_state.joint_state.name = joint_names; + req.start_state.joint_state.position = start_joint_values; + req.goal_constraints.clear(); + req.group_name = PLANNING_GROUP; + + geometry_msgs::Pose pose_msg_start; + const Eigen::Isometry3d& end_effector_transform_start = robot_state->getGlobalLinkTransform(link_model_names.back()); + pose_msg_start = tf2::toMsg(end_effector_transform_start); + + // Set the goal state + // ======================================================================================== + std::vector goal_joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1 }; + robot_state->setJointGroupPositions(joint_model_group, goal_joint_values); + robot_state->update(); + moveit_msgs::Constraints joint_goal = + kinematic_constraints::constructGoalConstraints(*robot_state, joint_model_group); + req.goal_constraints.push_back(joint_goal); + req.goal_constraints[0].name = "goal_pos"; + // Set joint tolerance + std::vector goal_joint_constraint = req.goal_constraints[0].joint_constraints; + for (std::size_t x = 0; x < goal_joint_constraint.size(); ++x) + { + req.goal_constraints[0].joint_constraints[x].tolerance_above = 0.001; + req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.001; + } + + geometry_msgs::Pose pose_msg_goal; + const Eigen::Isometry3d& end_effector_transform_goal = robot_state->getGlobalLinkTransform(link_model_names.back()); + pose_msg_goal = tf2::toMsg(end_effector_transform_goal); + + // Reference Trajectory. The type should be defined in the yaml file. + // ======================================================================================== + // type: JOINT_INTERPOLATED + req.reference_trajectories.resize(1); + req.reference_trajectories[0].joint_trajectory.resize(1); + req.reference_trajectories[0].joint_trajectory[0].joint_names = joint_names; + req.reference_trajectories[0].joint_trajectory[0].points.resize(1); + req.reference_trajectories[0].joint_trajectory[0].points[0].positions = goal_joint_values; + + // Solve the problem + // ======================================================================================== + { + planning_scene_monitor::LockedPlanningSceneRO lscene(psm); + // Now, call the pipeline and check whether planning was successful. + planning_pipeline->generatePlan(lscene, req, res); + } + + // Check that the planning was successful + if (res.error_code_.val != res.error_code_.SUCCESS) + { + ROS_ERROR_STREAM_NAMED(NODE_NAME, "Could not compute plan successfully"); + return; + } + + // Check the difference between the last step in the solution and the goal + // ======================================================================================== + moveit_msgs::RobotTrajectory solution_trajectory; + res.trajectory_->getRobotTrajectoryMsg(solution_trajectory); + std::vector joints_values_last_step = solution_trajectory.joint_trajectory.points.back().positions; + + for (std::size_t joint_index = 0; joint_index < joints_values_last_step.size(); ++joint_index) + { + double goal_error = + abs(joints_values_last_step[joint_index] - req.goal_constraints[0].joint_constraints[joint_index].position); + std::cerr << "goal_error: " << goal_error << std::endl; + EXPECT_LT(goal_error, GOAL_TOLERANCE); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "trajectory_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + int result = RUN_ALL_TESTS(); + ros::shutdown(); + + return result; +} diff --git a/moveit_planners/trajopt/trajopt_interface/test/trajectory_test.test b/moveit_planners/trajopt/trajopt_interface/test/trajectory_test.test new file mode 100644 index 0000000000..46432b0b86 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/test/trajectory_test.test @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/moveit_planners/trajopt/trajopt_interface/test/trajopt_planning.yaml b/moveit_planners/trajopt/trajopt_interface/test/trajopt_planning.yaml new file mode 100644 index 0000000000..5bc754f01d --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/test/trajopt_planning.yaml @@ -0,0 +1,58 @@ +trajopt_param: + improve_ratio_threshold: 0.25 # minimum ratio true_improve/approx_improve to accept step. [Research Paper] TrueImprove/ModelImprove > c + min_trust_box_size: 1e-4 # if trust region gets any smaller, exit and report convergence. [Research Paper] xtol + min_approx_improve: 1e-4 # if model improves less than this, exit and report convergence + min_approx_improve_frac: -.Inf # if model improves less than this, exit and report convergence + max_iter: 100 # The max number of iterations + trust_shrink_ratio: 0.1 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio. [Research Paper] tao- + trust_expand_ratio: 1.5 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio. [Research Paper] tao+ + cnt_tolerance: 1e-4 # after convergence of penalty subproblem, if constraint violation is less than this, we're done. [Research Paper] ctol + max_merit_coeff_increases: 5 # number of times that we jack up penalty coefficient. [Reasearch Paper] Max iteration in PenaltyIteration loop + merit_coeff_increase_ratio: 10 # ratio that we increate coeff each time. [Researcy Paper] Penalty scaling factory (italic kappa) + max_time: .inf # not yet implemented + merit_error_coeff: 10 # initial penalty coefficient. [Research Paper] mu_0 + trust_box_size: 1e-1 # current size of trust region (component-wise). [Research Paper] s + +problem_info: + basic_info: + n_steps: 20 # 2 * steps_per_phase + dt_upper_lim: 2.0 # The upper limit of 1/dt values allowed in the optimization + dt_lower_lim: 100.0 # The lower limit of 1/dt values allowed in the optimization + start_fixed: true # if true, start pose is the current pose at timestep = 0 + # if false, the start pose is the one given by req.start_state + use_time: false # if false, it means the timestep is unit, meaning x1-x0 is the velocity for example + # if true, then cost_infos ro cnt_infos should have TT_USE_TIME for their term_type + convex_solver: 3 # which convex solver to use + # 1 = AUTO_SOLVER, 2 = BPMPD, 3 = OSQP, 4 = QPOASES, 5 = GUROBI + + init_info: + type: 2 # 1 = STATIONARY, 2 = JOINT_INTERPOLATED, 3 = GIVEN_TRAJ + # request.start_state should be provided for JOINT_INTERPOLATED and GIVEN_TRAJ + dt: 0.5 + +joint_pos_term_info: + start_pos: # from req.start_state + name: start_pos + first_timestep: 10 # First time step to which the term is applied. + last_timestep: 10 # Last time step to which the term is applied. + # if start_fixed is trure then we can not set the first_timestep to 0 for start_pos as it is going + # to be ignored and only the current pose would be the constraint at timestep = 0. + term_type: 2 # 1 = TT_COST, 2 = TT_CNT, 3 = TT_USE_TIME + middle_pos: + name: middle_pos + first_timestep: 15 + last_timestep: 15 + term_type: 2 + goal_pos: + name: goal_pos + first_timestep: 19 + last_timestep: 19 + term_type: 2 + goal_tmp: # If the constraint from request does not have any name, for example when using MotionPlanning Display in Rviz, + # goal_tmp is assigned as the name of the constraint. + # In this case: start_fixed = false and start_pos should be applied at timestep 0 + # OR: start_fixed = true and start_pos can be applies at any timestep + name: goal_tmp + first_timestep: 19 # n_steps - 1 + last_timestep: 19 # n_steps - 1 + term_type: 2 diff --git a/moveit_planners/trajopt/trajopt_interface/test/urdf/panda_arm.urdf.xacro b/moveit_planners/trajopt/trajopt_interface/test/urdf/panda_arm.urdf.xacro new file mode 100644 index 0000000000..9987fadb50 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/test/urdf/panda_arm.urdf.xacro @@ -0,0 +1,5 @@ + + + + + diff --git a/moveit_planners/trajopt/trajopt_interface/test/urdf/panda_arm.xacro b/moveit_planners/trajopt/trajopt_interface/test/urdf/panda_arm.xacro new file mode 100644 index 0000000000..672c881caa --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/test/urdf/panda_arm.xacro @@ -0,0 +1,171 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_planners/trajopt/trajopt_interface/trajopt_interface_plugin_description.xml b/moveit_planners/trajopt/trajopt_interface/trajopt_interface_plugin_description.xml new file mode 100644 index 0000000000..051864285a --- /dev/null +++ b/moveit_planners/trajopt/trajopt_interface/trajopt_interface_plugin_description.xml @@ -0,0 +1,7 @@ + + + + The trajopt motion planner plugin which implements sequential convex optimization to solve motion planning problem. + + + diff --git a/moveit_planners/trajopt/trajopt_sco/CMakeLists.txt b/moveit_planners/trajopt/trajopt_sco/CMakeLists.txt new file mode 100644 index 0000000000..3f7094bf65 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/CMakeLists.txt @@ -0,0 +1,143 @@ +cmake_minimum_required(VERSION 2.8.3) +project(trajopt_sco) + +add_compile_options(-std=c++11 -Wall -Wextra) + +# Download and unpack osqp at configure time +configure_file(osqp_configure.in + ${CMAKE_CURRENT_BINARY_DIR}/CMakeLists.txt) +execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) +execute_process(COMMAND ${CMAKE_COMMAND} --build . + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + +find_package(osqp CONFIG + PATHS "${CMAKE_CURRENT_BINARY_DIR}/osqp/src/osqp-build/osqp-install" + NO_DEFAULT_PATH) + +if(!${osqp_FOUND}) + message([FATAL_ERROR] "OSQP was not found. Check trajopt_utils CMakeLists to make sure OSQP is installed in build directory") +endif() + +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules/") +find_package(catkin REQUIRED COMPONENTS trajopt_utils) +find_package(GUROBI QUIET) +find_package(qpOASES QUIET) +find_package(Eigen3 REQUIRED) + +find_package(PkgConfig REQUIRED) +pkg_check_modules(JSONCPP jsoncpp) + +set(SCO_SOURCE_FILES + src/solver_interface.cpp + src/solver_utils.cpp + src/modeling.cpp + src/expr_ops.cpp + src/expr_vec_ops.cpp + src/optimizers.cpp + src/modeling_utils.cpp + src/num_diff.cpp +) + +find_library(BPMPD_LIBRARY bpmpd) +option(HAVE_BPMPD "Whether the BPMPD library is installed" BPMPD_LIBRARY) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS trajopt_utils + DEPENDS + EIGEN3 +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + SYSTEM ${EIGEN3_INCLUDE_DIRS} + SYSTEM ${JSONCPP_INCLUDE_DIRS} +) + +if (HAVE_BPMPD) + add_executable(bpmpd_caller src/bpmpd_caller.cpp) + + target_link_libraries(bpmpd_caller ${BPMPD_LIBRARY} -static) + target_compile_options(bpmpd_caller PRIVATE -Wsuggest-override -Wconversion -Wsign-conversion) + + list(APPEND SCO_SOURCE_FILES src/bpmpd_interface.cpp) + set_property(SOURCE src/bpmpd_interface.cpp APPEND PROPERTY COMPILE_DEFINITIONS BPMPD_CALLER="\\\"${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}/bpmpd_caller\\\"") + + #TODO: Levi check if this is correct. + set(BPMPD_WORKING_DIR "${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}/") + set_property(SOURCE src/bpmpd_caller.cpp APPEND PROPERTY COMPILE_DEFINITIONS BPMPD_WORKING_DIR="${BPMPD_WORKING_DIR}") + file(COPY src/bpmpd.par DESTINATION ${BPMPD_WORKING_DIR}) + + set_property(SOURCE src/solver_interface.cpp APPEND PROPERTY COMPILE_DEFINITIONS HAVE_BPMPD) +endif() + +if (GUROBI_FOUND) + include_directories(${GUROBI_INCLUDE_DIR}) + set_property(SOURCE src/solver_interface.cpp APPEND PROPERTY COMPILE_DEFINITIONS HAVE_GUROBI) + list(APPEND SCO_SOURCE_FILES src/gurobi_interface.cpp) +endif(GUROBI_FOUND) + +if (osqp_FOUND) + set_property(SOURCE src/solver_interface.cpp APPEND PROPERTY COMPILE_DEFINITIONS HAVE_OSQP) + list(APPEND SCO_SOURCE_FILES src/osqp_interface.cpp) +endif() + +if (qpOASES_FOUND) + include_directories(${qpOASES_INCLUDE_DIRS}) + set_property(SOURCE src/solver_interface.cpp APPEND PROPERTY COMPILE_DEFINITIONS HAVE_QPOASES) + list(APPEND SCO_SOURCE_FILES src/qpoases_interface.cpp) +endif() + +add_library(${PROJECT_NAME} ${SCO_SOURCE_FILES}) + +set (SCO_LINK_LIBS ${catkin_LIBRARIES} ${CMAKE_DL_LIBS}) +if (GUROBI_FOUND) + list(APPEND SCO_LINK_LIBS ${GUROBI_LIBRARIES}) +endif() +if (HAVE_BPMPD) + list(APPEND SCO_LINK_LIBS ${BPMPD_LIBRARY}) +endif() +if (osqp_FOUND) + target_link_libraries(${PROJECT_NAME} PRIVATE osqp::osqpstatic) +endif() +if (qpOASES_FOUND) + target_link_libraries(${PROJECT_NAME} PRIVATE ${qpOASES_LIBRARIES}) +endif() + +target_link_libraries(${PROJECT_NAME} PRIVATE ${JSONCPP_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} PUBLIC ${SCO_LINK_LIBS}) +target_compile_options(${PROJECT_NAME} PRIVATE -Wsuggest-override -Wconversion -Wsign-conversion) + +# Mark executables and/or libraries for installation +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" + ) + +if (CATKIN_ENABLE_TESTING) + set(SCO_TEST_SOURCE + test/unit.cpp + test/small-problems-unit.cpp + test/solver-interface-unit.cpp + test/solver-utils-unit.cpp + ) + + catkin_add_gtest(${PROJECT_NAME}-test ${SCO_TEST_SOURCE}) + target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) + if (osqp_FOUND) + target_link_libraries(${PROJECT_NAME}-test osqp::osqpstatic) + endif() +endif() diff --git a/moveit_planners/trajopt/trajopt_sco/LICENSE b/moveit_planners/trajopt/trajopt_sco/LICENSE new file mode 100644 index 0000000000..9103917b0f --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/LICENSE @@ -0,0 +1,11 @@ +Copyright (c) 2013, John Schulman +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +http://opensource.org/licenses/BSD-2-Clause \ No newline at end of file diff --git a/moveit_planners/trajopt/trajopt_sco/README.md b/moveit_planners/trajopt/trajopt_sco/README.md new file mode 100644 index 0000000000..1c18708b1f --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/README.md @@ -0,0 +1,3 @@ +TODO(ommmid): Remove code duplication. +This package temporarily copied into moveit until the repo trajopt_ros does not depend on Tesseract +See ros-industrial-consortium/trajopt_ros#122 \ No newline at end of file diff --git a/moveit_planners/trajopt/trajopt_sco/cmake/Modules/FindGUROBI.cmake b/moveit_planners/trajopt/trajopt_sco/cmake/Modules/FindGUROBI.cmake new file mode 100644 index 0000000000..5a0b849f15 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/cmake/Modules/FindGUROBI.cmake @@ -0,0 +1,43 @@ +# - Try to find GUROBI +# +# This modules reads hints about search locations from the following +# environment variables: +# GUROBI_HOME +# +# e.g. GUROBI_HOME=/opt/gurobobi810/linux64 +# +# The module will define: +# GUROBI_FOUND - If we found Gurobi in the system +# GUROBI_INCLUDE_DIR - Gurobi include directories +# GUROBI_LIBRARIES - libraries needed to use Gurobi +# +# This module will NOT enforce a specific GUROBI version, rather it will +# only work with latest GUROBI version (8.1.0) + +find_path(GUROBI_INCLUDE_DIR NAMES gurobi_c++.h + HINTS + ENV GUROBI_HOME + PATH_SUFFIXES include + ) + +find_library(GUROBI_LIBRARY NAMES gurobi81 + HINTS + ENV GUROBI_HOME + PATH_SUFFIXES lib + ) + +find_library(GUROBI_CXX_LIBRARY NAMES gurobi_c++ + HINTS + ENV GUROBI_HOME + PATH_SUFFIXES lib + ) + +set(GUROBI_LIBRARIES ${GUROBI_LIBRARY} ${GUROBI_CXX_LIBRARY}) + +include(FindPackageHandleStandardArgs) +# handle the QUIETLY and REQUIRED arguments and set GUROBI_FOUND to TRUE +# if all listed variables are TRUE +find_package_handle_standard_args(GUROBI DEFAULT_MSG + GUROBI_LIBRARY GUROBI_CXX_LIBRARY GUROBI_INCLUDE_DIR) + +mark_as_advanced(GUROBI_INCLUDE_DIR GUROBI_LIBRARY GUROBI_CXX_LIBRARY GUROBI_LIBRARIES) diff --git a/moveit_planners/trajopt/trajopt_sco/cmake/Modules/FindqpOASES.cmake b/moveit_planners/trajopt/trajopt_sco/cmake/Modules/FindqpOASES.cmake new file mode 100644 index 0000000000..457824faf3 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/cmake/Modules/FindqpOASES.cmake @@ -0,0 +1,51 @@ +#.rst: +# FindqpOASES +# ----------- +# +# Try to find the qpOASES library. +# Once done this will define the following variables:: +# +# qpOASES_FOUND - System has qpOASES +# qpOASES_INCLUDE_DIRS - qpOASES include directory +# qpOASES_LIBRARIES - qpOASES libraries +# +# qpOASES does not have an "install" step, and the includes are in the source +# tree, while the libraries are in the build tree. +# Therefore the environment and cmake variables `qpOASES_SOURCE_DIR` and +# `qpOASES_BINARY_DIR` will be used to locate the includes and libraries. + +#============================================================================= +# Copyright 2014 iCub Facility, Istituto Italiano di Tecnologia +# Authors: Daniele E. Domenichelli +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file Copyright.txt for details. +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# (To distribute this file outside of YCM, substitute the full +# License text for the above reference.) + + +include(FindPackageHandleStandardArgs) + +find_path(qpOASES_INCLUDEDIR + NAMES qpOASES.hpp + HINTS "${qpOASES_SOURCE_DIR}" + ENV qpOASES_SOURCE_DIR + PATH_SUFFIXES include) +find_library(qpOASES_LIB + NAMES qpOASES + HINTS "${qpOASES_BINARY_DIR}" + ENV qpOASES_BINARY_DIR + PATH_SUFFIXES lib + libs) + +set(qpOASES_INCLUDE_DIRS ${qpOASES_INCLUDEDIR}) +set(qpOASES_LIBRARIES ${qpOASES_LIB}) + +find_package_handle_standard_args(qpOASES DEFAULT_MSG qpOASES_LIBRARIES + qpOASES_INCLUDE_DIRS) +set(qpOASES_FOUND ${QPOASES_FOUND}) diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp new file mode 100644 index 0000000000..e0516e880d --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp @@ -0,0 +1,40 @@ +#pragma once +#include +#include + +namespace sco +{ +class BPMPDModel : public Model +{ +public: + VarVector m_vars; + CntVector m_cnts; + AffExprVector m_cntExprs; + ConstraintTypeVector m_cntTypes; + DblVec m_soln; + DblVec m_lbs, m_ubs; + + QuadExpr m_objective; + + int m_pipeIn, m_pipeOut, m_pid; + + BPMPDModel(); + ~BPMPDModel() override; + + Var addVar(const std::string& name) override; + Cnt addEqCnt(const AffExpr&, const std::string& name) override; + Cnt addIneqCnt(const AffExpr&, const std::string& name) override; + Cnt addIneqCnt(const QuadExpr&, const std::string& name) override; + void removeVars(const VarVector& vars) override; + void removeCnts(const CntVector& cnts) override; + + void update() override; + void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) override; + DblVec getVarValues(const VarVector& vars) const override; + virtual CvxOptStatus optimize() override; + virtual void setObjective(const AffExpr&) override; + virtual void setObjective(const QuadExpr&) override; + virtual void writeToFile(const std::string& fname) override; + virtual VarVector getVars() const override; +}; +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp new file mode 100644 index 0000000000..a9c296970a --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp @@ -0,0 +1,158 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +namespace bpmpd_io +{ +enum SerMode +{ + DESER, + SER +}; + +template +void ser(int fp, T& x, SerMode mode) +{ + switch (mode) + { + case SER: + { + T xcopy = x; + long n = write(fp, &xcopy, sizeof(T)); + assert(n == sizeof(T)); + break; + } + case DESER: + { + long n = read(fp, &x, sizeof(T)); + assert(n == sizeof(T)); + break; + } + } +} + +template +void ser(int fp, std::vector& x, SerMode mode) +{ + unsigned long size = x.size(); + ser(fp, size, mode); + switch (mode) + { + case SER: + { + long n = write(fp, x.data(), sizeof(T) * size); + assert(static_cast(n) == sizeof(T) * size); + break; + } + case DESER: + { + x.resize(size); + long n = read(fp, x.data(), sizeof(T) * size); + assert(static_cast(n) == sizeof(T) * size); + break; + } + } +} + +struct bpmpd_input +{ + int m, n, nz, qn, qnz; + std::vector acolcnt, acolidx; + std::vector acolnzs; + std::vector qcolcnt, qcolidx; + std::vector qcolnzs; + std::vector rhs, obj, lbound, ubound; + + bpmpd_input() + { + } + bpmpd_input(int m, int n, int nz, int qn, int qnz, const std::vector& acolcnt, const std::vector& acolidx, + const std::vector& acolnzs, const std::vector& qcolcnt, const std::vector& qcolidx, + const std::vector& qcolnzs, const std::vector& rhs, const std::vector& obj, + const std::vector& lbound, const std::vector& ubound) + : m(m) + , n(n) + , nz(nz) + , qn(qn) + , qnz(qnz) + , acolcnt(acolcnt) + , acolidx(acolidx) + , acolnzs(acolnzs) + , qcolcnt(qcolcnt) + , qcolidx(qcolidx) + , qcolnzs(qcolnzs) + , rhs(rhs) + , obj(obj) + , lbound(lbound) + , ubound(ubound) + { + } +}; + +const char EXIT_CHAR = 123; +const char CHECK_CHAR = 111; + +void ser(int fp, bpmpd_input& bi, SerMode mode) +{ + char scorrect = 'z', s = (mode == SER) ? scorrect : 0; + ser(fp, s, mode); + if (s == EXIT_CHAR) + { + exit(0); + } + + ser(fp, bi.m, mode); + ser(fp, bi.n, mode); + ser(fp, bi.nz, mode); + ser(fp, bi.qn, mode); + ser(fp, bi.qnz, mode); + ser(fp, bi.acolcnt, mode); + ser(fp, bi.acolidx, mode); + ser(fp, bi.acolnzs, mode); + ser(fp, bi.qcolcnt, mode); + ser(fp, bi.qcolidx, mode); + ser(fp, bi.qcolnzs, mode); + ser(fp, bi.rhs, mode); + ser(fp, bi.obj, mode); + ser(fp, bi.lbound, mode); + ser(fp, bi.ubound, mode); +} + +struct bpmpd_output +{ + std::vector primal, dual; + std::vector status; + int code; + double opt; + bpmpd_output() + { + } + bpmpd_output(const std::vector& primal, const std::vector& dual, const std::vector& status, + int code, double opt) + : primal(primal), dual(dual), status(status), code(code), opt(opt) + { + } +}; + +void ser(int fp, bpmpd_output& bo, SerMode mode) +{ + char scorrect = CHECK_CHAR, s = (mode == SER) ? scorrect : 0; + ser(fp, s, mode); + if (s == EXIT_CHAR) + { + exit(0); + } + ser(fp, bo.primal, mode); + ser(fp, bo.dual, mode); + ser(fp, bo.status, mode); + ser(fp, bo.code, mode); + ser(fp, bo.opt, mode); +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_op_overloads.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_op_overloads.hpp new file mode 100644 index 0000000000..70e89ad02e --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_op_overloads.hpp @@ -0,0 +1,136 @@ +#pragma once +#include + +namespace sco +{ +inline AffExpr operator+(const Var& x, double y) +{ + return exprAdd(AffExpr(x), y); +} +inline AffExpr operator+(const AffExpr& x, double y) +{ + return exprAdd(x, y); +} +inline QuadExpr operator+(const QuadExpr& x, double y) +{ + return exprAdd(x, y); +} +inline AffExpr operator+(const Var& x, const Var& y) +{ + return exprAdd(AffExpr(x), y); +} +inline AffExpr operator+(const AffExpr& x, const Var& y) +{ + return exprAdd(x, y); +} +inline QuadExpr operator+(const QuadExpr& x, const Var& y) +{ + return exprAdd(x, y); +} +inline AffExpr operator+(const Var& x, const AffExpr& y) +{ + return exprAdd(AffExpr(x), y); +} +inline AffExpr operator+(const AffExpr& x, const AffExpr& y) +{ + return exprAdd(x, y); +} +inline QuadExpr operator+(const QuadExpr& x, const AffExpr& y) +{ + return exprAdd(x, y); +} +inline QuadExpr operator+(const Var& x, const QuadExpr& y) +{ + return exprAdd(QuadExpr(x), y); +} +inline QuadExpr operator+(const AffExpr& x, const QuadExpr& y) +{ + return exprAdd(QuadExpr(x), y); +} +inline QuadExpr operator+(const QuadExpr& x, const QuadExpr& y) +{ + return exprAdd(x, y); +} +inline AffExpr operator-(const Var& x, double y) +{ + return exprSub(AffExpr(x), y); +} +inline AffExpr operator-(const AffExpr& x, double y) +{ + return exprSub(x, y); +} +inline QuadExpr operator-(const QuadExpr& x, double y) +{ + return exprSub(x, y); +} +inline AffExpr operator-(const Var& x, const Var& y) +{ + return exprSub(AffExpr(x), y); +} +inline AffExpr operator-(const AffExpr& x, const Var& y) +{ + return exprSub(x, y); +} +inline QuadExpr operator-(const QuadExpr& x, const Var& y) +{ + return exprSub(x, y); +} +inline AffExpr operator-(const Var& x, const AffExpr& y) +{ + return exprSub(AffExpr(x), y); +} +inline AffExpr operator-(const AffExpr& x, const AffExpr& y) +{ + return exprSub(x, y); +} +inline QuadExpr operator-(const QuadExpr& x, const AffExpr& y) +{ + return exprSub(x, y); +} +inline QuadExpr operator-(const Var& x, const QuadExpr& y) +{ + return exprSub(QuadExpr(x), y); +} +inline QuadExpr operator-(const AffExpr& x, const QuadExpr& y) +{ + return exprSub(QuadExpr(x), y); +} +inline QuadExpr operator-(const QuadExpr& x, const QuadExpr& y) +{ + return exprSub(x, y); +} +/////////////// + +inline AffExpr operator*(double a, const Var& b) +{ + return exprMult(b, a); +} +inline AffExpr operator*(double a, const AffExpr& b) +{ + return exprMult(b, a); +} +inline QuadExpr operator*(double a, const QuadExpr& b) +{ + return exprMult(b, a); +} +inline AffExpr operator*(const Var& a, double b) +{ + return exprMult(a, b); +} +inline AffExpr operator*(const AffExpr& a, double b) +{ + return exprMult(a, b); +} +inline QuadExpr operator*(const QuadExpr& a, double b) +{ + return exprMult(a, b); +} +inline AffExpr operator-(const Var& a) +{ + return exprMult(a, -1); +} +inline AffExpr operator-(const AffExpr& a) +{ + return exprMult(a, -1); +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_ops.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_ops.hpp new file mode 100644 index 0000000000..bb934c6131 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_ops.hpp @@ -0,0 +1,193 @@ +#pragma once +#include +#include + +namespace sco +{ +////// In-place operations /////// + +// multiplication +inline void exprScale(AffExpr& v, double a) +{ + v.constant *= a; + for (unsigned i = 0; i < v.coeffs.size(); ++i) + v.coeffs[i] *= a; +} +inline void exprScale(QuadExpr& q, double a) +{ + exprScale(q.affexpr, a); + for (unsigned i = 0; i < q.coeffs.size(); ++i) + q.coeffs[i] *= a; +} + +// addition +inline void exprInc(AffExpr& a, double b) +{ + a.constant += b; +} +inline void exprInc(AffExpr& a, const AffExpr& b) +{ + a.constant += b.constant; + a.coeffs.insert(a.coeffs.end(), b.coeffs.begin(), b.coeffs.end()); + a.vars.insert(a.vars.end(), b.vars.begin(), b.vars.end()); +} +inline void exprInc(AffExpr& a, const Var& b) +{ + exprInc(a, AffExpr(b)); +} +inline void exprInc(QuadExpr& a, double b) +{ + exprInc(a.affexpr, b); +} +inline void exprInc(QuadExpr& a, const Var& b) +{ + exprInc(a.affexpr, AffExpr(b)); +} +inline void exprInc(QuadExpr& a, const AffExpr& b) +{ + exprInc(a.affexpr, b); +} +inline void exprInc(QuadExpr& a, const QuadExpr& b) +{ + exprInc(a.affexpr, b.affexpr); + a.coeffs.insert(a.coeffs.end(), b.coeffs.begin(), b.coeffs.end()); + a.vars1.insert(a.vars1.end(), b.vars1.begin(), b.vars1.end()); + a.vars2.insert(a.vars2.end(), b.vars2.begin(), b.vars2.end()); +} + +// subtraction +inline void exprDec(AffExpr& a, double b) +{ + a.constant -= b; +} +inline void exprDec(AffExpr& a, AffExpr b) +{ + exprScale(b, -1); + exprInc(a, b); +} +inline void exprDec(AffExpr& a, const Var& b) +{ + exprDec(a, AffExpr(b)); +} +inline void exprDec(QuadExpr& a, double b) +{ + exprDec(a.affexpr, b); +} +inline void exprDec(QuadExpr& a, const Var& b) +{ + exprDec(a.affexpr, b); +} +inline void exprDec(QuadExpr& a, const AffExpr& b) +{ + exprDec(a.affexpr, b); +} +inline void exprDec(QuadExpr& a, QuadExpr b) +{ + exprScale(b, -1); + exprInc(a, b); +} + +///////////////////// + +inline AffExpr exprMult(const Var& a, double b) +{ + AffExpr c(a); + exprScale(c, b); + return c; +} +// multiplication +inline AffExpr exprMult(AffExpr a, double b) +{ + exprScale(a, b); + return a; +} +inline QuadExpr exprMult(QuadExpr a, double b) +{ + exprScale(a, b); + return a; +} + +inline AffExpr exprAdd(AffExpr a, double b) +{ + exprInc(a, b); + return a; +} +inline AffExpr exprAdd(AffExpr a, const Var& b) +{ + exprInc(a, b); + return a; +} +inline AffExpr exprAdd(AffExpr a, const AffExpr& b) +{ + exprInc(a, b); + return a; +} +inline QuadExpr exprAdd(QuadExpr a, double b) +{ + exprInc(a, b); + return a; +} +inline QuadExpr exprAdd(QuadExpr a, const Var& b) +{ + exprInc(a, b); + return a; +} +inline QuadExpr exprAdd(QuadExpr a, const AffExpr& b) +{ + exprInc(a, b); + return a; +} +inline QuadExpr exprAdd(QuadExpr a, const QuadExpr& b) +{ + exprInc(a, b); + return a; +} + +inline AffExpr exprSub(AffExpr a, double b) +{ + exprDec(a, b); + return a; +} +inline AffExpr exprSub(AffExpr a, const Var& b) +{ + exprDec(a, b); + return a; +} +inline AffExpr exprSub(AffExpr a, const AffExpr& b) +{ + exprDec(a, b); + return a; +} +inline QuadExpr exprSub(QuadExpr a, double b) +{ + exprDec(a, b); + return a; +} +inline QuadExpr exprSub(QuadExpr a, const Var& b) +{ + exprDec(a, b); + return a; +} +inline QuadExpr exprSub(QuadExpr a, const AffExpr& b) +{ + exprDec(a, b); + return a; +} +inline QuadExpr exprSub(QuadExpr a, const QuadExpr& b) +{ + exprDec(a, b); + return a; +} + +////////////////////// +/** + * @brief Multiplies two AffExpr. Does not consider any optimizations for shared variables + * @return The QuadExpr result of the multiplication + */ +QuadExpr exprMult(const AffExpr&, const AffExpr&); + +QuadExpr exprSquare(const Var&); +QuadExpr exprSquare(const AffExpr&); + +AffExpr cleanupAff(const AffExpr&); +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_vec_ops.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_vec_ops.hpp new file mode 100644 index 0000000000..050cea9a95 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_vec_ops.hpp @@ -0,0 +1,30 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include + +namespace sco +{ +#if 0 +Matrix3d leftCrossProdMat(const Vector3d& x); +Matrix3d rightCrossProdMat(const Vector3d& x); + +ExprVector exprMatMult(const MatrixXd& A, const VarVector& x); +ExprVector exprMatMult(const MatrixXd& A, const ExprVector& x); + +ExprVector exprCross(const VectorXd& x, const VarVector& y); +ExprVector exprCross(const VarVector& x, const VectorXd& y); +ExprVector exprCross(const VectorXd& x, const ExprVector& y); +ExprVector exprCross(const ExprVector& x, const VectorXd& y); + +#endif +AffExpr varDot(const Eigen::VectorXd& x, const VarVector& v); +AffExpr exprDot(const Eigen::VectorXd& x, const AffExprVector& v); +#if 0 +QuadExpr varNorm2(const VarVector& v); +QuadExpr exprNorm2(const ExprVector& v); +#endif +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp new file mode 100644 index 0000000000..39809169ca --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp @@ -0,0 +1,53 @@ +#pragma once +#include + +/** + +@file gurobi_interface.hpp + +Gurobi backend + +*/ + +struct _GRBmodel; +typedef struct _GRBmodel GRBmodel; + +namespace sco +{ +class GurobiModel : public Model +{ +public: + GRBmodel* m_model; + VarVector m_vars; + CntVector m_cnts; + + GurobiModel(); + + Var addVar(const std::string& name) override; + Var addVar(const std::string& name, double lower, double upper) override; + + Cnt addEqCnt(const AffExpr&, const std::string& name) override; + Cnt addIneqCnt(const AffExpr&, const std::string& name) override; + Cnt addIneqCnt(const QuadExpr&, const std::string& name) override; + + void removeVars(const VarVector&) override; + void removeCnts(const CntVector&) override; + + void update() override; + void setVarBounds(const VarVector&, const DblVec& lower, const DblVec& upper) override; + DblVec getVarValues(const VarVector&) const override; + + CvxOptStatus optimize() override; + /** Don't use this function, because it adds constraints that aren't tracked + */ + CvxOptStatus optimizeFeasRelax(); + + void setObjective(const AffExpr&) override; + void setObjective(const QuadExpr&) override; + void writeToFile(const std::string& fname) override; + + VarVector getVars() const override; + + ~GurobiModel(); +}; +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/modeling.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/modeling.hpp new file mode 100644 index 0000000000..0db00f76f4 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/modeling.hpp @@ -0,0 +1,325 @@ +#pragma once + +/* + * Model a non-convex optimization problem by defining Cost and Constraint + * objects + * which know how to generate a convex approximation + * + * + */ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include + +namespace sco +{ +/** +Stores convex terms in a objective +For non-quadratic terms like hinge(x) and abs(x), it needs to add auxilliary +variables and linear constraints to the model +Note: When this object is deleted, the constraints and variables it added to the +model are removed + */ +class ConvexObjective +{ +public: + ConvexObjective(Model* model) : model_(model) + { + } + void addAffExpr(const AffExpr&); + void addQuadExpr(const QuadExpr&); + void addHinge(const AffExpr&, double coeff); + void addAbs(const AffExpr&, double coeff); + void addHinges(const AffExprVector&); + void addL1Norm(const AffExprVector&); + void addL2Norm(const AffExprVector&); + void addMax(const AffExprVector&); + + bool inModel() + { + return model_ != nullptr; + } + void addConstraintsToModel(); + void removeFromModel(); + double value(const DblVec& x); + + ~ConvexObjective(); + + Model* model_; + QuadExpr quad_; + VarVector vars_; + AffExprVector eqs_; + AffExprVector ineqs_; + CntVector cnts_; + +private: + ConvexObjective() + { + } + ConvexObjective(ConvexObjective&) + { + } +}; + +/** +Stores convex inequality constraints and affine equality constraints. +Actually only affine inequality constraints are currently implemented. +*/ +class ConvexConstraints +{ +public: + ConvexConstraints(Model* model) : model_(model) + { + } + /** Expression that should == 0 */ + void addEqCnt(const AffExpr&); + /** Expression that should <= 0 */ + void addIneqCnt(const AffExpr&); + void setModel(Model* model) + { + assert(!inModel()); + model_ = model; + } + bool inModel() + { + return model_ != nullptr; + } + void addConstraintsToModel(); + void removeFromModel(); + + DblVec violations(const DblVec& x); + double violation(const DblVec& x); + + ~ConvexConstraints(); + AffExprVector eqs_; + AffExprVector ineqs_; + +private: + Model* model_; + CntVector cnts_; + ConvexConstraints() : model_(nullptr) + { + } + ConvexConstraints(ConvexConstraints&) + { + } +}; + +/** +Non-convex cost function, which knows how to calculate its convex approximation +(convexify() method) +*/ +class Cost +{ +public: + /** Evaluate at solution vector x*/ + virtual double value(const DblVec&) = 0; + /** Convexify at solution vector x*/ + virtual ConvexObjectivePtr convex(const DblVec& x, Model* model) = 0; + /** Get problem variables associated with this cost */ + virtual VarVector getVars() = 0; + std::string name() + { + return name_; + } + void setName(const std::string& name) + { + name_ = name; + } + Cost() : name_("unnamed") + { + } + Cost(const std::string& name) : name_(name) + { + } + virtual ~Cost() = default; + +protected: + std::string name_; +}; + +/** +Non-convex vector-valued constraint function, which knows how to calculate its +convex approximation +*/ +class Constraint +{ +public: + /** inequality vs equality */ + virtual ConstraintType type() = 0; + /** Evaluate at solution vector x*/ + virtual DblVec value(const DblVec& x) = 0; + /** Convexify at solution vector x*/ + virtual ConvexConstraintsPtr convex(const DblVec& x, Model* model) = 0; + /** Calculate constraint violations (positive part for inequality constraint, + * absolute value for inequality constraint)*/ + DblVec violations(const DblVec& x); + /** Sum of violations */ + double violation(const DblVec& x); + /** Get problem variables associated with this constraint */ + virtual VarVector getVars() = 0; + std::string name() + { + return name_; + } + void setName(const std::string& name) + { + name_ = name; + } + Constraint() : name_("unnamed") + { + } + Constraint(const std::string& name) : name_(name) + { + } + virtual ~Constraint() + { + } + +protected: + std::string name_; +}; + +class EqConstraint : public Constraint +{ +public: + ConstraintType type() override + { + return EQ; + } + EqConstraint() : Constraint() + { + } + EqConstraint(const std::string& name) : Constraint(name) + { + } +}; + +class IneqConstraint : public Constraint +{ +public: + ConstraintType type() override + { + return INEQ; + } + IneqConstraint() : Constraint() + { + } + IneqConstraint(const std::string& name) : Constraint(name) + { + } +}; + +/** +Non-convex optimization problem +*/ +class OptProb +{ +public: + OptProb(ModelType convex_solver = ModelType::AUTO_SOLVER); + virtual ~OptProb() = default; + + /** create variables with bounds [-INFINITY, INFINITY] */ + VarVector createVariables(const std::vector& names); + /** create variables with bounds [lb[i], ub[i] */ + VarVector createVariables(const std::vector& names, const DblVec& lb, const DblVec& ub); + /** set the lower bounds of all the variables */ + void setLowerBounds(const DblVec& lb); + /** set the upper bounds of all the variables */ + void setUpperBounds(const DblVec& ub); + /** set lower bounds of some of the variables */ + void setLowerBounds(const DblVec& lb, const VarVector& vars); + /** set upper bounds of some of the variables */ + void setUpperBounds(const DblVec& ub, const VarVector& vars); + /** Note: in the current implementation, this function just adds the + * constraint to the + * model. So if you're not careful, you might end up with an infeasible + * problem. */ + void addLinearConstraint(const AffExpr&, ConstraintType type); + /** Add nonlinear cost function */ + void addCost(CostPtr); + /** Add nonlinear constraint function */ + void addConstraint(ConstraintPtr); + void addEqConstraint(ConstraintPtr); + void addIneqConstraint(ConstraintPtr); + /** Find closest point to solution vector x that satisfies linear inequality + * constraints */ + DblVec getCentralFeasiblePoint(const DblVec& x); + DblVec getClosestFeasiblePoint(const DblVec& x); + + std::vector getConstraints() const; + const std::vector& getCosts() + { + return costs_; + } + const std::vector& getIneqConstraints() + { + return ineqcnts_; + } + const std::vector& getEqConstraints() + { + return eqcnts_; + } + const DblVec& getLowerBounds() + { + return lower_bounds_; + } + const DblVec& getUpperBounds() + { + return upper_bounds_; + } + ModelPtr getModel() + { + return model_; + } + const VarVector& getVars() + { + return vars_; + } + int getNumCosts() + { + return static_cast(costs_.size()); + } + int getNumConstraints() + { + return static_cast(eqcnts_.size() + ineqcnts_.size()); + } + int getNumVars() + { + return static_cast(vars_.size()); + } + +protected: + ModelPtr model_; + VarVector vars_; + DblVec lower_bounds_; + DblVec upper_bounds_; + std::vector costs_; + std::vector eqcnts_; + std::vector ineqcnts_; + + OptProb(OptProb&); +}; + +template +inline void setVec(DblVec& x, const VarVector& vars, const VecType& vals) +{ + assert(vars.size() == vals.size()); + for (size_t i = 0; i < vars.size(); ++i) + { + x[static_cast(vars[i].var_rep->index)] = vals[i]; + } +} +template +inline OutVecType getVec1(const DblVec& x, const VarVector& vars) +{ + OutVecType out(vars.size()); + for (unsigned i = 0; i < vars.size(); ++i) + out[i] = x[vars[i].var_rep->index]; + return out; +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/modeling_utils.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/modeling_utils.hpp new file mode 100644 index 0000000000..c8f19e5b69 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/modeling_utils.hpp @@ -0,0 +1,109 @@ +#pragma once +#include +#include +#include +/** +@file modeling_utils.hpp +@brief Build problem from user-defined functions +Utilities for creating Cost and Constraint objects from functions +using numerical derivatives or user-defined analytic derivatives. + */ + +namespace sco +{ +enum PenaltyType +{ + SQUARED, + ABS, + HINGE +}; + +/** +x is the big solution vector of the whole problem. vars are variables that +index into the vector x +this function extracts (from x) the values of the variables in vars + */ +Eigen::VectorXd getVec(const DblVec& x, const VarVector& vars); +/** +Same idea as above, but different output type + */ +DblVec getDblVec(const DblVec& x, const VarVector& vars); + +AffExpr affFromValGrad(double y, const Eigen::VectorXd& x, const Eigen::VectorXd& dydx, const VarVector& vars); + +class CostFromFunc : public Cost +{ +public: + /// supply function, obtain derivative and hessian numerically + CostFromFunc(ScalarOfVectorPtr f, const VarVector& vars, const std::string& name, bool full_hessian = false); + double value(const DblVec& x) override; + ConvexObjectivePtr convex(const DblVec& x, Model* model) override; + VarVector getVars() override + { + return vars_; + } + +protected: + ScalarOfVectorPtr f_; + VarVector vars_; + bool full_hessian_; + double epsilon_; +}; + +class CostFromErrFunc : public Cost +{ +public: + /// supply error function, obtain derivative numerically + CostFromErrFunc(VectorOfVectorPtr f, const VarVector& vars, const Eigen::VectorXd& coeffs, PenaltyType pen_type, + const std::string& name); + /// supply error function and gradient + CostFromErrFunc(VectorOfVectorPtr f, MatrixOfVectorPtr dfdx, const VarVector& vars, const Eigen::VectorXd& coeffs, + PenaltyType pen_type, const std::string& name); + double value(const DblVec& x) override; + ConvexObjectivePtr convex(const DblVec& x, Model* model) override; + VarVector getVars() override + { + return vars_; + } + +protected: + VectorOfVectorPtr f_; + MatrixOfVectorPtr dfdx_; + VarVector vars_; + Eigen::VectorXd coeffs_; + PenaltyType pen_type_; + double epsilon_; +}; + +class ConstraintFromErrFunc : public Constraint +{ +public: + /// supply error function, obtain derivative numerically + ConstraintFromErrFunc(VectorOfVectorPtr f, const VarVector& vars, const Eigen::VectorXd& coeffs, ConstraintType type, + const std::string& name); + /// supply error function and gradient + ConstraintFromErrFunc(VectorOfVectorPtr f, MatrixOfVectorPtr dfdx, const VarVector& vars, + const Eigen::VectorXd& coeffs, ConstraintType type, const std::string& name); + DblVec value(const DblVec& x) override; + ConvexConstraintsPtr convex(const DblVec& x, Model* model) override; + ConstraintType type() override + { + return type_; + } + VarVector getVars() override + { + return vars_; + } + +protected: + VectorOfVectorPtr f_; + MatrixOfVectorPtr dfdx_; + VarVector vars_; + Eigen::VectorXd coeffs_; + ConstraintType type_; + double epsilon_; + Eigen::VectorXd scaling_; +}; + +std::string AffExprToString(const AffExpr& aff); +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/num_diff.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/num_diff.hpp new file mode 100644 index 0000000000..388d30562f --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/num_diff.hpp @@ -0,0 +1,79 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +/* + * Numerical derivatives + */ + +namespace sco +{ +class ScalarOfVector; +class VectorOfVector; +class MatrixOfVector; +typedef std::shared_ptr ScalarOfVectorPtr; +typedef std::shared_ptr VectorOfVectorPtr; +typedef std::shared_ptr MatrixOfVectorPtr; + +class ScalarOfVector +{ +public: + virtual double operator()(const Eigen::VectorXd& x) const = 0; + double call(const Eigen::VectorXd& x) const + { + return operator()(x); + } + virtual ~ScalarOfVector() + { + } + typedef std::function func; + static ScalarOfVectorPtr construct(const func&); + // typedef VectorXd (*c_func)(const VectorXd&); + // static ScalarOfVectorPtr construct(const c_func&); +}; +class VectorOfVector +{ +public: + virtual Eigen::VectorXd operator()(const Eigen::VectorXd& x) const = 0; + Eigen::VectorXd call(const Eigen::VectorXd& x) const + { + return operator()(x); + } + virtual ~VectorOfVector() + { + } + typedef std::function func; + static VectorOfVectorPtr construct(const func&); + // typedef VectorXd (*c_func)(const VectorXd&); + // static VectorOfVectorPtr construct(const c_func&); +}; +class MatrixOfVector +{ +public: + virtual Eigen::MatrixXd operator()(const Eigen::VectorXd& x) const = 0; + Eigen::MatrixXd call(const Eigen::VectorXd& x) const + { + return operator()(x); + } + virtual ~MatrixOfVector() + { + } + typedef std::function func; + static MatrixOfVectorPtr construct(const func&); + // typedef VectorMatrixXd (*c_func)(const VectorXd&); + // static MatrixOfVectorPtr construct(const c_func&); +}; + +Eigen::VectorXd calcForwardNumGrad(const ScalarOfVector& f, const Eigen::VectorXd& x, double epsilon); +Eigen::MatrixXd calcForwardNumJac(const VectorOfVector& f, const Eigen::VectorXd& x, double epsilon); +void calcGradAndDiagHess(const ScalarOfVector& f, const Eigen::VectorXd& x, double epsilon, double& y, + Eigen::VectorXd& grad, Eigen::VectorXd& hess); +void calcGradHess(ScalarOfVectorPtr f, const Eigen::VectorXd& x, double epsilon, double& y, Eigen::VectorXd& grad, + Eigen::MatrixXd& hess); +VectorOfVectorPtr forwardNumGrad(ScalarOfVectorPtr f, double epsilon); +MatrixOfVectorPtr forwardNumJac(VectorOfVectorPtr f, double epsilon); +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/optimizers.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/optimizers.hpp new file mode 100644 index 0000000000..6d4d9a3915 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/optimizers.hpp @@ -0,0 +1,149 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +/* + * Algorithms for non-convex, constrained optimization + */ + +namespace sco +{ +enum OptStatus +{ + OPT_CONVERGED, + OPT_SCO_ITERATION_LIMIT, // hit iteration limit before convergence + OPT_PENALTY_ITERATION_LIMIT, + OPT_FAILED, + INVALID +}; +static const char* OptStatus_strings[] = { "CONVERGED", "SCO_ITERATION_LIMIT", "PENALTY_ITERATION_LIMIT", "FAILED", + "INVALID" }; +inline std::string statusToString(OptStatus status) +{ + return OptStatus_strings[status]; +} +struct OptResults +{ + DblVec x; // solution estimate + OptStatus status; + double total_cost; + DblVec cost_vals; + DblVec cnt_viols; + int n_func_evals, n_qp_solves; + void clear() + { + x.clear(); + status = INVALID; + cost_vals.clear(); + cnt_viols.clear(); + n_func_evals = 0; + n_qp_solves = 0; + } + OptResults() + { + clear(); + } +}; +std::ostream& operator<<(std::ostream& o, const OptResults& r); + +class Optimizer +{ + /* + * Solves an optimization problem + */ +public: + virtual ~Optimizer() = default; + virtual OptStatus optimize() = 0; + virtual void setProblem(OptProbPtr prob) + { + prob_ = prob; + } + void initialize(const DblVec& x); + DblVec& x() + { + return results_.x; + } + OptResults& results() + { + return results_; + } + typedef std::function Callback; + void addCallback(const Callback& f); // called before each iteration +protected: + std::vector callbacks_; + void callCallbacks(); + OptProbPtr prob_; + OptResults results_; +}; + +struct BasicTrustRegionSQPParameters +{ + double improve_ratio_threshold; // minimum ratio true_improve/approx_improve + // to accept step + double min_trust_box_size; // if trust region gets any smaller, exit and + // report convergence + double min_approx_improve; // if model improves less than this, exit and + // report convergence + double min_approx_improve_frac; // if model improves less than this, exit and + // report convergence + double max_iter; // The max number of iterations + double trust_shrink_ratio; // if improvement is less than + // improve_ratio_threshold, shrink trust region by + // this ratio + double trust_expand_ratio; // if improvement is less than + // improve_ratio_threshold, shrink trust region by + // this ratio + double cnt_tolerance; // after convergence of penalty subproblem, if + // constraint violation is less than this, we're done + double max_merit_coeff_increases; // number of times that we jack up penalty + // coefficient + double merit_coeff_increase_ratio; // ratio that we increate coeff each time + double max_time; // not yet implemented + double merit_error_coeff; // initial penalty coefficient + double trust_box_size; // current size of trust region (component-wise) + + BasicTrustRegionSQPParameters(); +}; + +class BasicTrustRegionSQP : public Optimizer +{ + /* + * Alternates between convexifying objectives and constraints and then solving + * convex subproblem + * Uses a merit function to decide whether or not to accept the step + * merit function = objective + merit_err_coeff * | constraint_error | + * Note: sometimes the convexified objectives and constraints lead to an + * infeasible subproblem + * In that case, you should turn them into penalties and solve that problem + * (todo: implement penalty-based sqp that gracefully handles infeasible + * constraints) + */ +public: + BasicTrustRegionSQP(); + BasicTrustRegionSQP(OptProbPtr prob); + void setProblem(OptProbPtr prob) override; + void setParameters(const BasicTrustRegionSQPParameters& param) + { + param_ = param; + } + const BasicTrustRegionSQPParameters& getParameters() const + { + return param_; + } + BasicTrustRegionSQPParameters& getParameters() + { + return param_; + } + OptStatus optimize() override; + +protected: + void adjustTrustRegion(double ratio); + void setTrustBoxConstraints(const DblVec& x); + ModelPtr model_; + BasicTrustRegionSQPParameters param_; +}; +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/osqp_interface.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/osqp_interface.hpp new file mode 100644 index 0000000000..b17f7256a7 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/osqp_interface.hpp @@ -0,0 +1,84 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include + +namespace sco +{ +/** + * OSQPModel uses the BSD solver OSQP to solve a linearly constrained QP. + * OSQP solves a problem in the form: + * ``` + * min 1/2*x'Px + q'x + * s.t. l <= Ax <= u + * ``` + * + * More informations about the solver are available at: + * https://osqp.org/docs/ + */ +class OSQPModel : public Model +{ + OSQPSettings osqp_settings_; /**< OSQP Settings */ + /** OSQPData. Some fields here (`osp_data_.A` and `osp_data_.P`) are + * automatically allocated by OSQP, but deallocated by us. */ + OSQPData osqp_data_; + /** OSQP Workspace. Memory here is managed by OSQP */ + OSQPWorkspace* osqp_workspace_; + + /** Updates OSQP quadratic cost matrix from QuadExpr expression. + * Transforms QuadExpr objective_ into the OSQP CSC matrix P_ */ + void updateObjective(); + + /** Updates qpOASES constraints from AffExpr expression. + * Transforms AffExpr cntr_exprs_ and box bounds lbs_ and ubs_ into the + * OSQP CSC matrix A_, and vectors lbA_ and ubA_ */ + void updateConstraints(); + + /** Creates or updates the solver and its workspace */ + void createOrUpdateSolver(); + + VarVector vars_; /**< model variables */ + CntVector cnts_; /**< model's constraints sizes */ + DblVec lbs_, ubs_; /**< variables bounds */ + AffExprVector cnt_exprs_; /**< constraints expressions */ + ConstraintTypeVector cnt_types_; /**< constraints types */ + DblVec solution_; /**< optimizizer's solution for current model */ + + std::vector P_row_indices_; /**< row indices for P, CSC format */ + std::vector P_column_pointers_; /**< column pointers for P, CSC format */ + DblVec P_csc_data_; /**< P values in CSC format */ + Eigen::VectorXd q_; /**< linear part of the objective */ + + std::vector A_row_indices_; /**< row indices for constraint matrix, CSC format */ + std::vector A_column_pointers_; /**< column pointers for constraint matrix, CSC format */ + DblVec A_csc_data_; /**< constraint matrix values in CSC format */ + DblVec l_, u_; /**< linear constraints upper and lower limits */ + + QuadExpr objective_; /**< objective QuadExpr expression */ + +public: + OSQPModel(); + virtual ~OSQPModel(); + + Var addVar(const std::string& name) override; + Cnt addEqCnt(const AffExpr&, const std::string& name) override; + Cnt addIneqCnt(const AffExpr&, const std::string& name) override; + Cnt addIneqCnt(const QuadExpr&, const std::string& name) override; + void removeVars(const VarVector& vars) override; + void removeCnts(const CntVector& cnts) override; + + void update() override; + void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) override; + DblVec getVarValues(const VarVector& vars) const override; + virtual CvxOptStatus optimize() override; + virtual void setObjective(const AffExpr&) override; + virtual void setObjective(const QuadExpr&) override; + virtual void writeToFile(const std::string& fname) override; + virtual VarVector getVars() const override; +}; +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp new file mode 100644 index 0000000000..ddf1ce9153 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp @@ -0,0 +1,95 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include + +namespace sco +{ +/** + * qpOASESModel uses the LGPL solver qpOASES to solve a linearly constrained QP. + * qpOASES solves a problem in the form: + * ``` + * min 1/2*x'Hx + x'g + * s.t. lb <= x <= ub + * lbA <= Ax <= ubA + * ``` + * + * More informations about the solver are available at: + * https://projects.coin-or.org/qpOASES + */ +class qpOASESModel : public Model +{ + /** pointer to a qpOASES Sequential Quadratic Problem*/ + std::shared_ptr qpoases_problem_; + qpOASES::Options qpoases_options_; /**< qpOASES solver options */ + + qpOASES::SymSparseMat H_; /**< Quadratic cost matrix */ + qpOASES::SparseMatrix A_; /**< Constraints matrix */ + + /** Updates qpOASES Hessian matrix from QuadExpr expression. + * Transforms QuadExpr objective_ into the qpOASES sparse matrix H_*/ + void updateObjective(); + + /** Updates qpOASES constraints from AffExpr expression. + * Transforms AffExpr cntr_exprs_ into the qpOASES sparse matrix A_, and + * vectors lbA_ and ubA_ */ + void updateConstraints(); + + /** + * Instantiates a new qpOASES problem if it has not been instantiated yet + * or if the size of the problem has changed. + * + * @returns true if a new qpOASES problem has been instantiated + */ + bool updateSolver(); + + /** + * Instantiates a new qpOASES problem + */ + void createSolver(); + + VarVector vars_; /**< model variables */ + CntVector cnts_; /**< model's constraints sizes */ + DblVec lb_, ub_; /**< variables bounds */ + AffExprVector cnt_exprs_; /**< constraints expressions */ + ConstraintTypeVector cnt_types_; /**< constraints types */ + DblVec solution_; /**< optimizizer's solution for current model */ + + IntVec H_row_indices_; /**< row indices for Hessian, CSC format */ + IntVec H_column_pointers_; /**< column pointers for Hessian, CSC format */ + DblVec H_csc_data_; /**< Hessian values in CSC format */ + Eigen::VectorXd g_; /**< gradient of the optimization problem */ + + IntVec A_row_indices_; /**< row indices for constraint matrix, CSC format */ + IntVec A_column_pointers_; /**< column pointers for constraint matrix, CSC format */ + DblVec A_csc_data_; /**< constraint matrix values in CSC format */ + DblVec lbA_, ubA_; /**< linear constraints upper and lower limits */ + + QuadExpr objective_; /**< objective QuadExpr expression */ + +public: + qpOASESModel(); + virtual ~qpOASESModel(); + + Var addVar(const std::string& name) override; + Cnt addEqCnt(const AffExpr&, const std::string& name) override; + Cnt addIneqCnt(const AffExpr&, const std::string& name) override; + Cnt addIneqCnt(const QuadExpr&, const std::string& name) override; + void removeVars(const VarVector& vars) override; + void removeCnts(const CntVector& cnts) override; + + void update() override; + void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) override; + DblVec getVarValues(const VarVector& vars) const override; + virtual CvxOptStatus optimize() override; + virtual void setObjective(const AffExpr&) override; + virtual void setObjective(const QuadExpr&) override; + virtual void writeToFile(const std::string& fname) override; + virtual VarVector getVars() const override; +}; +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/sco_common.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/sco_common.hpp new file mode 100644 index 0000000000..3769654ff7 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/sco_common.hpp @@ -0,0 +1,61 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include + +namespace sco +{ +typedef std::vector DblVec; +typedef std::vector IntVec; +typedef std::vector VarVector; +typedef std::vector AffExprVector; +typedef std::vector QuadExprVector; +typedef std::vector CntVector; + +inline double vecSum(const DblVec& v) +{ + double out = 0; + for (unsigned i = 0; i < v.size(); ++i) + out += v[i]; + return out; +} +inline double vecAbsSum(const DblVec& v) +{ + double out = 0; + for (unsigned i = 0; i < v.size(); ++i) + out += fabs(v[i]); + return out; +} +inline double pospart(double x) +{ + return (x > 0) ? x : 0; +} +inline double sq(double x) +{ + return x * x; +} +inline double vecHingeSum(const DblVec& v) +{ + double out = 0; + for (unsigned i = 0; i < v.size(); ++i) + out += pospart(v[i]); + return out; +} +inline double vecMax(const DblVec& v) +{ + return *std::max_element(v.begin(), v.end()); +} +inline double vecDot(const DblVec& a, const DblVec& b) +{ + assert(a.size() == b.size()); + double out = 0; + for (unsigned i = 0; i < a.size(); ++i) + out += a[i] * b[i]; + return out; +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/sco_fwd.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/sco_fwd.hpp new file mode 100644 index 0000000000..982c8a9ee0 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/sco_fwd.hpp @@ -0,0 +1,48 @@ +// autogenerated forward declaration header +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +namespace sco +{ +class GurobiModel; +typedef std::shared_ptr GurobiModelPtr; +class ConvexObjective; +typedef std::shared_ptr ConvexObjectivePtr; +class ConvexConstraints; +typedef std::shared_ptr ConvexConstraintsPtr; +class Cost; +typedef std::shared_ptr CostPtr; +class Constraint; +typedef std::shared_ptr ConstraintPtr; +class EqConstraint; +typedef std::shared_ptr EqConstraintPtr; +class IneqConstraint; +typedef std::shared_ptr IneqConstraintPtr; +class OptProb; +typedef std::shared_ptr OptProbPtr; +class CostFromFunc; +typedef std::shared_ptr CostFromFuncPtr; +class ConstraintFromErrFunc; +typedef std::shared_ptr ConstraintFromFuncPtr; +class Optimizer; +typedef std::shared_ptr OptimizerPtr; +class BasicTrustRegionSQP; +typedef std::shared_ptr BasicTrustRegionSQPPtr; +class Model; +typedef std::shared_ptr ModelPtr; +struct VarRep; +typedef std::shared_ptr VarRepPtr; +struct Var; +typedef std::shared_ptr VarPtr; +struct CntRep; +typedef std::shared_ptr CntRepPtr; +struct Cnt; +typedef std::shared_ptr CntPtr; +struct AffExpr; +typedef std::shared_ptr AffExprPtr; +struct QuadExpr; +typedef std::shared_ptr QuadExprPtr; +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/solver_interface.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/solver_interface.hpp new file mode 100644 index 0000000000..6e7ae0b8cc --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/solver_interface.hpp @@ -0,0 +1,247 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP +#include + +/** +@file solver_interface.hpp +@brief Interface to convex solvers + + This is based on Gurobi's nice c++ API (though the SCO Gurobi backend uses the +Gurobi c api). + However, our intention is to allow for different solvers to be used as +backends. + */ + +namespace sco +{ +enum ConstraintType +{ + EQ, + INEQ +}; + +typedef std::vector ConstraintTypeVector; + +enum CvxOptStatus +{ + CVX_SOLVED, + CVX_INFEASIBLE, + CVX_FAILED +}; + +/** @brief Convex optimization problem + +Gotchas: +- after adding a variable, need to call update() before doing anything else with +that variable + + */ +class Model +{ +public: + virtual Var addVar(const std::string& name) = 0; + virtual Var addVar(const std::string& name, double lb, double ub); + + virtual Cnt addEqCnt(const AffExpr&, const std::string& name) = 0; // expr == 0 + virtual Cnt addIneqCnt(const AffExpr&, const std::string& name) = 0; // expr <= 0 + virtual Cnt addIneqCnt(const QuadExpr&, const std::string& name) = 0; // expr <= 0 + + virtual void removeVar(const Var& var); + virtual void removeCnt(const Cnt& cnt); + virtual void removeVars(const VarVector& vars) = 0; + virtual void removeCnts(const CntVector& cnts) = 0; + + virtual void update() = 0; // call after adding/deleting stuff + virtual void setVarBounds(const Var& var, double lower, double upper); + virtual void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) = 0; + virtual double getVarValue(const Var& var) const; + virtual DblVec getVarValues(const VarVector& vars) const = 0; + virtual CvxOptStatus optimize() = 0; + + virtual void setObjective(const AffExpr&) = 0; + virtual void setObjective(const QuadExpr&) = 0; + virtual void writeToFile(const std::string& fname) = 0; + + virtual VarVector getVars() const = 0; + + virtual ~Model() + { + } +}; + +struct VarRep +{ + VarRep(int _index, const std::string& _name, void* _creator) + : index(_index), name(_name), removed(false), creator(_creator) + { + } + int index; + std::string name; + bool removed; + void* creator; +}; + +struct Var +{ + VarRep* var_rep; + Var() : var_rep(nullptr) + { + } + Var(VarRep* var_rep) : var_rep(var_rep) + { + } + Var(const Var& other) : var_rep(other.var_rep) + { + } + double value(const double* x) const + { + return x[var_rep->index]; + } + double value(const DblVec& x) const + { + assert(var_rep->index < static_cast(x.size())); + return x[static_cast(var_rep->index)]; + } +}; + +struct CntRep +{ + CntRep(int _index, void* _creator) : index(_index), removed(false), creator(_creator) + { + } + int index; + bool removed; + void* creator; + ConstraintType type; + std::string expr; // todo placeholder +}; + +struct Cnt +{ + CntRep* cnt_rep; + Cnt() : cnt_rep(nullptr) + { + } + Cnt(CntRep* cnt_rep) : cnt_rep(cnt_rep) + { + } + Cnt(const Cnt& other) : cnt_rep(other.cnt_rep) + { + } +}; + +struct AffExpr +{ // affine expression + double constant; + DblVec coeffs; + VarVector vars; + AffExpr() : constant(0) + { + } + explicit AffExpr(double a) : constant(a) + { + } + explicit AffExpr(const Var& v) : constant(0), coeffs(1, 1), vars(1, v) + { + } + AffExpr(const AffExpr& other) : constant(other.constant), coeffs(other.coeffs), vars(other.vars) + { + } + size_t size() const + { + return coeffs.size(); + } + double value(const double* x) const; + double value(const DblVec& x) const; +}; + +struct QuadExpr +{ + AffExpr affexpr; + DblVec coeffs; + VarVector vars1; + VarVector vars2; + QuadExpr() + { + } + explicit QuadExpr(double a) : affexpr(a) + { + } + explicit QuadExpr(const Var& v) : affexpr(v) + { + } + explicit QuadExpr(const AffExpr& aff) : affexpr(aff) + { + } + size_t size() const + { + return coeffs.size(); + } + double value(const double* x) const; + double value(const DblVec& x) const; +}; + +std::ostream& operator<<(std::ostream&, const Var&); +std::ostream& operator<<(std::ostream&, const Cnt&); +std::ostream& operator<<(std::ostream&, const AffExpr&); +std::ostream& operator<<(std::ostream&, const QuadExpr&); + +class ModelType +{ +public: + enum Value + { + GUROBI, + BPMPD, + OSQP, + QPOASES, + AUTO_SOLVER + }; + + static const std::vector MODEL_NAMES_; + + ModelType(); + ModelType(const ModelType::Value& v); + ModelType(const int& v); + ModelType(const std::string& s); + operator int() const; + bool operator==(const ModelType::Value& a) const; + bool operator==(const ModelType& a) const; + bool operator!=(const ModelType& a) const; + void fromJson(const Json::Value& v); + friend std::ostream& operator<<(std::ostream& os, const ModelType& cs); + +private: + Value value_; +}; + +std::vector availableSolvers(); + +std::ostream& operator<<(std::ostream& os, const ModelType& cs); + +ModelPtr createModel(ModelType model_type = ModelType::AUTO_SOLVER); + +IntVec vars2inds(const VarVector& vars); + +IntVec cnts2inds(const CntVector& cnts); + +/** + * @brief simplify2 gets as input a list of indices, corresponding to non-zero + * values in vals, checks that all indexed values are actually non-zero, + * and if they are not, removes them from vals and inds, so that + * inds_out.size() <= inds.size(). Also, it will compact vals so that + * vals_out.size() == inds_out.size() + * + * @param[in,out] inds indices of non-vero variables in vals + * @param[in,out] val values + */ +void simplify2(IntVec& inds, DblVec& vals); +} diff --git a/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/solver_utils.hpp b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/solver_utils.hpp new file mode 100644 index 0000000000..42fcd3e34e --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/solver_utils.hpp @@ -0,0 +1,142 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include + +namespace sco +{ +/** + * @brief transform an `AffExpr` to an `Eigen::SparseVector` + * + * @param [in] expr an `AffExpr` expression + * @param [out] sparse_vector vector where to store results. It will be resized + * to the correct size. + * @param [in] n_vars the number of variables in expr. It is usually equal + * to `expr.size()`, but it might be larger. + */ +void exprToEigen(const AffExpr& expr, Eigen::SparseVector& sparse_vector, const int& n_vars); + +/** + * @brief transform a `QuadExpr` to an `Eigen::SparseMatrix` plus + * `Eigen::VectorXd` + * + * @param [in] expr a `QuadExpr` expression + * @param [out] sparse_matrix matrix where to store results. It will be resized + * to the correct size. + * @param [out] vector vector where to store the affine part of the + * `QuadExpr`. It will be resized to the correct size. + * @param [in] n_vars the number of variables in expr. It is usually equal + * to `expr.size()`, but it might be larger. + * @param [in] matrix_is_halved if `true`, sparse_matrix will be premultiplied + * by the coefficient `2`. + * @param [in] force_diagonal if true, we will forcibly add elements to the + * diagonal of the sparse matrix, adding `0.` if needed + */ +void exprToEigen(const QuadExpr& expr, Eigen::SparseMatrix& sparse_matrix, Eigen::VectorXd& vector, + const int& n_vars, const bool& matrix_is_halved = false, const bool& force_diagonal = false); + +/** + * @brief transform a vector of `AffExpr` to an `Eigen::SparseMatrix` plus an + * `Eigen::VectorXd`. + * Notice that the underlying transformation is so that the the affine + * expressions of the type `ax + b` will be stacked in matrix form into + * `Ax + b` and then transformed into the expression `Ax <= -b` so that + * `vector[i] = -expr_vec[i].constant` + * @param [in] expr_vec a an `AffExprVector` + * @param [out] sparse_matrix matrix where to store results. It will be resized + * to the correct size. + * @param [out] vector vector where to store all the constants of each `AffExpr`, + * ordered by their appearance order in `expr_vec`. + * It will be resized to the correct size. + * @param [in] n_vars the larger number of variables in expr. + * It is usually the same for each `expr` in `expr_vec`, + * and equal to `expr.size()`, but it might be larger. + */ +void exprToEigen(const AffExprVector& expr_vec, Eigen::SparseMatrix& sparse_matrix, Eigen::VectorXd& vector, + const int& n_vars = -1); +/** + * @brief Converts triplets to an `Eigen::SparseMatrix`. + * @param [in] rows_i a vector of row indices + * @param [in] cols_j a vector of columns indices + * @param [in] values_ij a vector of values, so that: + * `M[rows_i[k], cols_j[k]] = values_ij[k]` + * @param [in,out] sparse_matrix must be of the right size, as we should not + * be guessing the right size of sparse_matrix from + * a sparse triplet representation. + */ +void tripletsToEigen(const IntVec& rows_i, const IntVec& cols_j, const DblVec& values_ij, + Eigen::SparseMatrix& sparse_matrix); + +/** + * @brief Converts an `Eigen::SparseMatrix` into triplets format + * @param [in] sparse_matrix an `Eigen::SparseMatrix` + * @param [out] rows_i a vector of row indices + * @param [out] cols_j a vector of columns indices + * @param [out] values_ij a vector of values, so that: + * `M[rows_i[k], cols_j[k]] = values_ij[k]` + */ +void eigenToTriplets(const Eigen::SparseMatrix& sparse_matrix, IntVec& rows_i, IntVec& cols_j, + DblVec& values_ij); + +/** + * @brief converts a sparse matrix into compressed + * sparse column representation (CSC). + * + * @param [out] row_indices row indices for a CSC matrix + * @param [out] column_pointers column pointer for a CSC matrix + * @param [out] values pointer to non-zero elements in CSC representation + * @param [in,out] sparse_matrix input matrix: will be compressed + */ +template +void eigenToCSC(Eigen::SparseMatrix& sparse_matrix, std::vector& row_indices, + std::vector& column_pointers, DblVec& values) +{ + Eigen::SparseMatrix sm_t; + auto sm_ref = std::ref(sparse_matrix); + + if (eigenUpLoType > 0) + { + sm_t = sparse_matrix.triangularView(); + sm_ref = std::ref(sm_t); + } + sm_ref.get().makeCompressed(); + + if (sizeof(T) != sizeof(int)) + { + IntVec row_indices_int, column_pointers_int; + auto si_p = sm_ref.get().innerIndexPtr(); + row_indices_int.assign(si_p, si_p + sm_ref.get().nonZeros()); + row_indices.clear(); + row_indices.reserve(row_indices_int.size()); + for (const auto& v : row_indices_int) + row_indices.push_back(static_cast(v)); + + si_p = sm_ref.get().outerIndexPtr(); + column_pointers_int.assign(si_p, si_p + sm_ref.get().outerSize()); + column_pointers.clear(); + column_pointers.reserve(column_pointers_int.size()); + for (const auto& v : column_pointers_int) + column_pointers.push_back(static_cast(v)); + } + else + { + auto si_p = sm_ref.get().innerIndexPtr(); + row_indices.assign(si_p, si_p + sm_ref.get().nonZeros()); + + si_p = sm_ref.get().outerIndexPtr(); + column_pointers.assign(si_p, si_p + sm_ref.get().outerSize()); + } + + // while Eigen does not enforce this, CSC format requires that column + // pointers ends with the number of non-zero elements + column_pointers.push_back(sm_ref.get().nonZeros()); + + auto csc_v = sm_ref.get().valuePtr(); + values.assign(csc_v, csc_v + sm_ref.get().nonZeros()); +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/osqp_configure.in b/moveit_planners/trajopt/trajopt_sco/osqp_configure.in new file mode 100644 index 0000000000..655e362901 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/osqp_configure.in @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.2) + +project(osqp) + +include(ExternalProject) + +ExternalProject_Add(osqp + PREFIX osqp + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=osqp-install + # Github tar balls don't code in submodules, so need to + # download via git + GIT_REPOSITORY https://github.com/oxfordcontrol/osqp.git + GIT_TAG v0.5.0 # currently, not compatible with v0.6.0 +) \ No newline at end of file diff --git a/moveit_planners/trajopt/trajopt_sco/package.xml b/moveit_planners/trajopt/trajopt_sco/package.xml new file mode 100644 index 0000000000..eaba8862f4 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/package.xml @@ -0,0 +1,19 @@ + + + trajopt_sco + 0.1.0 + The trajopt_sco package + Levi Armstrong + BSD + + catkin + trajopt_utils + + rosunit + + + + + + + diff --git a/moveit_planners/trajopt/trajopt_sco/src/bpmpd.par b/moveit_planners/trajopt/trajopt_sco/src/bpmpd.par new file mode 100644 index 0000000000..342c55aee8 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/bpmpd.par @@ -0,0 +1,189 @@ +! Parameter file for multiply predictor - corrector method + +MPS and Input/Output definitions + +BEGIN_PROB ! Begin marker +NAME = default ! Problem name, max. 36 characters +STARTSOL = ! Warm start solution file name +MAXMN = 250000. ! Upper bound for M+N +MINMAX = 1. ! =1. (mininmize) =-1. (maximize) +OUTPUT = 1. ! Output level 0 :only statistic, 1 :solution +OBJNAM = ! Objective function name (first N type row) +RHSNAM = ! RHS name (first) +BNDNAM = ! BOUND name (first) +RNGNAM = ! RANGE name (first) +EXPLSLACK = 1. ! Include slack as variables +BIGBOUND = 1.0d+15 ! Skip bounds and ranges exceeding this limit +SMALLVAL = 1.0d-12 ! Skip matrix (and bound) values under this limit +ITERLOG = 3. ! 0:no report, 1: stdout, 2: logfile, 3:both + ! 4:advanced debug info 8: debug on steplengths + ! 256: generating bpmpd.qps at start +END_PROB + + +Optimization parameter section + + +BEGIN_OPT + +! Supernode parameters + +PSUPNODE = 4. ! Primer supernode length +SSUPNODE = 4. ! Secunder supernode length +CACHESIZ = 256. ! Size of the cache in kilobytes +PUSHFACT = 2. ! 0: pull Cholesky + ! 1: single-pass push Cholesky + ! 2: multi-pass push Cholesky + +! Density handling and factorization type parameters + +MAXDENSE = 0.20 ! maximal dense columns rate +DENSGAP = 3.50 ! density gap parameter +DENSLEN = 10. ! Value for the possibily dense columns +SUPDENS = 350. ! 'Super' dense column length +SETLAM = 0. ! 0 : density gap heuristic, ( AAT ) + ! -1 : use LAM, ( AAT ) + +! Pivot and factorization parameters + +TPIV1 = 1.0D-03 ! First threshold pivot tolerance +TPIV2 = 1.0D-08 ! Second threshold pivot tolerance +TABS = 1.0D-12 ! Abs. pivot tolerance for the first factorization +TRABS = 1.0D-15 ! Abs. pivot tolerance during the algorithm +LAM = 1.0D-05 ! Minimum value of lambda +TFIND = 25. ! Pivot search loop count +ORDERING = 0. ! 0 : Selected automatically + ! 1 : Minimum degree + ! 2 : Minimum local fill-in + ! 3 : Nested dissection (METIS at the moment) + ! -1 : No ordering + +! Stopping criterion parameters + +TOPT1 = 1.0D-08 ! Relative duality gap tolerance +TOPT2 = 1.0D-25 ! Average complementary gap tolerance +TFEAS1 = 1.0D-07 ! Relative primal feasibility tolerance +TFEAS2 = 1.0D-07 ! Relative dual feasibility tolerance +INFTOL = 1.0D+04 ! Infeasibility check tolerance +TSDIR = 1.0D-16 ! Search direction maximum norm tolerance +MAXITER = 99. ! Iteration limit + +! Numerical tolerances + +TPLUS = 1.0D-10 ! Relative addition tolerance +TZER = 1.0D-35 ! Relative zero tolerance + +! Iterative refinement tolerances + +TRESX = 1.0D-10 ! Acceptable residual in the primal space +TRESY = 1.0D-10 ! Acceptable residual in the dual space +MAXREF = 5. ! Maximal number of refinement +REFMET = 3. ! Refinement method: 1: Iterative improvement + ! 2: Preconditioned QMR + ! 4: Preconditioned CGM + +! Scaling parameters ! Scaling methods: + ! 0. : No scaling + ! 1. : Simple scaling to rowmax=1, colmax=1 + ! 2. : Geometric mean scaling + simple scaling + ! 3. : Curtis-Reid's algorithm + simple scaling + ! 4. : Geometric mean scaling only + ! 5. : Curtis-Reid's algorithm only +OBJNORM = 1.0D+02 ! Scaling the objective to this max. norm +RHSNORM = 0.0D+00 ! Scaling the rhs to this max. norm +SIGNORE = 1.0D-12 ! Ignore values during scaling under this parameter + ! BEFORE AGGREGATOR +SPASSES1 = 5. ! Maximum number of passes ( <128 ) +SMETHOD1 = 2. ! Scaling method + ! AFTER AGGREGATOR +SPASSES2 = 0. ! Maximum number of passes ( <128 ) +SMETHOD2 = 0. ! Scaling method + +! Complementary gap ballancing + +MINSTEP = 5.0d-01 ! Minimum stepsize +COMPLMIN = 1.0d-03 ! Min/Max complementarity component threshold + +! Predictor-corrector and barrier parameters + +STOPCOR = 1.00D-03 ! Correction stop parameter +BARSET = 2.00D-01 ! Barrier set-up limit +BARGROW = 1.00D+03 ! Barrier grow controll +BARMIN = 1.00D-15 ! Minimum barrier threshold +MINCORR = -1. ! Number of the minimum corrections +MAXCORR = 1. ! Number of the maximum corrections +INIBARR = 0.0d+0 ! Use initial barrier parameter + +! Centrality corrections parameters + +TARGET = 9.00D-02 ! Trial steplength improvement +TSMALL = 2.00D-01 ! Small complementarity bound +TLARGE = 2.00D+01 ! Large compelmentarity bound +CENTER = 5.00D+00 ! Centrality force +CORSTOP = 1.01D+00 ! Correction stop parameter +MINCCORR = 0. ! Number of the minimum corrections +MAXCCORR = 9. ! Number of the maximum corrections + +! Steplenth parameters + +PRDARE = 0.999D+00 ! Maximal primal steplength +DUDARE = 0.999D+00 ! Maximal dual steplength + +! Variable fixing tolerances + +TFIXVAR = 1.0D-20 ! Variable reset parameter +TFIXSLACK = 1.0D-20 ! Slack reset parameter +DSLACKLIM = 1.0D-20 ! Dual slack variable limit + +! Starting point paramerers + +PRMIN = 150.00 ! Minimum initial variable value +UPMAX = 50000.00 ! Maximum initial variable value +DUMIN = 150.00 ! Minimum initial slack value +LSQWEIGHT = 10.00 ! LSQ weight over quadratic objective +SMETHOD = 2. ! Starting method (1. or 2.) +SAFEMET = -3. ! Safe method (1. 2. or 3.) + +! +! Presolv parameters +! + +PRESOLV = 2047. ! 1 : singleton row check (895) + ! 2 : singleton column check + ! 4 : min/max row value check + ! 8 : cheap dual tests + ! 16 : dual check + ! 32 : primal bound check and relaxation + ! 64 : search identical variables + ! 128 : setting vars free in row doubletons + ! 256 : elimination of free variables + ! 512 : linear dependency check + ! 1024 : sparser + ! 2048 : Restore original bounds after elimination + ! 4096 : extended dual tests + +BNDLOOP = 5. ! Maximum loops in bound check +DULOOP = 10. ! Maximum loops in dual check +PRIMALBND = 1000. ! Maximal upper bound allowed during bndcheck +DUALBND = 10000. ! Maximal upper bound allowed during ducheck +PRESFEAS = 1.0d-8 ! Feasibility tolerance during presolve +PIVRTOL = 1.0D-2 ! Relative pivot tolerance in aggragation +PIVATOL = 1.0D-4 ! Absolute pivot tolerance in aggregation +PIVXTOL = 1.0D-8 ! Absolute pivot tolerance in linear dep. check +FILLTOL = 4. ! Fill-in controll during aggregation + +! +! Regularization parameters +! + +SOFTREG = 1.0D-12 ! Soft regularization parameter +HARDREG = 1.0D-16 ! Hard regularization parameter +SCFREE = 1.0D-06 ! Regularization for free variables +REGULARIZE= 27. ! Regularization method + ! 1. Pre-regularize free variables + ! 2. Pre-regularize equalities, AS + ! 4. Pre-regularize equalities, NE + ! 8. Post-regularize free variables + ! 16. Post-regularize equalities, AS + ! 32. Post-regularize equalities, NE +END_OPT diff --git a/moveit_planners/trajopt/trajopt_sco/src/bpmpd_caller.cpp b/moveit_planners/trajopt/trajopt_sco/src/bpmpd_caller.cpp new file mode 100644 index 0000000000..235f359b53 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/bpmpd_caller.cpp @@ -0,0 +1,63 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include + +extern "C" { +extern void bpmpd(int*, int*, int*, int*, int*, int*, int*, double*, int*, int*, double*, double*, double*, double*, + double*, double*, double*, int*, double*, int*, double*, int*); +} + +int main(int /*argc*/, char** /*argv*/) +{ + std::string working_dir = BPMPD_WORKING_DIR; + int err = chdir(working_dir.c_str()); + if (err != 0) + { + std::cerr << "error going to BPMPD working dir\n"; + std::cerr << strerror(err) << std::endl; + abort(); + } + // int counter=0; + while (true) + { + bpmpd_io::bpmpd_input bi; + bpmpd_io::ser(STDIN_FILENO, bi, bpmpd_io::DESER); + + int memsiz = 0; + double BIG = 1e30; + bpmpd_io::bpmpd_output bo; + bo.primal.resize(static_cast(bi.m + bi.n)); + bo.dual.resize(static_cast(bi.m + bi.n)); + bo.status.resize(static_cast(bi.m + bi.n)); + +#define DBG(expr) // cerr << #expr << ": " << CSTR(expr) << std::endl + DBG(bi.m); + DBG(bi.n); + DBG(bi.nz); + DBG(bi.qn); + DBG(bi.qnz); + DBG(bi.acolcnt); + DBG(bi.acolidx); + DBG(bi.acolnzs); + DBG(bi.qcolcnt); + DBG(bi.qcolidx); + DBG(bi.qcolnzs); + DBG(bi.rhs); + DBG(bi.obj); + DBG(bi.lbound); + DBG(bi.ubound); + + bpmpd(&bi.m, &bi.n, &bi.nz, &bi.qn, &bi.qnz, bi.acolcnt.data(), bi.acolidx.data(), bi.acolnzs.data(), + bi.qcolcnt.data(), bi.qcolidx.data(), bi.qcolnzs.data(), bi.rhs.data(), bi.obj.data(), bi.lbound.data(), + bi.ubound.data(), bo.primal.data(), bo.dual.data(), bo.status.data(), &BIG, &bo.code, &bo.opt, &memsiz); + + bpmpd_io::ser(STDOUT_FILENO, bo, bpmpd_io::SER); + } +} diff --git a/moveit_planners/trajopt/trajopt_sco/src/bpmpd_interface.cpp b/moveit_planners/trajopt/trajopt_sco/src/bpmpd_interface.cpp new file mode 100644 index 0000000000..b17bb6ff2f --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/bpmpd_interface.cpp @@ -0,0 +1,517 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +/** +This is a readme file for the linux DLL version of BPMPD version 2.21 +The DLL solves linear and convex quadratic problems. + + +The objective function is assumed to be in the form: + + 1 + min c'x + - x'Qx + 2 + + +The interface (Fortran-like) for BPMPD: + + subroutine bpmpd(m,n,nz,qn,qnz, + acolcnt,acolidx,acolnzs, + qcolcnt,qcolidx,qcolnzs, + rhs,obj,lbound,ubound,primal,dual,status, + big,code,opt,memsiz) + +integer*4 m,n,nz,qn,qnz,acolcnt(n),acolidx(nz),qcolcnt(n),qcolidx(qnz), + status(n+m),code, +real*8 acolnzs(nz),qcolnzs(qnz),rhs(m),obj(n),lbound(n+m),ubound(n+m), + primal(n+m),dual(n+m),big,opt + + +or C-like: + +void bpmpd(int *m, int *n, int *nz, int *qn, int *qnz, int *acolcnt, + int *acolidx, double *acolnzs, int *qcolcnt, int *qcolidx, + double *qcolnzs, double *rhs, double *obj, double *lbound, + double *ubound, double *primal, double *dual, int *status, + double *big, int *code, double *opt, int *memsiz) + +where + + memsiz : Number of bytes to be allocated by bpmpd. If memsiz <= 0 + bpmpd determines the allocatable memory itself. As output + memsiz contains the memory in bytes which was used during + the process. + + m : number of rows in the constraint matrix + n : number of columns in the constraint matrix + nz : number of nonzeros in the constraint matrix + qn : number of quadratic variables + qnz : number of nonzeros in the lower triangular of Q (with diagonals) + + acolcnt : number of nonzeros in each column of A + acolidx : A matrix index values + acolnzs : A matrix nonzero values + It is supposed that the columns are placed continuousely from + the first positions of the parallel arrays acolidx-acolnzs. + + qcolcnt : number of nonzeros in each column of the lower triangular part of Q + qcolidx : Q matrix index values + qcolnzs : Q matrix nonzero values + It is supposed that the the lower triangular part (with diagonal) + of Q is provided and its columns are placed continuousely from + the first positions of the parallel arrays qcolidx-qcolnzs. + + rhs : right hand side + obj : objective function + lbound : 1 .. n lower bounds of the variables + n+1 .. n+m lower bounds on rows (slacks) + ubound : upper bounds (like lbound) + + primal : optimal primal values + dual : optimal dual values + status : basic/nonbasic status estimation of variables (1/0) + + big : Represents infinity (recommended big = 1.0d+30 ) + code : return code + if code < 0 --> not enought memory + if code = 1 --> solver stopped at feasible point (suboptimal +solution) + if code = 2 --> optimal solution found + if code = 3 --> problem dual infeasible + if code = 4 --> problem primal infeasible + + opt : final primal objective value + +The DLL will read the parameter file called bpmpd.par (if presented) and +write a log file called bpmpd.log. + + + As example the LP problem : + + min x1 + x1*x1 + 2*x1*x2 + 2*x2*x2 + x4*x4 + + 1x1 + 2x2 + 0x3 -4x4 >= 0 + 3x1 + 0x2 - 2x3 -1x4 <= 100 + 30 >= 1x1 + 3x2 + 3x3 -2x4 >= 10 + + x1,x2,x3 >=0 + x1 <= 20 + + Then the input parameters: + + m=3, n=4, nz=10, qn=3, qnz=4 + + ubound : (20, big, big, big, big, 0, 20) + lbound : ( 0, 0, 0, -big, 0, -big, 0) + rhs : ( 0, 100, 10) + obj : ( 1, 0, 0, 0) + +acolcnt : ( 3, 2, 2, 3) +acolidx : ( 1, 2, 3, 1, 3, 2, 3, 1, 2, 3) +acolnzs : ( 1, 3, 1, 2, 3, -2, 3, -4,-1,-2) + +qcolcnt : ( 2, 1, 0, 1) +qcolidx : ( 1,2, 2, 4) +qcolnzs : ( 2,2, 4, 2) + + 1 1 1 0 2 2 0 +(Note : 2*x1*x2 = x1*x2+x2*x1 thus - * Q = 1 2 0 and Q = 2 4 0 ) + 2 0 0 1 0 0 2 + +NOTE: +The subroutine solves minimization problems, therefore you +have to change the sign of the objective, if you have a maximization +problem. + +NOTE: +The DLL routine will create a log file called "bpmpd.log" at every call. +The old bpmpd.log file will be overwritten. + +NOTE: +A sample driver solving the above problem is included as well as the +logfile it generates. + **/ + +// extern "C" { +// extern void bpmpd(int *, int *, int *, int *, int *, int *, int *, +// double *, int *, int *, double *, double *, double *, double *, +// double *, double *, double *, int *, double *, int *, double *, int +// *); +// } + +namespace sco +{ +static double BPMPD_BIG = 1e+30; + +ModelPtr createBPMPDModel() +{ + ModelPtr out(new BPMPDModel()); + return out; +} + +#define READ 0 +#define WRITE 1 + +pid_t popen2(const char* command, int* infp, int* outfp) +{ + int p_stdin[2], p_stdout[2]; + pid_t pid; + + if (pipe(p_stdin) != 0 || pipe(p_stdout) != 0) + return -1; + + pid = fork(); + + if (pid < 0) + { + assert(0); + return pid; + } + else if (pid == 0) + { + close(p_stdin[WRITE]); + dup2(p_stdin[READ], READ); + close(p_stdout[READ]); + dup2(p_stdout[WRITE], WRITE); + + execl("/bin/sh", "sh", "-c", command, nullptr); + perror("execl"); + exit(1); + } + + if (infp == nullptr) + close(p_stdin[WRITE]); + else + *infp = p_stdin[WRITE]; + + if (outfp == nullptr) + close(p_stdout[READ]); + else + *outfp = p_stdout[READ]; + + return pid; +} + +static pid_t gPID = 0; +static int gPipeIn = 0; +static int gPipeOut = 0; + +void fexit() +{ + char text[1] = { bpmpd_io::EXIT_CHAR }; + long n = write(gPipeIn, text, 1); + ALWAYS_ASSERT(n == 1); +} + +BPMPDModel::BPMPDModel() : m_pipeIn(0), m_pipeOut(0) +{ + if (gPID == 0) + { + atexit(fexit); + gPID = popen2(BPMPD_CALLER, &gPipeIn, &gPipeOut); + } +} + +BPMPDModel::~BPMPDModel() +{ + // char text[1] = {123}; + // write(gPipeIn, text, 1); + // // kill(m_pid, SIGKILL); // TODO: WHY DOES THIS KILL THE PARENT PROCESS?! + // close(m_pipeIn); + // close(m_pipeOut); + // m_pipeIn=0; + // m_pipeOut=0; +} + +Var BPMPDModel::addVar(const std::string& name) +{ + m_vars.push_back(new VarRep(static_cast(m_vars.size()), name, this)); + m_lbs.push_back(-BPMPD_BIG); + m_ubs.push_back(BPMPD_BIG); + return m_vars.back(); +} +Cnt BPMPDModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) +{ + m_cnts.push_back(new CntRep(static_cast(m_cnts.size()), this)); + m_cntExprs.push_back(expr); + m_cntTypes.push_back(EQ); + return m_cnts.back(); +} +Cnt BPMPDModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/) +{ + m_cnts.push_back(new CntRep(static_cast(m_cnts.size()), this)); + m_cntExprs.push_back(expr); + m_cntTypes.push_back(INEQ); + return m_cnts.back(); +} +Cnt BPMPDModel::addIneqCnt(const QuadExpr&, const std::string& /*name*/) +{ + assert(0 && "NOT IMPLEMENTED"); + return nullptr; +} +void BPMPDModel::removeVars(const VarVector& vars) +{ + IntVec inds = vars2inds(vars); + for (unsigned i = 0; i < vars.size(); ++i) + vars[i].var_rep->removed = true; +} + +void BPMPDModel::removeCnts(const CntVector& cnts) +{ + IntVec inds = cnts2inds(cnts); + for (unsigned i = 0; i < cnts.size(); ++i) + cnts[i].cnt_rep->removed = true; +} + +void BPMPDModel::update() +{ + { + size_t inew = 0; + for (unsigned iold = 0; iold < m_vars.size(); ++iold) + { + const Var& var = m_vars[iold]; + if (!var.var_rep->removed) + { + m_vars[inew] = var; + m_lbs[inew] = m_lbs[iold]; + m_ubs[inew] = m_ubs[iold]; + var.var_rep->index = static_cast(inew); + ++inew; + } + else + delete var.var_rep; + } + m_vars.resize(inew); + m_lbs.resize(inew); + m_ubs.resize(inew); + } + { + size_t inew = 0; + for (unsigned iold = 0; iold < m_cnts.size(); ++iold) + { + const Cnt& cnt = m_cnts[iold]; + if (!cnt.cnt_rep->removed) + { + m_cnts[inew] = cnt; + m_cntExprs[inew] = m_cntExprs[iold]; + m_cntTypes[inew] = m_cntTypes[iold]; + cnt.cnt_rep->index = static_cast(inew); + ++inew; + } + else + delete cnt.cnt_rep; + } + m_cnts.resize(inew); + m_cntExprs.resize(inew); + m_cntTypes.resize(inew); + } +} + +void BPMPDModel::setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) +{ + for (unsigned i = 0; i < vars.size(); ++i) + { + size_t varind = static_cast(vars[i].var_rep->index); + m_lbs[varind] = lower[i]; + m_ubs[varind] = upper[i]; + } +} +DblVec BPMPDModel::getVarValues(const VarVector& vars) const +{ + DblVec out(vars.size()); + for (unsigned i = 0; i < vars.size(); ++i) + { + size_t varind = static_cast(vars[i].var_rep->index); + out[i] = m_soln[varind]; + } + return out; +} + +#define DBG(expr) // cout << #expr << ": " << CSTR(expr) << std::endl + +CvxOptStatus BPMPDModel::optimize() +{ + update(); + // + // + // int m, n, nz, qn, qnz, acolcnt[maxn+1], acolidx[maxnz], qcolcnt[maxn+1], + // qcolidx[maxqnz], + // status[maxn+maxm], code, memsiz; + // double acolnzs[maxnz], qcolnzs[maxqnz], rhs[maxm], obj[maxn], + // lbound[maxn+maxm], + // ubound[maxn+maxm], primal[maxn+maxm], dual[maxn+maxm], big, opt; + + size_t n = m_vars.size(); + size_t m = m_cnts.size(); + + IntVec acolcnt(n), acolidx, qcolcnt(n), qcolidx, status(m + n); + DblVec acolnzs, qcolnzs, rhs(m), obj(n, 0), lbound(m + n), ubound(m + n), primal(m + n), dual(m + n); + + DBG(m_lbs); + DBG(m_ubs); + for (size_t iVar = 0; iVar < n; ++iVar) + { + lbound[iVar] = fmax(m_lbs[iVar], -BPMPD_BIG); + ubound[iVar] = fmin(m_ubs[iVar], BPMPD_BIG); + } + + std::vector var2cntinds(n); + std::vector var2cntvals(n); + for (size_t iCnt = 0; iCnt < m; ++iCnt) + { + const AffExpr& aff = m_cntExprs[iCnt]; + // cout << "adding constraint " << aff << endl; + IntVec inds = vars2inds(aff.vars); + + for (size_t i = 0; i < aff.vars.size(); ++i) + { + var2cntinds[static_cast(inds[i])].push_back(static_cast(iCnt)); + var2cntvals[static_cast(inds[i])].push_back(aff.coeffs[i]); // xxx maybe repeated/ + } + + lbound[n + iCnt] = (m_cntTypes[iCnt] == INEQ) ? -BPMPD_BIG : 0; + ubound[n + iCnt] = 0; + rhs[iCnt] = -aff.constant; + } + + for (size_t iVar = 0; iVar < n; ++iVar) + { + simplify2(var2cntinds[iVar], var2cntvals[iVar]); + acolcnt[iVar] = static_cast(var2cntinds[iVar].size()); + acolidx.insert(acolidx.end(), var2cntinds[iVar].begin(), var2cntinds[iVar].end()); + acolnzs.insert(acolnzs.end(), var2cntvals[iVar].begin(), var2cntvals[iVar].end()); + } + // cout << CSTR(acolidx) << endl; + // cout << CSTR(acolnzs) << endl; + + std::vector var2qcoeffs(n); + std::vector var2qinds(n); + for (size_t i = 0; i < m_objective.size(); ++i) + { + size_t idx1 = static_cast(m_objective.vars1[i].var_rep->index); + size_t idx2 = static_cast(m_objective.vars2[i].var_rep->index); + if (idx1 < idx2) + { + var2qinds[idx1].push_back(static_cast(idx2)); + var2qcoeffs[idx1].push_back(m_objective.coeffs[i]); + } + else if (idx1 == idx2) + { + var2qinds[idx1].push_back(static_cast(idx2)); + var2qcoeffs[idx1].push_back(m_objective.coeffs[i] * 2); + } + else + { + var2qinds[idx2].push_back(static_cast(idx1)); + var2qcoeffs[idx2].push_back(m_objective.coeffs[i]); + } + } + + for (size_t iVar = 0; iVar < n; ++iVar) + { + simplify2(var2qinds[iVar], var2qcoeffs[iVar]); + qcolidx.insert(qcolidx.end(), var2qinds[iVar].begin(), var2qinds[iVar].end()); + qcolnzs.insert(qcolnzs.end(), var2qcoeffs[iVar].begin(), var2qcoeffs[iVar].end()); + qcolcnt[iVar] = static_cast(var2qinds[iVar].size()); + } + + for (size_t i = 0; i < m_objective.affexpr.size(); ++i) + { + obj[static_cast(m_objective.affexpr.vars[i].var_rep->index)] += m_objective.affexpr.coeffs[i]; + } + +#define VECINC(vec) \ + for (unsigned i = 0; i < vec.size(); ++i) \ + ++vec[i]; + VECINC(acolidx); + VECINC(qcolidx); +#undef VECINC + + // cout << "objective: " << m_objective << endl; + + int nz = static_cast(acolnzs.size()), qn = static_cast(n), qnz = static_cast(qcolnzs.size()); + + DBG(m); + DBG(n); + DBG(nz); + DBG(qn); + DBG(qnz); + DBG(acolcnt); + DBG(acolidx); + DBG(acolnzs); + DBG(qcolcnt); + DBG(qcolidx); + DBG(qcolnzs); + DBG(rhs); + DBG(obj); + DBG(lbound); + DBG(ubound); + +#if 0 + bpmpd(&m, &n, &nz, &qn, &qnz, acolcnt.data(), acolidx.data(), acolnzs.data(), qcolcnt.data(), qcolidx.data(), qcolnzs.data(), + rhs.data(), obj.data(), lbound.data(), ubound.data(), + primal.data(), dual.data(), status.data(), &BIG, &code, &opt, &memsiz); + + // opt += m_objective.affexpr.constant; + m_soln = DblVec(primal.begin(), primal.begin()+n); + + + if (1) { + DBG(primal); + DBG(dual); + DBG(status); + DBG(code); + DBG(opt); +#undef DBG + +#else + + bpmpd_io::bpmpd_input bi(static_cast(m), static_cast(n), nz, qn, qnz, acolcnt, acolidx, acolnzs, qcolcnt, + qcolidx, qcolnzs, rhs, obj, lbound, ubound); + bpmpd_io::ser(gPipeIn, bi, bpmpd_io::SER); + + // std::cout << "serialization time:" << end-start << std::endl; + + bpmpd_io::bpmpd_output bo; + bpmpd_io::ser(gPipeOut, bo, bpmpd_io::DESER); + + m_soln = DblVec(bo.primal.begin(), bo.primal.begin() + static_cast(n)); + int retcode = bo.code; + + if (retcode == 2) + return CVX_SOLVED; + else if (retcode == 3 || retcode == 4) + return CVX_INFEASIBLE; + else + return CVX_FAILED; + +#endif + + // exit(0); +} +void BPMPDModel::setObjective(const AffExpr& expr) +{ + m_objective.affexpr = expr; +} +void BPMPDModel::setObjective(const QuadExpr& expr) +{ + m_objective = expr; +} +void BPMPDModel::writeToFile(const std::string& /*fname*/) +{ + // assert(0 && "NOT IMPLEMENTED"); +} +VarVector BPMPDModel::getVars() const +{ + return m_vars; +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/src/expr_ops.cpp b/moveit_planners/trajopt/trajopt_sco/src/expr_ops.cpp new file mode 100644 index 0000000000..eeab09ba62 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/expr_ops.cpp @@ -0,0 +1,106 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include + +static inline double sq(double x) +{ + return x * x; +} +namespace sco +{ +QuadExpr exprMult(const AffExpr& affexpr1, const AffExpr& affexpr2) +{ + QuadExpr out; + size_t naff1 = affexpr1.coeffs.size(); + size_t naff2 = affexpr2.coeffs.size(); + size_t nquad = naff1 * naff2; + + // Multiply the constants of the two expr + out.affexpr.constant = affexpr1.constant * affexpr2.constant; + + // Account for vars in each expr multiplied by the constant in the other expr + out.affexpr.vars.reserve(naff1 + naff2); + out.affexpr.vars.insert(out.affexpr.vars.end(), affexpr1.vars.begin(), affexpr1.vars.end()); + out.affexpr.vars.insert(out.affexpr.vars.end(), affexpr2.vars.begin(), affexpr2.vars.end()); + out.affexpr.coeffs.resize(naff1 + naff2); + for (size_t i = 0; i < naff1; ++i) + out.affexpr.coeffs[i] = affexpr2.constant * affexpr1.coeffs[i]; + for (size_t i = 0; i < naff2; ++i) + out.affexpr.coeffs[i + naff1] = affexpr1.constant * affexpr2.coeffs[i]; + + // Account for the vars in each expr that are multipled by another var in the other expr + out.coeffs.reserve(nquad); + out.vars1.reserve(nquad); + out.vars2.reserve(nquad); + for (size_t i = 0; i < naff1; ++i) + { + for (size_t j = 0; j < naff2; ++j) + { + out.vars1.push_back(affexpr1.vars[i]); + out.vars2.push_back(affexpr2.vars[j]); + out.coeffs.push_back(affexpr1.coeffs[i] * affexpr2.coeffs[j]); + } + } + return out; +} + +QuadExpr exprSquare(const Var& a) +{ + QuadExpr out; + out.coeffs.push_back(1); + out.vars1.push_back(a); + out.vars2.push_back(a); + return out; +} + +QuadExpr exprSquare(const AffExpr& affexpr) +{ + QuadExpr out; + size_t naff = affexpr.coeffs.size(); + size_t nquad = (naff * (naff + 1)) / 2; + + out.affexpr.constant = sq(affexpr.constant); + + out.affexpr.vars = affexpr.vars; + out.affexpr.coeffs.resize(naff); + for (size_t i = 0; i < naff; ++i) + out.affexpr.coeffs[i] = 2 * affexpr.constant * affexpr.coeffs[i]; + + out.coeffs.reserve(nquad); + out.vars1.reserve(nquad); + out.vars2.reserve(nquad); + for (size_t i = 0; i < naff; ++i) + { + out.vars1.push_back(affexpr.vars[i]); + out.vars2.push_back(affexpr.vars[i]); + out.coeffs.push_back(sq(affexpr.coeffs[i])); + for (size_t j = i + 1; j < naff; ++j) + { + out.vars1.push_back(affexpr.vars[i]); + out.vars2.push_back(affexpr.vars[j]); + out.coeffs.push_back(2 * affexpr.coeffs[i] * affexpr.coeffs[j]); + } + } + return out; +} + +AffExpr cleanupAff(const AffExpr& a) +{ + AffExpr out; + for (size_t i = 0; i < a.size(); ++i) + { + if (fabs(a.coeffs[i]) > 1e-7) + { + out.coeffs.push_back(a.coeffs[i]); + out.vars.push_back(a.vars[i]); + } + } + out.constant = a.constant; + return out; +} + +/////////////////////////////////////////////////////////////// +} // namespace sco diff --git a/moveit_planners/trajopt/trajopt_sco/src/expr_vec_ops.cpp b/moveit_planners/trajopt/trajopt_sco/src/expr_vec_ops.cpp new file mode 100644 index 0000000000..a3ce509fc0 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/expr_vec_ops.cpp @@ -0,0 +1,12 @@ +#include +namespace sco +{ +AffExpr varDot(const Eigen::VectorXd& x, const VarVector& v) +{ + AffExpr out; + out.constant = 0; + out.vars = v; + out.coeffs = DblVec(x.data(), x.data() + x.size()); // this creates a vector + return out; +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/src/gurobi_interface.cpp b/moveit_planners/trajopt/trajopt_sco/src/gurobi_interface.cpp new file mode 100644 index 0000000000..c86dd714d0 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/gurobi_interface.cpp @@ -0,0 +1,299 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +extern "C" { +#include "gurobi_c.h" +} +#include +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include + +namespace sco +{ +GRBenv* gEnv; + +#if 0 +void simplify(IntVec& inds, DblVec& vals) { + // first find the largest element of inds + // make an array of that size + // + + assert(inds.size() == vals.size()); + int min_ind = *std::min_element(inds.begin(), inds.end()); + int max_ind = *std::max_element(inds.begin(), inds.end()); + int orig_size = inds.size(); + int work_size = max_ind - min_ind + 1; + DblVec work_vals(work_size, 0); + + for (int i=0; i < orig_size; ++i) { + work_vals[inds[i] - min_ind] += vals[i]; + } + + + int i_new = 0; + for (int i=0; i < work_size; ++i) { + if (work_vals[i] != 0) { + vals[i_new] = work_vals[i]; + inds[i_new] = i + min_ind; + ++i_new; + } + } + + inds.resize(i_new); + vals.resize(i_new); + +} +#endif + +#define ENSURE_SUCCESS(expr) \ + do \ + { \ + bool error = expr; \ + if (error) \ + { \ + printf("GRB error: %s while evaluating %s at %s:%i\n", GRBgeterrormsg(gEnv), #expr, __FILE__, __LINE__); \ + abort(); \ + } \ + } while (0) + +ModelPtr createGurobiModel() +{ + ModelPtr out(new GurobiModel()); + return out; +} + +GurobiModel::GurobiModel() +{ + if (!gEnv) + { + GRBloadenv(&gEnv, nullptr); + if (util::GetLogLevel() < util::LevelDebug) + { + ENSURE_SUCCESS(GRBsetintparam(gEnv, "OutputFlag", 0)); + } + } + GRBnewmodel(gEnv, &m_model, "problem", 0, nullptr, nullptr, nullptr, nullptr, nullptr); +} + +Var GurobiModel::addVar(const std::string& name) +{ + ENSURE_SUCCESS(GRBaddvar(m_model, 0, nullptr, nullptr, 0, -GRB_INFINITY, GRB_INFINITY, GRB_CONTINUOUS, + const_cast(name.c_str()))); + m_vars.push_back(new VarRep(static_cast(m_vars.size()), name, this)); + return m_vars.back(); +} + +Var GurobiModel::addVar(const std::string& name, double lb, double ub) +{ + ENSURE_SUCCESS(GRBaddvar(m_model, 0, nullptr, nullptr, 0, lb, ub, GRB_CONTINUOUS, const_cast(name.c_str()))); + m_vars.push_back(new VarRep(static_cast(m_vars.size()), name, this)); + return m_vars.back(); +} + +Cnt GurobiModel::addEqCnt(const AffExpr& expr, const std::string& name) +{ + LOG_TRACE("adding eq constraint: %s = 0", CSTR(expr)); + IntVec inds = vars2inds(expr.vars); + DblVec vals = expr.coeffs; + simplify2(inds, vals); + ENSURE_SUCCESS(GRBaddconstr(m_model, inds.size(), const_cast(inds.data()), const_cast(vals.data()), + GRB_EQUAL, -expr.constant, const_cast(name.c_str()))); + m_cnts.push_back(new CntRep(static_cast(m_cnts.size()), this)); + return m_cnts.back(); +} +Cnt GurobiModel::addIneqCnt(const AffExpr& expr, const std::string& name) +{ + LOG_TRACE("adding ineq: %s <= 0", CSTR(expr)); + IntVec inds = vars2inds(expr.vars); + DblVec vals = expr.coeffs; + simplify2(inds, vals); + ENSURE_SUCCESS(GRBaddconstr(m_model, inds.size(), inds.data(), vals.data(), GRB_LESS_EQUAL, -expr.constant, + const_cast(name.c_str()))); + m_cnts.push_back(new CntRep(static_cast(m_cnts.size()), this)); + return m_cnts.back(); +} +Cnt GurobiModel::addIneqCnt(const QuadExpr& qexpr, const std::string& name) +{ + int numlnz = qexpr.affexpr.size(); + IntVec linds = vars2inds(qexpr.affexpr.vars); + DblVec lvals = qexpr.affexpr.coeffs; + IntVec inds1 = vars2inds(qexpr.vars1); + IntVec inds2 = vars2inds(qexpr.vars2); + ENSURE_SUCCESS(GRBaddqconstr(m_model, numlnz, linds.data(), lvals.data(), qexpr.size(), inds1.data(), inds2.data(), + const_cast(qexpr.coeffs.data()), GRB_LESS_EQUAL, -qexpr.affexpr.constant, + const_cast(name.c_str()))); + return Cnt(); +} + +void resetIndices(VarVector& vars) +{ + for (size_t i = 0; i < vars.size(); ++i) + vars[i].var_rep[i].index = i; +} +void resetIndices(CntVector& cnts) +{ + for (size_t i = 0; i < cnts.size(); ++i) + cnts[i].cnt_rep[i].index = i; +} + +void GurobiModel::removeVars(const VarVector& vars) +{ + IntVec inds = vars2inds(vars); + ENSURE_SUCCESS(GRBdelvars(m_model, inds.size(), inds.data())); + for (int i = 0; i < vars.size(); ++i) + vars[i].var_rep->removed = true; +} + +void GurobiModel::removeCnts(const CntVector& cnts) +{ + IntVec inds = cnts2inds(cnts); + ENSURE_SUCCESS(GRBdelconstrs(m_model, inds.size(), inds.data())); + for (int i = 0; i < cnts.size(); ++i) + cnts[i].cnt_rep->removed = true; +} + +#if 0 +void GurobiModel::setVarBounds(const Var& var, double lower, double upper) { + assert(var.var_rep->creator == this); + ENSURE_SUCCESS(GRBsetdblattrelement(m_model, GRB_DBL_ATTR_LB, var.var_rep->index, lower)); + ENSURE_SUCCESS(GRBsetdblattrelement(m_model, GRB_DBL_ATTR_UB, var.var_rep->index, upper)); +} +#endif + +void GurobiModel::setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) +{ + assert(vars.size() == lower.size() && vars.size() == upper.size()); + IntVec inds = vars2inds(vars); + ENSURE_SUCCESS( + GRBsetdblattrlist(m_model, GRB_DBL_ATTR_LB, inds.size(), inds.data(), const_cast(lower.data()))); + ENSURE_SUCCESS( + GRBsetdblattrlist(m_model, GRB_DBL_ATTR_UB, inds.size(), inds.data(), const_cast(upper.data()))); +} + +#if 0 +double GurobiModel::getVarValue(const Var& var) const { + assert(var.var_rep->creator == this); + double out; + ENSURE_SUCCESS(GRBgetdblattrelement(m_model, GRB_DBL_ATTR_X, var.var_rep->index, &out)); + return out; +} +#endif + +DblVec GurobiModel::getVarValues(const VarVector& vars) const +{ + assert((vars.size() == 0) || (vars[0].var_rep->creator == this)); + IntVec inds = vars2inds(vars); + DblVec out(inds.size()); + ENSURE_SUCCESS(GRBgetdblattrlist(m_model, GRB_DBL_ATTR_X, inds.size(), inds.data(), out.data())); + return out; +} + +CvxOptStatus GurobiModel::optimize() +{ + ENSURE_SUCCESS(GRBoptimize(m_model)); + int status; + GRBgetintattr(m_model, GRB_INT_ATTR_STATUS, &status); + if (status == GRB_OPTIMAL) + { + double objval; + GRBgetdblattr(m_model, GRB_DBL_ATTR_OBJVAL, &objval); + LOG_DEBUG("solver objective value: %.3e", objval); + return CVX_SOLVED; + } + else if (status == GRB_INFEASIBLE) + { + GRBcomputeIIS(m_model); + return CVX_INFEASIBLE; + } + else + return CVX_FAILED; +} +CvxOptStatus GurobiModel::optimizeFeasRelax() +{ + double lbpen = GRB_INFINITY, ubpen = GRB_INFINITY, rhspen = 1; + ENSURE_SUCCESS( + GRBfeasrelax(m_model, 0 /*sum of viol*/, 0 /*just minimize cost of viol*/, &lbpen, &ubpen, &rhspen, nullptr)); + return optimize(); +} + +void GurobiModel::setObjective(const AffExpr& expr) +{ + GRBdelq(m_model); + + int nvars; + GRBgetintattr(m_model, GRB_INT_ATTR_NUMVARS, &nvars); + assert(nvars == m_vars.size()); + + DblVec obj(nvars, 0); + for (size_t i = 0; i < expr.size(); ++i) + { + obj[expr.vars[i].var_rep->index] += expr.coeffs[i]; + } + ENSURE_SUCCESS(GRBsetdblattrarray(m_model, "Obj", 0, nvars, obj.data())); + GRBsetdblattr(m_model, "ObjCon", expr.constant); +} + +void GurobiModel::setObjective(const QuadExpr& quad_expr) +{ + setObjective(quad_expr.affexpr); + IntVec inds1 = vars2inds(quad_expr.vars1); + IntVec inds2 = vars2inds(quad_expr.vars2); + GRBaddqpterms(m_model, quad_expr.coeffs.size(), const_cast(inds1.data()), const_cast(inds2.data()), + const_cast(quad_expr.coeffs.data())); +} + +void GurobiModel::writeToFile(const std::string& fname) +{ + ENSURE_SUCCESS(GRBwrite(m_model, fname.c_str())); +} +void GurobiModel::update() +{ + ENSURE_SUCCESS(GRBupdatemodel(m_model)); + + { + int inew = 0; + for (const Var& var : m_vars) + { + if (!var.var_rep->removed) + { + m_vars[inew] = var; + var.var_rep->index = inew; + ++inew; + } + else + delete var.var_rep; + } + m_vars.resize(inew); + } + { + int inew = 0; + for (const Cnt& cnt : m_cnts) + { + if (!cnt.cnt_rep->removed) + { + m_cnts[inew] = cnt; + cnt.cnt_rep->index = inew; + ++inew; + } + else + delete cnt.cnt_rep; + } + m_cnts.resize(inew); + } +} + +VarVector GurobiModel::getVars() const +{ + return m_vars; +} +GurobiModel::~GurobiModel() +{ + ENSURE_SUCCESS(GRBfreemodel(m_model)); +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/src/modeling.cpp b/moveit_planners/trajopt/trajopt_sco/src/modeling.cpp new file mode 100644 index 0000000000..81a2140b5e --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/modeling.cpp @@ -0,0 +1,298 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include +#include +#include + +namespace sco +{ +void ConvexObjective::addAffExpr(const AffExpr& affexpr) +{ + exprInc(quad_, affexpr); +} +void ConvexObjective::addQuadExpr(const QuadExpr& quadexpr) +{ + exprInc(quad_, quadexpr); +} +void ConvexObjective::addHinge(const AffExpr& affexpr, double coeff) +{ + Var hinge = model_->addVar("hinge", 0, INFINITY); + vars_.push_back(hinge); + ineqs_.push_back(affexpr); + exprDec(ineqs_.back(), hinge); + AffExpr hinge_cost = exprMult(AffExpr(hinge), coeff); + exprInc(quad_, hinge_cost); +} + +void ConvexObjective::addAbs(const AffExpr& affexpr, double coeff) +{ + Var neg = model_->addVar("neg", 0, INFINITY); + Var pos = model_->addVar("pos", 0, INFINITY); + vars_.push_back(neg); + vars_.push_back(pos); + AffExpr neg_plus_pos; + neg_plus_pos.coeffs = DblVec(2, coeff); + neg_plus_pos.vars.push_back(neg); + neg_plus_pos.vars.push_back(pos); + exprInc(quad_, neg_plus_pos); + AffExpr affeq = affexpr; + affeq.vars.push_back(neg); + affeq.vars.push_back(pos); + affeq.coeffs.push_back(1); + affeq.coeffs.push_back(-1); + eqs_.push_back(affeq); +} + +void ConvexObjective::addHinges(const AffExprVector& ev) +{ + for (size_t i = 0; i < ev.size(); ++i) + addHinge(ev[i], 1); +} + +void ConvexObjective::addL1Norm(const AffExprVector& ev) +{ + for (size_t i = 0; i < ev.size(); ++i) + addAbs(ev[i], 1); +} + +void ConvexObjective::addL2Norm(const AffExprVector& ev) +{ + for (size_t i = 0; i < ev.size(); ++i) + exprInc(quad_, exprSquare(ev[i])); +} + +void ConvexObjective::addMax(const AffExprVector& ev) +{ + Var m = model_->addVar("max", -INFINITY, INFINITY); + for (size_t i = 0; i < ev.size(); ++i) + { + ineqs_.push_back(ev[i]); + exprDec(ineqs_.back(), m); + } +} + +void ConvexObjective::addConstraintsToModel() +{ + cnts_.reserve(eqs_.size() + ineqs_.size()); + for (const AffExpr& aff : eqs_) + { + cnts_.push_back(model_->addEqCnt(aff, "")); + } + for (const AffExpr& aff : ineqs_) + { + cnts_.push_back(model_->addIneqCnt(aff, "")); + } +} + +void ConvexObjective::removeFromModel() +{ + model_->removeCnts(cnts_); + model_->removeVars(vars_); + model_ = nullptr; +} +ConvexObjective::~ConvexObjective() +{ + if (inModel()) + removeFromModel(); +} + +void ConvexConstraints::addEqCnt(const AffExpr& aff) +{ + eqs_.push_back(aff); +} +void ConvexConstraints::addIneqCnt(const AffExpr& aff) +{ + ineqs_.push_back(aff); +} +void ConvexConstraints::addConstraintsToModel() +{ + cnts_.reserve(eqs_.size() + ineqs_.size()); + for (const AffExpr& aff : eqs_) + { + cnts_.push_back(model_->addEqCnt(aff, "")); + } + for (const AffExpr& aff : ineqs_) + { + cnts_.push_back(model_->addIneqCnt(aff, "")); + } +} + +void ConvexConstraints::removeFromModel() +{ + model_->removeCnts(cnts_); + model_ = nullptr; +} + +DblVec ConvexConstraints::violations(const DblVec& x) +{ + DblVec out; + out.reserve(eqs_.size() + ineqs_.size()); + for (const AffExpr& aff : eqs_) + out.push_back(fabs(aff.value(x.data()))); + for (const AffExpr& aff : ineqs_) + out.push_back(pospart(aff.value(x.data()))); + return out; +} +double ConvexConstraints::violation(const DblVec& x) +{ + return vecSum(violations(x)); +} +ConvexConstraints::~ConvexConstraints() +{ + if (inModel()) + removeFromModel(); +} + +double ConvexObjective::value(const DblVec& x) +{ + return quad_.value(x); +} +DblVec Constraint::violations(const DblVec& x) +{ + DblVec val = value(x); + DblVec out(val.size()); + + if (type() == EQ) + { + for (size_t i = 0; i < val.size(); ++i) + out[i] = fabs(val[i]); + } + else + { // type() == INEQ + for (size_t i = 0; i < val.size(); ++i) + out[i] = pospart(val[i]); + } + + return out; +} + +double Constraint::violation(const DblVec& x) +{ + return vecSum(violations(x)); +} +OptProb::OptProb(ModelType convex_solver) : model_(createModel(convex_solver)) +{ +} +VarVector OptProb::createVariables(const std::vector& var_names) +{ + return createVariables(var_names, DblVec(var_names.size(), -INFINITY), DblVec(var_names.size(), INFINITY)); +} + +VarVector OptProb::createVariables(const std::vector& var_names, const DblVec& lb, const DblVec& ub) +{ + size_t n_add = var_names.size(), n_cur = vars_.size(); + assert(lb.size() == n_add); + assert(ub.size() == n_add); + vars_.reserve(n_cur + n_add); + lower_bounds_.reserve(n_cur + n_add); + upper_bounds_.reserve(n_cur + n_add); + for (size_t i = 0; i < var_names.size(); ++i) + { + vars_.push_back(model_->addVar(var_names[i], lb[i], ub[i])); + lower_bounds_.push_back(lb[i]); + upper_bounds_.push_back(ub[i]); + } + model_->update(); + return VarVector(vars_.end() - static_cast(n_add), vars_.end()); +} + +void OptProb::setLowerBounds(const DblVec& lb) +{ + assert(lb.size() == vars_.size()); + lower_bounds_ = lb; +} + +void OptProb::setUpperBounds(const DblVec& ub) +{ + assert(ub.size() == vars_.size()); + upper_bounds_ = ub; +} + +void OptProb::setLowerBounds(const DblVec& lb, const VarVector& vars) +{ + setVec(lower_bounds_, vars, lb); +} +void OptProb::setUpperBounds(const DblVec& ub, const VarVector& vars) +{ + setVec(upper_bounds_, vars, ub); +} +void OptProb::addCost(CostPtr cost) +{ + costs_.push_back(cost); +} +void OptProb::addConstraint(ConstraintPtr cnt) +{ + if (cnt->type() == EQ) + addEqConstraint(cnt); + else + addIneqConstraint(cnt); +} + +void OptProb::addEqConstraint(ConstraintPtr cnt) +{ + assert(cnt->type() == EQ); + eqcnts_.push_back(cnt); +} + +void OptProb::addIneqConstraint(ConstraintPtr cnt) +{ + assert(cnt->type() == INEQ); + ineqcnts_.push_back(cnt); +} + +std::vector OptProb::getConstraints() const +{ + std::vector out; + out.reserve(eqcnts_.size() + ineqcnts_.size()); + out.insert(out.end(), eqcnts_.begin(), eqcnts_.end()); + out.insert(out.end(), ineqcnts_.begin(), ineqcnts_.end()); + return out; +} + +void OptProb::addLinearConstraint(const AffExpr& expr, ConstraintType type) +{ + if (type == EQ) + model_->addEqCnt(expr, ""); + else + model_->addIneqCnt(expr, ""); +} + +DblVec OptProb::getCentralFeasiblePoint(const DblVec& x) +{ + assert(x.size() == lower_bounds_.size()); + DblVec center(x.size()); + for (unsigned i = 0; i < x.size(); ++i) + center[i] = (lower_bounds_[i] + upper_bounds_[i]) / 2; + return getClosestFeasiblePoint(center); +} + +DblVec OptProb::getClosestFeasiblePoint(const DblVec& x) +{ + LOG_DEBUG("getClosestFeasiblePoint"); + assert(vars_.size() == x.size()); + QuadExpr obj; + for (unsigned i = 0; i < x.size(); ++i) + { + exprInc(obj, exprSquare(exprSub(AffExpr(vars_[i]), x[i]))); + } + model_->setVarBounds(vars_, lower_bounds_, upper_bounds_); + model_->setObjective(obj); + CvxOptStatus status = model_->optimize(); + if (status != CVX_SOLVED) + { + model_->writeToFile("/tmp/fail.lp"); + PRINT_AND_THROW("couldn't find a feasible point. there's probably a " + "problem with variable bounds (e.g. joint limits). wrote " + "to /tmp/fail.lp"); + } + return model_->getVarValues(vars_); +} +} // namespace sco diff --git a/moveit_planners/trajopt/trajopt_sco/src/modeling_utils.cpp b/moveit_planners/trajopt/trajopt_sco/src/modeling_utils.cpp new file mode 100644 index 0000000000..421d3e79e5 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/modeling_utils.cpp @@ -0,0 +1,254 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +namespace sco +{ +const double DEFAULT_EPSILON = 1e-5; + +// convert a VarVector to a DblVec. Var has more properties than a single double +Eigen::VectorXd getVec(const DblVec& x, const VarVector& vars) +{ + Eigen::VectorXd out(vars.size()); + for (unsigned i = 0; i < vars.size(); ++i) + out[i] = x[static_cast(vars[i].var_rep->index)]; + return out; +} + +DblVec getDblVec(const DblVec& x, const VarVector& vars) +{ + DblVec out(vars.size()); + for (unsigned i = 0; i < vars.size(); ++i) + out[i] = x[static_cast(vars[i].var_rep->index)]; + return out; +} + +AffExpr affFromValGrad(double y, const Eigen::VectorXd& x, const Eigen::VectorXd& dydx, const VarVector& vars) +{ + AffExpr aff; + aff.constant = y - dydx.dot(x); + aff.coeffs = util::toDblVec(dydx); + aff.vars = vars; + aff = cleanupAff(aff); + return aff; +} + +CostFromFunc::CostFromFunc(ScalarOfVectorPtr f, const VarVector& vars, const std::string& name, bool full_hessian) + : Cost(name), f_(f), vars_(vars), full_hessian_(full_hessian), epsilon_(DEFAULT_EPSILON) +{ +} + +double CostFromFunc::value(const DblVec& xin) +{ + Eigen::VectorXd x = getVec(xin, vars_); + return f_->call(x); +} + +ConvexObjectivePtr CostFromFunc::convex(const DblVec& xin, Model* model) +{ + Eigen::VectorXd x = getVec(xin, vars_); + + ConvexObjectivePtr out(new ConvexObjective(model)); + if (!full_hessian_) + { + double val; + Eigen::VectorXd grad, hess; + calcGradAndDiagHess(*f_, x, epsilon_, val, grad, hess); + hess = hess.cwiseMax(Eigen::VectorXd::Zero(hess.size())); + QuadExpr& quad = out->quad_; + quad.affexpr.constant = val - grad.dot(x) + .5 * x.dot(hess.cwiseProduct(x)); + quad.affexpr.vars = vars_; + quad.affexpr.coeffs = util::toDblVec(grad - hess.cwiseProduct(x)); + quad.vars1 = vars_; + quad.vars2 = vars_; + quad.coeffs = util::toDblVec(hess * .5); + } + else + { + double val; + Eigen::VectorXd grad; + Eigen::MatrixXd hess; + calcGradHess(f_, x, epsilon_, val, grad, hess); + + Eigen::MatrixXd pos_hess = Eigen::MatrixXd::Zero(x.size(), x.size()); + Eigen::SelfAdjointEigenSolver es(hess); + Eigen::VectorXd eigvals = es.eigenvalues(); + Eigen::MatrixXd eigvecs = es.eigenvectors(); + for (long int i = 0, end = x.size(); i != end; ++i) + { // tricky --- eigen size() is signed + if (eigvals(i) > 0) + pos_hess += eigvals(i) * eigvecs.col(i) * eigvecs.col(i).transpose(); + } + + QuadExpr& quad = out->quad_; + quad.affexpr.constant = val - grad.dot(x) + .5 * x.dot(pos_hess * x); + quad.affexpr.vars = vars_; + quad.affexpr.coeffs = util::toDblVec(grad - pos_hess * x); + + size_t nquadterms = static_cast((x.size() * (x.size() - 1)) / 2); + quad.coeffs.reserve(nquadterms); + quad.vars1.reserve(nquadterms); + quad.vars2.reserve(nquadterms); + for (long int i = 0, end = x.size(); i != end; ++i) + { // tricky --- eigen size() is signed + quad.vars1.push_back(vars_[static_cast(i)]); + quad.vars2.push_back(vars_[static_cast(i)]); + quad.coeffs.push_back(pos_hess(i, i) / 2); + for (long int j = i + 1; j != end; ++j) + { // tricky --- eigen size() is signed + quad.vars1.push_back(vars_[static_cast(i)]); + quad.vars2.push_back(vars_[static_cast(j)]); + quad.coeffs.push_back(pos_hess(i, j)); + } + } + } + + return out; +} + +CostFromErrFunc::CostFromErrFunc(VectorOfVectorPtr f, const VarVector& vars, const Eigen::VectorXd& coeffs, + PenaltyType pen_type, const std::string& name) + : Cost(name), f_(f), vars_(vars), coeffs_(coeffs), pen_type_(pen_type), epsilon_(DEFAULT_EPSILON) +{ +} +CostFromErrFunc::CostFromErrFunc(VectorOfVectorPtr f, MatrixOfVectorPtr dfdx, const VarVector& vars, + const Eigen::VectorXd& coeffs, PenaltyType pen_type, const std::string& name) + : Cost(name), f_(f), dfdx_(dfdx), vars_(vars), coeffs_(coeffs), pen_type_(pen_type), epsilon_(DEFAULT_EPSILON) +{ +} +double CostFromErrFunc::value(const DblVec& xin) +{ + Eigen::VectorXd x = getVec(xin, vars_); + Eigen::VectorXd err = f_->call(x); + + switch (pen_type_) + { + case SQUARED: + err = err.array().square(); + break; + case ABS: + err = err.array().abs(); + break; + case HINGE: + err = err.cwiseMax(Eigen::VectorXd::Zero(err.size())); + break; + default: + assert(0 && "unreachable"); + } + + if (coeffs_.size() > 0) + err.array() *= coeffs_.array(); + + return err.array().sum(); +} +ConvexObjectivePtr CostFromErrFunc::convex(const DblVec& xin, Model* model) +{ + Eigen::VectorXd x = getVec(xin, vars_); + Eigen::MatrixXd jac = (dfdx_) ? dfdx_->call(x) : calcForwardNumJac(*f_, x, epsilon_); + ConvexObjectivePtr out(new ConvexObjective(model)); + Eigen::VectorXd y = f_->call(x); + for (int i = 0; i < jac.rows(); ++i) + { + AffExpr aff = affFromValGrad(y[i], x, jac.row(i), vars_); + double weight = 1; + if (coeffs_.size() > 0) + { + if (coeffs_[i] == 0) + continue; + + weight = coeffs_[i]; + } + switch (pen_type_) + { + case SQUARED: + { + QuadExpr quad = exprSquare(aff); + exprScale(quad, weight); + out->addQuadExpr(quad); + break; + } + case ABS: + { + exprScale(aff, weight); + out->addAbs(aff, 1); + break; + } + case HINGE: + { + exprScale(aff, weight); + out->addHinge(aff, 1); + break; + } + default: + assert(0 && "unreachable"); + } + } + return out; +} + +ConstraintFromErrFunc::ConstraintFromErrFunc(VectorOfVectorPtr f, const VarVector& vars, const Eigen::VectorXd& coeffs, + ConstraintType type, const std::string& name) + : Constraint(name), f_(f), vars_(vars), coeffs_(coeffs), type_(type), epsilon_(DEFAULT_EPSILON) +{ +} + +ConstraintFromErrFunc::ConstraintFromErrFunc(VectorOfVectorPtr f, MatrixOfVectorPtr dfdx, const VarVector& vars, + const Eigen::VectorXd& coeffs, ConstraintType type, + const std::string& name) + : Constraint(name), f_(f), dfdx_(dfdx), vars_(vars), coeffs_(coeffs), type_(type), epsilon_(DEFAULT_EPSILON) +{ +} + +DblVec ConstraintFromErrFunc::value(const DblVec& xin) +{ + Eigen::VectorXd x = getVec(xin, vars_); + Eigen::VectorXd err = f_->call(x); + if (coeffs_.size() > 0) + err.array() *= coeffs_.array(); + return util::toDblVec(err); +} + +ConvexConstraintsPtr ConstraintFromErrFunc::convex(const DblVec& xin, Model* model) +{ + Eigen::VectorXd x = getVec(xin, vars_); + Eigen::MatrixXd jac = (dfdx_) ? dfdx_->call(x) : calcForwardNumJac(*f_, x, epsilon_); + ConvexConstraintsPtr out(new ConvexConstraints(model)); + Eigen::VectorXd y = f_->call(x); + for (int i = 0; i < jac.rows(); ++i) + { + AffExpr aff = affFromValGrad(y[i], x, jac.row(i), vars_); + if (coeffs_.size() > 0) + { + if (coeffs_[i] == 0) + continue; + exprScale(aff, coeffs_[i]); + } + if (type() == INEQ) + out->addIneqCnt(aff); + else + out->addEqCnt(aff); + } + return out; +} + +std::string AffExprToString(const AffExpr& aff) +{ + std::string out; + for (size_t i = 0; i < aff.vars.size(); i++) + { + if (i != 0) + out.append(" + "); + std::string term = std::to_string(aff.coeffs[i]) + "*" + aff.vars[i].var_rep->name; + out.append(term); + } + out.append(" + " + std::to_string(aff.constant)); + return out; +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/src/num_diff.cpp b/moveit_planners/trajopt/trajopt_sco/src/num_diff.cpp new file mode 100644 index 0000000000..4277cf5035 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/num_diff.cpp @@ -0,0 +1,130 @@ +#include + +namespace sco +{ +ScalarOfVectorPtr ScalarOfVector::construct(const func& f) +{ + struct F : public ScalarOfVector + { + func f; + F(const func& _f) : f(_f) + { + } + double operator()(const Eigen::VectorXd& x) const override + { + return f(x); + } + }; + ScalarOfVector* sov = new F(f); // to avoid erroneous clang warning + return ScalarOfVectorPtr(sov); +} +VectorOfVectorPtr VectorOfVector::construct(const func& f) +{ + struct F : public VectorOfVector + { + func f; + F(const func& _f) : f(_f) + { + } + Eigen::VectorXd operator()(const Eigen::VectorXd& x) const override + { + return f(x); + } + }; + VectorOfVector* vov = new F(f); // to avoid erroneous clang warning + return VectorOfVectorPtr(vov); +} + +Eigen::VectorXd calcForwardNumGrad(const ScalarOfVector& f, const Eigen::VectorXd& x, double epsilon) +{ + Eigen::VectorXd out(x.size()); + Eigen::VectorXd xpert = x; + double y = f(x); + for (int i = 0; i < x.size(); ++i) + { + xpert(i) = x(i) + epsilon; + double ypert = f(xpert); + out(i) = (ypert - y) / epsilon; + xpert(i) = x(i); + } + return out; +} +Eigen::MatrixXd calcForwardNumJac(const VectorOfVector& f, const Eigen::VectorXd& x, double epsilon) +{ + Eigen::VectorXd y = f(x); + Eigen::MatrixXd out(y.size(), x.size()); + Eigen::VectorXd xpert = x; + for (int i = 0; i < x.size(); ++i) + { + xpert(i) = x(i) + epsilon; + Eigen::VectorXd ypert = f(xpert); + out.col(i) = (ypert - y) / epsilon; + xpert(i) = x(i); + } + return out; +} + +void calcGradAndDiagHess(const ScalarOfVector& f, const Eigen::VectorXd& x, double epsilon, double& y, + Eigen::VectorXd& grad, Eigen::VectorXd& hess) +{ + y = f(x); + grad.resize(x.size()); + hess.resize(x.size()); + Eigen::VectorXd xpert = x; + for (int i = 0; i < x.size(); ++i) + { + xpert(i) = x(i) + epsilon / 2; + double yplus = f(xpert); + xpert(i) = x(i) - epsilon / 2; + double yminus = f(xpert); + grad(i) = (yplus - yminus) / epsilon; + hess(i) = (yplus + yminus - 2 * y) / (epsilon * epsilon / 4); + xpert(i) = x(i); + } +} + +void calcGradHess(ScalarOfVectorPtr f, const Eigen::VectorXd& x, double epsilon, double& y, Eigen::VectorXd& grad, + Eigen::MatrixXd& hess) +{ + y = f->call(x); + VectorOfVectorPtr grad_func = forwardNumGrad(f, epsilon); + grad = grad_func->call(x); + hess = calcForwardNumJac(*grad_func, x, epsilon); + hess = (hess + hess.transpose()) / 2; +} + +struct ForwardNumGrad : public VectorOfVector +{ + ScalarOfVectorPtr f_; + double epsilon_; + ForwardNumGrad(ScalarOfVectorPtr f, double epsilon) : f_(f), epsilon_(epsilon) + { + } + Eigen::VectorXd operator()(const Eigen::VectorXd& x) const override + { + return calcForwardNumGrad(*f_, x, epsilon_); + } +}; + +struct ForwardNumJac : public MatrixOfVector +{ + VectorOfVectorPtr f_; + double epsilon_; + ForwardNumJac(VectorOfVectorPtr f, double epsilon) : f_(f), epsilon_(epsilon) + { + } + Eigen::MatrixXd operator()(const Eigen::VectorXd& x) const override + { + return calcForwardNumJac(*f_, x, epsilon_); + } +}; + +VectorOfVectorPtr forwardNumGrad(ScalarOfVectorPtr f, double epsilon) +{ + return VectorOfVectorPtr(new ForwardNumGrad(f, epsilon)); +} +MatrixOfVectorPtr forwardNumJac(VectorOfVectorPtr f, double epsilon) +{ + return MatrixOfVectorPtr(new ForwardNumJac(f, epsilon)); +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/src/optimizers.cpp b/moveit_planners/trajopt/trajopt_sco/src/optimizers.cpp new file mode 100644 index 0000000000..0d07502ca4 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/optimizers.cpp @@ -0,0 +1,468 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sco +{ +std::ostream& operator<<(std::ostream& o, const OptResults& r) +{ + o << "Optimization results:" << std::endl + << "status: " << statusToString(r.status) << std::endl + << "cost values: " << util::Str(r.cost_vals) << std::endl + << "constraint violations: " << util::Str(r.cnt_viols) << std::endl + << "n func evals: " << r.n_func_evals << std::endl + << "n qp solves: " << r.n_qp_solves << std::endl; + return o; +} + +////////////////////////////////////////////////// +////////// private utility functions for sqp ///////// +////////////////////////////////////////////////// + +static DblVec evaluateCosts(const std::vector& costs, const DblVec& x) +{ + DblVec out(costs.size()); + for (size_t i = 0; i < costs.size(); ++i) + { + out[i] = costs[i]->value(x); + } + return out; +} +static DblVec evaluateConstraintViols(const std::vector& constraints, const DblVec& x) +{ + DblVec out(constraints.size()); + for (size_t i = 0; i < constraints.size(); ++i) + { + out[i] = constraints[i]->violation(x); + } + return out; +} +static std::vector convexifyCosts(const std::vector& costs, const DblVec& x, Model* model) +{ + std::vector out(costs.size()); + for (size_t i = 0; i < costs.size(); ++i) + { + out[i] = costs[i]->convex(x, model); + } + return out; +} +static std::vector convexifyConstraints(const std::vector& cnts, const DblVec& x, + Model* model) +{ + std::vector out(cnts.size()); + for (size_t i = 0; i < cnts.size(); ++i) + { + out[i] = cnts[i]->convex(x, model); + } + return out; +} + +DblVec evaluateModelCosts(const std::vector& costs, const DblVec& x) +{ + DblVec out(costs.size()); + for (size_t i = 0; i < costs.size(); ++i) + { + out[i] = costs[i]->value(x); + } + return out; +} +DblVec evaluateModelCntViols(const std::vector& cnts, const DblVec& x) +{ + DblVec out(cnts.size()); + for (size_t i = 0; i < cnts.size(); ++i) + { + out[i] = cnts[i]->violation(x); + } + return out; +} + +static std::vector getCostNames(const std::vector& costs) +{ + std::vector out(costs.size()); + for (size_t i = 0; i < costs.size(); ++i) + out[i] = costs[i]->name(); + return out; +} +static std::vector getCntNames(const std::vector& cnts) +{ + std::vector out(cnts.size()); + for (size_t i = 0; i < cnts.size(); ++i) + out[i] = cnts[i]->name(); + return out; +} + +void printCostInfo(const DblVec& old_cost_vals, const DblVec& model_cost_vals, const DblVec& new_cost_vals, + const DblVec& old_cnt_vals, const DblVec& model_cnt_vals, const DblVec& new_cnt_vals, + const std::vector& cost_names, const std::vector& cnt_names, + double merit_coeff) +{ + std::printf("%15s | %10s | %10s | %10s | %10s\n", "", "oldexact", "dapprox", "dexact", "ratio"); + std::printf("%15s | %10s---%10s---%10s---%10s\n", "COSTS", "----------", "----------", "----------", "----------"); + for (size_t i = 0; i < old_cost_vals.size(); ++i) + { + double approx_improve = old_cost_vals[i] - model_cost_vals[i]; + double exact_improve = old_cost_vals[i] - new_cost_vals[i]; + if (fabs(approx_improve) > 1e-8) + std::printf("%15s | %10.3e | %10.3e | %10.3e | %10.3e\n", cost_names[i].c_str(), old_cost_vals[i], approx_improve, + exact_improve, exact_improve / approx_improve); + else + std::printf("%15s | %10.3e | %10.3e | %10.3e | %10s\n", cost_names[i].c_str(), old_cost_vals[i], approx_improve, + exact_improve, " ------ "); + } + if (cnt_names.size() == 0) + return; + std::printf("%15s | %10s---%10s---%10s---%10s\n", "CONSTRAINTS", "----------", "----------", "----------", "---------" + "-"); + for (size_t i = 0; i < old_cnt_vals.size(); ++i) + { + double approx_improve = old_cnt_vals[i] - model_cnt_vals[i]; + double exact_improve = old_cnt_vals[i] - new_cnt_vals[i]; + if (fabs(approx_improve) > 1e-8) + std::printf("%15s | %10.3e | %10.3e | %10.3e | %10.3e\n", cnt_names[i].c_str(), merit_coeff * old_cnt_vals[i], + merit_coeff * approx_improve, merit_coeff * exact_improve, exact_improve / approx_improve); + else + std::printf("%15s | %10.3e | %10.3e | %10.3e | %10s\n", cnt_names[i].c_str(), merit_coeff * old_cnt_vals[i], + merit_coeff * approx_improve, merit_coeff * exact_improve, " ------ "); + } +} + +// todo: use different coeffs for each constraint +std::vector cntsToCosts(const std::vector& cnts, double err_coeff, + Model* model) +{ + std::vector out; + for (const ConvexConstraintsPtr& cnt : cnts) + { + ConvexObjectivePtr obj(new ConvexObjective(model)); + for (const AffExpr& aff : cnt->eqs_) + { + obj->addAbs(aff, err_coeff); + } + for (const AffExpr& aff : cnt->ineqs_) + { + obj->addHinge(aff, err_coeff); + } + out.push_back(obj); + } + return out; +} + +void Optimizer::addCallback(const Callback& cb) +{ + callbacks_.push_back(cb); +} +void Optimizer::callCallbacks() +{ + for (unsigned i = 0; i < callbacks_.size(); ++i) + { + callbacks_[i](prob_.get(), results_); + } +} + +void Optimizer::initialize(const DblVec& x) +{ + if (!prob_) + PRINT_AND_THROW("need to set the problem before initializing"); + if (prob_->getVars().size() != x.size()) + PRINT_AND_THROW(boost::format("initialization vector has wrong length. expected %i got %i") % + prob_->getVars().size() % x.size()); + results_.clear(); + results_.x = x; +} + +BasicTrustRegionSQPParameters::BasicTrustRegionSQPParameters() +{ + improve_ratio_threshold = 0.25; + min_trust_box_size = 1e-4; + min_approx_improve = 1e-4; + min_approx_improve_frac = -INFINITY; + max_iter = 50; + trust_shrink_ratio = 0.1; + trust_expand_ratio = 1.5; + cnt_tolerance = 1e-4; + max_merit_coeff_increases = 5; + merit_coeff_increase_ratio = 10; + max_time = INFINITY; + merit_error_coeff = 10; + trust_box_size = 1e-1; +} + +BasicTrustRegionSQP::BasicTrustRegionSQP() +{ +} +BasicTrustRegionSQP::BasicTrustRegionSQP(OptProbPtr prob) +{ + setProblem(prob); +} +void BasicTrustRegionSQP::setProblem(OptProbPtr prob) +{ + Optimizer::setProblem(prob); + model_ = prob->getModel(); +} + +void BasicTrustRegionSQP::adjustTrustRegion(double ratio) +{ + param_.trust_box_size *= ratio; +} +void BasicTrustRegionSQP::setTrustBoxConstraints(const DblVec& x) +{ + const VarVector& vars = prob_->getVars(); + assert(vars.size() == x.size()); + const DblVec &lb = prob_->getLowerBounds(), ub = prob_->getUpperBounds(); + DblVec lbtrust(x.size()), ubtrust(x.size()); + for (size_t i = 0; i < x.size(); ++i) + { + lbtrust[i] = fmax(x[i] - param_.trust_box_size, lb[i]); + ubtrust[i] = fmin(x[i] + param_.trust_box_size, ub[i]); + } + model_->setVarBounds(vars, lbtrust, ubtrust); +} + +#if 0 +struct MultiCritFilter { + /** + * Checks if you're making an improvement on a multidimensional objective + * Given a set of past error vectors, the improvement is defined as + * min_{olderrvec in past_err_vecs} | olderrvec - errvec |^+ + */ + vector errvecs; + double improvement(const DblVec& errvec) { + double leastImprovement=INFINITY; + for (const DblVec& olderrvec : errvecs) { + double improvement=0; + for (int i=0; i < errvec.size(); ++i) improvement += pospart(olderrvec[i] - errvec[i]); + leastImprovement = fmin(leastImprovement, improvement); + } + return leastImprovement; + } + void insert(const DblVec& x) {errvecs.push_back(x);} + bool empty() {return errvecs.size() > 0;} +}; +#endif + +OptStatus BasicTrustRegionSQP::optimize() +{ + std::vector cost_names = getCostNames(prob_->getCosts()); + std::vector constraints = prob_->getConstraints(); + std::vector cnt_names = getCntNames(constraints); + + if (results_.x.size() == 0) + PRINT_AND_THROW("you forgot to initialize!"); + if (!prob_) + PRINT_AND_THROW("you forgot to set the optimization problem"); + + results_.x = prob_->getClosestFeasiblePoint(results_.x); + + assert(results_.x.size() == prob_->getVars().size()); + assert(prob_->getCosts().size() > 0 || constraints.size() > 0); + + OptStatus retval = INVALID; + + + //Strategy: with a start merit_increase value, start iteration (iter = 1). if the trust region is expanded, it goes to the next iteration (iter = 2) + // if the improv is not good enough, it stays in the while loop (it does not go to the next iter) and keep shrinking the trust region. if by this + // decreasing the improv is not achieved, then it says it is converged becasue the trust region is tiny. Then it increase the merit_increase and repeat + // the process + for (int merit_increases = 0; merit_increases < param_.max_merit_coeff_increases; ++merit_increases) // I think: PenaltyIteration + { /* merit adjustment loop */ + LOG_INFO("merit increases: %i", merit_increases); + for (int iter = 1;; ++iter) // I think: ConvexifyIteration + { /* sqp loop */ + callCallbacks(); + + LOG_DEBUG("current iterate: %s", CSTR(results_.x)); + LOG_INFO("iteration %i", iter); + + // speedup: if you just evaluated the cost when doing the line search, use + // that + if (results_.cost_vals.empty() && results_.cnt_viols.empty()) + { // only happens on the first iteration + results_.cnt_viols = evaluateConstraintViols(constraints, results_.x); + results_.cost_vals = evaluateCosts(prob_->getCosts(), results_.x); + assert(results_.n_func_evals == 0); + ++results_.n_func_evals; + } + + // DblVec new_cnt_viols = evaluateConstraintViols(constraints, results_.x); + // DblVec new_cost_vals = evaluateCosts(prob_->getCosts(), results_.x); + // cout << "costs" << endl; + // for (int i=0; i < new_cnt_viols.size(); ++i) { + // cout << cnt_names[i] << " " << new_cnt_viols[i] - + // results_.cnt_viols[i] << endl; + // } + // for (int i=0; i < new_cost_vals.size(); ++i) { + // cout << cost_names[i] << " " << new_cost_vals[i] - + // results_.cost_vals[i] << endl; + // } + + std::vector cost_models = convexifyCosts(prob_->getCosts(), results_.x, model_.get()); + std::vector cnt_models = convexifyConstraints(constraints, results_.x, model_.get()); + std::vector cnt_cost_models = cntsToCosts(cnt_models, param_.merit_error_coeff, model_.get()); + model_->update(); + for (ConvexObjectivePtr& cost : cost_models) + cost->addConstraintsToModel(); + for (ConvexObjectivePtr& cost : cnt_cost_models) + cost->addConstraintsToModel(); + model_->update(); + QuadExpr objective; + for (ConvexObjectivePtr& co : cost_models) + exprInc(objective, co->quad_); + for (ConvexObjectivePtr& co : cnt_cost_models) + exprInc(objective, co->quad_); + + // objective = cleanupExpr(objective); + model_->setObjective(objective); + + // if (logging::filter() >= IPI_LEVEL_DEBUG) { + // DblVec model_cost_vals; + // for (ConvexObjectivePtr& cost : cost_models) { + // model_cost_vals.push_back(cost->value(x)); + // } + // LOG_DEBUG("model costs %s should equalcosts %s", + // printer(model_cost_vals), printer(cost_vals)); + // } + + while (param_.trust_box_size >= param_.min_trust_box_size) // I think: TrustRegionIteration + { + setTrustBoxConstraints(results_.x); + CvxOptStatus status = model_->optimize(); + ++results_.n_qp_solves; + if (status != CVX_SOLVED) + { + LOG_ERROR("convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see " + "solver output. saving model to /tmp/fail.lp and IIS to " + "/tmp/fail.ilp"); + model_->writeToFile("/tmp/fail.lp"); + model_->writeToFile("/tmp/fail.ilp"); + retval = OPT_FAILED; + goto cleanup; + } + DblVec model_var_vals = model_->getVarValues(model_->getVars()); + + DblVec model_cost_vals = evaluateModelCosts(cost_models, model_var_vals); + DblVec model_cnt_viols = evaluateModelCntViols(cnt_models, model_var_vals); + + // the n variables of the OptProb happen to be the first n variables in + // the Model + DblVec new_x(model_var_vals.begin(), model_var_vals.begin() + static_cast(results_.x.size())); + + if (util::GetLogLevel() >= util::LevelDebug) + { + DblVec cnt_costs1 = evaluateModelCosts(cnt_cost_models, model_var_vals); + DblVec cnt_costs2 = model_cnt_viols; + for (unsigned i = 0; i < cnt_costs2.size(); ++i) + cnt_costs2[i] *= param_.merit_error_coeff; + LOG_DEBUG("SHOULD BE ALMOST THE SAME: %s ?= %s", CSTR(cnt_costs1), CSTR(cnt_costs2)); + // not exactly the same because cnt_costs1 is based on aux variables, + // but they might not be at EXACTLY the right value + } + + DblVec new_cost_vals = evaluateCosts(prob_->getCosts(), new_x); + DblVec new_cnt_viols = evaluateConstraintViols(constraints, new_x); + ++results_.n_func_evals; + + double old_merit = vecSum(results_.cost_vals) + param_.merit_error_coeff * vecSum(results_.cnt_viols); + double model_merit = vecSum(model_cost_vals) + param_.merit_error_coeff * vecSum(model_cnt_viols); + double new_merit = vecSum(new_cost_vals) + param_.merit_error_coeff * vecSum(new_cnt_viols); + double approx_merit_improve = old_merit - model_merit; + double exact_merit_improve = old_merit - new_merit; + double merit_improve_ratio = exact_merit_improve / approx_merit_improve; + + if (util::GetLogLevel() >= util::LevelInfo) + { + LOG_INFO(" "); + printCostInfo(results_.cost_vals, model_cost_vals, new_cost_vals, results_.cnt_viols, model_cnt_viols, + new_cnt_viols, cost_names, cnt_names, param_.merit_error_coeff); + std::printf("%15s | %10.3e | %10.3e | %10.3e | %10.3e\n", "TOTAL", old_merit, approx_merit_improve, + exact_merit_improve, merit_improve_ratio); + } + + if (approx_merit_improve < -1e-5) + { + LOG_ERROR("approximate merit function got worse (%.3e). " + "(convexification is probably wrong to zeroth order)", + approx_merit_improve); + } + if (approx_merit_improve < param_.min_approx_improve) + { + LOG_INFO("converged because improvement was small (%.3e < %.3e)", approx_merit_improve, + param_.min_approx_improve); + retval = OPT_CONVERGED; + goto penaltyadjustment; + } + if (approx_merit_improve / old_merit < param_.min_approx_improve_frac) + { + LOG_INFO("converged because improvement ratio was small (%.3e < %.3e)", approx_merit_improve / old_merit, + param_.min_approx_improve_frac); + retval = OPT_CONVERGED; + goto penaltyadjustment; + } + else if (exact_merit_improve < 0 || merit_improve_ratio < param_.improve_ratio_threshold) + { + adjustTrustRegion(param_.trust_shrink_ratio); + LOG_INFO("shrunk trust region. new box size: %.4f", param_.trust_box_size); + } + else + { + results_.x = new_x; + results_.cost_vals = new_cost_vals; + results_.cnt_viols = new_cnt_viols; + adjustTrustRegion(param_.trust_expand_ratio); + LOG_INFO("expanded trust region. new box size: %.4f", param_.trust_box_size); + break; + } + } + + if (param_.trust_box_size < param_.min_trust_box_size) + { + LOG_INFO("converged because trust region is tiny"); + retval = OPT_CONVERGED; + goto penaltyadjustment; + } + else if (iter >= param_.max_iter) + { + LOG_INFO("iteration limit"); + retval = OPT_SCO_ITERATION_LIMIT; + goto cleanup; + } + } + + penaltyadjustment: + if (results_.cnt_viols.empty() || vecMax(results_.cnt_viols) < param_.cnt_tolerance) + { + if (results_.cnt_viols.size() > 0) + LOG_INFO("woo-hoo! constraints are satisfied (to tolerance %.2e)", param_.cnt_tolerance); + goto cleanup; + } + else + { + LOG_INFO("not all constraints are satisfied. increasing penalties"); + param_.merit_error_coeff *= param_.merit_coeff_increase_ratio; + param_.trust_box_size = fmax(param_.trust_box_size, param_.min_trust_box_size / param_.trust_shrink_ratio * 1.5); + } + } + retval = OPT_PENALTY_ITERATION_LIMIT; + LOG_INFO("optimization couldn't satisfy all constraints"); + +cleanup: + assert(retval != INVALID && "should never happen"); + results_.status = retval; + results_.total_cost = vecSum(results_.cost_vals); + LOG_INFO("\n==================\n%s==================", CSTR(results_)); + callCallbacks(); + + return retval; +} +} // namespace sco diff --git a/moveit_planners/trajopt/trajopt_sco/src/osqp_interface.cpp b/moveit_planners/trajopt/trajopt_sco/src/osqp_interface.cpp new file mode 100644 index 0000000000..a3dffa4c7b --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/osqp_interface.cpp @@ -0,0 +1,271 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +namespace sco +{ +double OSQP_INFINITY = std::numeric_limits::infinity(); + +ModelPtr createOSQPModel() +{ + ModelPtr out(new OSQPModel()); + return out; +} + +OSQPModel::OSQPModel() +{ + // Define Solver settings as default + // see https://osqp.org/docs/interfaces/solver_settings.html#solver-settings + osqp_set_default_settings(&osqp_settings_); + // tuning parameters to be less accurate, but add a polishing step + osqp_settings_.eps_abs = 1e-4; + osqp_settings_.eps_rel = 1e-6; + osqp_settings_.max_iter = 8192; + osqp_settings_.polish = 1; + osqp_settings_.verbose = false; + + // Initialize data + osqp_data_.A = nullptr; + osqp_data_.P = nullptr; + osqp_workspace_ = nullptr; +} + +OSQPModel::~OSQPModel() +{ + // Cleanup + if (osqp_workspace_ != nullptr) + osqp_cleanup(osqp_workspace_); + if (osqp_data_.A != nullptr) + c_free(osqp_data_.A); + if (osqp_data_.P != nullptr) + c_free(osqp_data_.P); +} + +Var OSQPModel::addVar(const std::string& name) +{ + vars_.push_back(new VarRep(vars_.size(), name, this)); + lbs_.push_back(-OSQP_INFINITY); + ubs_.push_back(OSQP_INFINITY); + return vars_.back(); +} + +Cnt OSQPModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) +{ + cnts_.push_back(new CntRep(cnts_.size(), this)); + cnt_exprs_.push_back(expr); + cnt_types_.push_back(EQ); + return cnts_.back(); +} + +Cnt OSQPModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/) +{ + cnts_.push_back(new CntRep(cnts_.size(), this)); + cnt_exprs_.push_back(expr); + cnt_types_.push_back(INEQ); + return cnts_.back(); +} + +Cnt OSQPModel::addIneqCnt(const QuadExpr&, const std::string& /*name*/) +{ + throw std::runtime_error("NOT IMPLEMENTED"); + return Cnt(); +} + +void OSQPModel::removeVars(const VarVector& vars) +{ + IntVec inds = vars2inds(vars); + for (unsigned i = 0; i < vars.size(); ++i) + vars[i].var_rep->removed = true; +} + +void OSQPModel::removeCnts(const CntVector& cnts) +{ + IntVec inds = cnts2inds(cnts); + for (unsigned i = 0; i < cnts.size(); ++i) + cnts[i].cnt_rep->removed = true; +} + +void OSQPModel::updateObjective() +{ + const size_t n = vars_.size(); + osqp_data_.n = n; + + Eigen::SparseMatrix sm; + exprToEigen(objective_, sm, q_, n, true); + eigenToCSC(sm, P_row_indices_, P_column_pointers_, P_csc_data_); + + if (osqp_data_.P != nullptr) + c_free(osqp_data_.P); + osqp_data_.P = csc_matrix(osqp_data_.n, osqp_data_.n, P_csc_data_.size(), P_csc_data_.data(), P_row_indices_.data(), + P_column_pointers_.data()); + + osqp_data_.q = q_.data(); +} + +void OSQPModel::updateConstraints() +{ + const size_t n = vars_.size(); + const size_t m = cnts_.size(); + osqp_data_.m = m + n; + + Eigen::SparseMatrix sm; + Eigen::VectorXd v; + exprToEigen(cnt_exprs_, sm, v, n); + Eigen::SparseMatrix sm_e(m + n, n); + Eigen::SparseMatrix sm_e2 = sm; + sm.conservativeResize(m + n, Eigen::NoChange_t(n)); + + l_.clear(); + l_.resize(m + n, -OSQP_INFINITY); + u_.clear(); + u_.resize(m + n, OSQP_INFINITY); + + for (int i_cnt = 0; i_cnt < m; ++i_cnt) + { + l_[i_cnt] = (cnt_types_[i_cnt] == INEQ) ? -OSQP_INFINITY : v[i_cnt]; + u_[i_cnt] = v[i_cnt]; + } + + for (int i_bnd = 0; i_bnd < n; ++i_bnd) + { + l_[i_bnd + m] = fmax(lbs_[i_bnd], -OSQP_INFINITY); + u_[i_bnd + m] = fmin(ubs_[i_bnd], OSQP_INFINITY); + sm.insert(i_bnd + m, i_bnd) = 1.; + } + + eigenToCSC(sm, A_row_indices_, A_column_pointers_, A_csc_data_); + + if (osqp_data_.A != nullptr) + c_free(osqp_data_.A); + osqp_data_.A = csc_matrix(osqp_data_.m, osqp_data_.n, A_csc_data_.size(), A_csc_data_.data(), A_row_indices_.data(), + A_column_pointers_.data()); + + osqp_data_.l = l_.data(); + osqp_data_.u = u_.data(); +} + +void OSQPModel::createOrUpdateSolver() +{ + updateObjective(); + updateConstraints(); + + // TODO atm we are not updating the workspace, but recreating it each time. + // In the future, we will checking sparsity did not change and update instead + if (osqp_workspace_ != nullptr) + osqp_cleanup(osqp_workspace_); + // Setup workspace - this should be called only once + osqp_workspace_ = osqp_setup(&osqp_data_, &osqp_settings_); +} + +void OSQPModel::update() +{ + { + int inew = 0; + for (unsigned iold = 0; iold < vars_.size(); ++iold) + { + const Var& var = vars_[iold]; + if (!var.var_rep->removed) + { + vars_[inew] = var; + lbs_[inew] = lbs_[iold]; + ubs_[inew] = ubs_[iold]; + var.var_rep->index = inew; + ++inew; + } + else + delete var.var_rep; + } + vars_.resize(inew); + lbs_.resize(inew); + ubs_.resize(inew); + } + { + int inew = 0; + for (unsigned iold = 0; iold < cnts_.size(); ++iold) + { + const Cnt& cnt = cnts_[iold]; + if (!cnt.cnt_rep->removed) + { + cnts_[inew] = cnt; + cnt_exprs_[inew] = cnt_exprs_[iold]; + cnt_types_[inew] = cnt_types_[iold]; + cnt.cnt_rep->index = inew; + ++inew; + } + else + delete cnt.cnt_rep; + } + cnts_.resize(inew); + cnt_exprs_.resize(inew); + cnt_types_.resize(inew); + } +} + +void OSQPModel::setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) +{ + for (unsigned i = 0; i < vars.size(); ++i) + { + const int varind = vars[i].var_rep->index; + lbs_[varind] = lower[i]; + ubs_[varind] = upper[i]; + } +} +DblVec OSQPModel::getVarValues(const VarVector& vars) const +{ + DblVec out(vars.size()); + for (unsigned i = 0; i < vars.size(); ++i) + { + const int varind = vars[i].var_rep->index; + out[i] = solution_[varind]; + } + return out; +} + +CvxOptStatus OSQPModel::optimize() +{ + update(); + createOrUpdateSolver(); + + // Solve Problem + const int retcode = osqp_solve(osqp_workspace_); + + if (retcode == 0) + { + // opt += m_objective.affexpr.constant; + solution_ = DblVec(osqp_workspace_->solution->x, osqp_workspace_->solution->x + vars_.size()); + int status = osqp_workspace_->info->status_val; + if (status == OSQP_SOLVED || status == OSQP_SOLVED_INACCURATE) + return CVX_SOLVED; + else if (status == OSQP_PRIMAL_INFEASIBLE || status == OSQP_PRIMAL_INFEASIBLE_INACCURATE || + status == OSQP_DUAL_INFEASIBLE || status == OSQP_DUAL_INFEASIBLE_INACCURATE) + return CVX_INFEASIBLE; + } + return CVX_FAILED; +} +void OSQPModel::setObjective(const AffExpr& expr) +{ + objective_.affexpr = expr; +} +void OSQPModel::setObjective(const QuadExpr& expr) +{ + objective_ = expr; +} +void OSQPModel::writeToFile(const std::string& /*fname*/) +{ + return; // NOT IMPLEMENTED +} +VarVector OSQPModel::getVars() const +{ + return vars_; +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/src/qpoases_interface.cpp b/moveit_planners/trajopt/trajopt_sco/src/qpoases_interface.cpp new file mode 100644 index 0000000000..d3140cf0ee --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/qpoases_interface.cpp @@ -0,0 +1,262 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +using namespace qpOASES; + +namespace sco +{ +double QPOASES_INFTY = qpOASES::INFTY; + +ModelPtr createqpOASESModel() +{ + ModelPtr out(new qpOASESModel()); + return out; +} + +qpOASESModel::qpOASESModel() +{ + // set to be fast. More details at: + // https://www.coin-or.org/qpOASES/doc/3.2/doxygen/classOptions.html + // https://projects.coin-or.org/qpOASES/browser/stable/3.2/src/Options.cpp#L191 + qpoases_options_.setToMPC(); + qpoases_options_.printLevel = qpOASES::PL_NONE; + // enable regularisation to deal with degenerate Hessians + qpoases_options_.enableRegularisation = qpOASES::BT_TRUE; + qpoases_options_.ensureConsistency(); +} + +qpOASESModel::~qpOASESModel() +{ +} +Var qpOASESModel::addVar(const std::string& name) +{ + vars_.push_back(new VarRep(vars_.size(), name, this)); + lb_.push_back(-QPOASES_INFTY); + ub_.push_back(QPOASES_INFTY); + return vars_.back(); +} + +Cnt qpOASESModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) +{ + cnts_.push_back(new CntRep(cnts_.size(), this)); + cnt_exprs_.push_back(expr); + cnt_types_.push_back(EQ); + return cnts_.back(); +} + +Cnt qpOASESModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/) +{ + cnts_.push_back(new CntRep(cnts_.size(), this)); + cnt_exprs_.push_back(expr); + cnt_types_.push_back(INEQ); + return cnts_.back(); +} + +Cnt qpOASESModel::addIneqCnt(const QuadExpr&, const std::string& /*name*/) +{ + assert(0 && "NOT IMPLEMENTED"); + return 0; +} + +void qpOASESModel::removeVars(const VarVector& vars) +{ + IntVec inds = vars2inds(vars); + for (unsigned i = 0; i < vars.size(); ++i) + vars[i].var_rep->removed = true; +} + +void qpOASESModel::removeCnts(const CntVector& cnts) +{ + IntVec inds = cnts2inds(cnts); + for (unsigned i = 0; i < cnts.size(); ++i) + cnts[i].cnt_rep->removed = true; +} + +void qpOASESModel::updateObjective() +{ + const size_t n = vars_.size(); + + Eigen::SparseMatrix sm; + exprToEigen(objective_, sm, g_, n, true, true); + eigenToCSC(sm, H_row_indices_, H_column_pointers_, H_csc_data_); + + H_ = SymSparseMat(vars_.size(), vars_.size(), H_row_indices_.data(), H_column_pointers_.data(), H_csc_data_.data()); + H_.createDiagInfo(); +} + +void qpOASESModel::updateConstraints() +{ + const size_t n = vars_.size(); + const size_t m = cnts_.size(); + + lbA_.clear(); + lbA_.resize(m, -QPOASES_INFTY); + ubA_.clear(); + ubA_.resize(m, QPOASES_INFTY); + + Eigen::SparseMatrix sm; + Eigen::VectorXd v; + exprToEigen(cnt_exprs_, sm, v, n); + + for (int i_cnt = 0; i_cnt < m; ++i_cnt) + { + lbA_[i_cnt] = (cnt_types_[i_cnt] == INEQ) ? -QPOASES_INFTY : v[i_cnt]; + ubA_[i_cnt] = v[i_cnt]; + } + + eigenToCSC(sm, A_row_indices_, A_column_pointers_, A_csc_data_); + A_ = SparseMatrix(cnts_.size(), vars_.size(), A_row_indices_.data(), A_column_pointers_.data(), A_csc_data_.data()); +} + +bool qpOASESModel::updateSolver() +{ + bool solver_updated = false; + if (!qpoases_problem_ || vars_.size() != qpoases_problem_->getNV() || cnts_.size() != qpoases_problem_->getNC()) + { + // Create Problem - this should be called only once + qpoases_problem_.reset(new SQProblem(vars_.size(), cnts_.size())); + qpoases_problem_->setOptions(qpoases_options_); + solver_updated = true; + } + return solver_updated; +} + +void qpOASESModel::createSolver() +{ + qpoases_problem_.reset(); + updateSolver(); +} + +void qpOASESModel::update() +{ + { + int inew = 0; + for (unsigned iold = 0; iold < vars_.size(); ++iold) + { + const Var& var = vars_[iold]; + if (!var.var_rep->removed) + { + vars_[inew] = var; + lb_[inew] = lb_[iold]; + ub_[inew] = ub_[iold]; + var.var_rep->index = inew; + ++inew; + } + else + delete var.var_rep; + } + vars_.resize(inew); + lb_.resize(inew, QPOASES_INFTY); + ub_.resize(inew, -QPOASES_INFTY); + } + { + int inew = 0; + for (unsigned iold = 0; iold < cnts_.size(); ++iold) + { + const Cnt& cnt = cnts_[iold]; + if (!cnt.cnt_rep->removed) + { + cnts_[inew] = cnt; + cnt_exprs_[inew] = cnt_exprs_[iold]; + cnt_types_[inew] = cnt_types_[iold]; + cnt.cnt_rep->index = inew; + ++inew; + } + else + delete cnt.cnt_rep; + } + cnts_.resize(inew); + cnt_exprs_.resize(inew); + cnt_types_.resize(inew); + } +} + +void qpOASESModel::setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) +{ + for (unsigned i = 0; i < vars.size(); ++i) + { + const int varind = vars[i].var_rep->index; + lb_[varind] = lower[i]; + ub_[varind] = upper[i]; + } +} +DblVec qpOASESModel::getVarValues(const VarVector& vars) const +{ + DblVec out(vars.size()); + for (unsigned i = 0; i < vars.size(); ++i) + { + const int varind = vars[i].var_rep->index; + out[i] = solution_[varind]; + } + return out; +} + +CvxOptStatus qpOASESModel::optimize() +{ + update(); + updateObjective(); + updateConstraints(); + updateSolver(); + qpOASES::returnValue val = qpOASES::RET_QP_SOLUTION_STARTED; + + // Solve Problem + int nWSR = 255; + if (qpoases_problem_->isInitialised()) + { + val = qpoases_problem_->hotstart(&H_, g_.data(), &A_, lb_.data(), ub_.data(), lbA_.data(), ubA_.data(), nWSR, + nullptr); + } + + if (val != qpOASES::SUCCESSFUL_RETURN) + { + // TODO ATM this means we are creating a new solver even if updateSolver + // returned true and the problem is not initialized. Still, it makes + // tests pass. + createSolver(); + + val = qpoases_problem_->init(&H_, g_.data(), &A_, lb_.data(), ub_.data(), lbA_.data(), ubA_.data(), nWSR, nullptr); + } + + if (val == qpOASES::SUCCESSFUL_RETURN) + { + // opt += m_objective.affexpr.constant; + solution_.resize(vars_.size(), 0.); + val = qpoases_problem_->getPrimalSolution(solution_.data()); + return CVX_SOLVED; + } + else if (val == qpOASES::RET_INIT_FAILED_INFEASIBILITY) + { + return CVX_INFEASIBLE; + } + else + { + return CVX_FAILED; + } +} +void qpOASESModel::setObjective(const AffExpr& expr) +{ + objective_.affexpr = expr; +} +void qpOASESModel::setObjective(const QuadExpr& expr) +{ + objective_ = expr; +} +void qpOASESModel::writeToFile(const std::string& /*fname*/) +{ + return; // NOT IMPLEMENTED +} +VarVector qpOASESModel::getVars() const +{ + return vars_; +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/src/solver_interface.cpp b/moveit_planners/trajopt/trajopt_sco/src/solver_interface.cpp new file mode 100644 index 0000000000..0162da16bf --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/solver_interface.cpp @@ -0,0 +1,326 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include + +namespace sco +{ +const std::vector ModelType::MODEL_NAMES_ = { "GUROBI", "BPMPD", "OSQP", "QPOASES", "AUTO_SOLVER" }; + +IntVec vars2inds(const VarVector& vars) +{ + IntVec inds(vars.size()); + for (size_t i = 0; i < inds.size(); ++i) + inds[i] = vars[i].var_rep->index; + return inds; +} +IntVec cnts2inds(const CntVector& cnts) +{ + IntVec inds(cnts.size()); + for (size_t i = 0; i < inds.size(); ++i) + inds[i] = cnts[i].cnt_rep->index; + return inds; +} + +void simplify2(IntVec& inds, DblVec& vals) +{ + typedef std::map Int2Double; + Int2Double ind2val; + for (unsigned i = 0; i < inds.size(); ++i) + { + if (vals[i] != 0.0) + ind2val[inds[i]] += vals[i]; + } + inds.resize(ind2val.size()); + vals.resize(ind2val.size()); + long unsigned int i_new = 0; + for (Int2Double::value_type& iv : ind2val) + { + inds[i_new] = iv.first; + vals[i_new] = iv.second; + ++i_new; + } +} + +double AffExpr::value(const double* x) const +{ + double out = constant; + for (size_t i = 0; i < size(); ++i) + { + out += coeffs[i] * vars[i].value(x); + } + return out; +} +double AffExpr::value(const DblVec& x) const +{ + double out = constant; + for (size_t i = 0; i < size(); ++i) + { + out += coeffs[i] * vars[i].value(x); + } + return out; +} +double QuadExpr::value(const DblVec& x) const +{ + double out = affexpr.value(x); + for (size_t i = 0; i < size(); ++i) + { + out += coeffs[i] * vars1[i].value(x) * vars2[i].value(x); + } + return out; +} +double QuadExpr::value(const double* x) const +{ + double out = affexpr.value(x); + for (size_t i = 0; i < size(); ++i) + { + out += coeffs[i] * vars1[i].value(x) * vars2[i].value(x); + } + return out; +} + +Var Model::addVar(const std::string& name, double lb, double ub) +{ + Var v = addVar(name); + setVarBounds(v, lb, ub); + return v; +} +void Model::removeVar(const Var& var) +{ + VarVector vars(1, var); + removeVars(vars); +} +void Model::removeCnt(const Cnt& cnt) +{ + CntVector cnts(1, cnt); + removeCnts(cnts); +} + +double Model::getVarValue(const Var& var) const +{ + VarVector vars(1, var); + return getVarValues(vars)[0]; +} + +void Model::setVarBounds(const Var& var, double lower, double upper) +{ + DblVec lowers(1, lower), uppers(1, upper); + VarVector vars(1, var); + setVarBounds(vars, lowers, uppers); +} + +std::ostream& operator<<(std::ostream& o, const Var& v) +{ + if (v.var_rep != nullptr) + o << v.var_rep->name; + else + o << "nullvar"; + return o; +} + +std::ostream& operator<<(std::ostream& o, const Cnt& c) +{ + o << c.cnt_rep->expr << ((c.cnt_rep->type == EQ) ? " == 0" : " <= 0"); + return o; +} + +std::ostream& operator<<(std::ostream& o, const AffExpr& e) +{ + o << e.constant; + for (size_t i = 0; i < e.size(); ++i) + { + o << " + " << e.coeffs[i] << "*" << e.vars[i]; + } + return o; +} + +std::ostream& operator<<(std::ostream& o, const QuadExpr& e) +{ + o << e.affexpr; + for (size_t i = 0; i < e.size(); ++i) + { + o << " + " << e.coeffs[i] << "*" << e.vars1[i] << "*" << e.vars2[i]; + } + return o; +} + +std::ostream& operator<<(std::ostream& o, const ModelType& cs) +{ + size_t cs_ivalue_ = static_cast(cs.value_); + if (cs_ivalue_ > cs.MODEL_NAMES_.size()) + { + std::stringstream conversion_error; + conversion_error << "Error converting ModelType to string - " + << "enum value is " << cs_ivalue_ << std::endl; + throw std::runtime_error(conversion_error.str()); + } + o << ModelType::MODEL_NAMES_[cs_ivalue_]; + return o; +} + +ModelType::ModelType() +{ + value_ = ModelType::AUTO_SOLVER; +} +ModelType::ModelType(const ModelType::Value& v) +{ + value_ = v; +} +ModelType::ModelType(const int& v) +{ + value_ = static_cast(v); +} +ModelType::ModelType(const std::string& s) +{ + for (unsigned int i = 0; i < ModelType::MODEL_NAMES_.size(); ++i) + { + if (s == ModelType::MODEL_NAMES_[i]) + { + value_ = static_cast(i); + return; + } + } + PRINT_AND_THROW(boost::format("invalid solver name:\"%s\"") % s); +} + +ModelType::operator int() const +{ + return static_cast(value_); +} +bool ModelType::operator==(const ModelType::Value& a) const +{ + return value_ == a; +} +bool ModelType::operator==(const ModelType& a) const +{ + return value_ == a.value_; +} +bool ModelType::operator!=(const ModelType& a) const +{ + return value_ != a.value_; +} +void ModelType::fromJson(const Json::Value& v) +{ + try + { + std::string ref = v.asString(); + ModelType cs(ref); + value_ = cs.value_; + } + catch (const std::runtime_error&) + { + PRINT_AND_THROW(boost::format("expected: %s, got %s") % ("string") % (v)); + } +} + +std::vector availableSolvers() +{ + std::vector has_solver(ModelType::AUTO_SOLVER, false); +#ifdef HAVE_GUROBI + has_solver[ModelType::GUROBI] = true; +#endif +#ifdef HAVE_BPMPD + has_solver[ModelType::BPMPD] = true; +#endif +#ifdef HAVE_OSQP + has_solver[ModelType::OSQP] = true; +#endif +#ifdef HAVE_QPOASES + has_solver[ModelType::QPOASES] = true; +#endif + size_t n_available_solvers = 0; + for (auto i = 0; i < ModelType::AUTO_SOLVER; ++i) + if (has_solver[static_cast(i)]) + ++n_available_solvers; + std::vector available_solvers(n_available_solvers, ModelType::AUTO_SOLVER); + + size_t j = 0; + for (int i = 0; i < static_cast(ModelType::AUTO_SOLVER); ++i) + if (has_solver[static_cast(i)]) + available_solvers[j++] = static_cast(i); + return available_solvers; +} + +ModelPtr createModel(ModelType model_type) +{ +#ifdef HAVE_GUROBI + extern ModelPtr createGurobiModel(); +#endif +#ifdef HAVE_BPMPD + extern ModelPtr createBPMPDModel(); +#endif +#ifdef HAVE_OSQP + extern ModelPtr createOSQPModel(); +#endif +#ifdef HAVE_QPOASES + extern ModelPtr createqpOASESModel(); +#endif + + char* solver_env = getenv("TRAJOPT_CONVEX_SOLVER"); + + ModelType solver = model_type; + + if (solver == ModelType::AUTO_SOLVER) + { + if (solver_env and std::string(solver_env) != "AUTO_SOLVER") + { + try + { + solver = ModelType(std::string(solver_env)); + } + catch (std::runtime_error&) + { + PRINT_AND_THROW(boost::format("invalid solver \"%s\"specified by TRAJOPT_CONVEX_SOLVER") % solver_env); + } + } + else + { + solver = availableSolvers()[0]; + } + } + +#ifndef HAVE_GUROBI + if (solver == ModelType::GUROBI) + PRINT_AND_THROW("you didn't build with GUROBI support"); +#endif +#ifndef HAVE_BPMPD + if (solver == ModelType::BPMPD) + PRINT_AND_THROW("you don't have BPMPD support on this platform"); +#endif +#ifndef HAVE_OSQP + if (solver == ModelType::OSQP) + PRINT_AND_THROW("you don't have OSQP support on this platform"); +#endif +#ifndef HAVE_QPOASES + if (solver == ModelType::QPOASES) + PRINT_AND_THROW("you don't have qpOASES support on this platform"); +#endif + +#ifdef HAVE_GUROBI + if (solver == ModelType::GUROBI) + return createGurobiModel(); +#endif +#ifdef HAVE_BPMPD + if (solver == ModelType::BPMPD) + return createBPMPDModel(); +#endif +#ifdef HAVE_OSQP + if (solver == ModelType::OSQP) + return createOSQPModel(); +#endif +#ifdef HAVE_QPOASES + if (solver == ModelType::QPOASES) + return createqpOASESModel(); +#endif + std::stringstream solver_instatiation_error; + solver_instatiation_error << "Failed to create solver: unknown solver " << solver << std::endl; + PRINT_AND_THROW(solver_instatiation_error.str()); + return ModelPtr(); +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/src/solver_utils.cpp b/moveit_planners/trajopt/trajopt_sco/src/solver_utils.cpp new file mode 100644 index 0000000000..4fbdea121a --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/src/solver_utils.cpp @@ -0,0 +1,124 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include + +namespace sco +{ +void exprToEigen(const AffExpr& expr, Eigen::SparseVector& sparse_vector, const int& n_vars) +{ + sparse_vector.resize(n_vars); + sparse_vector.reserve(static_cast(expr.size())); + for (size_t i = 0; i < expr.size(); ++i) + { + int i_var_index = expr.vars[i].var_rep->index; + if (i_var_index >= n_vars) + { + std::stringstream msg; + msg << "Coefficient " << i << "has index " << i_var_index << " but n_vars is " << n_vars; + throw std::runtime_error(msg.str()); + } + if (expr.coeffs[i] != 0.) + sparse_vector.coeffRef(i_var_index) += expr.coeffs[i]; + } +} + +void exprToEigen(const QuadExpr& expr, Eigen::SparseMatrix& sparse_matrix, Eigen::VectorXd& vector, + const int& n_vars, const bool& matrix_is_halved, const bool& force_diagonal) +{ + IntVec ind1 = vars2inds(expr.vars1); + IntVec ind2 = vars2inds(expr.vars2); + sparse_matrix.resize(n_vars, n_vars); + sparse_matrix.reserve(static_cast(2 * expr.size())); + + Eigen::SparseVector vector_sparse; + exprToEigen(expr.affexpr, vector_sparse, n_vars); + vector = vector_sparse; + + for (size_t i = 0; i < expr.coeffs.size(); ++i) + { + if (expr.coeffs[i] != 0.0) + { + if (ind1[i] == ind2[i]) + sparse_matrix.coeffRef(ind1[i], ind2[i]) += expr.coeffs[i]; + else + { + int c, r; + if (ind1[i] < ind2[i]) + { + r = ind1[i]; + c = ind2[i]; + } + else + { + r = ind2[i]; + c = ind1[i]; + } + sparse_matrix.coeffRef(r, c) += expr.coeffs[i]; + } + } + } + + auto sparse_matrix_T = Eigen::SparseMatrix(sparse_matrix.transpose()); + sparse_matrix = sparse_matrix + sparse_matrix_T; + + if (!matrix_is_halved) + sparse_matrix = 0.5 * sparse_matrix; + + if (force_diagonal) + for (int k = 0; k < n_vars; ++k) + sparse_matrix.coeffRef(k, k) += 0.0; +} + +void exprToEigen(const AffExprVector& expr_vec, Eigen::SparseMatrix& sparse_matrix, Eigen::VectorXd& vector, + const int& n_vars) +{ + vector.resize(static_cast(expr_vec.size())); + vector.setZero(); + sparse_matrix.resize(static_cast(expr_vec.size()), n_vars); + sparse_matrix.reserve(static_cast(expr_vec.size()) * n_vars); + + for (long int i = 0; i < static_cast(expr_vec.size()); ++i) + { + const AffExpr& expr = expr_vec[static_cast(i)]; + Eigen::SparseVector expr_vector; + exprToEigen(expr, expr_vector, n_vars); + for (Eigen::SparseVector::InnerIterator it(expr_vector); it; ++it) + sparse_matrix.coeffRef(i, it.index()) = it.value(); + vector[i] = -expr.constant; + } +} + +void tripletsToEigen(const IntVec& rows_i, const IntVec& cols_j, const DblVec& values_ij, + Eigen::SparseMatrix& sparse_matrix) +{ + typedef Eigen::Triplet T; + std::vector> triplets; + for (unsigned int i = 0; i < values_ij.size(); ++i) + triplets.push_back(T(rows_i[i], cols_j[i], values_ij[i])); + sparse_matrix.setFromTriplets(triplets.begin(), triplets.end()); +} + +void eigenToTriplets(const Eigen::SparseMatrix& sparse_matrix, IntVec& rows_i, IntVec& cols_j, + DblVec& values_ij) +{ + auto& sm = sparse_matrix; + rows_i.reserve(rows_i.size() + static_cast(sm.nonZeros())); + cols_j.reserve(cols_j.size() + static_cast(sm.nonZeros())); + values_ij.reserve(values_ij.size() + static_cast(sm.nonZeros())); + for (int k = 0; k < sm.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(sm, k); it; ++it) + { + rows_i.push_back(static_cast(it.row())); + cols_j.push_back(static_cast(it.col())); + values_ij.push_back(it.value()); + } + } +} +} diff --git a/moveit_planners/trajopt/trajopt_sco/test/small-problems-unit.cpp b/moveit_planners/trajopt/trajopt_sco/test/small-problems-unit.cpp new file mode 100644 index 0000000000..7e7356a99e --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/test/small-problems-unit.cpp @@ -0,0 +1,190 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include +#include +#include +#include +#include + +using namespace util; +using namespace std; +using namespace sco; +using namespace Eigen; + +class SQP : public testing::TestWithParam +{ +protected: + SQP() + { + } +}; + +void setupProblem(OptProbPtr& probptr, size_t nvars, ModelType convex_solver) +{ + probptr.reset(new OptProb(convex_solver)); + vector var_names; + for (size_t i = 0; i < nvars; ++i) + { + var_names.push_back((boost::format("x_%i") % i).str()); + } + probptr->createVariables(var_names); +} + +void expectAllNear(const DblVec& x, const DblVec& y, double abstol) +{ + EXPECT_EQ(x.size(), y.size()); + stringstream ss; + LOG_INFO("checking %s ?= %s", CSTR(x), CSTR(y)); + for (size_t i = 0; i < x.size(); ++i) + EXPECT_NEAR(x[i], y[i], abstol); +} + +double f_QuadraticSeparable(const VectorXd& x) +{ + return x(0) * x(0) + sq(x(1) - 1) + sq(x(2) - 2); +} +TEST_P(SQP, QuadraticSeparable) +{ + // if the problem is exactly a QP, it should be solved in one iteration + OptProbPtr prob; + setupProblem(prob, 3, GetParam()); + prob->addCost(CostPtr(new CostFromFunc(ScalarOfVector::construct(&f_QuadraticSeparable), prob->getVars(), "f"))); + BasicTrustRegionSQP solver(prob); + BasicTrustRegionSQPParameters& params = solver.getParameters(); + params.trust_box_size = 100; + DblVec x = { 3, 4, 5 }; + solver.initialize(x); + OptStatus status = solver.optimize(); + ASSERT_EQ(status, OPT_CONVERGED); + expectAllNear(solver.x(), { 0, 1, 2 }, 1e-3); + // todo: checks on number of iterations and function evaluates +} +double f_QuadraticNonseparable(const VectorXd& x) +{ + return sq(x(0) - x(1) + 3 * x(2)) + sq(x(0) - 1) + sq(x(2) - 2); +} +TEST_P(SQP, QuadraticNonseparable) +{ + OptProbPtr prob; + setupProblem(prob, 3, GetParam()); + prob->addCost( + CostPtr(new CostFromFunc(ScalarOfVector::construct(&f_QuadraticNonseparable), prob->getVars(), "f", true))); + BasicTrustRegionSQP solver(prob); + BasicTrustRegionSQPParameters& params = solver.getParameters(); + params.trust_box_size = 100; + params.min_trust_box_size = 1e-5; + params.min_approx_improve = 1e-6; + DblVec x = { 3, 4, 5 }; + solver.initialize(x); + OptStatus status = solver.optimize(); + ASSERT_EQ(status, OPT_CONVERGED); + expectAllNear(solver.x(), { 1, 7, 2 }, .01); + // todo: checks on number of iterations and function evaluates +} + +void testProblem(ScalarOfVectorPtr f, VectorOfVectorPtr g, ConstraintType cnt_type, const DblVec& init, + const DblVec& sol, ModelType convex_solver) +{ + OptProbPtr prob; + size_t n = init.size(); + assert(sol.size() == n); + setupProblem(prob, n, convex_solver); + prob->addCost(CostPtr(new CostFromFunc(f, prob->getVars(), "f", true))); + prob->addConstraint(ConstraintPtr(new ConstraintFromErrFunc(g, prob->getVars(), VectorXd(), cnt_type, "g"))); + BasicTrustRegionSQP solver(prob); + BasicTrustRegionSQPParameters& params = solver.getParameters(); + params.max_iter = 1000; + params.min_trust_box_size = 1e-5; + params.min_approx_improve = 1e-10; + params.merit_error_coeff = 1; + + solver.initialize(init); + OptStatus status = solver.optimize(); + EXPECT_EQ(status, OPT_CONVERGED); + expectAllNear(solver.x(), sol, .01); +} +// http://www.ai7.uni-bayreuth.de/test_problem_coll.pdf + +double f_TP1(const VectorXd& x) +{ + return 1 * sq(x(1) - sq(x(0))) + sq(1 - x(0)); +} +VectorXd g_TP1(const VectorXd& x) +{ + VectorXd out(1); + out(0) = -1.5 - x(1); + return out; +} +double f_TP2(const VectorXd& x) +{ + return 100 * sq(x(1) - sq(x(0))) + sq(1 - x(0)); +} +VectorXd g_TP2(const VectorXd& x) +{ + VectorXd out(1); + out(0) = -1.5 - x(1); + return out; +} +double f_TP3(const VectorXd& x) +{ + return (x(1) + 1e-5 * sq(x(1) - x(0))); +} +VectorXd g_TP3(const VectorXd& x) +{ + VectorXd out(1); + out(0) = 0 - x(1); + return out; +} +double f_TP6(const VectorXd& x) +{ + return sq(1 - x(0)); +} +VectorXd g_TP6(const VectorXd& x) +{ + VectorXd out(1); + out(0) = 10 * (x(1) - sq(x(0))); + return out; +} +double f_TP7(const VectorXd& x) +{ + return log(1 + sq(x(0))) - x(1); +} +VectorXd g_TP7(const VectorXd& x) +{ + VectorXd out(1); + out(0) = sq(1 + sq(x(0))) + sq(x(1)) - 4; + return out; +} + +TEST_P(SQP, TP1) +{ + testProblem(ScalarOfVector::construct(&f_TP1), VectorOfVector::construct(&g_TP1), INEQ, { -2, 1 }, { 1, 1 }, + GetParam()); +} +TEST_P(SQP, TP3) +{ + testProblem(ScalarOfVector::construct(&f_TP3), VectorOfVector::construct(&g_TP3), INEQ, { 10, 1 }, { 0, 0 }, + GetParam()); +} +TEST_P(SQP, TP6) +{ + testProblem(ScalarOfVector::construct(&f_TP6), VectorOfVector::construct(&g_TP6), EQ, { 10, 1 }, { 1, 1 }, + GetParam()); +} +TEST_P(SQP, TP7) +{ + testProblem(ScalarOfVector::construct(&f_TP7), VectorOfVector::construct(&g_TP7), EQ, { 2, 2 }, { 0., sqrtf(3.) }, + GetParam()); +} + +INSTANTIATE_TEST_CASE_P(AllSolvers, SQP, testing::ValuesIn(availableSolvers())); diff --git a/moveit_planners/trajopt/trajopt_sco/test/solver-interface-unit.cpp b/moveit_planners/trajopt/trajopt_sco/test/solver-interface-unit.cpp new file mode 100644 index 0000000000..6f118b1e3c --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/test/solver-interface-unit.cpp @@ -0,0 +1,246 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +namespace sco +{ +extern void simplify2(IntVec& inds, DblVec& vals); +} + +using namespace sco; + +class SolverInterface : public testing::TestWithParam +{ +protected: + SolverInterface() + { + } +}; + +TEST(SolverInterface, simplify2) +{ + IntVec indices = { 0, 1, 3 }; + DblVec values = { 1e-7, 1e3, 0., 0., 0. }; + simplify2(indices, values); + + EXPECT_EQ(indices.size(), 2); + EXPECT_EQ(values.size(), 2); + EXPECT_TRUE((indices == IntVec{ 0, 1 })); + EXPECT_TRUE((values == DblVec{ 1e-7, 1e3 })); +} + +TEST_P(SolverInterface, setup_problem) +{ + ModelPtr solver = createModel(GetParam()); + VarVector vars; + for (int i = 0; i < 3; ++i) + { + char namebuf[5]; + sprintf(namebuf, "v%i", i); + vars.push_back(solver->addVar(namebuf)); + } + solver->update(); + + AffExpr aff; + std::cout << aff << std::endl; + for (int i = 0; i < 3; ++i) + { + exprInc(aff, vars[i]); + solver->setVarBounds(vars[i], 0, 10); + } + aff.constant -= 3; + std::cout << aff << std::endl; + QuadExpr affsquared = exprSquare(aff); + solver->setObjective(affsquared); + solver->update(); + LOG_INFO("objective: %s", CSTR(affsquared)); + LOG_INFO("please manually check that /tmp/solver-interface-test.lp matches this"); + solver->writeToFile("/tmp/solver-interface-test.lp"); + + solver->optimize(); + + DblVec soln(3); + for (int i = 0; i < 3; ++i) + { + soln[i] = solver->getVarValue(vars[i]); + } + EXPECT_NEAR(aff.value(soln), 0, 1e-6); + + solver->removeVar(vars[2]); + solver->update(); + EXPECT_EQ(solver->getVars().size(), 2); +} + +// Tests multiplying larger terms +TEST_P(SolverInterface, DISABLED_ExprMult_test1) // QuadExpr not PSD +{ + ModelPtr solver = createModel(GetParam()); + VarVector vars; + for (int i = 0; i < 3; ++i) + { + char namebuf[5]; + sprintf(namebuf, "v%i", i); + vars.push_back(solver->addVar(namebuf)); + } + for (int i = 3; i < 6; ++i) + { + char namebuf[5]; + sprintf(namebuf, "v%i", i); + vars.push_back(solver->addVar(namebuf)); + } + solver->update(); + + AffExpr aff1; + std::cout << aff1 << std::endl; + for (int i = 0; i < 3; ++i) + { + exprInc(aff1, vars[i]); + solver->setVarBounds(vars[i], 0, 10); + } + aff1.constant -= 3; + std::cout << aff1 << std::endl; + + AffExpr aff2; + std::cout << aff2 << std::endl; + for (int i = 3; i < 6; ++i) + { + exprInc(aff2, vars[i]); + solver->setVarBounds(vars[i], 0, 10); + } + aff2.constant -= 5; + + std::cout << aff2 << std::endl; + QuadExpr aff12 = exprMult(aff1, aff2); + solver->setObjective(aff12); + solver->update(); + LOG_INFO("objective: %s", CSTR(aff12)); + LOG_INFO("please manually check that /tmp/solver-interface-test.lp matches this"); + solver->writeToFile("/tmp/solver-interface-test.lp"); + + solver->optimize(); + + DblVec soln(3); + for (int i = 0; i < 3; ++i) + { + soln[i] = solver->getVarValue(vars[i]); + } + EXPECT_NEAR(aff1.value(soln), 0, 1e-6); + + solver->removeVar(vars[2]); + solver->update(); + EXPECT_EQ(solver->getVars().size(), 5); +} + +// Tests multiplication with 2 variables: v1=10, v2=20 => (2*v1)(v2) = 400 +TEST_P(SolverInterface, ExprMult_test2) +{ + const double v1_val = 10; + const double v2_val = 20; + const double v1_coeff = 2; + const double v2_coeff = 1; + const double aff1_const = 0; + const double aff2_const = 0; + + ModelPtr solver = createModel(GetParam()); + VarVector vars; + vars.push_back(solver->addVar("v1")); + vars.push_back(solver->addVar("v2")); + + solver->update(); + + AffExpr aff1; + AffExpr aff2; + + exprInc(aff1, vars[0]); + solver->setVarBounds(vars[0], v1_val, v1_val); + aff1.constant = aff1_const; + aff1.coeffs[0] = v1_coeff; + + exprInc(aff2, vars[1]); + solver->setVarBounds(vars[1], v2_val, v2_val); + aff2.constant = aff2_const; + aff2.coeffs[0] = v2_coeff; + + std::cout << "aff1: " << aff1 << std::endl; + std::cout << "aff2: " << aff2 << std::endl; + QuadExpr aff12 = exprMult(aff1, aff2); + solver->setObjective(aff12); + solver->update(); + LOG_INFO("objective: %s", CSTR(aff12)); + LOG_INFO("please manually check that /tmp/solver-interface-test.lp matches this"); + solver->writeToFile("/tmp/solver-interface-test.lp"); + + solver->optimize(); + + DblVec soln(2); + for (int i = 0; i < 2; ++i) + { + soln[i] = solver->getVarValue(vars[i]); + std::cout << soln[i] << std::endl; + } + std::cout << "Result: " << aff12.value(soln) << std::endl; + double answer = (v1_coeff * v1_val + aff1_const) * (v2_coeff * v2_val + aff2_const); + EXPECT_NEAR(aff12.value(soln), answer, 1e-6); +} + +// Tests multiplication with a constant: v1=10, v2=20 => (3*v1-3)(2*v2-5) = 945 +TEST_P(SolverInterface, ExprMult_test3) +{ + const double v1_val = 10; + const double v2_val = 20; + const double v1_coeff = 3; + const double v2_coeff = 2; + const double aff1_const = -3; + const double aff2_const = -5; + + ModelPtr solver = createModel(GetParam()); + VarVector vars; + vars.push_back(solver->addVar("v1")); + vars.push_back(solver->addVar("v2")); + + solver->update(); + + AffExpr aff1; + AffExpr aff2; + + exprInc(aff1, vars[0]); + solver->setVarBounds(vars[0], v1_val, v1_val); + aff1.constant = aff1_const; + aff1.coeffs[0] = v1_coeff; + + exprInc(aff2, vars[1]); + solver->setVarBounds(vars[1], v2_val, v2_val); + aff2.constant = aff2_const; + aff2.coeffs[0] = v2_coeff; + + std::cout << "aff1: " << aff1 << std::endl; + std::cout << "aff2: " << aff2 << std::endl; + QuadExpr aff12 = exprMult(aff1, aff2); + solver->setObjective(aff12); + solver->update(); + LOG_INFO("objective: %s", CSTR(aff12)); + LOG_INFO("please manually check that /tmp/solver-interface-test.lp matches this"); + solver->writeToFile("/tmp/solver-interface-test.lp"); + + solver->optimize(); + + DblVec soln(2); + for (int i = 0; i < 2; ++i) + { + soln[i] = solver->getVarValue(vars[i]); + std::cout << soln[i] << std::endl; + } + std::cout << "Result: " << aff12.value(soln) << std::endl; + double answer = (v1_coeff * v1_val + aff1_const) * (v2_coeff * v2_val + aff2_const); + EXPECT_NEAR(aff12.value(soln), answer, 1e-6); +} + +INSTANTIATE_TEST_CASE_P(AllSolvers, SolverInterface, testing::ValuesIn(availableSolvers())); diff --git a/moveit_planners/trajopt/trajopt_sco/test/solver-utils-unit.cpp b/moveit_planners/trajopt/trajopt_sco/test/solver-utils-unit.cpp new file mode 100644 index 0000000000..084648dd37 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/test/solver-utils-unit.cpp @@ -0,0 +1,261 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include +#include +#include +#include +#include + +using namespace sco; + +TEST(solver_utils, exprToEigen) +{ + int n_vars = 2; + std::vector x_info; + VarVector x; + for (int i = 0; i < n_vars; ++i) + { + std::stringstream var_name; + var_name << "x_" << i; + VarRepPtr x_el(new VarRep(i, var_name.str(), nullptr)); + x_info.push_back(x_el); + x.push_back(Var(x_el.get())); + } + + // x_affine = [3, 2]*x + 1 + AffExpr x_affine; + x_affine.vars = x; + x_affine.coeffs = DblVec{ 3, 2 }; + x_affine.constant = 1; + + std::cout << "x_affine= " << x_affine << std::endl; + std::cout << "expecting A = [3, 2];" << std::endl << " u = [1]" << std::endl; + Eigen::MatrixXd m_A_expected(1, n_vars); + m_A_expected << 3, 2; + Eigen::VectorXd v_u_expected(1); + v_u_expected << -1; + + Eigen::SparseVector v_A; + exprToEigen(x_affine, v_A, n_vars); + ASSERT_EQ(v_A.size(), m_A_expected.cols()); + Eigen::VectorXd v_A_d = v_A; + Eigen::VectorXd m_A_r = m_A_expected.row(0); + EXPECT_TRUE(v_A_d.isApprox(m_A_r)) << "error converting x_affine" + << " to Eigen::SparseVector. " + << "v_A :" << std::endl + << v_A << std::endl; + + Eigen::SparseMatrix m_A; + Eigen::VectorXd v_u; + AffExprVector x_affine_vector(1, x_affine); + exprToEigen(x_affine_vector, m_A, v_u, n_vars); + ASSERT_EQ(v_u_expected.size(), v_u.size()); + EXPECT_TRUE(v_u_expected == v_u) << "v_u_expected != v_u" << std::endl << "v_u:" << std::endl << v_u << std::endl; + EXPECT_EQ(m_A.nonZeros(), 2) << "m_A.nonZeros() != 2" << std::endl; + ASSERT_EQ(m_A.rows(), m_A_expected.rows()); + ASSERT_EQ(m_A.cols(), m_A_expected.cols()); + EXPECT_TRUE(m_A.isApprox(m_A_expected)) << "error converting x_affine to " + << "Eigen::SparseMatrix. m_A :" << std::endl + << m_A << std::endl; + + QuadExpr x_squared = exprSquare(x_affine); + std::cout << "x_squared= " << x_squared << std::endl; + std::cout << "expecting Q = [9, 6;" << std::endl + << " 6, 4]" << std::endl + << " q = [6, 4]" << std::endl; + Eigen::MatrixXd m_Q_expected(2, 2); + m_Q_expected << 9, 6, 6, 4; + DblVec v_q_expected{ 6, 4 }; + + Eigen::SparseMatrix m_Q; + Eigen::VectorXd v_q; + exprToEigen(x_squared, m_Q, v_q, n_vars); + { + Eigen::Map v_q_e_eig(v_q_expected.data(), v_q_expected.size()); + ASSERT_EQ(v_q.size(), v_q_e_eig.size()); + EXPECT_TRUE(v_q_e_eig.isApprox(v_q)) << "v_q_expected != v_q" << std::endl + << "v_q:" << std::endl + << v_q << std::endl; + } + EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to " + << "Eigen::SparseMatrix. m_Q :" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q.nonZeros(), 4) << "m_Q.nonZeros() != 4" << std::endl; + + exprToEigen(x_squared, m_Q, v_q, n_vars, true); + { + Eigen::Map v_q_e_eig(v_q_expected.data(), v_q_expected.size()); + EXPECT_TRUE(v_q_e_eig.isApprox(v_q)) << "v_q_expected != v_q" << std::endl + << "v_q:" << std::endl + << v_q << std::endl; + } + EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to" + << " Eigen::SparseMatrix. m_Q :" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q.nonZeros(), 4) << "m_Q.nonZeros() != 4" << std::endl; + + x_affine.coeffs = DblVec{ 0, 2 }; + std::cout << "x_affine= " << x_affine << std::endl; + std::cout << "expecting A = [0, 2];" << std::endl; + x_squared = exprSquare(x_affine); + std::cout << "x_squared= " << x_squared << std::endl; + std::cout << "expecting Q = [0, 0;" << std::endl << " 0, 4]" << std::endl; + m_Q_expected.setZero(); + m_Q_expected << 0, 0, 0, 4; + exprToEigen(x_squared, m_Q, v_q, n_vars, false, false); + EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to " + << "Eigen::SparseMatrix. m_Q :" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q.nonZeros(), 1) << "m_Q.nonZeros() != 1" << std::endl; + + exprToEigen(x_squared, m_Q, v_q, n_vars, true, false); + EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to" + << " Eigen::SparseMatrix. m_Q :" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q.nonZeros(), 1) << "m_Q.nonZeros() != 1" << std::endl; + + exprToEigen(x_squared, m_Q, v_q, n_vars, false, true); + EXPECT_TRUE(m_Q.isApprox(m_Q_expected)) << "error converting x_squared to " + << "Eigen::SparseMatrix. m_Q :" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q.nonZeros(), 2) << "m_Q.nonZeros() != 2" << std::endl; + + exprToEigen(x_squared, m_Q, v_q, n_vars, true, true); + EXPECT_TRUE(m_Q.isApprox(2 * m_Q_expected)) << "error converting x_squared to" + << "Eigen::SparseMatrix. m_Q :" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q.nonZeros(), 2) << "m_Q.nonZeros() != 2" << std::endl; +} + +TEST(solver_utils, eigenToTriplets) +{ + Eigen::MatrixXd m_Q(2, 2); + m_Q << 9, 0, 6, 4; + Eigen::SparseMatrix m_Q_sparse_expected = m_Q.sparseView(); + IntVec m_Q_i, m_Q_j; + DblVec m_Q_ij; + eigenToTriplets(m_Q_sparse_expected, m_Q_i, m_Q_j, m_Q_ij); + Eigen::SparseMatrix m_Q_sparse(2, 2); + tripletsToEigen(m_Q_i, m_Q_j, m_Q_ij, m_Q_sparse); + EXPECT_TRUE(m_Q_sparse.isApprox(m_Q)) << "m_Q != m_Q_sparse when converting " + << "m_Q -> triplets -> m_Q_sparse. m_Q:" << std::endl + << m_Q << std::endl; + EXPECT_EQ(m_Q_sparse.nonZeros(), 3) << "m_Q.nonZeros() != 3" << std::endl; +} + +TEST(solver_utils, eigenToCSC) +{ + DblVec P; + IntVec rows_i; + IntVec cols_p; + { + /* + * M = [ 1, 2, 3, + * 1, 0, 9, + * 1, 8, 0] + */ + Eigen::MatrixXd M(3, 3); + M << 1, 2, 3, 1, 0, 9, 1, 8, 0; + Eigen::SparseMatrix Ms = M.sparseView(); + + eigenToCSC(Ms, rows_i, cols_p, P); + + EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()"; + EXPECT_TRUE((P == DblVec{ 1, 1, 1, 2, 8, 3, 9 })) << "bad P:\n" << CSTR(P); + EXPECT_TRUE((rows_i == IntVec{ 0, 1, 2, 0, 2, 0, 1 })) << "bad rows_i:\n" << CSTR(rows_i); + EXPECT_TRUE((cols_p == IntVec{ 0, 3, 5, 7 })) << "cols_p not in " + << "CRC form:\n" + << CSTR(cols_p); + } + { + /* + * M = [ 0, 2, 0, + * 7, 0, 0, + * 0, 0, 0] + */ + Eigen::SparseMatrix M(3, 3); + M.coeffRef(0, 1) = 2; + M.coeffRef(1, 0) = 7; + + eigenToCSC(M, rows_i, cols_p, P); + + EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()"; + EXPECT_TRUE((P == DblVec{ 7, 2 })) << "bad P:\n" << CSTR(P); + EXPECT_TRUE((rows_i == IntVec{ 1, 0 })) << "rows_i != data_j:\n" + << CSTR(rows_i) << " vs\n" + << CSTR((IntVec{ 1, 0 })); + EXPECT_TRUE((cols_p == IntVec{ 0, 1, 2, 2 })) << "cols_p not in " + << "CRC form:\n" + << CSTR(cols_p); + + std::vector rows_i_ll, cols_p_ll; + std::vector rows_i_ll_exp{ 1, 0 }; + std::vector cols_p_ll_exp{ 0, 1, 2, 2 }; + + eigenToCSC(M, rows_i_ll, cols_p_ll, P); + EXPECT_TRUE(rows_i_ll.size() == P.size()) << "rows_i_ll.size() != P.size()"; + EXPECT_TRUE((rows_i_ll == rows_i_ll_exp)); + EXPECT_TRUE((cols_p_ll == cols_p_ll_exp)); + + std::vector rows_i_ull, cols_p_ull; + std::vector rows_i_ull_exp{ 1, 0 }; + std::vector cols_p_ull_exp{ 0, 1, 2, 2 }; + eigenToCSC(M, rows_i_ull, cols_p_ull, P); + EXPECT_TRUE(rows_i_ull.size() == P.size()) << "rows_i_ll.size() != P.size()"; + EXPECT_TRUE((rows_i_ull == rows_i_ull_exp)); + EXPECT_TRUE((cols_p_ull == cols_p_ull_exp)); + } + { + /* + * M = [ 0, 0, 0, + * 0, 0, 0, + * 0, 6, 0] + */ + Eigen::SparseMatrix M(3, 3); + M.coeffRef(2, 1) = 6; + + eigenToCSC(M, rows_i, cols_p, P); + + EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()"; + EXPECT_TRUE((P == DblVec{ 6 })) << "bad P:\n" << CSTR(P); + EXPECT_TRUE((rows_i == IntVec{ 2 })) << "rows_i != data_j:\n" << CSTR(rows_i) << " vs\n" << CSTR((IntVec{ 1, 0 })); + EXPECT_TRUE((cols_p == IntVec{ 0, 0, 1, 1 })) << "cols_p not in " + << "CRC form:\n" + << CSTR(cols_p); + } +} + +TEST(solver_utils, eigenToCSC_upper_triangular) +{ + /* + * M = [ 1, 2, 0, + * 2, 4, 0, + * 0, 0, 9] + */ + Eigen::MatrixXd M(3, 3); + M << 1, 2, 0, 2, 4, 0, 0, 0, 9; + Eigen::SparseMatrix Ms = M.sparseView(); + + DblVec P; + IntVec rows_i; + IntVec cols_p; + eigenToCSC(Ms, rows_i, cols_p, P); + + EXPECT_TRUE(rows_i.size() == P.size()) << "rows_i.size() != P.size()"; + EXPECT_TRUE((P == DblVec{ 1, 2, 4, 9 })) << "bad P:\n" << CSTR(P); + EXPECT_TRUE((rows_i == IntVec{ 0, 0, 1, 2 })) << "rows_i != expected" + << ":\n" + << CSTR(rows_i) << " vs\n" + << CSTR((IntVec{ 0, 0, 1, 2 })); + EXPECT_TRUE((cols_p == IntVec{ 0, 1, 3, 4 })) << "cols_p not in " + << "CRC form:\n" + << CSTR(cols_p); +} diff --git a/moveit_planners/trajopt/trajopt_sco/test/unit.cpp b/moveit_planners/trajopt/trajopt_sco/test/unit.cpp new file mode 100644 index 0000000000..539179562b --- /dev/null +++ b/moveit_planners/trajopt/trajopt_sco/test/unit.cpp @@ -0,0 +1,10 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/trajopt/trajopt_utils/CMakeLists.txt b/moveit_planners/trajopt/trajopt_utils/CMakeLists.txt new file mode 100644 index 0000000000..c7beec9325 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 2.8.3) +project(trajopt_utils) + +add_compile_options(-std=c++11 -Wall -Wextra) + +include(ExternalProject) + +ExternalProject_Add(osqp + PREFIX osqp + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=osqp-install + # Github tar balls don't code in submodules, so need to + # download via git + GIT_REPOSITORY https://github.com/oxfordcontrol/osqp.git + GIT_TAG v0.5.0 # currently, not compatible with v0.6.0 +) + +find_package(catkin REQUIRED COMPONENTS) +find_package(Eigen3 REQUIRED) +find_package(Boost COMPONENTS system python thread program_options REQUIRED) + +set(UTILS_SOURCE_FILES + src/stl_to_string.cpp + src/clock.cpp + src/config.cpp + src/logging.cpp +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} +# CATKIN_DEPENDS roscpp + DEPENDS + EIGEN3 +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + SYSTEM ${EIGEN3_INCLUDE_DIRS} + SYSTEM ${Boost_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} ${UTILS_SOURCE_FILES}) +target_link_libraries(${PROJECT_NAME} ${Boost_PROGRAM_OPTIONS_LIBRARY}) +target_compile_options(${PROJECT_NAME} PRIVATE -Wsuggest-override -Wconversion -Wsign-conversion) + +# Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" + ) diff --git a/moveit_planners/trajopt/trajopt_utils/LICENSE b/moveit_planners/trajopt/trajopt_utils/LICENSE new file mode 100644 index 0000000000..9103917b0f --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/LICENSE @@ -0,0 +1,11 @@ +Copyright (c) 2013, John Schulman +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +http://opensource.org/licenses/BSD-2-Clause \ No newline at end of file diff --git a/moveit_planners/trajopt/trajopt_utils/README.md b/moveit_planners/trajopt/trajopt_utils/README.md new file mode 100644 index 0000000000..1c18708b1f --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/README.md @@ -0,0 +1,3 @@ +TODO(ommmid): Remove code duplication. +This package temporarily copied into moveit until the repo trajopt_ros does not depend on Tesseract +See ros-industrial-consortium/trajopt_ros#122 \ No newline at end of file diff --git a/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/basic_array.hpp b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/basic_array.hpp new file mode 100644 index 0000000000..e9e6413e62 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/basic_array.hpp @@ -0,0 +1,147 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +namespace util +{ +template +struct BasicArray +{ + int m_nRow; + int m_nCol; + std::vector m_data; + + BasicArray() : m_nRow(0), m_nCol(0) + { + } + BasicArray(int nRow, int nCol) : m_nRow(nRow), m_nCol(nCol) + { + m_data.resize(m_nRow * m_nCol); + } + BasicArray(int nRow, int nCol, const T* data) : m_nRow(nRow), m_nCol(nCol), m_data(data, data + nRow * nCol) + { + } + BasicArray(const BasicArray& x) : m_nRow(x.m_nRow), m_nCol(x.m_nCol), m_data(x.m_data) + { + } + void resize(int nRow, int nCol) + { + m_nRow = nRow; + m_nCol = nCol; + m_data.resize(static_cast(m_nRow * m_nCol)); + } + + int rows() const + { + return m_nRow; + } + int cols() const + { + return m_nCol; + } + int size() const + { + return m_data.size(); + } + BasicArray block(int startRow, int startCol, int nRow, int nCol) const + { + BasicArray out; + out.resize(nRow, nCol); + for (int iRow = 0; iRow < nRow; ++iRow) + { + for (int iCol = 0; iCol < nCol; ++iCol) + { + out(iRow, iCol) = at(iRow + startRow, iCol + startCol); + } + } + return out; + } + std::vector rblock(int startRow, int startCol, int nCol) const + { + std::vector out(static_cast(nCol)); + for (int iCol = 0; iCol < nCol; ++iCol) + { + out[static_cast(iCol)] = at(static_cast(startRow), static_cast(iCol + startCol)); + } + return out; + } + std::vector cblock(int startRow, int startCol, int nRow) const + { + std::vector out(nRow); + for (int iRow = 0; iRow < nRow; ++iRow) + { + out[iRow] = at(iRow + startRow, startCol); + } + return out; + } + BasicArray middleRows(int start, int n) + { + BasicArray out; + out.resize(n, m_nCol); + for (int i = start; i < start + n; ++i) + { + for (int j = 0; j < m_nCol; ++j) + { + out(i, j) = at(i, j); + } + } + return out; + } + BasicArray topRows(int n) + { + return middleRows(0, n); + } + BasicArray bottomRows(int n) + { + return middleRows(m_nRow - n, n); + } + const T& at(int row, int col) const + { + return m_data.at(static_cast(row * m_nCol + col)); + } + T& at(int row, int col) + { + return m_data.at(static_cast(row * m_nCol + col)); + } + const T& operator()(int row, int col) const + { + return m_data.at(static_cast(row * m_nCol + col)); + } + T& operator()(int row, int col) + { + return m_data.at(static_cast(row * m_nCol + col)); + } + std::vector col(int col) + { + std::vector out; + out.reserve(m_nRow); + for (int row = 0; row < m_nRow; row++) + out.push_back(at(row, col)); + return out; + } + + std::vector row(int row) + { + std::vector out; + out.reserve(static_cast(m_nCol)); + for (int col = 0; col < m_nCol; col++) + out.push_back(at(row, col)); + return out; + } + + std::vector flatten() + { + return m_data; + } + T* data() + { + return m_data.data(); + } + T* data() const + { + return m_data.data(); + } +}; +} // namespace util diff --git a/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/clock.hpp b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/clock.hpp new file mode 100644 index 0000000000..1fcf7c201f --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/clock.hpp @@ -0,0 +1,7 @@ +#pragma once + +namespace util +{ +void StartClock(); +double GetClock(); +} diff --git a/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/config.hpp b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/config.hpp new file mode 100644 index 0000000000..54b5a4a598 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/config.hpp @@ -0,0 +1,80 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include + +namespace util +{ +namespace po = boost::program_options; + +struct ParameterBase +{ + std::string m_name; + std::string m_desc; + ParameterBase(const std::string& name, const std::string& desc) : m_name(name), m_desc(desc) + { + } + virtual void addToBoost(po::options_description&) = 0; + virtual ~ParameterBase() + { + } +}; +typedef std::shared_ptr ParameterBasePtr; + +template +struct ParameterVec : ParameterBase +{ + std::vector* m_value; + ParameterVec(std::string name, std::vector* value, std::string desc) : ParameterBase(name, desc), m_value(value) + { + } + void addToBoost(po::options_description& od) + { + od.add_options()(m_name.c_str(), po::value(m_value)->default_value(*m_value, Str(*m_value))->multitoken(), + m_desc.c_str()); + } +}; + +template +struct Parameter : ParameterBase +{ + T* m_value; + Parameter(std::string name, T* value, std::string desc) : ParameterBase(name, desc), m_value(value) + { + } + void addToBoost(po::options_description& od) + { + od.add_options()(m_name.c_str(), po::value(m_value)->default_value(*m_value, Str(*m_value)), m_desc.c_str()); + } +}; + +struct Config +{ + std::vector params; + void add(ParameterBase* param) + { + params.push_back(ParameterBasePtr(param)); + } +}; + +struct CommandParser +{ + std::vector configs; + void addGroup(const Config& config) + { + configs.push_back(config); + } + CommandParser(const Config& config) + { + addGroup(config); + } + void read(int argc, char* argv[]); +}; +} diff --git a/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/eigen_conversions.hpp b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/eigen_conversions.hpp new file mode 100644 index 0000000000..c8b89ceb92 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/eigen_conversions.hpp @@ -0,0 +1,18 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +namespace util +{ +inline std::vector toDblVec(const Eigen::Matrix& x) +{ + return std::vector(x.data(), x.data() + x.size()); +} +inline Eigen::VectorXd toVectorXd(const std::vector& x) +{ + return Eigen::Map(x.data(), static_cast(x.size())); +} +} diff --git a/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/eigen_slicing.hpp b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/eigen_slicing.hpp new file mode 100644 index 0000000000..ac1e4fccdb --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/eigen_slicing.hpp @@ -0,0 +1,27 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +namespace util +{ +template +VectorT fancySlice(const VectorT& x, const std::vector& inds) +{ + VectorT out(inds.size()); + for (int i = 0; i < inds.size(); ++i) + out[i] = x[inds[i]]; + return out; +} + +template +std::vector flatnonzero(const VectorT& x) +{ + std::vector out; + for (int i = 0; i < x.size(); ++i) + if (x[i] != 0) + out.push_back(i); + return out; +} +} diff --git a/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/interpolation.hpp b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/interpolation.hpp new file mode 100644 index 0000000000..79d7f7550e --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/interpolation.hpp @@ -0,0 +1,48 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +namespace util +{ +template +Eigen::VectorXi searchsorted(const VectorT& x, const VectorT& y) +{ + // y(i-1) <= x(out(i)) < y(i) + int nX = x.size(); + int nY = y.size(); + + Eigen::VectorXi out(nX); + int iY = 0; + for (int iX = 0; iX < nX; iX++) + { + while (iY < nY && x[iX] > y[iY]) + iY++; + out(iX) = iY; + } + return out; +} + +template +MatrixT interp2d(const VectorT& xNew, const VectorT& xOld, const MatrixT& yOld) +{ + int nNew = xNew.size(); + int nOld = xOld.size(); + MatrixT yNew(nNew, yOld.cols()); + Eigen::VectorXi new2old = searchsorted(xNew, xOld); + for (int iNew = 0; iNew < nNew; iNew++) + { + int iOldAbove = new2old(iNew); + if (iOldAbove == 0) + yNew.row(iNew) = yOld.row(0); + else if (iOldAbove == nOld) + yNew.row(iNew) = yOld.row(nOld - 1); + else + { + double t = (xNew(iNew) - xOld(iOldAbove - 1)) / (xOld(iOldAbove) - xOld(iOldAbove - 1)); + yNew.row(iNew) = yOld.row(iOldAbove - 1) * (1 - t) + yOld.row(iOldAbove) * t; + } + } + return yNew; +} +} diff --git a/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/logging.hpp b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/logging.hpp new file mode 100644 index 0000000000..482c193d1a --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/logging.hpp @@ -0,0 +1,74 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +namespace util +{ +enum LogLevel +{ + LevelFatal = 0, + LevelError = 1, + LevelWarn = 2, + LevelInfo = 3, + LevelDebug = 4, + LevelTrace = 5 +}; + +extern LogLevel gLogLevel; +inline LogLevel GetLogLevel() +{ + return gLogLevel; +} +#define FATAL_PREFIX "\x1b[31m[FATAL] " +#define ERROR_PREFIX "\x1b[31m[ERROR] " +#define WARN_PREFIX "\x1b[33m[WARN] " +#define INFO_PREFIX "[INFO] " +#define DEBUG_PREFIX "\x1b[32m[DEBUG] " +#define TRACE_PREFIX "\x1b[34m[TRACE] " +#define LOG_SUFFIX "\x1b[0m\n" + +#define LOG_FATAL(msg, ...) \ + if (util::GetLogLevel() >= util::LevelFatal) \ + { \ + printf(FATAL_PREFIX); \ + printf(msg, ##__VA_ARGS__); \ + printf(LOG_SUFFIX); \ + } +#define LOG_ERROR(msg, ...) \ + if (util::GetLogLevel() >= util::LevelError) \ + { \ + printf(ERROR_PREFIX); \ + printf(msg, ##__VA_ARGS__); \ + printf(LOG_SUFFIX); \ + } +#define LOG_WARN(msg, ...) \ + if (util::GetLogLevel() >= util::LevelWarn) \ + { \ + printf(WARN_PREFIX); \ + printf(msg, ##__VA_ARGS__); \ + printf(LOG_SUFFIX); \ + } +#define LOG_INFO(msg, ...) \ + if (util::GetLogLevel() >= util::LevelInfo) \ + { \ + printf(INFO_PREFIX); \ + printf(msg, ##__VA_ARGS__); \ + printf(LOG_SUFFIX); \ + } +#define LOG_DEBUG(msg, ...) \ + if (util::GetLogLevel() >= util::LevelDebug) \ + { \ + printf(DEBUG_PREFIX); \ + printf(msg, ##__VA_ARGS__); \ + printf(LOG_SUFFIX); \ + } +#define LOG_TRACE(msg, ...) \ + if (util::GetLogLevel() >= util::LevelTrace) \ + { \ + printf(TRACE_PREFIX); \ + printf(msg, ##__VA_ARGS__); \ + printf(LOG_SUFFIX); \ + } +} diff --git a/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/macros.h b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/macros.h new file mode 100644 index 0000000000..47332a1735 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/macros.h @@ -0,0 +1,71 @@ +#pragma once +#include + +#define TRAJOPT_IGNORE_WARNINGS_PUSH \ + _Pragma("GCC diagnostic push") _Pragma("GCC diagnostic ignored \"-Wall\"") \ + _Pragma("GCC diagnostic ignored \"-Wint-to-pointer-cast\"") \ + _Pragma("GCC diagnostic ignored \"-Wunused-parameter\"") \ + _Pragma("GCC diagnostic ignored \"-Wsuggest-override\"") \ + _Pragma("GCC diagnostic ignored \"-Wconversion\"") \ + _Pragma("GCC diagnostic ignored \"-Wfloat-conversion\"") \ + _Pragma("GCC diagnostic ignored \"-Wsign-conversion\"") + +#define TRAJOPT_IGNORE_WARNINGS_POP _Pragma("GCC diagnostic pop") + +// Generic helper definitions for shared library support +#if defined _WIN32 || defined __CYGWIN__ +#define TRAJOPT_HELPER_DLL_IMPORT __declspec(dllimport) +#define TRAJOPT_HELPER_DLL_EXPORT __declspec(dllexport) +#define TRAJOPT_HELPER_DLL_LOCAL +#else +#if __GNUC__ >= 4 +#define TRAJOPT_HELPER_DLL_IMPORT __attribute__((visibility("default"))) +#define TRAJOPT_HELPER_DLL_EXPORT __attribute__((visibility("default"))) +#define TRAJOPT_HELPER_DLL_LOCAL __attribute__((visibility("hidden"))) +#else +#define TRAJOPT_HELPER_DLL_IMPORT +#define TRAJOPT_HELPER_DLL_EXPORT +#define TRAJOPT_HELPER_DLL_LOCAL +#endif +#endif + +// Now we use the generic helper definitions above to define TRAJOPT_API and TRAJOPT_LOCAL. +// TRAJOPT_API is used for the public API symbols. It either DLL imports or DLL exports (or does nothing for static +// build) +// TRAJOPT_LOCAL is used for non-api symbols. + +#define TRAJOPT_DLL + +#ifdef TRAJOPT_DLL // defined if TRAJOPT is compiled as a DLL +#ifdef TRAJOPT_DLL_EXPORTS // defined if we are building the TRAJOPT DLL (instead of using it) +#define TRAJOPT_API TRAJOPT_HELPER_DLL_EXPORT +#else +#define TRAJOPT_API TRAJOPT_HELPER_DLL_IMPORT +#endif // TRAJOPT_DLL_EXPORTS +#define TRAJOPT_LOCAL TRAJOPT_HELPER_DLL_LOCAL +#else // TRAJOPT_DLL is not defined: this means TRAJOPT is a static lib. +#define TRAJOPT_API +#define TRAJOPT_LOCAL +#endif // TRAJOPT_DLL + +#define PRINT_AND_THROW(s) \ + do \ + { \ + std::cerr << "\033[1;31mERROR " << s << "\033[0m\n"; \ + std::cerr << "at " << __FILE__ << ":" << __LINE__ << std::endl; \ + std::stringstream ss; \ + ss << s; \ + throw std::runtime_error(ss.str()); \ + } while (0) +#define FAIL_IF_FALSE(expr) \ + if (!(expr)) \ + { \ + PRINT_AND_THROW("expected true: " #expr); \ + } + +#define ALWAYS_ASSERT(exp) \ + if (!(exp)) \ + { \ + printf("%s failed in file %s at line %i\n", #exp, __FILE__, __LINE__); \ + abort(); \ + } diff --git a/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/math.hpp b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/math.hpp new file mode 100644 index 0000000000..1c71ef8f1c --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/math.hpp @@ -0,0 +1,13 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +namespace util +{ +float randf() +{ + return (float)rand() / (float)RAND_MAX; +} +} diff --git a/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/stl_to_string.hpp b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/stl_to_string.hpp new file mode 100644 index 0000000000..e6bbe304fa --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/stl_to_string.hpp @@ -0,0 +1,73 @@ +#pragma once +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +namespace util +{ +// std::string Str(const vector& x); +// std::string Str(const vector& x); +// std::string Str(const vector& x); + +template +std::string Str(const std::vector& x) +{ + std::stringstream ss; + ss << "("; + if (x.size() > 0) + ss << x[0]; + for (size_t i = 1; i < x.size(); ++i) + ss << ", " << x[i]; + ss << ")"; + return ss.str(); +} + +template +std::string Str(const std::set& x) +{ + std::stringstream ss; + ss << "{"; + typename std::set::const_iterator it = x.begin(); + if (x.size() > 0) + { + ss << *it; + ++it; + for (; it != x.end(); ++it) + ss << ", " << *it; + } + ss << "}"; + return ss.str(); +} + +template +std::string Str(const T& x) +{ + std::stringstream ss; + ss << x; + return ss.str(); +} +#define CSTR(x) util::Str(x).c_str() + +template +std::string Str(const typename std::map& x) +{ + std::stringstream ss; + ss << "{"; + typename std::map::const_iterator it = x.begin(); + if (x.size() > 0) + { + ss << Str(it->first) << " : " << Str(it->second); + ++it; + for (; it != x.end(); ++it) + ss << ", " << Str(it->first) << " : " << Str(it->second); + } + ss << "}"; + return ss.str(); +} +} diff --git a/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/vector_ops.hpp b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/vector_ops.hpp new file mode 100644 index 0000000000..5a17476b3b --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/include/trajopt_utils/vector_ops.hpp @@ -0,0 +1,20 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + +namespace util +{ +std::vector arange(int n) +{ + std::vector out(static_cast(n)); + for (int i = 0; i < n; ++i) + out[static_cast(i)] = i; + return out; +} + +inline bool doubleEquals(double x, double y, double eps = 1E-5) +{ + return std::abs(x - y) < eps; +} +} diff --git a/moveit_planners/trajopt/trajopt_utils/package.xml b/moveit_planners/trajopt/trajopt_utils/package.xml new file mode 100644 index 0000000000..0c38901c12 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/package.xml @@ -0,0 +1,16 @@ + + + trajopt_utils + 0.1.0 + The trajopt_utils package + Levi Armstrong + BSD + + catkin + + + + + + + diff --git a/moveit_planners/trajopt/trajopt_utils/src/clock.cpp b/moveit_planners/trajopt/trajopt_utils/src/clock.cpp new file mode 100644 index 0000000000..0ef0f05fc1 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/src/clock.cpp @@ -0,0 +1,40 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include + +namespace util +{ +static long unsigned int startTime = 0; + +/* + * Starts the clock! Call this once at the beginning of the program. + * Calling again will reset the clock to 0; but doing so is not + * thread-safe if other threads may be calling GetClock(); (which + * is thread-safe since it only reads values and calls thread-safe + * functions in the kernel). + */ +// time in units of seconds since some time in the past +void StartClock() +{ + // determine start time + struct timeval startTimeStruct; + gettimeofday(&startTimeStruct, nullptr); + startTime = static_cast(startTimeStruct.tv_sec * 1e6l + startTimeStruct.tv_usec); +} + +/* + * Returns the current time since the call to StartClock(); + */ +double GetClock() +{ + struct timeval startTimeStruct; + unsigned long int curTime; + gettimeofday(&startTimeStruct, nullptr); + curTime = static_cast(startTimeStruct.tv_sec * 1e6l + startTimeStruct.tv_usec); + return (1e-6) * static_cast(curTime - startTime); +} +} diff --git a/moveit_planners/trajopt/trajopt_utils/src/config.cpp b/moveit_planners/trajopt/trajopt_utils/src/config.cpp new file mode 100644 index 0000000000..07b687ff97 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/src/config.cpp @@ -0,0 +1,27 @@ +#include + +namespace util +{ +void CommandParser::read(int argc, char* argv[]) +{ + // create boost options_description based on variables, parser + po::options_description od; + od.add_options()("help,h", "produce help message"); + for (std::size_t i = 0; i < configs.size(); ++i) + { + for (std::size_t j = 0; j < configs[i].params.size(); ++j) + { + configs[i].params[j]->addToBoost(od); + } + } + po::variables_map vm; + po::store(po::command_line_parser(argc, argv).options(od).run(), vm); + if (vm.count("help")) + { + std::cout << "usage: " << argv[0] << " [options]" << std::endl; + std::cout << od << std::endl; + exit(0); + } + po::notify(vm); +} +} diff --git a/moveit_planners/trajopt/trajopt_utils/src/logging.cpp b/moveit_planners/trajopt/trajopt_utils/src/logging.cpp new file mode 100644 index 0000000000..f8510478ac --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/src/logging.cpp @@ -0,0 +1,54 @@ +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include + +namespace util +{ +LogLevel gLogLevel; + +int LoggingInit() +{ + const char* VALID_THRESH_VALUES = "FATAL ERROR WARN INFO DEBUG TRACE"; + + char* lvlc = getenv("TRAJOPT_LOG_THRESH"); + std::string lvlstr; + if (lvlc == nullptr) + { + std::printf("You can set logging level with TRAJOPT_LOG_THRESH. Valid values: " + "%s. Defaulting to ERROR\n", + VALID_THRESH_VALUES); +#ifdef NDEBUG + lvlstr = "ERROR"; +#else + lvlstr = "DEBUG"; +#endif + } + else + lvlstr = std::string(lvlc); + if (lvlstr == "FATAL") + gLogLevel = LevelFatal; + else if (lvlstr == "ERROR") + gLogLevel = LevelError; + else if (lvlstr == "WARN") + gLogLevel = LevelWarn; + else if (lvlstr == "INFO") + gLogLevel = LevelInfo; + else if (lvlstr == "DEBUG") + gLogLevel = LevelDebug; + else if (lvlstr == "TRACE") + gLogLevel = LevelTrace; + else + { + std::printf("Invalid value for environment variable TRAJOPT_LOG_THRESH: %s\n", lvlstr.c_str()); + std::printf("Valid values: %s\n", VALID_THRESH_VALUES); + abort(); + } + return 1; +} +int this_is_a_hack_but_rhs_executes_on_library_load = LoggingInit(); +} diff --git a/moveit_planners/trajopt/trajopt_utils/src/stl_to_string.cpp b/moveit_planners/trajopt/trajopt_utils/src/stl_to_string.cpp new file mode 100644 index 0000000000..ab8e0b9c2e --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/src/stl_to_string.cpp @@ -0,0 +1,33 @@ +#include + +namespace +{ +template +std::string Str_impl(const std::vector& x) +{ + std::stringstream ss; + ss << "("; + if (x.size() > 0) + ss << x[0]; + for (size_t i = 1; i < x.size(); ++i) + ss << ", " << x[i]; + ss << ")"; + return ss.str(); +} +} + +namespace util +{ +std::string Str(const std::vector& x) +{ + return Str_impl(x); +} +std::string Str(const std::vector& x) +{ + return Str_impl(x); +} +std::string Str(const std::vector& x) +{ + return Str_impl(x); +} +} diff --git a/moveit_planners/trajopt/trajopt_utils/src/test_logging.cpp b/moveit_planners/trajopt/trajopt_utils/src/test_logging.cpp new file mode 100644 index 0000000000..a991c81bf0 --- /dev/null +++ b/moveit_planners/trajopt/trajopt_utils/src/test_logging.cpp @@ -0,0 +1,12 @@ +#include + +int main() +{ + LOG_FATAL("fatal"); + LOG_ERROR("error"); + LOG_WARN("warn"); + LOG_INFO("info"); + LOG_DEBUG("debug"); + LOG_TRACE("trace"); + printf("hi\n"); +} diff --git a/moveit_plugins/README.md b/moveit_plugins/README.md index 62065de1f1..b6400b3a55 100644 --- a/moveit_plugins/README.md +++ b/moveit_plugins/README.md @@ -1,4 +1,4 @@ -# MoveIt! Plugins +# MoveIt Plugins This is a collection of plugins for MoveIt: - moveit_simple_controller_manager - A very basic controller manager plugin. Can connect to FollowJointTrajectoryAction and GripperCommandAction servers. diff --git a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst index 887619d6ee..5efd4dcc8e 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst @@ -2,6 +2,40 @@ Changelog for package moveit_fake_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + +1.1.0 (2020-09-04) +------------------ + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [fix] Handle "default" parameter in MoveitControllerManagers + MoveIt{Fake|Simple}ControllerManager::getControllerState() now correctly returns current state +* [fix] Handle incomplete group states +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Robert Haschke, Sean Yen, Yu, Yan, Luca Lach + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -63,7 +97,7 @@ Changelog for package moveit_fake_controller_manager 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt index 6408e9062b..5ffd372631 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_fake_controller_manager) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -15,7 +19,7 @@ find_package(catkin COMPONENTS roscpp REQUIRED) -include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) catkin_package( LIBRARIES @@ -23,6 +27,7 @@ catkin_package( CATKIN_DEPENDS moveit_core moveit_ros_planning + roscpp ) add_library(${PROJECT_NAME} @@ -31,7 +36,10 @@ add_library(${PROJECT_NAME} set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(FILES moveit_fake_controller_manager_plugin_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/moveit_plugins/moveit_fake_controller_manager/README.md b/moveit_plugins/moveit_fake_controller_manager/README.md index 102a296a23..cacbba6f9a 100644 --- a/moveit_plugins/moveit_fake_controller_manager/README.md +++ b/moveit_plugins/moveit_fake_controller_manager/README.md @@ -1,6 +1,6 @@ -# MoveIt! Fake Controller Manager +# MoveIt Fake Controller Manager -This package implements a series of fake trajectory controllers for MoveIt! -- to be used in simulation. +This package implements a series of fake trajectory controllers for MoveIt -- to be used in simulation. For example, the `demo.launch` generated by MoveIt's `setup_assistant`, employs fake controllers for nice visualization in `rviz`. For configuration, edit the file `config/fake_controllers.yaml`, and adjust the desired controller type. diff --git a/moveit_plugins/moveit_fake_controller_manager/package.xml b/moveit_plugins/moveit_fake_controller_manager/package.xml index 05c2662a48..fc7d207bc7 100644 --- a/moveit_plugins/moveit_fake_controller_manager/package.xml +++ b/moveit_plugins/moveit_fake_controller_manager/package.xml @@ -1,12 +1,12 @@ moveit_fake_controller_manager - 1.0.1 + 1.1.1 A fake controller manager plugin for MoveIt. Ioan Sucan Michael GΓΆrner - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp index baa51f4f16..9dc5217946 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controller_manager.cpp @@ -42,7 +42,6 @@ #include #include #include -#include namespace moveit_fake_controller_manager { @@ -96,8 +95,8 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont if (controller_list[i]["joints"].getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_ERROR_STREAM_NAMED("MoveItFakeControllerManager", "The list of joints for controller " - << name << " is not specified as an array"); + ROS_ERROR_STREAM_NAMED("MoveItFakeControllerManager", + "The list of joints for controller " << name << " is not specified as an array"); continue; } std::vector joints; @@ -115,6 +114,12 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont controllers_[name].reset(new InterpolatingController(name, joints, pub_)); else ROS_ERROR_STREAM("Unknown fake controller type: " << type); + + moveit_controller_manager::MoveItControllerManager::ControllerState state; + state.default_ = controller_list[i].hasMember("default") ? (bool)controller_list[i]["default"] : false; + state.active_ = true; + + controller_states_[name] = state; } catch (...) { @@ -135,10 +140,12 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont } robot_model_loader::RobotModelLoader robot_model_loader(ROBOT_DESCRIPTION); - const robot_model::RobotModelPtr& robot_model = robot_model_loader.getModel(); + const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel(); + moveit::core::RobotState robot_state(robot_model); typedef std::map JointPoseMap; JointPoseMap joints; + robot_state.setToDefaultValues(); // initialize all joint values (just in case...) for (int i = 0, end = param.size(); i != end; ++i) { try @@ -151,7 +158,6 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont continue; } moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group_name); - moveit::core::RobotState robot_state(robot_model); const std::vector& joint_names = jmg->getActiveJointModelNames(); if (!robot_state.setToDefaultValues(jmg, pose_name)) @@ -188,10 +194,10 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont } // fill the joint state - for (JointPoseMap::const_iterator it = joints.begin(), end = joints.end(); it != end; ++it) + for (const auto& name_pos_pair : joints) { - js.name.push_back(it->first); - js.position.push_back(it->second); + js.name.push_back(name_pos_pair.first); + js.position.push_back(name_pos_pair.second); } return js; } @@ -257,20 +263,15 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont } } - /* - * Controllers are all active and default. - */ moveit_controller_manager::MoveItControllerManager::ControllerState getControllerState(const std::string& name) override { - moveit_controller_manager::MoveItControllerManager::ControllerState state; - state.active_ = true; - state.default_ = true; - return state; + return controller_states_[name]; } /* Cannot switch our controllers */ - bool switchControllers(const std::vector& activate, const std::vector& deactivate) override + bool switchControllers(const std::vector& /*activate*/, + const std::vector& /*deactivate*/) override { return false; } @@ -279,6 +280,7 @@ class MoveItFakeControllerManager : public moveit_controller_manager::MoveItCont ros::NodeHandle node_handle_; ros::Publisher pub_; std::map controllers_; + std::map controller_states_; }; } // end namespace moveit_fake_controller_manager diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h index 89bf9f6f81..24e08d9712 100644 --- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h +++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h @@ -46,7 +46,7 @@ namespace moveit_fake_controller_manager { -MOVEIT_CLASS_FORWARD(BaseFakeController); +MOVEIT_CLASS_FORWARD(BaseFakeController); // Defines BaseFakeControllerPtr, ConstPtr, WeakPtr... etc // common base class to all fake controllers in this package class BaseFakeController : public moveit_controller_manager::MoveItControllerHandle @@ -70,7 +70,7 @@ class LastPointController : public BaseFakeController bool sendTrajectory(const moveit_msgs::RobotTrajectory& t) override; bool cancelExecution() override; - bool waitForExecution(const ros::Duration&) override; + bool waitForExecution(const ros::Duration& /*timeout*/) override; }; class ThreadedController : public BaseFakeController @@ -81,7 +81,7 @@ class ThreadedController : public BaseFakeController bool sendTrajectory(const moveit_msgs::RobotTrajectory& t) override; bool cancelExecution() override; - bool waitForExecution(const ros::Duration&) override; + bool waitForExecution(const ros::Duration& /*timeout*/) override; moveit_controller_manager::ExecutionStatus getLastExecutionStatus() override; protected: @@ -122,6 +122,6 @@ class InterpolatingController : public ThreadedController private: ros::WallRate rate_; }; -} +} // namespace moveit_fake_controller_manager #endif diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst index 4905296463..1240cc9447 100644 --- a/moveit_plugins/moveit_plugins/CHANGELOG.rst +++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst @@ -2,6 +2,31 @@ Changelog for package moveit_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + +1.1.0 (2020-09-04) +------------------ +* [feature] Use standard cmake text for metapackages (`#1620 `_) +* [feature] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Jonathan Binney, Robert Haschke, Sean Yen, Tyler Weaver + +1.0.6 (2020-08-19) +------------------ + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ diff --git a/moveit_plugins/moveit_plugins/CMakeLists.txt b/moveit_plugins/moveit_plugins/CMakeLists.txt index 03c972ba5f..9afdc1d9d8 100644 --- a/moveit_plugins/moveit_plugins/CMakeLists.txt +++ b/moveit_plugins/moveit_plugins/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_plugins) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml index 265fa36725..b4c96441b0 100644 --- a/moveit_plugins/moveit_plugins/package.xml +++ b/moveit_plugins/moveit_plugins/package.xml @@ -1,13 +1,13 @@ moveit_plugins - 1.0.1 - Metapackage for MoveIt! plugins. + 1.1.1 + Metapackage for MoveIt plugins. Michael Ferguson Ioan Sucan Michael GΓΆrner - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst index d6312849f9..3be2a43d9d 100644 --- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst +++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst @@ -2,6 +2,43 @@ Changelog for package moveit_ros_control_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + +1.1.0 (2020-09-04) +------------------ +* [feature] Optional cpp version setting (`#2166 `_) +* [feature] Remove support for Indigo's ros_control (`#2128 `_) +* [feature] Add support for pos_vel_controllers and pos_vel_acc_controllers (`#1806 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [maint] clang-tidy-fix `modernize-loop-convert` to entire code base (`#1419 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Henning Kayser, Jonathan Binney, Robert Haschke, Sandro MagalhΓ£es, Sean Yen, Tyler Weaver, Yu, Yan + +1.0.6 (2020-08-19) +------------------ + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [feature] Add support for pos_vel_controllers and pos_vel_acc_controllers (`#1806 `_) +* Contributors: Robert Haschke, Sandro MagalhΓ£es, Sean Yen + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt index e460655d1c..8408cc163a 100644 --- a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt +++ b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_control_interface) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) find_package(catkin REQUIRED COMPONENTS actionlib @@ -20,18 +24,15 @@ catkin_package( ${PROJECT_NAME}_plugin ${PROJECT_NAME}_trajectory_plugin CATKIN_DEPENDS - moveit_core + actionlib controller_manager_msgs + moveit_core trajectory_msgs DEPENDS Boost ) -# support indigo's ros_control - This can be removed upon EOL indigo -if("${controller_manager_msgs_VERSION}" VERSION_LESS "0.10.0") - add_definitions(-DMOVEIT_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) -endif() - -include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) add_library(${PROJECT_NAME}_plugin src/controller_manager_plugin.cpp @@ -68,7 +69,7 @@ target_link_libraries(${PROJECT_NAME}_trajectory_plugin install(TARGETS ${PROJECT_NAME}_plugin ${PROJECT_NAME}_trajectory_plugin ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) ## Mark cpp header files for installation diff --git a/moveit_plugins/moveit_ros_control_interface/README.md b/moveit_plugins/moveit_ros_control_interface/README.md index d13be1b5a2..355006d366 100644 --- a/moveit_plugins/moveit_ros_control_interface/README.md +++ b/moveit_plugins/moveit_ros_control_interface/README.md @@ -1,4 +1,4 @@ -# MoveIt! ROS Control Plugin +# MoveIt ROS Control Plugin This package provides plugins of base class `moveit_controller_manager::MoveItControllerManager` and a new plugin base class for `moveit_controller_manager::MoveItControllerHandle` allocators. The allocator class is necessary because `moveit_controller_manager::MoveItControllerHandle` needs a name passed to the constructor. @@ -7,7 +7,7 @@ Two variantes are provided, `moveit_ros_control_interface::MoveItControllerManag ## moveit_ros_control_interface::MoveItControllerManager This plugin interfaces a single ros_control-driven node in the namespace given in the `~ros_control_namespace` ROS parameter. -It polls all controllers via the `list_controllers` service and passes their properties to MoveIt!. +It polls all controllers via the `list_controllers` service and passes their properties to MoveIt. The polling is throttled to 1 Hertz. ### Handle plugins @@ -17,7 +17,7 @@ These plugins should be registered with lookup names that match the correspondin Currently plugins for `position_controllers/JointTrajectoryController`, `velocity_controllers/JointTrajectoryController` and `effort_controllers/JointTrajectoryController` are available, which simply wrap `moveit_simple_controller_manager::FollowJointTrajectoryControllerHandle` instances. ### Setup -In your MoveIt! launch file (e.g. `ROBOT_moveit_config/launch/ROBOT_moveit_controller_manager.launch.xml`) set the `moveit_controller_manager` parameter: +In your MoveIt launch file (e.g. `ROBOT_moveit_config/launch/ROBOT_moveit_controller_manager.launch.xml`) set the `moveit_controller_manager` parameter: ``` ``` @@ -42,8 +42,8 @@ controller_list: ``` ### Controller switching -MoveIt! can decide which controllers have to be started and stopped. -Since only controller names with registered allocator plugins are handed over to MoveIt!, this implementation takes care of stopping other conflicting controllers based on their claimed resources and the resources for the to-be-started controllers. +MoveIt can decide which controllers have to be started and stopped. +Since only controller names with registered allocator plugins are handed over to MoveIt, this implementation takes care of stopping other conflicting controllers based on their claimed resources and the resources for the to-be-started controllers. ### Namespaces All controller names get prefixed by the namespace of the ros_control node. @@ -56,7 +56,7 @@ It spawns `moveit_ros_control_interface::MoveItControllerManager` instances with ### Setup -Just set the `moveit_controller_manager` parameter in your MoveIt! launch file (e.g. `ROBOT_moveit_config/launch/ROBOT_moveit_controller_manager.launch.xml`) +Just set the `moveit_controller_manager` parameter in your MoveIt launch file (e.g. `ROBOT_moveit_config/launch/ROBOT_moveit_controller_manager.launch.xml`) ``` ``` diff --git a/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.h b/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.h index c9527dfacd..6bc78f7aac 100644 --- a/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.h +++ b/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.h @@ -34,15 +34,14 @@ /* Author: Mathias LΓΌdtke */ -#ifndef MOVEIT_ROS_CONTROL_INTERFACE_CONTROLLER_HANDLE_H -#define MOVEIT_ROS_CONTROL_INTERFACE_CONTROLLER_HANDLE_H +#pragma once #include #include namespace moveit_ros_control_interface { -MOVEIT_CLASS_FORWARD(ControllerHandleAllocator); +MOVEIT_CLASS_FORWARD(ControllerHandleAllocator); // Defines ControllerHandleAllocatorPtr, ConstPtr, WeakPtr... etc /** * Base class for MoveItControllerHandle allocators @@ -58,5 +57,3 @@ class ControllerHandleAllocator }; } // namespace moveit_ros_control_interface - -#endif // MOVEIT_ROS_CONTROL_INTERFACE_CONTROLLER_HANDLE_H diff --git a/moveit_plugins/moveit_ros_control_interface/moveit_core_plugins.xml b/moveit_plugins/moveit_ros_control_interface/moveit_core_plugins.xml index f7461f59d5..3bd7e971cc 100644 --- a/moveit_plugins/moveit_ros_control_interface/moveit_core_plugins.xml +++ b/moveit_plugins/moveit_ros_control_interface/moveit_core_plugins.xml @@ -1,10 +1,10 @@ - ros_control controller manager interface for MoveIt! + ros_control controller manager interface for MoveIt - multiple ros_control controller managers interface for MoveIt! + multiple ros_control controller managers interface for MoveIt diff --git a/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml b/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml index 2bbf76e67e..3bac259dd2 100644 --- a/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml +++ b/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml @@ -9,5 +9,11 @@ + + + + + + diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml index 98d0206d4b..ad88665726 100644 --- a/moveit_plugins/moveit_ros_control_interface/package.xml +++ b/moveit_plugins/moveit_ros_control_interface/package.xml @@ -1,13 +1,13 @@ moveit_ros_control_interface - 1.0.1 - ros_control controller manager interface for MoveIt! + 1.1.1 + ros_control controller manager interface for MoveIt Mathias LΓΌdtke Mathias LΓΌdtke - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp index 2a6e1ce8d8..c40ade2940 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp @@ -73,7 +73,7 @@ bool checkTimeout(ros::Time& t, double timeout, bool force = false) return false; } -MOVEIT_CLASS_FORWARD(MoveItControllerManager); +MOVEIT_CLASS_FORWARD(MoveItControllerManager); // Defines MoveItControllerManagerPtr, ConstPtr, WeakPtr... etc /** * \brief moveit_controller_manager::MoveItControllerManager sub class that interfaces one ros_control @@ -158,9 +158,6 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll } std::vector resources; -#if defined(MOVEIT_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) - resources = controller.resources; -#else // Collect claimed resources across different hardware interfaces for (const controller_manager_msgs::HardwareInterfaceResources& claimed_resource : controller.claimed_resources) { @@ -169,7 +166,6 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll resources.push_back(resource); } } -#endif moveit_controller_manager::MoveItControllerHandlePtr handle = alloc_it->second->alloc(name, resources); // allocate handle @@ -268,15 +264,11 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll ControllersMap::iterator it = managed_controllers_.find(name); if (it != managed_controllers_.end()) { -#if defined(MOVEIT_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) - joints = it->second.resources; -#else for (controller_manager_msgs::HardwareInterfaceResources& claimed_resource : it->second.claimed_resources) { std::vector& resources = claimed_resource.resources; joints.insert(joints.end(), resources.begin(), resources.end()); } -#endif } } @@ -316,15 +308,8 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll resources_bimap claimed_resources; // fill bimap with active controllers and their resources - for (std::pair& active_controller : - active_controllers_) + for (std::pair& active_controller : active_controllers_) { -#if defined(MOVEIT_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) - for (std::vector::iterator r = c->second.resources.begin(); r != c->second.resources.end(); ++r) - { - claimed_resources.insert(resources_bimap::value_type(c->second.name, *r)); - } -#else for (std::vector::iterator hir = active_controller.second.claimed_resources.begin(); hir != active_controller.second.claimed_resources.end(); ++hir) @@ -334,7 +319,6 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll claimed_resources.insert(resources_bimap::value_type(active_controller.second.name, resource)); } } -#endif } controller_manager_msgs::SwitchController srv; @@ -355,17 +339,6 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll if (c != managed_controllers_.end()) { // controller belongs to this manager srv.request.start_controllers.push_back(c->second.name); -#if defined(MOVEIT_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) - for (std::vector::iterator r = c->second.resources.begin(); r != c->second.resources.end(); ++r) - { // for all claimed resource - resources_bimap::right_const_iterator res = claimed_resources.right.find(*r); - if (res != claimed_resources.right.end()) - { // resource is claimed - srv.request.stop_controllers.push_back(res->second); // add claiming controller to stop list - claimed_resources.left.erase(res->second); // remove claimed resources - } - } -#else for (controller_manager_msgs::HardwareInterfaceResources& claimed_resource : c->second.claimed_resources) { for (const std::string& resource : claimed_resource.resources) @@ -378,7 +351,6 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll } } } -#endif } } srv.request.strictness = srv.request.STRICT; diff --git a/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp index fbe5c1c224..9f8a048aec 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp @@ -49,7 +49,7 @@ class JointTrajectoryControllerAllocator : public ControllerHandleAllocator { public: moveit_controller_manager::MoveItControllerHandlePtr alloc(const std::string& name, - const std::vector& resources) override + const std::vector& /* resources */) override { return std::make_shared( name, "follow_joint_trajectory"); diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst index 1f699681e5..99577cd5c9 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst @@ -2,6 +2,51 @@ Changelog for package moveit_simple_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + +1.1.0 (2020-09-04) +------------------ +* [feature] Optional cpp version setting (`#2166 `_) +* [feature] Allow different controllers for execution `#1832 `_) +* [feature] ControllerManager: wait for done-callback (`#1783 `_) +* [feature] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [fix] Fix binary artifact install locations. (`#1575 `_) +* [fix] add missing space to log (`#1477 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#1419 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Henning Kayser, Jonathan Binney, Leroy RΓΌgemer, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan, llach + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* Contributors: Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [fix] Handle "default" parameter in MoveitControllerManagers + MoveIt{Fake|Simple}ControllerManager::getControllerState() now correctly returns current state +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] `ControllerManager`: wait for done-callback (`#1783 `_) +* Contributors: Robert Haschke, Sean Yen, Luca Lach + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -67,7 +112,7 @@ Changelog for package moveit_simple_controller_manager 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt index af29fd503f..b89705f3ac 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_simple_controller_manager) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -17,12 +21,19 @@ find_package(catkin COMPONENTS control_msgs REQUIRED) -include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) catkin_package( - LIBRARIES ${PROJECT_NAME} - INCLUDE_DIRS include - CATKIN_DEPENDS moveit_core actionlib control_msgs + LIBRARIES + ${PROJECT_NAME} + INCLUDE_DIRS + include + CATKIN_DEPENDS + actionlib + control_msgs + moveit_core + roscpp ) add_library(${PROJECT_NAME} @@ -32,7 +43,10 @@ add_library(${PROJECT_NAME} set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(FILES moveit_simple_controller_manager_plugin_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h index bfdee6d098..bc93e429da 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h @@ -35,8 +35,7 @@ /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ -#ifndef MOVEIT_PLUGINS_ACTION_BASED_CONTROLLER_HANDLE -#define MOVEIT_PLUGINS_ACTION_BASED_CONTROLLER_HANDLE +#pragma once #include #include @@ -57,12 +56,13 @@ class ActionBasedControllerHandleBase : public moveit_controller_manager::MoveIt virtual void addJoint(const std::string& name) = 0; virtual void getJoints(std::vector& joints) = 0; - virtual void configure(XmlRpc::XmlRpcValue& config) + virtual void configure(XmlRpc::XmlRpcValue& /* config */) { } }; -MOVEIT_CLASS_FORWARD(ActionBasedControllerHandleBase); +MOVEIT_CLASS_FORWARD( + ActionBasedControllerHandleBase); // Defines ActionBasedControllerHandleBasePtr, ConstPtr, WeakPtr... etc /* * This is a simple base class, which handles all of the action creation/etc @@ -127,6 +127,12 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase { if (controller_action_client_ && !done_) return controller_action_client_->waitForResult(timeout); +#if 1 // TODO: remove when https://github.com/ros/actionlib/issues/155 is fixed + // workaround for actionlib issue: waitForResult() might return before our doneCB finished + ros::Time deadline = ros::Time::now() + ros::Duration(0.1); // limit waiting to 0.1s + while (!done_ && ros::ok() && ros::Time::now() < deadline) // Check the done_ flag explicitly, + ros::Duration(0.0001).sleep(); // which is eventually set in finishControllerExecution +#endif return true; } @@ -147,7 +153,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase protected: ros::NodeHandle nh_; - std::string getActionName(void) const + std::string getActionName() const { if (namespace_.empty()) return name_; @@ -186,5 +192,3 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase }; } // end namespace moveit_simple_controller_manager - -#endif // MOVEIT_PLUGINS_ACTION_BASED_CONTROLLER_HANDLE diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h index dc01ccf011..c6bc9e0f74 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h @@ -35,8 +35,7 @@ /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ -#ifndef MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE -#define MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE +#pragma once #include #include @@ -48,7 +47,7 @@ namespace moveit_simple_controller_manager * or anything using a control_mgs/FollowJointTrajectoryAction. */ class FollowJointTrajectoryControllerHandle - : public ActionBasedControllerHandle + : public ActionBasedControllerHandle { public: FollowJointTrajectoryControllerHandle(const std::string& name, const std::string& action_ns) @@ -77,5 +76,3 @@ class FollowJointTrajectoryControllerHandle }; } // end namespace moveit_simple_controller_manager - -#endif // MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h index 8574dee376..aae275a2e2 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h @@ -35,8 +35,7 @@ /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ -#ifndef MOVEIT_PLUGINS_GRIPPER_CONTROLLER_HANDLE -#define MOVEIT_PLUGINS_GRIPPER_CONTROLLER_HANDLE +#pragma once #include #include @@ -102,7 +101,7 @@ class GripperControllerHandle : public ActionBasedControllerHandle moveit_simple_controller_manager - 1.0.1 + 1.1.1 A generic, simple controller manager plugin for MoveIt. Michael Ferguson Michael Ferguson Michael GΓΆrner - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp index fecb380504..31cf634780 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp @@ -179,8 +179,9 @@ void FollowJointTrajectoryControllerHandle::configure(XmlRpc::XmlRpcValue& confi ROS_WARN_STREAM_NAMED(LOGNAME, "Invalid " << config_name); } -control_msgs::JointTolerance& FollowJointTrajectoryControllerHandle::getTolerance( - std::vector& tolerances, const std::string& name) +control_msgs::JointTolerance& +FollowJointTrajectoryControllerHandle::getTolerance(std::vector& tolerances, + const std::string& name) { auto it = std::lower_bound(tolerances.begin(), tolerances.end(), name, @@ -214,7 +215,7 @@ void FollowJointTrajectoryControllerHandle::controllerActiveCallback() } void FollowJointTrajectoryControllerHandle::controllerFeedbackCallback( - const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback) + const control_msgs::FollowJointTrajectoryFeedbackConstPtr& /* feedback */) { } diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp index 2f15a4189a..67d600dd16 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp @@ -86,8 +86,8 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo if (!isArray(controller_list[i]["joints"])) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "The list of joints for controller " << name - << " is not specified as an array"); + ROS_ERROR_STREAM_NAMED(LOGNAME, + "The list of joints for controller " << name << " is not specified as an array"); continue; } @@ -113,8 +113,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo static_cast(new_handle.get()) ->setCommandJoint(controller_list[i]["command_joint"]); else - static_cast(new_handle.get()) - ->setCommandJoint(controller_list[i]["joints"][0]); + static_cast(new_handle.get())->setCommandJoint(controller_list[i]["joints"][0]); } if (controller_list[i].hasMember("allow_failure")) @@ -145,7 +144,13 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo continue; } - /* add list of joints, used by controller manager and MoveIt! */ + moveit_controller_manager::MoveItControllerManager::ControllerState state; + state.default_ = controller_list[i].hasMember("default") ? (bool)controller_list[i]["default"] : false; + state.active_ = true; + + controller_states_[name] = state; + + /* add list of joints, used by controller manager and MoveIt */ for (int j = 0; j < controller_list[i]["joints"].size(); ++j) new_handle->addJoint(std::string(controller_list[i]["joints"][j])); @@ -213,27 +218,23 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo } else { - ROS_WARN_NAMED(LOGNAME, "The joints for controller '%s' are not known. Perhaps the controller configuration is " - "not loaded on the param server?", + ROS_WARN_NAMED(LOGNAME, + "The joints for controller '%s' are not known. Perhaps the controller configuration is " + "not loaded on the param server?", name.c_str()); joints.clear(); } } - /* - * Controllers are all active and default -- that's what makes this thing simple. - */ moveit_controller_manager::MoveItControllerManager::ControllerState getControllerState(const std::string& name) override { - moveit_controller_manager::MoveItControllerManager::ControllerState state; - state.active_ = true; - state.default_ = true; - return state; + return controller_states_[name]; } /* Cannot switch our controllers */ - bool switchControllers(const std::vector& activate, const std::vector& deactivate) override + bool switchControllers(const std::vector& /* activate */, + const std::vector& /* deactivate */) override { return false; } @@ -241,6 +242,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo protected: ros::NodeHandle node_handle_; std::map controllers_; + std::map controller_states_; }; } // end namespace moveit_simple_controller_manager diff --git a/moveit_ros/README.md b/moveit_ros/README.md index ba995b41ef..5755639551 100644 --- a/moveit_ros/README.md +++ b/moveit_ros/README.md @@ -1,6 +1,6 @@ -#MoveIt! ROS +#MoveIt ROS -This repository includes components of MoveIt! that use ROS. This is where much of the functionality MoveIt! provides is put together. Libraries from MoveIt! Core and various plugins are used to provide that functionality. +This repository includes components of MoveIt that use ROS. This is where much of the functionality MoveIt provides is put together. Libraries from MoveIt Core and various plugins are used to provide that functionality. - planning - planning interfaces - benchmarking diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst index 98e4914f29..017ee36224 100644 --- a/moveit_ros/benchmarks/CHANGELOG.rst +++ b/moveit_ros/benchmarks/CHANGELOG.rst @@ -2,6 +2,65 @@ Changelog for package moveit_ros_benchmarks ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [fix] python3 issues (`#2323 `_) +* [maint] Cleanup MSA includes (`#2351 `_) +* Contributors: Michael GΓΆrner, Robert Haschke + +1.1.0 (2020-09-04) +------------------ +* [feature] Benchmark combinations of predefined poses (`#1548 `_) +* [feature] Support benchmarking of full planning pipelines (`#1531 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix plot details, correcting xlabels positions and cleaning the graph (`#1658 `_) (`#1668 `_) +* [maint] Optional cpp version setting (`#2166 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#2004 `_, `#1419 `_) +* [maint] Fix usage of panda_moveit_config (`#1904 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Fix compiler warnings (`#1773 `_) +* [maint] Do not install helper scripts in global bin destination (`#1704 `_) +* [maint] Cleanup launch + config files (`#1631 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Dave Coleman, Henning Kayser, Jonathan Binney, Mahmoud Ahmed Selim, Markus Vieth, Michael GΓΆrner, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10, Fix warnings +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [feature] MoveIt benchmark improvements (`#1510 `_) + * Add pseudo experiment all_experiments to allow comparing all entries + * Expose loadBenchmarkQueryData() for setting up custom queries + * Add benchmark entry for comparing the 'final' result trajectory + * Add trajectory similarity function to measure repeatability + * Address requested changes + * Fill empty fields in all_experiments + * Improve variable and function names + * Add helper function computeTrajectoryDistance() +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Windows build fixes + * Fix header inclusion and other MSVC build errors (`#1636 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Henning Kayser, Michael GΓΆrner, Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ +* [maintenance] Removed unnecessary null pointer checks on deletion (`#1410 `_) +* Contributors: Mahmoud Ahmed Selim + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -69,7 +128,7 @@ Changelog for package moveit_ros_benchmarks 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman @@ -155,13 +214,13 @@ Changelog for package moveit_ros_benchmarks as a symlink pointing to the versioned file. Because this sets each library's SONAME to the *full version*, this enforces that *every* binary links with the versioned library file from now on and - has to be relinked with *each* new release of MoveIt!. + has to be relinked with *each* new release of MoveIt. The alternative would be to set the SONAME to `$MAJOR.$MINOR` and ignore the patch version, but because we currently stay with one `$MAJOR.$MINOR` number within each ROS distribution, we had (and likely will have) ABI changes in the `$PATCH` version releases too. The reason for this commit is that it is practically impossible to maintain full ABI compatibility within each ROS distribution and still add the the features/patches the community asks for. - This has resulted in more than one ABI-incompatible MoveIt! release in the recent past + This has resulted in more than one ABI-incompatible MoveIt release in the recent past within a ROS distribution. Because the libraries have not been versioned up to now, there was no way to indicate the incompatible changes and users who did not rebuild their whole workspace with the new release encountered weird and hard-to-track segfaults diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt index d6ce5fad68..f64a99da75 100644 --- a/moveit_ros/benchmarks/CMakeLists.txt +++ b/moveit_ros/benchmarks/CMakeLists.txt @@ -1,9 +1,13 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_benchmarks) set(MOVEIT_LIB_NAME moveit_ros_benchmarks) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -30,7 +34,8 @@ catkin_package( INCLUDE_DIRS include ) -include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) add_library(${MOVEIT_LIB_NAME} src/BenchmarkOptions.cpp src/BenchmarkExecutor.cpp) @@ -39,16 +44,26 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_executable(moveit_run_benchmark src/RunBenchmark.cpp) target_link_libraries(moveit_run_benchmark ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_executable(moveit_combine_predefined_poses_benchmark src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp) +target_link_libraries(moveit_combine_predefined_poses_benchmark ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) install( TARGETS - ${MOVEIT_LIB_NAME} moveit_run_benchmark + ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +install( + TARGETS + moveit_run_benchmark + moveit_combine_predefined_poses_benchmark RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) -install(PROGRAMS scripts/moveit_benchmark_statistics.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +catkin_install_python( + PROGRAMS scripts/moveit_benchmark_statistics.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY examples/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples) diff --git a/moveit_ros/benchmarks/README.md b/moveit_ros/benchmarks/README.md index 775e391586..4823b68253 100644 --- a/moveit_ros/benchmarks/README.md +++ b/moveit_ros/benchmarks/README.md @@ -1,5 +1,5 @@ -# MoveIt! ROS Benchmarks +# MoveIt ROS Benchmarks This package provides methods to benchmark motion planning algorithms and aggregate/plot statistics. Results can be viewed in [Planner Arena](http://plannerarena.org/). -For more information and usage example please see [moveit tutorials](http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/benchmarking_tutorial.html). +For more information and usage example please see [moveit tutorials](https://ros-planning.github.io/moveit_tutorials/doc/benchmarking/benchmarking_tutorial.html). diff --git a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp index b9ca44eb74..43916a8f69 100644 --- a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp +++ b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp @@ -88,8 +88,8 @@ void checkHeader(moveit_msgs::Constraints& c, const std::string& header_frame) c.orientation_constraints[i].header.stamp = ros::Time::now(); } } -} -} +} // namespace +} // namespace moveit_benchmarks moveit_benchmarks::BenchmarkExecution::BenchmarkExecution(const planning_scene::PlanningScenePtr& scene, warehouse_ros::DatabaseConnection::Ptr conn) @@ -883,8 +883,8 @@ void moveit_benchmarks::BenchmarkExecution::collectMetrics(RunData& rundata, namespace { -bool isIKSolutionCollisionFree(const planning_scene::PlanningScene* scene, robot_state::RobotState* state, - const robot_model::JointModelGroup* group, const double* ik_solution, bool* reachable) +bool isIKSolutionCollisionFree(const planning_scene::PlanningScene* scene, moveit::core::RobotState* state, + const moveit::core::JointModelGroup* group, const double* ik_solution, bool* reachable) { state->setJointGroupPositions(group, ik_solution); state->update(); @@ -894,7 +894,7 @@ bool isIKSolutionCollisionFree(const planning_scene::PlanningScene* scene, robot else return true; } -} +} // namespace void moveit_benchmarks::BenchmarkExecution::runPlanningBenchmark(BenchmarkRequest& req) { @@ -1001,8 +1001,9 @@ void moveit_benchmarks::BenchmarkExecution::runPlanningBenchmark(BenchmarkReques ROS_ERROR("Planning interface '%s' has no planners defined", it->first.c_str()); } else - ROS_WARN_STREAM("Planning interface '" << it->second->getDescription() << "' is not able to solve the specified " - "benchmark problem."); + ROS_WARN_STREAM("Planning interface '" << it->second->getDescription() + << "' is not able to solve the specified " + "benchmark problem."); } // error check @@ -1247,8 +1248,7 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR bool reachable = false; if (req.motion_plan_request.goal_constraints.size() > 0 && req.motion_plan_request.goal_constraints[0].position_constraints.size() > 0 && - req.motion_plan_request.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses.size() > - 0 && + req.motion_plan_request.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses.size() > 0 && req.motion_plan_request.goal_constraints[0].orientation_constraints.size() > 0) { // Compute IK on goal constraints @@ -1270,16 +1270,17 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR ik_pose.orientation.z = req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation.z; ik_pose.orientation.w = req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation.w; - robot_state::RobotState robot_state(planning_scene_->getCurrentState()); - robot_state::robotStateMsgToRobotState(req.motion_plan_request.start_state, robot_state); + moveit::core::RobotState robot_state(planning_scene_->getCurrentState()); + moveit::core::robotStateMsgToRobotState(req.motion_plan_request.start_state, robot_state); // Compute IK ROS_INFO_STREAM("Processing goal " << req.motion_plan_request.goal_constraints[0].name << " ..."); ros::WallTime startTime = ros::WallTime::now(); - success = robot_state.setFromIK( - robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, - req.motion_plan_request.num_planning_attempts, req.motion_plan_request.allowed_planning_time, - boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable)); + success = + robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, + req.motion_plan_request.num_planning_attempts, + req.motion_plan_request.allowed_planning_time, + boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable)); if (success) { ROS_INFO(" Success!"); @@ -1358,17 +1359,18 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR ik_pose.orientation.w = req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation.w; - robot_state::RobotState robot_state(planning_scene_->getCurrentState()); - robot_state::robotStateMsgToRobotState(req.motion_plan_request.start_state, robot_state); + moveit::core::RobotState robot_state(planning_scene_->getCurrentState()); + moveit::core::robotStateMsgToRobotState(req.motion_plan_request.start_state, robot_state); // Compute IK ROS_INFO_STREAM("Processing trajectory waypoint " << req.motion_plan_request.trajectory_constraints.constraints[tc].name << " ..."); startTime = ros::WallTime::now(); - success = robot_state.setFromIK( - robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, - req.motion_plan_request.num_planning_attempts, req.motion_plan_request.allowed_planning_time, - boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable)); + success = + robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose, + req.motion_plan_request.num_planning_attempts, + req.motion_plan_request.allowed_planning_time, + boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable)); double duration = (ros::WallTime::now() - startTime).toSec(); if (success) diff --git a/moveit_ros/benchmarks/examples/demo1.yaml b/moveit_ros/benchmarks/examples/demo1.yaml index 92227c3814..d31a58594a 100644 --- a/moveit_ros/benchmarks/examples/demo1.yaml +++ b/moveit_ros/benchmarks/examples/demo1.yaml @@ -1,5 +1,5 @@ # This is an example configuration that loads the "Kitchen" scene from the -# local MoveIt! warehouse and benchmarks the "manipulator" group in the Pick1 +# local MoveIt warehouse and benchmarks the "panda_arm" group in the Pick1 # query using the Start1 initial state (all pre-stored in the local warehouse) # Three different planners from OMPL are run a total of 50 times each, with a @@ -18,8 +18,8 @@ benchmark_config: output_directory: /tmp/moveit_benchmarks/ queries: Pick1 start_states: Start1 - planners: - - plugin: ompl_interface/OMPLPlanner + planning_pipelines: + - name: ompl planners: - RRTConnectkConfigDefault - BKPIECEkConfigDefault diff --git a/moveit_ros/benchmarks/examples/demo2.yaml b/moveit_ros/benchmarks/examples/demo2.yaml index 9151abf075..cb25a6c6c6 100644 --- a/moveit_ros/benchmarks/examples/demo2.yaml +++ b/moveit_ros/benchmarks/examples/demo2.yaml @@ -1,5 +1,5 @@ # This is an example configuration that loads the "Kitchen" scene from the -# local MoveIt! warehouse and benchmarks the "manipulator" group over all pairs +# local MoveIt warehouse and benchmarks the "manipulator" group over all pairs # of motion plan queries and start states in the Kitchen scene. # Five planners from two different plugins are run a total of 50 times each, with a @@ -18,12 +18,12 @@ benchmark_config: output_directory: /home/benchmarks/ queries: .* start_states: .* - planners: - - plugin: ompl_interface/OMPLPlanner + planning_pipelines: + - name: ompl planners: - RRTConnectkConfigDefault - BKPIECEkConfigDefault - KPIECEkConfigDefault - - plugin: my_plugin_ns/my_plugin + - name: my_pipeline planners: - MyPlanner diff --git a/moveit_ros/benchmarks/examples/demo_fanuc.launch b/moveit_ros/benchmarks/examples/demo_fanuc.launch index 75b4f91b8d..7fa8f55c94 100644 --- a/moveit_ros/benchmarks/examples/demo_fanuc.launch +++ b/moveit_ros/benchmarks/examples/demo_fanuc.launch @@ -3,17 +3,17 @@ - + - + - + diff --git a/moveit_ros/benchmarks/examples/demo_obstacles.yaml b/moveit_ros/benchmarks/examples/demo_obstacles.yaml index 70c7604e03..d6a3c3b504 100644 --- a/moveit_ros/benchmarks/examples/demo_obstacles.yaml +++ b/moveit_ros/benchmarks/examples/demo_obstacles.yaml @@ -1,5 +1,5 @@ # This is an example configuration that loads the "Kitchen" scene from the -# local MoveIt! warehouse and benchmarks the "manipulator" group over all pairs +# local MoveIt warehouse and benchmarks the "manipulator" group over all pairs # of motion plan queries and start states in the Kitchen scene. # Five planners from two different plugins are run a total of 50 times each, with a @@ -18,16 +18,16 @@ benchmark_config: output_directory: /tmp/moveit_benchmarks/ queries: .* start_states: .* - planners: - - plugin: ompl_interface/OMPLPlanner + planning_pipelines: + - name: ompl planners: - RRTConnectkConfigDefault - BKPIECEkConfigDefault - KPIECEkConfigDefault - - plugin: chomp_interface/CHOMPPlanner + - name: chomp planners: - CHOMP - - plugin: stomp_moveit/StompPlannerManager + - name: stomp planners: - STOMP diff --git a/moveit_ros/benchmarks/examples/demo_panda.launch b/moveit_ros/benchmarks/examples/demo_panda.launch index f82b5ef8c8..08a1c33c9d 100644 --- a/moveit_ros/benchmarks/examples/demo_panda.launch +++ b/moveit_ros/benchmarks/examples/demo_panda.launch @@ -3,17 +3,17 @@ - + - + - + diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch b/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch index 161cd4478b..6cb02f534b 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch @@ -3,29 +3,30 @@ - + - - - - + + - - - + + + + + - - - + + + - - + + + + - diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml b/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml index 00ac5066e6..5a101151f9 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml @@ -1,5 +1,5 @@ # This is an example configuration that loads the "Kitchen" scene from the -# local MoveIt! warehouse and benchmarks the "manipulator" group over all pairs +# local MoveIt warehouse and benchmarks the "manipulator" group over all pairs # of motion plan queries and start states in the Kitchen scene. # Five planners from two different plugins are run a total of 50 times each, with a @@ -18,16 +18,15 @@ benchmark_config: output_directory: /tmp/moveit_benchmarks/ queries: .* start_states: .* - planners: - - plugin: ompl_interface/OMPLPlanner + planning_pipelines: + - name: ompl planners: - RRTConnectkConfigDefault - BKPIECEkConfigDefault - KPIECEkConfigDefault - - plugin: chomp_interface/CHOMPPlanner + - name: chomp planners: - CHOMP - - plugin: stomp_moveit/StompPlannerManager + - name: stomp planners: - STOMP - diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch b/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch index 205bab2081..46f3e1a5c5 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch +++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners_obstacles.launch @@ -3,25 +3,27 @@ - + - - - - + + - - - + + + + + - - - + + + - - + + + + diff --git a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch new file mode 100644 index 0000000000..1f04aedd5e --- /dev/null +++ b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.yaml b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.yaml new file mode 100644 index 0000000000..94f7803595 --- /dev/null +++ b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.yaml @@ -0,0 +1,34 @@ +# This is an example configuration that loads the "Kitchen" scene from the +# local MoveIt warehouse and benchmarks the "manipulator" group over all pairs +# of motion plan queries and start states in the Kitchen scene. + +# Five planners from two different plugins are run a total of 50 times each, with a +# maximum of 10 seconds per run. Output is stored in the /tmp/moveit_benchmarks directory. + +benchmark_config: + warehouse: + host: 127.0.0.1 + port: 33829 + scene_name: Kitchen # Required + parameters: + name: KitchenPick1 + runs: 50 + group: panda_arm # Required + timeout: 10.0 + output_directory: /tmp/moveit_benchmarks/ + predefined_poses_group: panda_arm_hand # Group where the predefined poses are specified + predefined_poses: # List of named targets + - ready + - extended + planning_pipelines: + - name: ompl + planners: + - RRTConnectkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - name: chomp + planners: + - CHOMP + - name: stomp + planners: + - STOMP diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index ed88584133..d0af01a6b6 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2015, Rice University -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Rice University nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Rice University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Rice University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ryan Luna */ -#ifndef MOVEIT_ROS_BENCHMARKS_BENCHMARK_EXECUTOR_ -#define MOVEIT_ROS_BENCHMARKS_BENCHMARK_EXECUTOR_ +#pragma once #include @@ -46,7 +45,7 @@ #include #include #include -#include +#include #include #include @@ -54,7 +53,6 @@ #include #include #include -#include namespace moveit_ros_benchmarks { @@ -91,8 +89,7 @@ class BenchmarkExecutor /// Definition of a post-run benchmark event function. Invoked immediately after each planner calls solve(). typedef boost::function + const planning_interface::MotionPlanDetailedResponse& response, PlannerRunData& run_data)> PostRunEventFunction; BenchmarkExecutor(const std::string& robot_description_param = "robot_description"); @@ -141,9 +138,30 @@ class BenchmarkExecutor virtual bool initializeBenchmarks(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg, std::vector& queries); + /// Initialize benchmark query data from start states and constraints + virtual bool loadBenchmarkQueryData(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg, + std::vector& start_states, + std::vector& path_constraints, + std::vector& goal_constraints, + std::vector& traj_constraints, + std::vector& queries); + virtual void collectMetrics(PlannerRunData& metrics, const planning_interface::MotionPlanDetailedResponse& mp_res, bool solved, double total_time); + /// Compute the similarity of each (final) trajectory to all other (final) trajectories in the experiment and write + /// the results to planner_data metrics + void computeAveragePathSimilarities(PlannerBenchmarkData& planner_data, + const std::vector& responses, + const std::vector& solved); + + /// Helper function used by computeAveragePathSimilarities() for computing a heuristic distance metric between two + /// robot trajectories. This function aligns both trajectories in a greedy fashion and computes the mean waypoint + /// distance averaged over all aligned waypoints. Using a greedy approach is more efficient than dynamic time warping, + /// and seems to be sufficient for similar trajectories. + bool computeTrajectoryDistance(const robot_trajectory::RobotTrajectory& traj_first, + const robot_trajectory::RobotTrajectory& traj_second, double& result_distance); + virtual void writeOutput(const BenchmarkRequest& brequest, const std::string& start_time, double benchmark_duration); void shiftConstraintsByOffset(moveit_msgs::Constraints& constraints, const std::vector& offset); @@ -192,8 +210,7 @@ class BenchmarkExecutor BenchmarkOptions options_; - std::shared_ptr> planner_plugin_loader_; - std::map planner_interfaces_; + std::map planning_pipelines_; std::vector benchmark_data_; @@ -204,6 +221,4 @@ class BenchmarkExecutor std::vector query_start_fns_; std::vector query_end_fns_; }; -} - -#endif +} // namespace moveit_ros_benchmarks diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h index 0bc10321af..e90a5076ac 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2015, Rice University -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Rice University nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Rice University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Rice University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ryan Luna */ -#ifndef MOVEIT_ROS_BENCHMARK_BENCHMARK_OPTIONS_ -#define MOVEIT_ROS_BENCHMARK_BENCHMARK_OPTIONS_ +#pragma once #include #include @@ -48,31 +47,57 @@ namespace moveit_ros_benchmarks class BenchmarkOptions { public: + /** \brief Constructor */ BenchmarkOptions(); + /** \brief Constructor accepting a custom namespace for parameter lookup */ BenchmarkOptions(const std::string& ros_namespace); + /** \brief Destructor */ virtual ~BenchmarkOptions(); + /** \brief Set the ROS namespace the node handle should use for parameter lookup */ void setNamespace(const std::string& ros_namespace); + /** \brief Get the name of the warehouse database host server */ const std::string& getHostName() const; + /** \brief Get the port of the warehouse database host server */ int getPort() const; + /** \brief Get the reference name of the planning scene stored inside the warehouse database */ const std::string& getSceneName() const; + /** \brief Get the specified number of benchmark query runs */ int getNumRuns() const; + /** \brief Get the maximum timeout per planning attempt */ double getTimeout() const; + /** \brief Get the reference name of the benchmark */ const std::string& getBenchmarkName() const; + /** \brief Get the name of the planning group to run the benchmark with */ const std::string& getGroupName() const; + /** \brief Get the target directory for the generated benchmark result data */ const std::string& getOutputDirectory() const; + /** \brief Get the regex expression for matching the names of all queries to run */ const std::string& getQueryRegex() const; + /** \brief Get the regex expression for matching the names of all start states to plan from */ const std::string& getStartStateRegex() const; + /** \brief Get the regex expression for matching the names of all goal constraints to plan to */ const std::string& getGoalConstraintRegex() const; + /** \brief Get the regex expression for matching the names of all path constraints to plan with */ const std::string& getPathConstraintRegex() const; + /** \brief Get the regex expression for matching the names of all trajectory constraints to plan with */ const std::string& getTrajectoryConstraintRegex() const; + /** \brief Get the names of all predefined poses to consider for planning */ + const std::vector& getPredefinedPoses() const; + /** \brief Get the name of the planning group for which the predefined poses are defined */ + const std::string& getPredefinedPosesGroup() const; + /** \brief Get the constant position/orientation offset to be used for shifting all goal constraints */ void getGoalOffsets(std::vector& offsets) const; - const std::map>& getPlannerConfigurations() const; - void getPlannerPluginList(std::vector& plugin_list) const; + /** \brief Get all planning pipeline names mapped to their parameter configuration */ + const std::map>& getPlanningPipelineConfigurations() const; + /** \brief Get all planning pipeline names */ + void getPlanningPipelineNames(std::vector& planning_pipeline_names) const; + /* \brief Get the frame id of the planning workspace */ const std::string& getWorkspaceFrameID() const; + /* \brief Get the parameter set of the planning workspace */ const moveit_msgs::WorkspaceParameters& getWorkspaceParameters() const; protected: @@ -101,13 +126,13 @@ class BenchmarkOptions std::string goal_constraint_regex_; std::string path_constraint_regex_; std::string trajectory_constraint_regex_; + std::vector predefined_poses_; + std::string predefined_poses_group_; double goal_offsets[6]; /// planner configurations - std::map> planners_; + std::map> planning_pipelines_; moveit_msgs::WorkspaceParameters workspace_; }; -} - -#endif +} // namespace moveit_ros_benchmarks diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index cbb64dbe81..4ee7757f63 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -1,13 +1,13 @@ moveit_ros_benchmarks - 1.0.1 + 1.1.1 - Enhanced tools for benchmarks in MoveIt! + Enhanced tools for benchmarks in MoveIt Ryan Luna Dave Coleman - MoveIt! Release Team + MoveIt Release Team http://moveit.ros.org https://github.com/ros-planning/moveit/issues diff --git a/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py b/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py index fd84005a33..50896a037d 100755 --- a/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py +++ b/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py @@ -112,7 +112,7 @@ def readBenchmarkLog(dbname, filenames): # create all tables if they don't already exist c.executescript("""CREATE TABLE IF NOT EXISTS experiments - (id INTEGER PRIMARY KEY AUTOINCREMENT, name VARCHAR(512), + (id INTEGER PRIMARY KEY ON CONFLICT REPLACE AUTOINCREMENT, name VARCHAR(512), totaltime REAL, timelimit REAL, memorylimit REAL, runcount INTEGER, version VARCHAR(128), hostname VARCHAR(1024), cpuinfo TEXT, date DATETIME, seed INTEGER, setup TEXT); @@ -130,7 +130,19 @@ def readBenchmarkLog(dbname, filenames): (runid INTEGER, time REAL, PRIMARY KEY (runid, time), FOREIGN KEY (runid) REFERENCES runs(id) ON DELETE CASCADE)""") - for filename in filenames: + # add placeholder entry for all_experiments + allExperimentsName = "all_experiments" + allExperimentsValues = { + "totaltime": 0.0, "timelimit": 0.0, "memorylimit": 0.0, "runcount": 0, "version": "0.0.0", + "hostname": "", "cpuinfo": "", "date": 0, "seed": 0, "setup": "" + } + addAllExperiments = len(filenames) > 0 + if addAllExperiments: + c.execute('INSERT INTO experiments VALUES (?,?,?,?,?,?,?,?,?,?,?,?)', + (None, allExperimentsName) + tuple(allExperimentsValues.values())) + allExperimentsId = c.lastrowid + + for i, filename in enumerate(filenames): print('Processing ' + filename) logfile = open(filename,'r') start_pos = logfile.tell() @@ -155,7 +167,19 @@ def readBenchmarkLog(dbname, filenames): nrruns = -1 if nrrunsOrNone != None: nrruns = int(nrrunsOrNone) + allExperimentsValues['runcount'] += nrruns totaltime = float(readRequiredLogValue("total time", logfile, 0, {-3 : "collect", -2 : "the", -1 : "data"})) + # fill in fields of all_experiments + allExperimentsValues['totaltime'] += totaltime + allExperimentsValues['memorylimit'] = max(allExperimentsValues['memorylimit'], totaltime) + allExperimentsValues['timelimit'] = max(allExperimentsValues['timelimit'], totaltime) + # copy the fields of the first file to all_experiments so that they are not empty + if (i==0): + allExperimentsValues['version'] = version + allExperimentsValues['date'] = date + allExperimentsValues['setup'] = expsetup + allExperimentsValues['hostname'] = hostname + allExperimentsValues['cpuinfo'] = cpuinfo numEnums = 0 numEnumsOrNone = readOptionalLogValue(logfile, 0, {-2 : "enum"}) if numEnumsOrNone != None: @@ -213,13 +237,17 @@ def readBenchmarkLog(dbname, filenames): numRuns = int(logfile.readline().split()[0]) runIds = [] for j in range(numRuns): - values = tuple([experimentId, plannerId] + \ - [None if len(x) == 0 or x == 'nan' or x == 'inf' else x - for x in logfile.readline().split('; ')[:-1]]) + runValues = [None if len(x) == 0 or x == 'nan' or x == 'inf' else x + for x in logfile.readline().split('; ')[:-1]] + values = tuple([experimentId, plannerId] + runValues) c.execute(insertFmtStr, values) # extract primary key of each run row so we can reference them # in the planner progress data table if needed runIds.append(c.lastrowid) + # add all run data to all_experiments + if addAllExperiments: + values = tuple([allExperimentsId, plannerId] + runValues) + c.execute(insertFmtStr, values) nextLine = logfile.readline().strip() @@ -259,6 +287,15 @@ def readBenchmarkLog(dbname, filenames): logfile.readline() logfile.close() + + if addAllExperiments: + updateString = "UPDATE experiments SET" + for i, (key, val) in enumerate(allExperimentsValues.items()): + if i > 0: + updateString += "," + updateString += " " + str(key) + "='" + str(val) + "'" + updateString += "WHERE id='" + str(allExperimentsId) + "'" + c.execute(updateString) conn.commit() c.close() @@ -306,7 +343,7 @@ def plotAttribute(cur, planners, attribute, typename): color=matplotlib.cm.hot(int(floor(i*256/numValues))), label=descriptions[i]) heights = heights + measurements[i] - xtickNames = plt.xticks([x+width/2. for x in ind], labels, rotation=30) + xtickNames = plt.xticks([x + width / 2. for x in ind], labels, rotation=30, fontsize=8, ha='right') ax.set_ylabel(attribute.replace('_',' ') + ' (%)') box = ax.get_position() ax.set_position([box.x0, box.y0, box.width * 0.8, box.height]) @@ -318,7 +355,11 @@ def plotAttribute(cur, planners, attribute, typename): measurementsPercentage = [sum(m) * 100. / len(m) for m in measurements] ind = range(len(measurements)) plt.bar(ind, measurementsPercentage, width) - xtickNames = plt.xticks([x + width / 2. for x in ind], labels, rotation=30, fontsize=8) + ### uncommenting this line will remove the term 'kConfigDefault' from the labels for OMPL Solvers. + ### Fits situations where you need more control in the plot, such as in an academic publication for example + #labels = [l.replace('kConfigDefault', '') for l in labels] + + xtickNames = plt.xticks([x + width / 2. for x in ind], labels, rotation=30, fontsize=8, ha='right') ax.set_ylabel(attribute.replace('_',' ') + ' (%)') plt.subplots_adjust(bottom=0.3) # Squish the plot into the upper 2/3 of the page. Leave room for labels else: @@ -330,19 +371,24 @@ def plotAttribute(cur, planners, attribute, typename): #xtickNames = plt.xticks(labels, rotation=30, fontsize=10) #plt.subplots_adjust(bottom=0.3) # Squish the plot into the upper 2/3 of the page. Leave room for labels - + + ### uncommenting this line will remove the term 'kConfigDefault' from the labels for OMPL Solvers. + ### Fits situations where you need more control in the plot, such as in an academic publication for example + #labels = [l.replace('kConfigDefault', '') for l in labels] + xtickNames = plt.setp(ax,xticklabels=labels) - plt.setp(xtickNames, rotation=30) + plt.setp(xtickNames, rotation=30, fontsize=8, ha='right') for tick in ax.xaxis.get_major_ticks(): # shrink the font size of the x tick labels tick.label.set_fontsize(8) plt.subplots_adjust(bottom=0.3) # Squish the plot into the upper 2/3 of the page. Leave room for labels - ax.set_xlabel('Motion planning algorithm') + ax.set_xlabel('Motion planning algorithm', fontsize=12) ax.yaxis.grid(True, linestyle='-', which='major', color='lightgrey', alpha=0.5) if max(nanCounts)>0: maxy = max([max(y) for y in measurements]) for i in range(len(labels)): x = i+width/2 if typename=='BOOLEAN' else i+1 - ax.text(x, .95*maxy, str(nanCounts[i]), horizontalalignment='center', size='small') + ### uncommenting the next line, the number of failed planning attempts will be added to each bar + # ax.text(x, .95*maxy, str(nanCounts[i]), horizontalalignment='center', size='small') plt.show() def plotProgressAttribute(cur, planners, attribute): @@ -541,9 +587,9 @@ def computeViews(dbname): (options, args) = parser.parse_args() if len(args) == 0: - parser.error("No arguments were provided. Please provide full path of log file") + parser.error("No arguments were provided. Please provide full path of log file") - if len(args) == 1: + if len(args) > 0: readBenchmarkLog(options.dbname, args) # If we update the database, we recompute the views as well options.view = True diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index aa59bed3f9..35e7e98a18 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2015, Rice University -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Rice University nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Rice University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Rice University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ryan Luna */ @@ -40,11 +40,18 @@ #include #include +// TODO: Remove if boost >= 1.72 (https://github.com/boostorg/timer/issues/12) +#define BOOST_ALLOW_DEPRECATED_HEADERS 1 #include +#undef BOOST_ALLOW_DEPRECATED_HEADERS #include #include #include +#ifndef _WIN32 #include +#else +#include +#endif using namespace moveit_ros_benchmarks; @@ -71,17 +78,6 @@ BenchmarkExecutor::BenchmarkExecutor(const std::string& robot_description_param) tcs_ = nullptr; psm_ = new planning_scene_monitor::PlanningSceneMonitor(robot_description_param); planning_scene_ = psm_->getPlanningScene(); - - // Initialize the class loader for planner plugins - try - { - planner_plugin_loader_.reset(new pluginlib::ClassLoader( - "moveit_core", "planning_interface::PlannerManager")); - } - catch (pluginlib::PluginlibException& ex) - { - ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); - } } BenchmarkExecutor::~BenchmarkExecutor() @@ -94,46 +90,39 @@ BenchmarkExecutor::~BenchmarkExecutor() delete psm_; } -void BenchmarkExecutor::initialize(const std::vector& plugin_classes) +void BenchmarkExecutor::initialize(const std::vector& planning_pipeline_names) { - planner_interfaces_.clear(); + planning_pipelines_.clear(); - // Load the planning plugins - const std::vector& classes = planner_plugin_loader_->getDeclaredClasses(); - - for (const std::string& planner_plugin_name : plugin_classes) + ros::NodeHandle pnh("~"); + for (const std::string& planning_pipeline_name : planning_pipeline_names) { - std::vector::const_iterator it = std::find(classes.begin(), classes.end(), planner_plugin_name); - if (it == classes.end()) - { - ROS_ERROR("Failed to find plugin_class %s", planner_plugin_name.c_str()); - return; - } - - try - { - planning_interface::PlannerManagerPtr p = planner_plugin_loader_->createUniqueInstance(planner_plugin_name); - p->initialize(planning_scene_->getRobotModel(), ""); + // Initialize planning pipelines from configured child namespaces + ros::NodeHandle child_nh(pnh, planning_pipeline_name); + planning_pipeline::PlanningPipelinePtr pipeline(new planning_pipeline::PlanningPipeline( + planning_scene_->getRobotModel(), child_nh, "planning_plugin", "request_adapters")); - p->getPlannerConfigurations(); - planner_interfaces_[planner_plugin_name] = p; - } - catch (pluginlib::PluginlibException& ex) + // Verify the pipeline has successfully initialized a planner + if (!pipeline->getPlannerManager()) { - ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what()); + ROS_ERROR("Failed to initialize planning pipeline '%s'", planning_pipeline_name.c_str()); + continue; } + + // Disable visualizations and store pipeline + pipeline->displayComputedMotionPlans(false); + pipeline->checkSolutionPaths(false); + planning_pipelines_[planning_pipeline_name] = pipeline; } - // error check - if (planner_interfaces_.empty()) - ROS_ERROR("No planning plugins have been loaded. Nothing to do for the benchmarking service."); + // Error check + if (planning_pipelines_.empty()) + ROS_ERROR("No planning pipelines have been loaded. Nothing to do for the benchmarking service."); else { - std::stringstream ss; - for (std::map::const_iterator it = planner_interfaces_.begin(); - it != planner_interfaces_.end(); ++it) - ss << it->first << " "; - ROS_INFO("Available planner instances: %s", ss.str().c_str()); + ROS_INFO("Available planning pipelines:"); + for (const std::pair& entry : planning_pipelines_) + ROS_INFO_STREAM("Pipeline: " << entry.first << ", Planner: " << entry.second->getPlannerPluginName()); } } @@ -206,9 +195,9 @@ void BenchmarkExecutor::addQueryCompletionEvent(const QueryCompletionEventFuncti bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& opts) { - if (planner_interfaces_.empty()) + if (planning_pipelines_.empty()) { - ROS_ERROR("No planning interfaces configured. Did you call BenchmarkExecutor::initialize?"); + ROS_ERROR("No planning pipelines configured. Did you call BenchmarkExecutor::initialize?"); return false; } @@ -217,7 +206,7 @@ bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& opts) if (initializeBenchmarks(opts, scene_msg, queries)) { - if (!queriesAndPlannersCompatible(queries, opts.getPlannerConfigurations())) + if (!queriesAndPlannersCompatible(queries, opts.getPlanningPipelineConfigurations())) return false; for (std::size_t i = 0; i < queries.size(); ++i) @@ -241,7 +230,7 @@ bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& opts) ROS_INFO("Benchmarking query '%s' (%lu of %lu)", queries[i].name.c_str(), i + 1, queries.size()); ros::WallTime start_time = ros::WallTime::now(); - runBenchmark(queries[i].request, options_.getPlannerConfigurations(), options_.getNumRuns()); + runBenchmark(queries[i].request, options_.getPlanningPipelineConfigurations(), options_.getNumRuns()); double duration = (ros::WallTime::now() - start_time).toSec(); for (QueryCompletionEventFunction& query_end_fn : query_end_fns_) @@ -256,17 +245,18 @@ bool BenchmarkExecutor::runBenchmarks(const BenchmarkOptions& opts) } bool BenchmarkExecutor::queriesAndPlannersCompatible(const std::vector& requests, - const std::map>& planners) + const std::map>& /*planners*/) { // Make sure that the planner interfaces can service the desired queries - for (std::map::const_iterator it = planner_interfaces_.begin(); - it != planner_interfaces_.end(); ++it) + for (const std::pair& pipeline_entry : planning_pipelines_) { for (const BenchmarkRequest& request : requests) { - if (!it->second->canServiceRequest(request.request)) + if (!pipeline_entry.second->getPlannerManager()->canServiceRequest(request.request)) { - ROS_ERROR("Interface '%s' cannot service the benchmark request '%s'", it->first.c_str(), request.name.c_str()); + ROS_ERROR("Interface '%s' in pipeline '%s' cannot service the benchmark request '%s'", + pipeline_entry.second->getPlannerPluginName().c_str(), pipeline_entry.first.c_str(), + request.name.c_str()); return false; } } @@ -278,48 +268,19 @@ bool BenchmarkExecutor::queriesAndPlannersCompatible(const std::vector& requests) { - if (!plannerConfigurationsExist(opts.getPlannerConfigurations(), opts.getGroupName())) + if (!plannerConfigurationsExist(opts.getPlanningPipelineConfigurations(), opts.getGroupName())) return false; - try - { - warehouse_ros::DatabaseConnection::Ptr conn = dbloader.loadDatabase(); - conn->setParams(opts.getHostName(), opts.getPort(), 20); - if (conn->connect()) - { - pss_ = new moveit_warehouse::PlanningSceneStorage(conn); - psws_ = new moveit_warehouse::PlanningSceneWorldStorage(conn); - rs_ = new moveit_warehouse::RobotStateStorage(conn); - cs_ = new moveit_warehouse::ConstraintsStorage(conn); - tcs_ = new moveit_warehouse::TrajectoryConstraintsStorage(conn); - } - else - { - ROS_ERROR("Failed to connect to DB"); - return false; - } - } - catch (std::exception& e) - { - ROS_ERROR("Failed to initialize benchmark server: '%s'", e.what()); - return false; - } - std::vector start_states; std::vector path_constraints; std::vector goal_constraints; std::vector traj_constraints; std::vector queries; - bool ok = loadPlanningScene(opts.getSceneName(), scene_msg) && loadStates(opts.getStartStateRegex(), start_states) && - loadPathConstraints(opts.getGoalConstraintRegex(), goal_constraints) && - loadPathConstraints(opts.getPathConstraintRegex(), path_constraints) && - loadTrajectoryConstraints(opts.getTrajectoryConstraintRegex(), traj_constraints) && - loadQueries(opts.getQueryRegex(), opts.getSceneName(), queries); - - if (!ok) + if (!loadBenchmarkQueryData(opts, scene_msg, start_states, path_constraints, goal_constraints, traj_constraints, + queries)) { - ROS_ERROR("Failed to load benchmark stuff"); + ROS_ERROR("Failed to load benchmark query data"); return false; } @@ -429,6 +390,44 @@ bool BenchmarkExecutor::initializeBenchmarks(const BenchmarkOptions& opts, movei return true; } +bool BenchmarkExecutor::loadBenchmarkQueryData(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg, + std::vector& start_states, + std::vector& path_constraints, + std::vector& goal_constraints, + std::vector& traj_constraints, + std::vector& queries) +{ + try + { + warehouse_ros::DatabaseConnection::Ptr warehouse_connection = dbloader.loadDatabase(); + warehouse_connection->setParams(opts.getHostName(), opts.getPort(), 20); + if (warehouse_connection->connect()) + { + pss_ = new moveit_warehouse::PlanningSceneStorage(warehouse_connection); + psws_ = new moveit_warehouse::PlanningSceneWorldStorage(warehouse_connection); + rs_ = new moveit_warehouse::RobotStateStorage(warehouse_connection); + cs_ = new moveit_warehouse::ConstraintsStorage(warehouse_connection); + tcs_ = new moveit_warehouse::TrajectoryConstraintsStorage(warehouse_connection); + } + else + { + ROS_ERROR("Failed to connect to DB"); + return false; + } + } + catch (std::exception& e) + { + ROS_ERROR("Failed to initialize benchmark server: '%s'", e.what()); + return false; + } + + return loadPlanningScene(opts.getSceneName(), scene_msg) && loadStates(opts.getStartStateRegex(), start_states) && + loadPathConstraints(opts.getGoalConstraintRegex(), goal_constraints) && + loadPathConstraints(opts.getPathConstraintRegex(), path_constraints) && + loadTrajectoryConstraints(opts.getTrajectoryConstraintRegex(), traj_constraints) && + loadQueries(opts.getQueryRegex(), opts.getSceneName(), queries); +} + void BenchmarkExecutor::shiftConstraintsByOffset(moveit_msgs::Constraints& constraints, const std::vector& offset) { @@ -475,6 +474,10 @@ void BenchmarkExecutor::createRequestCombinations(const BenchmarkRequest& breque { for (const StartState& start_state : start_states) { + // Skip start states that have the same name as the goal + if (start_state.name == brequest.name) + continue; + BenchmarkRequest new_brequest = brequest; new_brequest.request.start_state = start_state.state; @@ -495,33 +498,32 @@ void BenchmarkExecutor::createRequestCombinations(const BenchmarkRequest& breque } } -bool BenchmarkExecutor::plannerConfigurationsExist(const std::map>& planners, - const std::string& group_name) +bool BenchmarkExecutor::plannerConfigurationsExist( + const std::map>& pipeline_configurations, const std::string& group_name) { // Make sure planner plugins exist - for (std::map>::const_iterator it = planners.begin(); it != planners.end(); - ++it) + for (const std::pair>& pipeline_config_entry : pipeline_configurations) { - bool plugin_exists = false; - for (std::map::const_iterator planner_it = - planner_interfaces_.begin(); - planner_it != planner_interfaces_.end() && !plugin_exists; ++planner_it) + bool pipeline_exists = false; + for (const std::pair& pipeline_entry : + planning_pipelines_) { - plugin_exists = planner_it->first == it->first; + pipeline_exists = pipeline_entry.first == pipeline_config_entry.first; + if (pipeline_exists) + break; } - if (!plugin_exists) + if (!pipeline_exists) { - ROS_ERROR("Planning plugin '%s' does NOT exist", it->first.c_str()); + ROS_ERROR("Planning pipeline '%s' does NOT exist", pipeline_config_entry.first.c_str()); return false; } } - // Make sure planning algorithms exist within those plugins - for (std::map>::const_iterator it = planners.begin(); it != planners.end(); - ++it) + // Make sure planners exist within those pipelines + for (const std::pair>& entry : pipeline_configurations) { - planning_interface::PlannerManagerPtr pm = planner_interfaces_[it->first]; + planning_interface::PlannerManagerPtr pm = planning_pipelines_[entry.first]->getPlannerManager(); const planning_interface::PlannerConfigurationMap& config_map = pm->getPlannerConfigurations(); // if the planner is chomp or stomp skip this function and return true for checking planner configurations for the @@ -530,24 +532,23 @@ bool BenchmarkExecutor::plannerConfigurationsExist(const std::mapgetDescription().compare("stomp") || pm->getDescription().compare("chomp")) continue; - for (std::size_t i = 0; i < it->second.size(); ++i) + for (std::size_t i = 0; i < entry.second.size(); ++i) { bool planner_exists = false; - for (planning_interface::PlannerConfigurationMap::const_iterator map_it = config_map.begin(); - map_it != config_map.end() && !planner_exists; ++map_it) + for (const std::pair& config_entry : + config_map) { - std::string planner_name = group_name + "[" + it->second[i] + "]"; - planner_exists = (map_it->second.group == group_name && map_it->second.name == planner_name); + std::string planner_name = group_name + "[" + entry.second[i] + "]"; + planner_exists = (config_entry.second.group == group_name && config_entry.second.name == planner_name); } if (!planner_exists) { - ROS_ERROR("Planner '%s' does NOT exist for group '%s' in pipeline '%s'", it->second[i].c_str(), - group_name.c_str(), it->first.c_str()); + ROS_ERROR("Planner '%s' does NOT exist for group '%s' in pipeline '%s'", entry.second[i].c_str(), + group_name.c_str(), entry.first.c_str()); std::cout << "There are " << config_map.size() << " planner entries: " << std::endl; - for (planning_interface::PlannerConfigurationMap::const_iterator map_it = config_map.begin(); - map_it != config_map.end() && !planner_exists; ++map_it) - std::cout << map_it->second.name << std::endl; + for (const auto& config_map_entry : config_map) + std::cout << config_map_entry.second.name << std::endl; return false; } } @@ -747,33 +748,42 @@ bool BenchmarkExecutor::loadTrajectoryConstraints(const std::string& regex, } void BenchmarkExecutor::runBenchmark(moveit_msgs::MotionPlanRequest request, - const std::map>& planners, int runs) + const std::map>& pipeline_map, int runs) { benchmark_data_.clear(); unsigned int num_planners = 0; - for (const std::pair>& planner : planners) - num_planners += planner.second.size(); + for (const std::pair>& pipeline_entry : pipeline_map) + num_planners += pipeline_entry.second.size(); boost::progress_display progress(num_planners * runs, std::cout); - // Iterate through all planner plugins - for (const std::pair>& planner : planners) + // Iterate through all planning pipelines + for (const std::pair>& pipeline_entry : pipeline_map) { - // Iterate through all planners associated with the plugin - for (std::size_t i = 0; i < planner.second.size(); ++i) + planning_pipeline::PlanningPipelinePtr planning_pipeline = planning_pipelines_[pipeline_entry.first]; + // Use the planning context if the pipeline only contains the planner plugin + bool use_planning_context = planning_pipeline->getAdapterPluginNames().empty(); + // Iterate through all planners configured for the pipeline + for (const std::string& planner_id : pipeline_entry.second) { // This container stores all of the benchmark data for this planner PlannerBenchmarkData planner_data(runs); + // This vector stores all motion plan results for further evaluation + std::vector responses(runs); + std::vector solved(runs); - request.planner_id = planner.second[i]; + request.planner_id = planner_id; // Planner start events for (PlannerStartEventFunction& planner_start_fn : planner_start_fns_) planner_start_fn(request, planner_data); - planning_interface::PlanningContextPtr context = - planner_interfaces_[planner.first]->getPlanningContext(planning_scene_, request); + planning_interface::PlanningContextPtr planning_context; + if (use_planning_context) + planning_context = planning_pipeline->getPlannerManager()->getPlanningContext(planning_scene_, request); + + // Iterate runs for (int j = 0; j < runs; ++j) { // Pre-run events @@ -781,9 +791,24 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::MotionPlanRequest request, pre_event_fn(request); // Solve problem - planning_interface::MotionPlanDetailedResponse mp_res; ros::WallTime start = ros::WallTime::now(); - bool solved = context->solve(mp_res); + if (use_planning_context) + { + solved[j] = planning_context->solve(responses[j]); + } + else + { + // The planning pipeline does not support MotionPlanDetailedResponse + planning_interface::MotionPlanResponse response; + solved[j] = planning_pipeline->generatePlan(planning_scene_, request, response); + responses[j].error_code_ = response.error_code_; + if (response.trajectory_) + { + responses[j].description_.push_back("plan"); + responses[j].trajectory_.push_back(response.trajectory_); + responses[j].processing_time_.push_back(response.planning_time_); + } + } double total_time = (ros::WallTime::now() - start).toSec(); // Collect data @@ -791,14 +816,16 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::MotionPlanRequest request, // Post-run events for (PostRunEventFunction& post_event_fn : post_event_fns_) - post_event_fn(request, mp_res, planner_data[j]); - collectMetrics(planner_data[j], mp_res, solved, total_time); + post_event_fn(request, responses[j], planner_data[j]); + collectMetrics(planner_data[j], responses[j], solved[j], total_time); double metrics_time = (ros::WallTime::now() - start).toSec(); ROS_DEBUG("Spent %lf seconds collecting metrics", metrics_time); ++progress; } + computeAveragePathSimilarities(planner_data, responses, solved); + // Planner completion events for (PlannerCompletionEventFunction& planner_completion_fn : planner_completion_fns_) planner_completion_fn(request, planner_data); @@ -889,6 +916,15 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, metrics["path_" + mp_res.description_[j] + "_clearance REAL"] = moveit::core::toString(clearance); metrics["path_" + mp_res.description_[j] + "_smoothness REAL"] = moveit::core::toString(smoothness); metrics["path_" + mp_res.description_[j] + "_time REAL"] = moveit::core::toString(mp_res.processing_time_[j]); + + if (j == mp_res.trajectory_.size() - 1) + { + metrics["final_path_correct BOOLEAN"] = boost::lexical_cast(correct); + metrics["final_path_length REAL"] = moveit::core::toString(traj_len); + metrics["final_path_clearance REAL"] = moveit::core::toString(clearance); + metrics["final_path_smoothness REAL"] = moveit::core::toString(smoothness); + metrics["final_path_time REAL"] = moveit::core::toString(mp_res.processing_time_[j]); + } process_time -= mp_res.processing_time_[j]; } if (process_time <= 0.0) @@ -897,14 +933,129 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, } } +void BenchmarkExecutor::computeAveragePathSimilarities( + PlannerBenchmarkData& planner_data, const std::vector& responses, + const std::vector& solved) +{ + ROS_INFO("Computing result path similarity"); + const size_t result_count = planner_data.size(); + size_t unsolved = std::count_if(solved.begin(), solved.end(), [](bool s) { return !s; }); + std::vector average_distances(responses.size()); + for (size_t first_traj_i = 0; first_traj_i < result_count; ++first_traj_i) + { + // If trajectory was not solved there is no valid average distance so it's set to max double only + if (!solved[first_traj_i]) + { + average_distances[first_traj_i] = std::numeric_limits::max(); + continue; + } + // Iterate all result trajectories that haven't been compared yet + for (size_t second_traj_i = first_traj_i + 1; second_traj_i < result_count; ++second_traj_i) + { + // Ignore if other result has not been solved + if (!solved[second_traj_i]) + continue; + + // Get final trajectories + const robot_trajectory::RobotTrajectory& traj_first = *responses[first_traj_i].trajectory_.back(); + const robot_trajectory::RobotTrajectory& traj_second = *responses[second_traj_i].trajectory_.back(); + + // Compute trajectory distance + double trajectory_distance; + if (!computeTrajectoryDistance(traj_first, traj_second, trajectory_distance)) + continue; + + // Add average distance to counters of both trajectories + average_distances[first_traj_i] += trajectory_distance; + average_distances[second_traj_i] += trajectory_distance; + } + // Normalize average distance by number of actual comparisons + average_distances[first_traj_i] /= result_count - unsolved - 1; + } + + // Store results in planner_data + for (size_t i = 0; i < result_count; ++i) + planner_data[i]["average_waypoint_distance REAL"] = moveit::core::toString(average_distances[i]); +} + +bool BenchmarkExecutor::computeTrajectoryDistance(const robot_trajectory::RobotTrajectory& traj_first, + const robot_trajectory::RobotTrajectory& traj_second, + double& result_distance) +{ + // Abort if trajectories are empty + if (traj_first.empty() || traj_second.empty()) + return false; + + // Waypoint counter + size_t pos_first = 0; + size_t pos_second = 0; + const size_t max_pos_first = traj_first.getWayPointCount() - 1; + const size_t max_pos_second = traj_second.getWayPointCount() - 1; + + // Compute total distance between pairwise waypoints of both trajectories. + // The selection of waypoint pairs is based on what steps results in the minimal distance between the next pair of + // waypoints. We first check what steps are still possible or if we reached the end of the trajectories. Then we + // compute the pairwise waypoint distances of the pairs from increasing both, the first, or the second trajectory. + // Finally we select the pair that results in the minimal distance, summarize the total distance and iterate + // accordingly. After that we compute the average trajectory distance by normalizing over the number of steps. + double total_distance = 0; + size_t steps = 0; + double current_distance = traj_first.getWayPoint(pos_first).distance(traj_second.getWayPoint(pos_second)); + while (true) + { + // Keep track of total distance and number of comparisons + total_distance += current_distance; + ++steps; + if (pos_first == max_pos_first && pos_second == max_pos_second) // end reached + break; + + // Determine what steps are still possible + bool can_up_first = pos_first < max_pos_first; + bool can_up_second = pos_second < max_pos_second; + bool can_up_both = can_up_first && can_up_second; + + // Compute pair-wise waypoint distances (increasing both, first, or second trajectories). + double up_both = std::numeric_limits::max(); + double up_first = std::numeric_limits::max(); + double up_second = std::numeric_limits::max(); + if (can_up_both) + up_both = traj_first.getWayPoint(pos_first + 1).distance(traj_second.getWayPoint(pos_second + 1)); + if (can_up_first) + up_first = traj_first.getWayPoint(pos_first + 1).distance(traj_second.getWayPoint(pos_second)); + if (can_up_second) + up_second = traj_first.getWayPoint(pos_first).distance(traj_second.getWayPoint(pos_second + 1)); + + // Select actual step, store new distance value and iterate trajectory positions + if (can_up_both && up_both < up_first && up_both < up_second) + { + ++pos_first; + ++pos_second; + current_distance = up_both; + } + else if ((can_up_first && up_first < up_second) || !can_up_second) + { + ++pos_first; + current_distance = up_first; + } + else if (can_up_second) + { + ++pos_second; + current_distance = up_second; + } + } + // Normalize trajectory distance by number of comparison steps + result_distance = total_distance / static_cast(steps); + return true; +} + void BenchmarkExecutor::writeOutput(const BenchmarkRequest& brequest, const std::string& start_time, double benchmark_duration) { - const std::map>& planners = options_.getPlannerConfigurations(); + const std::map>& pipelines = options_.getPlanningPipelineConfigurations(); size_t num_planners = 0; - for (const std::pair>& planner : planners) - num_planners += planner.second.size(); + for (const std::pair>& pipeline : pipelines) + num_planners += pipeline.second.size(); std::string hostname = getHostname(); if (hostname.empty()) @@ -926,7 +1077,7 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& brequest, const std: return; } - out << "MoveIt! version " << MOVEIT_VERSION << std::endl; + out << "MoveIt version " << MOVEIT_VERSION << std::endl; out << "Experiment " << brequest.name << std::endl; out << "Running on " << hostname << std::endl; out << "Starting at " << start_time << std::endl; @@ -954,12 +1105,12 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& brequest, const std: out << num_planners << " planners" << std::endl; size_t run_id = 0; - for (const std::pair>& planner : planners) + for (const std::pair>& pipeline : pipelines) { - for (std::size_t i = 0; i < planner.second.size(); ++i, ++run_id) + for (std::size_t i = 0; i < pipeline.second.size(); ++i, ++run_id) { - // Write the name of the planner. - out << planner.second[i] << std::endl; + // Write the name of the planner and the used pipeline + out << pipeline.second[i] << " (" << pipeline.first << ")" << std::endl; // in general, we could have properties specific for a planner; // right now, we do not include such properties diff --git a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp index 134d1d4017..567eac3d5c 100644 --- a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2015, Rice University -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Rice University nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Rice University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Rice University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ryan Luna */ @@ -136,22 +136,32 @@ const std::string& BenchmarkOptions::getTrajectoryConstraintRegex() const return trajectory_constraint_regex_; } +const std::vector& BenchmarkOptions::getPredefinedPoses() const +{ + return predefined_poses_; +} + +const std::string& BenchmarkOptions::getPredefinedPosesGroup() const +{ + return predefined_poses_group_; +} + void BenchmarkOptions::getGoalOffsets(std::vector& offsets) const { offsets.resize(6); memcpy(&offsets[0], goal_offsets, 6 * sizeof(double)); } -const std::map>& BenchmarkOptions::getPlannerConfigurations() const +const std::map>& BenchmarkOptions::getPlanningPipelineConfigurations() const { - return planners_; + return planning_pipelines_; } -void BenchmarkOptions::getPlannerPluginList(std::vector& plugin_list) const +void BenchmarkOptions::getPlanningPipelineNames(std::vector& planning_pipeline_names) const { - plugin_list.clear(); - for (const std::pair>& planner : planners_) - plugin_list.push_back(planner.first); + planning_pipeline_names.clear(); + for (const std::pair>& planning_pipeline : planning_pipelines_) + planning_pipeline_names.push_back(planning_pipeline.first); } const std::string& BenchmarkOptions::getWorkspaceFrameID() const @@ -189,6 +199,8 @@ void BenchmarkOptions::readBenchmarkParameters(ros::NodeHandle& nh) nh.param(std::string("benchmark_config/parameters/path_constraints"), path_constraint_regex_, std::string("")); nh.param(std::string("benchmark_config/parameters/trajectory_constraints"), trajectory_constraint_regex_, std::string("")); + nh.param(std::string("benchmark_config/parameters/predefined_poses"), predefined_poses_, {}); + nh.param(std::string("benchmark_config/parameters/predefined_poses_group"), predefined_poses_group_, std::string("")); if (!nh.getParam(std::string("benchmark_config/parameters/group"), group_name_)) ROS_WARN("Benchmark group NOT specified"); @@ -237,48 +249,48 @@ void BenchmarkOptions::readWorkspaceParameters(ros::NodeHandle& nh) void BenchmarkOptions::readPlannerConfigs(ros::NodeHandle& nh) { - planners_.clear(); + planning_pipelines_.clear(); - XmlRpc::XmlRpcValue planner_configs; - if (nh.getParam("benchmark_config/planners", planner_configs)) + XmlRpc::XmlRpcValue pipeline_configs; + if (nh.getParam("benchmark_config/planning_pipelines", pipeline_configs)) { - if (planner_configs.getType() != XmlRpc::XmlRpcValue::TypeArray) + if (pipeline_configs.getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_ERROR("Expected a list of planner configurations to benchmark"); + ROS_ERROR("Expected a list of planning pipeline configurations to benchmark"); return; } - for (int i = 0; i < planner_configs.size(); ++i) // NOLINT(modernize-loop-convert) + for (int i = 0; i < pipeline_configs.size(); ++i) // NOLINT(modernize-loop-convert) { - if (planner_configs[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) + if (pipeline_configs[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_WARN("Improper YAML type for planner configurations"); + ROS_WARN("Improper YAML type for planning pipeline configurations"); continue; } - if (!planner_configs[i].hasMember("plugin") || !planner_configs[i].hasMember("planners")) + if (!pipeline_configs[i].hasMember("name") || !pipeline_configs[i].hasMember("planners")) { ROS_WARN("Malformed YAML for planner configurations"); continue; } - if (planner_configs[i]["planners"].getType() != XmlRpc::XmlRpcValue::TypeArray) + if (pipeline_configs[i]["planners"].getType() != XmlRpc::XmlRpcValue::TypeArray) { ROS_WARN("Expected a list of planners to benchmark"); continue; } - const std::string plugin = planner_configs[i]["plugin"]; - ROS_INFO("Reading in planner names for plugin '%s'", plugin.c_str()); + const std::string pipeline_name = pipeline_configs[i]["name"]; + ROS_INFO("Reading in planner names for planning pipeline '%s'", pipeline_name.c_str()); std::vector planners; - planners.reserve(planner_configs[i]["planners"].size()); - for (int j = 0; j < planner_configs[i]["planners"].size(); ++j) - planners.emplace_back(planner_configs[i]["planners"][j]); + planners.reserve(pipeline_configs[i]["planners"].size()); + for (int j = 0; j < pipeline_configs[i]["planners"].size(); ++j) + planners.emplace_back(pipeline_configs[i]["planners"][j]); for (std::size_t j = 0; j < planners.size(); ++j) ROS_INFO(" [%lu]: %s", j, planners[j].c_str()); - planners_[plugin] = planners; + planning_pipelines_[pipeline_name] = planners; } } } diff --git a/moveit_ros/benchmarks/src/RunBenchmark.cpp b/moveit_ros/benchmarks/src/RunBenchmark.cpp index 14082cbcdc..4a512c5c21 100644 --- a/moveit_ros/benchmarks/src/RunBenchmark.cpp +++ b/moveit_ros/benchmarks/src/RunBenchmark.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2015, Rice University -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Rice University nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Rice University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Rice University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ryan Luna */ @@ -51,9 +51,9 @@ int main(int argc, char** argv) // Setup benchmark server moveit_ros_benchmarks::BenchmarkExecutor server; - std::vector plugins; - opts.getPlannerPluginList(plugins); - server.initialize(plugins); + std::vector planning_pipelines; + opts.getPlanningPipelineNames(planning_pipelines); + server.initialize(planning_pipelines); // Running benchmarks if (!server.runBenchmarks(opts)) diff --git a/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp b/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp new file mode 100644 index 0000000000..914b189d03 --- /dev/null +++ b/moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp @@ -0,0 +1,147 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Henning Kayser */ +/* Description: A simple benchmark that plans trajectories for all combinations of specified predefined poses */ + +// MoveIt Benchmark +#include +#include + +// MoveIt +#include +#include +#include + +namespace moveit_ros_benchmarks +{ +constexpr char LOGNAME[] = "combine_predefined_poses_benchmark"; +class CombinePredefinedPosesBenchmark : public BenchmarkExecutor +{ +public: + bool loadBenchmarkQueryData(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg, + std::vector& start_states, std::vector& path_constraints, + std::vector& goal_constraints, + std::vector& traj_constraints, + std::vector& queries) override + { + // Load planning scene + if (!psm_) + psm_.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description")); + if (!psm_->newPlanningSceneMessage(scene_msg)) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to load planning scene"); + return false; + } + + // Load robot model + if (!psm_->getRobotModel()) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to load robot model"); + return false; + } + + // Select planning group to use for predefined poses + std::string predefined_poses_group = opts.getPredefinedPosesGroup(); + if (predefined_poses_group.empty()) + { + ROS_WARN_NAMED(LOGNAME, "Parameter predefined_poses_group is not set, using default planning group instead"); + predefined_poses_group = opts.getGroupName(); + } + const auto& joint_model_group = psm_->getRobotModel()->getJointModelGroup(predefined_poses_group); + if (!joint_model_group) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Robot model has no joint model group named '" << predefined_poses_group << "'"); + return false; + } + + // Iterate over all predefined poses and use each as start and goal states + moveit::core::RobotState robot_state(psm_->getRobotModel()); + start_states.clear(); + goal_constraints.clear(); + for (const auto& pose_id : opts.getPredefinedPoses()) + { + if (!robot_state.setToDefaultValues(joint_model_group, pose_id)) + { + ROS_WARN_STREAM_NAMED(LOGNAME, "Failed to set robot state to named target '" << pose_id << "'"); + continue; + } + // Create start state + start_states.emplace_back(); + start_states.back().name = pose_id; + moveit::core::robotStateToRobotStateMsg(robot_state, start_states.back().state); + + // Create goal constraints + goal_constraints.emplace_back(); + goal_constraints.back().name = pose_id; + goal_constraints.back().constraints.push_back( + kinematic_constraints::constructGoalConstraints(robot_state, joint_model_group)); + } + if (start_states.empty() || goal_constraints.empty()) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to init start and goal states from predefined_poses"); + return false; + } + + // We don't use path/trajectory constraints or custom queries + path_constraints.clear(); + traj_constraints.clear(); + queries.clear(); + return true; + } + +private: + planning_scene_monitor::PlanningSceneMonitorPtr psm_; +}; +} // namespace moveit_ros_benchmarks + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "moveit_run_benchmark"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + // Read benchmark options from param server + moveit_ros_benchmarks::BenchmarkOptions opts(ros::this_node::getName()); + // Setup benchmark server + moveit_ros_benchmarks::CombinePredefinedPosesBenchmark server; + + std::vector planning_pipelines; + opts.getPlanningPipelineNames(planning_pipelines); + server.initialize(planning_pipelines); + + // Running benchmarks + if (!server.runBenchmarks(opts)) + ROS_ERROR("Failed to run all benchmarks"); +} diff --git a/moveit_ros/manipulation/CHANGELOG.rst b/moveit_ros/manipulation/CHANGELOG.rst index 3a939ead3b..c531e54c7a 100644 --- a/moveit_ros/manipulation/CHANGELOG.rst +++ b/moveit_ros/manipulation/CHANGELOG.rst @@ -2,6 +2,37 @@ Changelog for package moveit_ros_manipulation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + +1.1.0 (2020-09-04) +------------------ + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-15) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -70,7 +101,7 @@ Changelog for package moveit_ros_manipulation 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_ros/manipulation/CMakeLists.txt b/moveit_ros/manipulation/CMakeLists.txt index 13db73b68f..5a6d452a53 100644 --- a/moveit_ros/manipulation/CMakeLists.txt +++ b/moveit_ros/manipulation/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_manipulation) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -31,9 +35,12 @@ catkin_package( LIBRARIES moveit_pick_place_planner CATKIN_DEPENDS + actionlib + dynamic_reconfigure moveit_core moveit_msgs moveit_ros_planning + roscpp DEPENDS EIGEN3 ) @@ -41,9 +48,9 @@ catkin_package( include_directories(pick_place/include) include_directories(move_group_pick_place_capability/include) -include_directories(${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS}) include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) add_subdirectory(pick_place) diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/CMakeLists.txt b/moveit_ros/manipulation/move_group_pick_place_capability/CMakeLists.txt index 8cdfecab66..036197fc58 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/CMakeLists.txt +++ b/moveit_ros/manipulation/move_group_pick_place_capability/CMakeLists.txt @@ -11,6 +11,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} moveit_pick_place_planner ${catkin_LIBR install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h b/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h index 8058e204a4..5feaa87575 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h +++ b/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_PICK_PLACE_CAPABILITY_NAMES -#define MOVEIT_MOVE_GROUP_PICK_PLACE_CAPABILITY_NAMES +#pragma once #include @@ -43,6 +42,4 @@ namespace move_group { static const std::string PICKUP_ACTION = "pickup"; // name of 'pickup' action static const std::string PLACE_ACTION = "place"; // name of 'place' action -} - -#endif +} // namespace move_group diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp index 4d413e1c9b..8e2d0c9409 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp +++ b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp @@ -54,8 +54,7 @@ void move_group::MoveGroupPickPlaceAction::initialize() // start the pickup action server pickup_action_server_.reset(new actionlib::SimpleActionServer( - root_node_handle_, PICKUP_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, _1), - false)); + root_node_handle_, PICKUP_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, _1), false)); pickup_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this)); pickup_action_server_->start(); @@ -283,8 +282,8 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanAndExecute( action_res.error_code = plan.error_code_; } -void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanAndExecute( - const moveit_msgs::PlaceGoalConstPtr& goal, moveit_msgs::PlaceResult& action_res) +void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanAndExecute(const moveit_msgs::PlaceGoalConstPtr& goal, + moveit_msgs::PlaceResult& action_res) { plan_execution::PlanExecution::Options opt; diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h index d76e6244f2..5945235850 100644 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h +++ b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_PICK_PLACE_ACTION_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_PICK_PLACE_ACTION_CAPABILITY_ +#pragma once #include #include @@ -57,8 +56,7 @@ class MoveGroupPickPlaceAction : public MoveGroupCapability void executePickupCallback(const moveit_msgs::PickupGoalConstPtr& goal); void executePlaceCallback(const moveit_msgs::PlaceGoalConstPtr& goal); - void executePickupCallbackPlanOnly(const moveit_msgs::PickupGoalConstPtr& goal, - moveit_msgs::PickupResult& action_res); + void executePickupCallbackPlanOnly(const moveit_msgs::PickupGoalConstPtr& goal, moveit_msgs::PickupResult& action_res); void executePickupCallbackPlanAndExecute(const moveit_msgs::PickupGoalConstPtr& goal, moveit_msgs::PickupResult& action_res); @@ -100,6 +98,4 @@ class MoveGroupPickPlaceAction : public MoveGroupCapability ros::ServiceClient grasp_planning_service_; }; -} - -#endif +} // namespace move_group diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml index d8168e7329..7362ba4306 100644 --- a/moveit_ros/manipulation/package.xml +++ b/moveit_ros/manipulation/package.xml @@ -1,14 +1,14 @@ moveit_ros_manipulation - 1.0.1 - Components of MoveIt! used for manipulation + 1.1.1 + Components of MoveIt used for manipulation Ioan Sucan Sachin Chitta Michael GΓΆrner Michael Ferguson - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/manipulation/pick_place/CMakeLists.txt b/moveit_ros/manipulation/pick_place/CMakeLists.txt index 4e41c8a4e5..fb371ae640 100644 --- a/moveit_ros/manipulation/pick_place/CMakeLists.txt +++ b/moveit_ros/manipulation/pick_place/CMakeLists.txt @@ -14,5 +14,8 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h index 2709de5136..8c7014495f 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_PICK_PLACE_APPROACH_AND_TRANSLATE_STAGE_ -#define MOVEIT_PICK_PLACE_APPROACH_AND_TRANSLATE_STAGE_ +#pragma once #include #include @@ -61,6 +60,4 @@ class ApproachAndTranslateStage : public ManipulationStage double max_step_; double jump_factor_; }; -} - -#endif +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h index f1bc6a920f..942e8aca59 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PICK_PLACE_MANIPULATION_PIPELINE_ -#define MOVEIT_PICK_PLACE_MANIPULATION_PIPELINE_ +#pragma once #include #include @@ -116,6 +115,4 @@ class ManipulationPipeline bool stop_processing_; }; -} - -#endif +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h index cc3ff2fe74..56dd3523a7 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_PICK_PLACE_MANIPULATION_PLAN_ -#define MOVEIT_PICK_PLACE_MANIPULATION_PLAN_ +#pragma once #include #include @@ -56,17 +55,17 @@ MOVEIT_STRUCT_FORWARD(ManipulationPlanSharedData); struct ManipulationPlanSharedData { ManipulationPlanSharedData() - : planning_group_(NULL) - , end_effector_group_(NULL) - , ik_link_(NULL) + : planning_group_(nullptr) + , end_effector_group_(nullptr) + , ik_link_(nullptr) , max_goal_sampling_attempts_(0) , minimize_object_distance_(false) { } - const robot_model::JointModelGroup* planning_group_; - const robot_model::JointModelGroup* end_effector_group_; - const robot_model::LinkModel* ik_link_; + const moveit::core::JointModelGroup* planning_group_; + const moveit::core::JointModelGroup* end_effector_group_; + const moveit::core::LinkModel* ik_link_; unsigned int max_goal_sampling_attempts_; @@ -126,9 +125,9 @@ struct ManipulationPlan // Allows for the sampling of a kineamtic state for a particular group of a robot constraint_samplers::ConstraintSamplerPtr goal_sampler_; - std::vector possible_goal_states_; + std::vector possible_goal_states_; - robot_state::RobotStatePtr approach_state_; + moveit::core::RobotStatePtr approach_state_; // The sequence of trajectories produced for execution std::vector trajectories_; @@ -142,6 +141,4 @@ struct ManipulationPlan // An id for this plan; this is usually the index of the Grasp / PlaceLocation in the input request std::size_t id_; }; -} - -#endif +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h index 5b36f9fb21..14636fe688 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_PICK_PLACE_MANIPULATION_STAGE_ -#define MOVEIT_PICK_PLACE_MANIPULATION_STAGE_ +#pragma once #include #include @@ -43,7 +42,7 @@ namespace pick_place { -MOVEIT_CLASS_FORWARD(ManipulationStage); +MOVEIT_CLASS_FORWARD(ManipulationStage); // Defines ManipulationStagePtr, ConstPtr, WeakPtr... etc class ManipulationStage { @@ -83,6 +82,4 @@ class ManipulationStage bool signal_stop_; bool verbose_; }; -} - -#endif +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h index 1507ebeaee..7781179ddf 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PICK_PLACE_PICK_PLACE_ -#define MOVEIT_PICK_PLACE_PICK_PLACE_ +#pragma once #include #include @@ -49,7 +48,7 @@ namespace pick_place { -MOVEIT_CLASS_FORWARD(PickPlace); +MOVEIT_CLASS_FORWARD(PickPlace); // Defines PickPlacePtr, ConstPtr, WeakPtr... etc class PickPlacePlanBase { @@ -93,7 +92,7 @@ class PickPlacePlanBase moveit_msgs::MoveItErrorCodes error_code_; }; -MOVEIT_CLASS_FORWARD(PickPlan); +MOVEIT_CLASS_FORWARD(PickPlan); // Defines PickPlanPtr, ConstPtr, WeakPtr... etc class PickPlan : public PickPlacePlanBase { @@ -102,7 +101,7 @@ class PickPlan : public PickPlacePlanBase bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::PickupGoal& goal); }; -MOVEIT_CLASS_FORWARD(PlacePlan); +MOVEIT_CLASS_FORWARD(PlacePlan); // Defines PlacePlanPtr, ConstPtr, WeakPtr... etc class PlacePlan : public PickPlacePlanBase { @@ -132,7 +131,7 @@ class PickPlace : private boost::noncopyable, public std::enable_shared_from_thi return planning_pipeline_; } - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return planning_pipeline_->getRobotModel(); } @@ -164,6 +163,4 @@ class PickPlace : private boost::noncopyable, public std::enable_shared_from_thi constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_; }; -} - -#endif +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h index baabe4963d..38be2ba658 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h @@ -1,41 +1,40 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PICK_PLACE_PICK_PLACE_PARAMS_ -#define MOVEIT_PICK_PLACE_PICK_PLACE_PARAMS_ +#pragma once namespace pick_place { @@ -51,6 +50,4 @@ struct PickPlaceParams // Get access to a global variable that contains the pick & place params. const PickPlaceParams& GetGlobalPickPlaceParams(); -} - -#endif +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h index 58a33e98d8..7b651e196d 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_PICK_PLACE_PLAN_STAGE_ -#define MOVEIT_PICK_PLACE_PLAN_STAGE_ +#pragma once #include #include @@ -57,6 +56,4 @@ class PlanStage : public ManipulationStage planning_scene::PlanningSceneConstPtr planning_scene_; planning_pipeline::PlanningPipelinePtr planning_pipeline_; }; -} - -#endif +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h index 7fe44f06bf..0e5ec7b0f3 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_PICK_PLACE_REACHABLE_VALID_POSE_FILTER_ -#define MOVEIT_PICK_PLACE_REACHABLE_VALID_POSE_FILTER_ +#pragma once #include #include @@ -53,12 +52,10 @@ class ReachableAndValidPoseFilter : public ManipulationStage bool evaluate(const ManipulationPlanPtr& plan) const override; private: - bool isEndEffectorFree(const ManipulationPlanPtr& plan, robot_state::RobotState& token_state) const; + bool isEndEffectorFree(const ManipulationPlanPtr& plan, moveit::core::RobotState& token_state) const; planning_scene::PlanningSceneConstPtr planning_scene_; collision_detection::AllowedCollisionMatrixConstPtr collision_matrix_; constraint_samplers::ConstraintSamplerManagerPtr constraints_sampler_manager_; }; -} - -#endif +} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp index df25737485..4a667fd489 100644 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp @@ -58,8 +58,8 @@ namespace { bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene, const collision_detection::AllowedCollisionMatrix* collision_matrix, bool verbose, - const trajectory_msgs::JointTrajectory* grasp_posture, robot_state::RobotState* state, - const robot_state::JointModelGroup* group, const double* joint_group_variable_values) + const trajectory_msgs::JointTrajectory* grasp_posture, moveit::core::RobotState* state, + const moveit::core::JointModelGroup* group, const double* joint_group_variable_values) { state->setJointGroupPositions(group, joint_group_variable_values); @@ -90,11 +90,11 @@ bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene, return planning_scene->isStateFeasible(*state); } -bool samplePossibleGoalStates(const ManipulationPlanPtr& plan, const robot_state::RobotState& reference_state, +bool samplePossibleGoalStates(const ManipulationPlanPtr& plan, const moveit::core::RobotState& reference_state, double min_distance, unsigned int attempts) { // initialize with scene state - robot_state::RobotStatePtr token_state(new robot_state::RobotState(reference_state)); + moveit::core::RobotStatePtr token_state(new moveit::core::RobotState(reference_state)); for (unsigned int j = 0; j < attempts; ++j) { double min_d = std::numeric_limits::infinity(); @@ -157,8 +157,8 @@ void addGripperTrajectory(const ManipulationPlanPtr& plan, // Check if a "closed" end effector configuration was specified if (!plan->retreat_posture_.joint_names.empty()) { - robot_state::RobotStatePtr ee_closed_state( - new robot_state::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint())); + moveit::core::RobotStatePtr ee_closed_state( + new moveit::core::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint())); robot_trajectory::RobotTrajectoryPtr ee_closed_traj(new robot_trajectory::RobotTrajectory( ee_closed_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName())); @@ -195,7 +195,7 @@ void addGripperTrajectory(const ManipulationPlanPtr& plan, bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const { - const robot_model::JointModelGroup* jmg = plan->shared_data_->planning_group_; + const moveit::core::JointModelGroup* jmg = plan->shared_data_->planning_group_; // compute what the maximum distance reported between any two states in the planning group could be, and keep 1% of // that; // this is the minimum distance between sampled goal states @@ -208,21 +208,23 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // if translation vectors are specified in the frame of the ik link name, then we assume the frame is local; // otherwise, the frame is global - bool approach_direction_is_global_frame = !robot_state::Transforms::sameFrame( + bool approach_direction_is_global_frame = !moveit::core::Transforms::sameFrame( plan->approach_.direction.header.frame_id, plan->shared_data_->ik_link_->getName()); - bool retreat_direction_is_global_frame = !robot_state::Transforms::sameFrame(plan->retreat_.direction.header.frame_id, - plan->shared_data_->ik_link_->getName()); + bool retreat_direction_is_global_frame = !moveit::core::Transforms::sameFrame( + plan->retreat_.direction.header.frame_id, plan->shared_data_->ik_link_->getName()); // transform the input vectors in accordance to frame specified in the header; if (approach_direction_is_global_frame) + // getFrameTransform() returns a valid isometry by contract approach_direction = - planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).rotation() * approach_direction; + planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).linear() * approach_direction; if (retreat_direction_is_global_frame) + // getFrameTransform() returns a valid isometry by contract retreat_direction = - planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).rotation() * retreat_direction; + planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).linear() * retreat_direction; // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping - robot_state::GroupStateValidityCallbackFn approach_valid_callback = + moveit::core::GroupStateValidityCallbackFn approach_valid_callback = boost::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, &plan->approach_posture_, _1, _2, _3); plan->goal_sampler_->setVerbose(verbose_); @@ -236,8 +238,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const if (plan->shared_data_->minimize_object_distance_) { static const double MAX_CLOSE_UP_DIST = 1.0; - robot_state::RobotStatePtr close_up_state(new robot_state::RobotState(*plan->possible_goal_states_[i])); - std::vector close_up_states; + moveit::core::RobotStatePtr close_up_state(new moveit::core::RobotState(*plan->possible_goal_states_[i])); + std::vector close_up_states; double d_close_up = moveit::core::CartesianInterpolator::computeCartesianPath( close_up_state.get(), plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_, approach_direction, approach_direction_is_global_frame, MAX_CLOSE_UP_DIST, @@ -248,9 +250,9 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const } // try to compute a straight line path that arrives at the goal using the specified approach direction - robot_state::RobotStatePtr first_approach_state(new robot_state::RobotState(*plan->possible_goal_states_[i])); + moveit::core::RobotStatePtr first_approach_state(new moveit::core::RobotState(*plan->possible_goal_states_[i])); - std::vector approach_states; + std::vector approach_states; double d_approach = moveit::core::CartesianInterpolator::computeCartesianPath( first_approach_state.get(), plan->shared_data_->planning_group_, approach_states, plan->shared_data_->ik_link_, -approach_direction, approach_direction_is_global_frame, @@ -273,14 +275,14 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // state validity checking during the retreat after the grasp must ensure the gripper posture is that of the // actual grasp - robot_state::GroupStateValidityCallbackFn retreat_valid_callback = + moveit::core::GroupStateValidityCallbackFn retreat_valid_callback = boost::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_, &plan->retreat_posture_, _1, _2, _3); // try to compute a straight line path that moves from the goal in a desired direction - robot_state::RobotStatePtr last_retreat_state( - new robot_state::RobotState(planning_scene_after_approach->getCurrentState())); - std::vector retreat_states; + moveit::core::RobotStatePtr last_retreat_state( + new moveit::core::RobotState(planning_scene_after_approach->getCurrentState())); + std::vector retreat_states; double d_retreat = moveit::core::CartesianInterpolator::computeCartesianPath( last_retreat_state.get(), plan->shared_data_->planning_group_, retreat_states, plan->shared_data_->ik_link_, retreat_direction, retreat_direction_is_global_frame, diff --git a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp index 1a3a6aab6b..29cab837f5 100644 --- a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp +++ b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Ioan Sucan */ @@ -209,8 +209,8 @@ void ManipulationPipeline::push(const ManipulationPlanPtr& plan) { boost::mutex::scoped_lock slock(queue_access_lock_); queue_.push_back(plan); - ROS_INFO_STREAM_NAMED("manipulation", "Added plan for pipeline '" << name_ << "'. Queue is now of size " - << queue_.size()); + ROS_INFO_STREAM_NAMED("manipulation", + "Added plan for pipeline '" << name_ << "'. Queue is now of size " << queue_.size()); queue_access_cond_.notify_all(); } diff --git a/moveit_ros/manipulation/pick_place/src/pick.cpp b/moveit_ros/manipulation/pick_place/src/pick.cpp index 5b922a6589..def95e4c2d 100644 --- a/moveit_ros/manipulation/pick_place/src/pick.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include namespace pick_place @@ -72,7 +73,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, std::string end_effector = goal.end_effector; if (end_effector.empty() && !planning_group.empty()) { - const robot_model::JointModelGroup* jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group); + const moveit::core::JointModelGroup* jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group); if (!jmg) { error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; @@ -90,7 +91,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, } else if (!end_effector.empty() && planning_group.empty()) { - const robot_model::JointModelGroup* jmg = planning_scene->getRobotModel()->getEndEffector(end_effector); + const moveit::core::JointModelGroup* jmg = planning_scene->getRobotModel()->getEndEffector(end_effector); if (!jmg) { error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; @@ -108,7 +109,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, ROS_INFO_STREAM_NAMED("manipulation", "Assuming the planning group for end effector '" << end_effector << "' is '" << planning_group << "'"); } - const robot_model::JointModelGroup* eef = + const moveit::core::JointModelGroup* eef = end_effector.empty() ? nullptr : planning_scene->getRobotModel()->getEndEffector(end_effector); if (!eef) { @@ -230,7 +231,7 @@ PickPlanPtr PickPlace::planPick(const planning_scene::PlanningSceneConstPtr& pla { PickPlanPtr p(new PickPlan(shared_from_this())); - if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff)) + if (moveit::core::isEmpty(goal.planning_options.planning_scene_diff)) p->plan(planning_scene, goal); else p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal); diff --git a/moveit_ros/manipulation/pick_place/src/pick_place.cpp b/moveit_ros/manipulation/pick_place/src/pick_place.cpp index 0c7ceb41d7..61fbcf68db 100644 --- a/moveit_ros/manipulation/pick_place/src/pick_place.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick_place.cpp @@ -38,7 +38,6 @@ #include #include #include -#include namespace pick_place { @@ -125,7 +124,7 @@ void PickPlace::visualizePlan(const ManipulationPlanPtr& plan) const continue; if (first) { - robot_state::robotStateToRobotStateMsg(traj.trajectory_->getFirstWayPoint(), dtraj.trajectory_start); + moveit::core::robotStateToRobotStateMsg(traj.trajectory_->getFirstWayPoint(), dtraj.trajectory_start); first = false; } dtraj.trajectory.resize(dtraj.trajectory.size() + 1); @@ -178,14 +177,14 @@ void PickPlace::visualizeGrasps(const std::vector& plans) c if (plans.empty()) return; - robot_state::RobotState state(getRobotModel()); + moveit::core::RobotState state(getRobotModel()); state.setToDefaultValues(); static std::vector colors(setupDefaultGraspColors()); visualization_msgs::MarkerArray ma; for (const ManipulationPlanPtr& plan : plans) { - const robot_model::JointModelGroup* jmg = plan->shared_data_->end_effector_group_; + const moveit::core::JointModelGroup* jmg = plan->shared_data_->end_effector_group_; if (jmg) { unsigned int type = std::min(plan->processing_stage_, colors.size() - 1); diff --git a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp index 250c42b621..addc270d44 100644 --- a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp +++ b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp @@ -61,7 +61,7 @@ class DynamicReconfigureImpl private: PickPlaceParams params_; - void dynamicReconfigureCallback(PickPlaceDynamicReconfigureConfig& config, uint32_t level) + void dynamicReconfigureCallback(PickPlaceDynamicReconfigureConfig& config, uint32_t /*level*/) { params_.max_goal_count_ = config.max_attempted_states_per_pose; params_.max_fail_ = config.max_consecutive_fail_attempts; diff --git a/moveit_ros/manipulation/pick_place/src/place.cpp b/moveit_ros/manipulation/pick_place/src/place.cpp index 098d5c1438..92eb153a37 100644 --- a/moveit_ros/manipulation/pick_place/src/place.cpp +++ b/moveit_ros/manipulation/pick_place/src/place.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -51,7 +52,7 @@ PlacePlan::PlacePlan(const PickPlaceConstPtr& pick_place) : PickPlacePlanBase(pi namespace { bool transformToEndEffectorGoal(const geometry_msgs::PoseStamped& goal_pose, - const robot_state::AttachedBody* attached_body, geometry_msgs::PoseStamped& place_pose) + const moveit::core::AttachedBody* attached_body, geometry_msgs::PoseStamped& place_pose) { const EigenSTL::vector_Isometry3d& fixed_transforms = attached_body->getFixedTransforms(); if (fixed_transforms.empty()) @@ -71,8 +72,8 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene double timeout = goal.allowed_planning_time; ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout); std::string attached_object_name = goal.attached_object_name; - const robot_model::JointModelGroup* jmg = nullptr; - const robot_model::JointModelGroup* eef = nullptr; + const moveit::core::JointModelGroup* jmg = nullptr; + const moveit::core::JointModelGroup* eef = nullptr; // if the group specified is actually an end-effector, we use it as such if (planning_scene->getRobotModel()->hasEndEffector(goal.group_name)) @@ -103,8 +104,8 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene const std::vector& eef_names = jmg->getAttachedEndEffectorNames(); if (eef_names.empty()) { - ROS_ERROR_STREAM_NAMED("manipulation", "There are no end-effectors specified for group '" << goal.group_name - << "'"); + ROS_ERROR_STREAM_NAMED("manipulation", + "There are no end-effectors specified for group '" << goal.group_name << "'"); error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } @@ -112,8 +113,8 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene // check to see if there is an end effector that has attached objects associaded, so we can complete the place for (const std::string& eef_name : eef_names) { - std::vector attached_bodies; - const robot_model::JointModelGroup* eg = planning_scene->getRobotModel()->getEndEffector(eef_name); + std::vector attached_bodies; + const moveit::core::JointModelGroup* eg = planning_scene->getRobotModel()->getEndEffector(eef_name); if (eg) { // see if there are objects attached to links in the eef @@ -121,11 +122,11 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene // is is often possible that the objects are attached to the same link that the eef itself is attached, // so we check for attached bodies there as well - const robot_model::LinkModel* attached_link_model = + const moveit::core::LinkModel* attached_link_model = planning_scene->getRobotModel()->getLinkModel(eg->getEndEffectorParentGroup().second); if (attached_link_model) { - std::vector attached_bodies2; + std::vector attached_bodies2; planning_scene->getCurrentState().getAttachedBodies(attached_bodies2, attached_link_model); attached_bodies.insert(attached_bodies.end(), attached_bodies2.begin(), attached_bodies2.end()); } @@ -171,14 +172,15 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene // if we know the attached object, but not the eef, we can try to identify that if (!attached_object_name.empty() && !eef) { - const robot_state::AttachedBody* attached_body = + const moveit::core::AttachedBody* attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name); if (attached_body) { // get the robot model link this attached body is associated to - const robot_model::LinkModel* link = attached_body->getAttachedLink(); + const moveit::core::LinkModel* link = attached_body->getAttachedLink(); // check to see if there is a unique end effector containing the link - const std::vector& eefs = planning_scene->getRobotModel()->getEndEffectors(); + const std::vector& eefs = + planning_scene->getRobotModel()->getEndEffectors(); for (const moveit::core::JointModelGroup* end_effector : eefs) if (end_effector->hasLinkModel(link->getName())) { @@ -223,7 +225,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene { // in the first try, look for objects attached to the eef, if the eef is known; // otherwise, look for attached bodies in the planning group itself - std::vector attached_bodies; + std::vector attached_bodies; planning_scene->getCurrentState().getAttachedBodies(attached_bodies, loop_count == 0 ? eef : jmg); loop_count++; @@ -239,7 +241,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene attached_object_name = attached_bodies[0]->getName(); } - const robot_state::AttachedBody* attached_body = + const moveit::core::AttachedBody* attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name); if (!attached_body) { @@ -369,7 +371,7 @@ PlacePlanPtr PickPlace::planPlace(const planning_scene::PlanningSceneConstPtr& p const moveit_msgs::PlaceGoal& goal) const { PlacePlanPtr p(new PlacePlan(shared_from_this())); - if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff)) + if (moveit::core::isEmpty(goal.planning_options.planning_scene_diff)) p->plan(planning_scene, goal); else p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal); diff --git a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp b/moveit_ros/manipulation/pick_place/src/plan_stage.cpp index 1d81f2e779..c704cd3ede 100644 --- a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/plan_stage.cpp @@ -65,8 +65,8 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const req.planner_id = plan->shared_data_->planner_id_; req.start_state.is_diff = true; - req.goal_constraints.resize( - 1, kinematic_constraints::constructGoalConstraints(*plan->approach_state_, plan->shared_data_->planning_group_)); + req.goal_constraints.resize(1, kinematic_constraints::constructGoalConstraints(*plan->approach_state_, + plan->shared_data_->planning_group_)); unsigned int attempts = 0; do // give the planner two chances { @@ -77,7 +77,7 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const // We have a valid motion plan, now apply pre-approach end effector posture (open gripper) if applicable if (!plan->approach_posture_.joint_names.empty()) { - robot_state::RobotStatePtr pre_approach_state(new robot_state::RobotState(res.trajectory_->getLastWayPoint())); + moveit::core::RobotStatePtr pre_approach_state(new moveit::core::RobotState(res.trajectory_->getLastWayPoint())); robot_trajectory::RobotTrajectoryPtr pre_approach_traj(new robot_trajectory::RobotTrajectory( pre_approach_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName())); pre_approach_traj->setRobotTrajectoryMsg(*pre_approach_state, plan->approach_posture_); @@ -95,8 +95,7 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const ROS_INFO_STREAM("Adding default duration of " << PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION << " seconds to the grasp closure time. Assign time_from_start " << "to your trajectory to avoid this."); - pre_approach_traj->addPrefixWayPoint(pre_approach_state, - PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION); + pre_approach_traj->addPrefixWayPoint(pre_approach_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION); } // Add the open gripper trajectory to the plan diff --git a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp index 36221b884e..986c05602e 100644 --- a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp +++ b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp @@ -55,8 +55,8 @@ namespace { bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene, const collision_detection::AllowedCollisionMatrix* collision_matrix, bool verbose, - const pick_place::ManipulationPlan* manipulation_plan, robot_state::RobotState* state, - const robot_model::JointModelGroup* group, const double* joint_group_variable_values) + const pick_place::ManipulationPlan* manipulation_plan, moveit::core::RobotState* state, + const moveit::core::JointModelGroup* group, const double* joint_group_variable_values) { collision_detection::CollisionRequest req; req.verbose = verbose; @@ -90,7 +90,7 @@ bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene, } // namespace bool pick_place::ReachableAndValidPoseFilter::isEndEffectorFree(const ManipulationPlanPtr& plan, - robot_state::RobotState& token_state) const + moveit::core::RobotState& token_state) const { tf2::fromMsg(plan->goal_pose_.pose, plan->transformed_goal_pose_); plan->transformed_goal_pose_ = @@ -107,14 +107,14 @@ bool pick_place::ReachableAndValidPoseFilter::isEndEffectorFree(const Manipulati bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr& plan) const { // initialize with scene state - robot_state::RobotStatePtr token_state(new robot_state::RobotState(planning_scene_->getCurrentState())); + moveit::core::RobotStatePtr token_state(new moveit::core::RobotState(planning_scene_->getCurrentState())); if (isEndEffectorFree(plan, *token_state)) { // update the goal pose message if anything has changed; this is because the name of the frame in the input goal // pose // can be that of objects in the collision world but most components are unaware of those transforms, // so we convert to a frame that is certainly known - if (!robot_state::Transforms::sameFrame(planning_scene_->getPlanningFrame(), plan->goal_pose_.header.frame_id)) + if (!moveit::core::Transforms::sameFrame(planning_scene_->getPlanningFrame(), plan->goal_pose_.header.frame_id)) { plan->goal_pose_.pose = tf2::toMsg(plan->transformed_goal_pose_); plan->goal_pose_.header.frame_id = planning_scene_->getPlanningFrame(); diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst index b2aa5af725..3a395043fe 100644 --- a/moveit_ros/move_group/CHANGELOG.rst +++ b/moveit_ros/move_group/CHANGELOG.rst @@ -2,6 +2,55 @@ Changelog for package moveit_ros_move_group ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [fix] Let the max number of contacts be the amount of world objects + link models with geometry (`#2355 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, Loy van Beek, Michael GΓΆrner, v4hn + +1.1.0 (2020-09-04) +------------------ +* [feature] Start new joint_state_publisher_gui on param use_gui (`#2257 `_) +* [feature] TfPublisher: fixup and add attached collsion objects (`#1792 `_) +* [feature] move_group capability for publishing planning scene frames to the tf system (`#1761 `_) +* [feature] get_planning_scene_service: return full scene when nothing was requested (`#1424 `_) +* [feature] Separate source file for CartesianInterpolator (`#1149 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix TfPublisher subframe publishing (`#2002 `_) +* [maint] Fix compiler warnings (`#1773 `_) +* [maint] clang-tidy fixes (`#2050 `_) (`#2004 `_, `#1419 `_) +* [maint] Optional cpp version setting (`#2166 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Move isEmpty() test functions to moveit_core/utils (`#1627 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Mike Lautman, Robert Haschke, Felix von Drigalski, Jens P, Jonathan Binney, JonasTietz, Michael GΓΆrner, Tyler Weaver, Yoan Mollard, Yu, Yan, v4hn + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10 +* Contributors: Robert Haschke, Michael GΓΆrner + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Move `get_planning_scene` service into `PlanningSceneMonitor` for reusability (`#1854 `_) +* [maint] Cleanup move_group capabilities (`#1515 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -66,7 +115,7 @@ Changelog for package moveit_ros_move_group 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index 71a0bf2759..a8c2098417 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_move_group) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -26,13 +30,17 @@ catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS + actionlib moveit_core moveit_ros_planning + roscpp + std_srvs tf2_geometry_msgs ) include_directories(include) -include_directories(${catkin_INCLUDE_DIRS} +include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) add_library(moveit_move_group_capabilities_base @@ -57,6 +65,7 @@ add_library(moveit_move_group_default_capabilities src/default_capabilities/get_planning_scene_service_capability.cpp src/default_capabilities/apply_planning_scene_service_capability.cpp src/default_capabilities/clear_octomap_service_capability.cpp + src/default_capabilities/tf_publisher_capability.cpp ) set_target_properties(moveit_move_group_default_capabilities PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") add_dependencies(moveit_move_group_default_capabilities ${catkin_EXPORTED_TARGETS}) @@ -66,10 +75,13 @@ target_link_libraries(move_group moveit_move_group_capabilities_base ${catkin_LI target_link_libraries(moveit_move_group_default_capabilities moveit_move_group_capabilities_base ${catkin_LIBRARIES} ${Boost_LIBRARIES}) target_link_libraries(list_move_group_capabilities ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS move_group list_move_group_capabilities moveit_move_group_capabilities_base moveit_move_group_default_capabilities +install(TARGETS move_group list_move_group_capabilities + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(TARGETS moveit_move_group_capabilities_base moveit_move_group_default_capabilities ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) @@ -85,4 +97,5 @@ if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) # this test is flaky # add_rostest(test/test_cancel_before_plan_execution.test) + add_rostest(test/test_check_state_validity_in_empty_scene.test) endif() diff --git a/moveit_ros/move_group/default_capabilities_plugin_description.xml b/moveit_ros/move_group/default_capabilities_plugin_description.xml index b7bd5d9f0d..aff9edd872 100644 --- a/moveit_ros/move_group/default_capabilities_plugin_description.xml +++ b/moveit_ros/move_group/default_capabilities_plugin_description.xml @@ -65,5 +65,11 @@ Provide a ROS service that allows for clearing the octomap + + + + Provide a capability that publishes PlanningScene frames to the tf system + + diff --git a/moveit_ros/move_group/include/moveit/move_group/capability_names.h b/moveit_ros/move_group/include/moveit/move_group/capability_names.h index 7b76ff727b..2e5e64153d 100644 --- a/moveit_ros/move_group/include/moveit/move_group/capability_names.h +++ b/moveit_ros/move_group/include/moveit/move_group/capability_names.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_DEFAULT_CAPABILITY_NAMES -#define MOVEIT_MOVE_GROUP_DEFAULT_CAPABILITY_NAMES +#pragma once #include @@ -63,6 +62,4 @@ static const std::string APPLY_PLANNING_SCENE_SERVICE_NAME = "apply_planning_scene"; // name of the service that applies a given planning scene static const std::string CLEAR_OCTOMAP_SERVICE_NAME = "clear_octomap"; // name of the service that can be used to clear the octomap -} - -#endif +} // namespace move_group diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h b/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h index e92735fe81..cf3798738b 100644 --- a/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h +++ b/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_CAPABILITY_ +#pragma once #include #include @@ -53,7 +52,7 @@ enum MoveGroupState LOOK }; -MOVEIT_CLASS_FORWARD(MoveGroupCapability); +MOVEIT_CLASS_FORWARD(MoveGroupCapability); // Defines MoveGroupCapabilityPtr, ConstPtr, WeakPtr... etc class MoveGroupCapability { @@ -98,6 +97,4 @@ class MoveGroupCapability std::string capability_name_; MoveGroupContextPtr context_; }; -} - -#endif +} // namespace move_group diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h index 845e155b3f..4648c0fc5b 100644 --- a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h +++ b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h @@ -34,30 +34,29 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_CONTEXT_ -#define MOVEIT_MOVE_GROUP_CONTEXT_ +#pragma once #include namespace planning_scene_monitor { -MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); +MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); // Defines PlanningSceneMonitorPtr, ConstPtr, WeakPtr... etc } namespace planning_pipeline { -MOVEIT_CLASS_FORWARD(PlanningPipeline); +MOVEIT_CLASS_FORWARD(PlanningPipeline); // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc } namespace plan_execution { -MOVEIT_CLASS_FORWARD(PlanExecution); -MOVEIT_CLASS_FORWARD(PlanWithSensing); -} +MOVEIT_CLASS_FORWARD(PlanExecution); // Defines PlanExecutionPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(PlanWithSensing); // Defines PlanWithSensingPtr, ConstPtr, WeakPtr... etc +} // namespace plan_execution namespace trajectory_execution_manager { -MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); +MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); // Defines TrajectoryExecutionManagerPtr, ConstPtr, WeakPtr... etc } namespace move_group @@ -80,6 +79,4 @@ struct MoveGroupContext bool allow_trajectory_execution_; bool debug_; }; -} - -#endif +} // namespace move_group diff --git a/moveit_ros/move_group/include/moveit/move_group/node_name.h b/moveit_ros/move_group/include/moveit/move_group/node_name.h index 220dca0498..79d0ca4f16 100644 --- a/moveit_ros/move_group/include/moveit/move_group/node_name.h +++ b/moveit_ros/move_group/include/moveit/move_group/node_name.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_NODE_NAME -#define MOVEIT_MOVE_GROUP_NODE_NAME +#pragma once #include @@ -43,5 +42,3 @@ namespace move_group { static const std::string NODE_NAME = "move_group"; // name of node } - -#endif diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index ffed75a12d..93f84d7ce3 100644 --- a/moveit_ros/move_group/package.xml +++ b/moveit_ros/move_group/package.xml @@ -1,13 +1,13 @@ moveit_ros_move_group - 1.0.1 + 1.1.1 The move_group node for MoveIt Ioan Sucan Sachin Chitta Michael Ferguson Michael GΓΆrner - MoveIt! Release Team + MoveIt Release Team BSD @@ -30,7 +30,7 @@ moveit_kinematics rostest - moveit_resources + moveit_resources_fanuc_moveit_config diff --git a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h index 3bcd7aeec2..858db10d64 100644 --- a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h @@ -34,8 +34,7 @@ /* Author: Michael 'v4hn' Goerner */ -#ifndef MOVEIT_MOVE_GROUP_APPLY_PLANNING_SCENE_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_APPLY_PLANNING_SCENE_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -58,6 +57,4 @@ class ApplyPlanningSceneService : public MoveGroupCapability ros::ServiceServer service_; }; -} - -#endif // MOVEIT_MOVE_GROUP_APPLY_PLANNING_SCENE_SERVICE_CAPABILITY_ +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 15d5d29c60..e2b939dadc 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -36,7 +36,7 @@ #include "cartesian_path_service_capability.h" #include -#include +#include #include #include #include @@ -49,8 +49,8 @@ namespace { bool isStateValid(const planning_scene::PlanningScene* planning_scene, - const kinematic_constraints::KinematicConstraintSet* constraint_set, robot_state::RobotState* state, - const robot_state::JointModelGroup* group, const double* ik_solution) + const kinematic_constraints::KinematicConstraintSet* constraint_set, moveit::core::RobotState* state, + const moveit::core::JointModelGroup* group, const double* ik_solution) { state->setJointGroupPositions(group, ik_solution); state->update(); @@ -80,10 +80,10 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath ROS_INFO_NAMED(getName(), "Received request to compute Cartesian path"); context_->planning_scene_monitor_->updateFrameTransforms(); - robot_state::RobotState start_state = + moveit::core::RobotState start_state = planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState(); - robot_state::robotStateMsgToRobotState(req.start_state, start_state); - if (const robot_model::JointModelGroup* jmg = start_state.getJointModelGroup(req.group_name)) + moveit::core::robotStateMsgToRobotState(req.start_state, start_state); + if (const moveit::core::JointModelGroup* jmg = start_state.getJointModelGroup(req.group_name)) { std::string link_name = req.link_name; if (link_name.empty() && !jmg->getLinkModelNames().empty()) @@ -93,8 +93,8 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath EigenSTL::vector_Isometry3d waypoints(req.waypoints.size()); const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame(); bool no_transform = req.header.frame_id.empty() || - robot_state::Transforms::sameFrame(req.header.frame_id, default_frame) || - robot_state::Transforms::sameFrame(req.header.frame_id, link_name); + moveit::core::Transforms::sameFrame(req.header.frame_id, default_frame) || + moveit::core::Transforms::sameFrame(req.header.frame_id, link_name); for (std::size_t i = 0; i < req.waypoints.size(); ++i) { @@ -128,10 +128,10 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath { if (!waypoints.empty()) { - robot_state::GroupStateValidityCallbackFn constraint_fn; + moveit::core::GroupStateValidityCallbackFn constraint_fn; std::unique_ptr ls; std::unique_ptr kset; - if (req.avoid_collisions || !kinematic_constraints::isEmpty(req.path_constraints)) + if (req.avoid_collisions || !moveit::core::isEmpty(req.path_constraints)) { ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)); kset.reset(new kinematic_constraints::KinematicConstraintSet((*ls)->getRobotModel())); @@ -141,16 +141,17 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath req.avoid_collisions ? static_cast(*ls).get() : nullptr, kset->empty() ? nullptr : kset.get(), _1, _2, _3); } - bool global_frame = !robot_state::Transforms::sameFrame(link_name, req.header.frame_id); - ROS_INFO_NAMED(getName(), "Attempting to follow %u waypoints for link '%s' using a step of %lf m " - "and jump threshold %lf (in %s reference frame)", + bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req.header.frame_id); + ROS_INFO_NAMED(getName(), + "Attempting to follow %u waypoints for link '%s' using a step of %lf m " + "and jump threshold %lf (in %s reference frame)", (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, req.jump_threshold, global_frame ? "global" : "link"); - std::vector traj; + std::vector traj; res.fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &start_state, jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame, moveit::core::MaxEEFStep(req.max_step), moveit::core::JumpThreshold(req.jump_threshold), constraint_fn); - robot_state::robotStateToRobotStateMsg(start_state, res.start_state); + moveit::core::robotStateToRobotStateMsg(start_state, res.start_state); robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req.group_name); for (const moveit::core::RobotStatePtr& traj_state : traj) @@ -169,7 +170,7 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath moveit_msgs::DisplayTrajectory disp; disp.model_id = context_->planning_scene_monitor_->getRobotModel()->getName(); disp.trajectory.resize(1, res.solution); - robot_state::robotStateToRobotStateMsg(rt.getFirstWayPoint(), disp.trajectory_start); + moveit::core::robotStateToRobotStateMsg(rt.getFirstWayPoint(), disp.trajectory_start); display_path_.publish(disp); } } diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h index cf1fbe9a47..aa000aad07 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_CARTESIAN_PATH_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_CARTESIAN_PATH_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -56,6 +55,4 @@ class MoveGroupCartesianPathService : public MoveGroupCapability ros::Publisher display_path_; bool display_computed_paths_; }; -} - -#endif +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp index 48aa594454..e2c8d1a814 100644 --- a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp @@ -46,7 +46,8 @@ void move_group::ClearOctomapService::initialize() service_ = root_node_handle_.advertiseService(CLEAR_OCTOMAP_SERVICE_NAME, &ClearOctomapService::clearOctomap, this); } -bool move_group::ClearOctomapService::clearOctomap(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) +bool move_group::ClearOctomapService::clearOctomap(std_srvs::Empty::Request& /*unused*/, + std_srvs::Empty::Response& /*unused*/) { if (!context_->planning_scene_monitor_) { diff --git a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h index 401a31f8f6..b43edafcf2 100644 --- a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h @@ -34,8 +34,7 @@ /* Author: David Hershberger */ -#ifndef MOVEIT_MOVE_GROUP_CLEAR_OCTOMAP_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_CLEAR_OCTOMAP_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -54,6 +53,4 @@ class ClearOctomapService : public MoveGroupCapability ros::ServiceServer service_; }; -} - -#endif // MOVEIT_MOVE_GROUP_CLEAR_OCTOMAP_SERVICE_CAPABILITY_ +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.h b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.h index c143b1e20b..f98965c8ba 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.h @@ -38,8 +38,7 @@ * Author: Kentaro Wada * */ -#ifndef MOVEIT_MOVE_GROUP_EXECUTE_TRAJECTORY_ACTION_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_EXECUTE_TRAJECTORY_ACTION_CAPABILITY_ +#pragma once #include #include @@ -66,5 +65,3 @@ class MoveGroupExecuteTrajectoryAction : public MoveGroupCapability }; } // namespace move_group - -#endif // MOVEIT_MOVE_GROUP_EXECUTE_TRAJECTORY_ACTION_CAPABILITY_ diff --git a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp index 89ccb8d817..5bdba89f0b 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp @@ -45,23 +45,9 @@ MoveGroupGetPlanningSceneService::MoveGroupGetPlanningSceneService() : MoveGroup void MoveGroupGetPlanningSceneService::initialize() { - get_scene_service_ = root_node_handle_.advertiseService( - GET_PLANNING_SCENE_SERVICE_NAME, &MoveGroupGetPlanningSceneService::getPlanningSceneService, this); + context_->planning_scene_monitor_->providePlanningSceneService(GET_PLANNING_SCENE_SERVICE_NAME); } -bool MoveGroupGetPlanningSceneService::getPlanningSceneService(moveit_msgs::GetPlanningScene::Request& req, - moveit_msgs::GetPlanningScene::Response& res) -{ - if (req.components.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS) - context_->planning_scene_monitor_->updateFrameTransforms(); - planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_); - - moveit_msgs::PlanningSceneComponents all_components; - all_components.components = UINT_MAX; // Return all scene components if nothing is specified. - ps->getPlanningSceneMsg(res.scene, req.components.components ? req.components : all_components); - - return true; -} } // namespace move_group #include diff --git a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h index 63fd7c35e7..96962faf68 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h @@ -34,11 +34,9 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_GET_PLANNING_SCENE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_GET_PLANNING_SCENE_CAPABILITY_ +#pragma once #include -#include namespace move_group { @@ -48,13 +46,5 @@ class MoveGroupGetPlanningSceneService : public MoveGroupCapability MoveGroupGetPlanningSceneService(); void initialize() override; - -private: - bool getPlanningSceneService(moveit_msgs::GetPlanningScene::Request& req, - moveit_msgs::GetPlanningScene::Response& res); - - ros::ServiceServer get_scene_service_; }; -} - -#endif +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index 73e3932e18..d977ace075 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -36,7 +36,7 @@ #include "kinematics_service_capability.h" #include -#include +#include #include #include @@ -58,7 +58,7 @@ namespace { bool isIKSolutionValid(const planning_scene::PlanningScene* planning_scene, const kinematic_constraints::KinematicConstraintSet* constraint_set, - robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, + moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, const double* ik_solution) { state->setJointGroupPositions(jmg, ik_solution); @@ -69,13 +69,13 @@ bool isIKSolutionValid(const planning_scene::PlanningScene* planning_scene, } // namespace void MoveGroupKinematicsService::computeIK(moveit_msgs::PositionIKRequest& req, moveit_msgs::RobotState& solution, - moveit_msgs::MoveItErrorCodes& error_code, robot_state::RobotState& rs, - const robot_state::GroupStateValidityCallbackFn& constraint) const + moveit_msgs::MoveItErrorCodes& error_code, moveit::core::RobotState& rs, + const moveit::core::GroupStateValidityCallbackFn& constraint) const { - const robot_state::JointModelGroup* jmg = rs.getJointModelGroup(req.group_name); + const moveit::core::JointModelGroup* jmg = rs.getJointModelGroup(req.group_name); if (jmg) { - robot_state::robotStateMsgToRobotState(req.robot_state, rs); + moveit::core::robotStateMsgToRobotState(req.robot_state, rs); const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame(); if (req.pose_stamped_vector.empty() || req.pose_stamped_vector.size() == 1) @@ -96,7 +96,7 @@ void MoveGroupKinematicsService::computeIK(moveit_msgs::PositionIKRequest& req, if (result_ik) { - robot_state::robotStateToRobotStateMsg(rs, solution, false); + moveit::core::robotStateToRobotStateMsg(rs, solution, false); error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; } else @@ -129,7 +129,7 @@ void MoveGroupKinematicsService::computeIK(moveit_msgs::PositionIKRequest& req, { if (rs.setFromIK(jmg, req_poses, req.ik_link_names, req.timeout.toSec(), constraint)) { - robot_state::robotStateToRobotStateMsg(rs, solution, false); + moveit::core::robotStateToRobotStateMsg(rs, solution, false); error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; } else @@ -148,22 +148,23 @@ bool MoveGroupKinematicsService::computeIKService(moveit_msgs::GetPositionIK::Re context_->planning_scene_monitor_->updateFrameTransforms(); // check if the planning scene needs to be kept locked; if so, call computeIK() in the scope of the lock - if (req.ik_request.avoid_collisions || !kinematic_constraints::isEmpty(req.ik_request.constraints)) + if (req.ik_request.avoid_collisions || !moveit::core::isEmpty(req.ik_request.constraints)) { planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_); kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel()); - robot_state::RobotState rs = ls->getCurrentState(); + moveit::core::RobotState rs = ls->getCurrentState(); kset.add(req.ik_request.constraints, ls->getTransforms()); computeIK(req.ik_request, res.solution, res.error_code, rs, - boost::bind(&isIKSolutionValid, req.ik_request.avoid_collisions ? - static_cast(ls).get() : - nullptr, + boost::bind(&isIKSolutionValid, + req.ik_request.avoid_collisions ? + static_cast(ls).get() : + nullptr, kset.empty() ? nullptr : &kset, _1, _2, _3)); } else { // compute unconstrained IK, no lock to planning scene maintained - robot_state::RobotState rs = + moveit::core::RobotState rs = planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState(); computeIK(req.ik_request, res.solution, res.error_code, rs); } @@ -185,13 +186,13 @@ bool MoveGroupKinematicsService::computeFKService(moveit_msgs::GetPositionFK::Re const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame(); bool do_transform = !req.header.frame_id.empty() && - !robot_state::Transforms::sameFrame(req.header.frame_id, default_frame) && + !moveit::core::Transforms::sameFrame(req.header.frame_id, default_frame) && context_->planning_scene_monitor_->getTFClient(); bool tf_problem = false; - robot_state::RobotState rs = + moveit::core::RobotState rs = planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState(); - robot_state::robotStateMsgToRobotState(req.robot_state, rs); + moveit::core::robotStateMsgToRobotState(req.robot_state, rs); for (std::size_t i = 0; i < req.fk_link_names.size(); ++i) if (rs.getRobotModel()->hasLinkModel(req.fk_link_names[i])) { diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h index d1f10a96a5..34c2be26e9 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_KINEMATICS_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_KINEMATICS_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -54,14 +53,12 @@ class MoveGroupKinematicsService : public MoveGroupCapability bool computeIKService(moveit_msgs::GetPositionIK::Request& req, moveit_msgs::GetPositionIK::Response& res); bool computeFKService(moveit_msgs::GetPositionFK::Request& req, moveit_msgs::GetPositionFK::Response& res); - void computeIK( - moveit_msgs::PositionIKRequest& req, moveit_msgs::RobotState& solution, moveit_msgs::MoveItErrorCodes& error_code, - robot_state::RobotState& rs, - const robot_state::GroupStateValidityCallbackFn& constraint = robot_state::GroupStateValidityCallbackFn()) const; + void computeIK(moveit_msgs::PositionIKRequest& req, moveit_msgs::RobotState& solution, + moveit_msgs::MoveItErrorCodes& error_code, moveit::core::RobotState& rs, + const moveit::core::GroupStateValidityCallbackFn& constraint = + moveit::core::GroupStateValidityCallbackFn()) const; ros::ServiceServer fk_service_; ros::ServiceServer ik_service_; }; -} - -#endif +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index cc6c36557a..22d8777d0a 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include namespace move_group @@ -102,10 +103,10 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const moveit_msgs::M ROS_INFO_NAMED(getName(), "Combined planning and execution request received for MoveGroup action. " "Forwarding to planning and execution pipeline."); - if (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff)) + if (moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) { planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); - const robot_state::RobotState& current_state = lscene->getCurrentState(); + const moveit::core::RobotState& current_state = lscene->getCurrentState(); // check to see if the desired constraints are already met for (std::size_t i = 0; i < goal->request.goal_constraints.size(); ++i) @@ -122,10 +123,9 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const moveit_msgs::M plan_execution::PlanExecution::Options opt; const moveit_msgs::MotionPlanRequest& motion_plan_request = - planning_scene::PlanningScene::isEmpty(goal->request.start_state) ? goal->request : - clearRequestStartState(goal->request); + moveit::core::isEmpty(goal->request.start_state) ? goal->request : clearRequestStartState(goal->request); const moveit_msgs::PlanningScene& planning_scene_diff = - planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ? + moveit::core::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ? goal->planning_options.planning_scene_diff : clearSceneRobotState(goal->planning_options.planning_scene_diff); @@ -168,7 +168,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const moveit_msgs::MoveGro // lock the scene so that it does not modify the world representation while diff() is called planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); const planning_scene::PlanningSceneConstPtr& the_scene = - (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff)) ? + (moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) ? static_cast(lscene) : lscene->diff(goal->planning_options.planning_scene_diff); planning_interface::MotionPlanResponse res; diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.h b/moveit_ros/move_group/src/default_capabilities/move_action_capability.h index d8af647a38..3f83999841 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_MOVE_ACTION_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_MOVE_ACTION_CAPABILITY_ +#pragma once #include #include @@ -70,6 +69,4 @@ class MoveGroupMoveAction : public MoveGroupCapability MoveGroupState move_state_; bool preempt_requested_; }; -} - -#endif +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h index b3d7a94250..5fb5bb5a0b 100644 --- a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_PLAN_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_PLAN_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -54,6 +53,4 @@ class MoveGroupPlanService : public MoveGroupCapability ros::ServiceServer plan_service_; }; -} - -#endif +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp index 9afab4f90e..c2c92caf51 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp @@ -55,7 +55,7 @@ void MoveGroupQueryPlannersService::initialize() &MoveGroupQueryPlannersService::setParams, this); } -bool MoveGroupQueryPlannersService::queryInterface(moveit_msgs::QueryPlannerInterfaces::Request& req, +bool MoveGroupQueryPlannersService::queryInterface(moveit_msgs::QueryPlannerInterfaces::Request& /*req*/, moveit_msgs::QueryPlannerInterfaces::Response& res) { const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager(); @@ -93,17 +93,17 @@ bool MoveGroupQueryPlannersService::getParams(moveit_msgs::GetPlannerParams::Req config.insert(it->second.config.begin(), it->second.config.end()); } - for (std::map::const_iterator it = config.begin(), end = config.end(); it != end; ++it) + for (const auto& key_value_pair : config) { - res.params.keys.push_back(it->first); - res.params.values.push_back(it->second); + res.params.keys.push_back(key_value_pair.first); + res.params.values.push_back(key_value_pair.second); } } return true; } bool MoveGroupQueryPlannersService::setParams(moveit_msgs::SetPlannerParams::Request& req, - moveit_msgs::SetPlannerParams::Response& res) + moveit_msgs::SetPlannerParams::Response& /*res*/) { const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager(); if (req.params.keys.size() != req.params.values.size()) diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h index 656c228a08..da31e1ca46 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Robert Haschke */ -#ifndef MOVEIT_MOVE_GROUP_QUERY_PLANNERS_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_QUERY_PLANNERS_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -62,6 +61,4 @@ class MoveGroupQueryPlannersService : public MoveGroupCapability ros::ServiceServer get_service_; ros::ServiceServer set_service_; }; -} - -#endif +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp index 16e6714665..1706f57b8d 100644 --- a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp @@ -36,7 +36,7 @@ #include "state_validation_service_capability.h" #include -#include +#include #include #include @@ -56,8 +56,8 @@ bool MoveGroupStateValidationService::computeService(moveit_msgs::GetStateValidi moveit_msgs::GetStateValidity::Response& res) { planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_); - robot_state::RobotState rs = ls->getCurrentState(); - robot_state::robotStateMsgToRobotState(req.robot_state, rs); + moveit::core::RobotState rs = ls->getCurrentState(); + moveit::core::robotStateMsgToRobotState(req.robot_state, rs); res.valid = true; @@ -66,8 +66,8 @@ bool MoveGroupStateValidationService::computeService(moveit_msgs::GetStateValidi creq.group_name = req.group_name; creq.cost = true; creq.contacts = true; - creq.max_contacts = ls->getWorld()->size(); - creq.max_cost_sources = creq.max_contacts + ls->getRobotModel()->getLinkModelsWithCollisionGeometry().size(); + creq.max_contacts = ls->getWorld()->size() + ls->getRobotModel()->getLinkModelsWithCollisionGeometry().size(); + creq.max_cost_sources = creq.max_contacts; creq.max_contacts *= creq.max_contacts; collision_detection::CollisionResult cres; @@ -100,7 +100,7 @@ bool MoveGroupStateValidationService::computeService(moveit_msgs::GetStateValidi } // evaluate constraints - if (!kinematic_constraints::isEmpty(req.constraints)) + if (!moveit::core::isEmpty(req.constraints)) { kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel()); kset.add(req.constraints, ls->getTransforms()); diff --git a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h index 2806a9586c..c814974de1 100644 --- a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVE_GROUP_STATE_VALIDATION_SERVICE_CAPABILITY_ -#define MOVEIT_MOVE_GROUP_STATE_VALIDATION_SERVICE_CAPABILITY_ +#pragma once #include #include @@ -54,6 +53,4 @@ class MoveGroupStateValidationService : public MoveGroupCapability ros::ServiceServer validity_service_; }; -} - -#endif +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp new file mode 100644 index 0000000000..f91b71c81f --- /dev/null +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -0,0 +1,137 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Hamburg University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Hamburg University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jonas Tietz */ + +#include "tf_publisher_capability.h" +#include +#include +#include +#include +#include +#include + +namespace move_group +{ +TfPublisher::TfPublisher() : MoveGroupCapability("TfPublisher") +{ +} + +TfPublisher::~TfPublisher() +{ + keep_running_ = false; + thread_.join(); +} + +namespace +{ +void publishSubframes(tf2_ros::TransformBroadcaster& broadcaster, const moveit::core::FixedTransformsMap& subframes, + const std::string& parent_object, const std::string& parent_frame, const ros::Time& stamp) +{ + geometry_msgs::TransformStamped transform; + for (auto& subframe : subframes) + { + transform = tf2::eigenToTransform(subframe.second); + transform.child_frame_id = parent_object + "/" + subframe.first; + transform.header.stamp = stamp; + transform.header.frame_id = parent_frame; + broadcaster.sendTransform(transform); + } +} +} // namespace + +void TfPublisher::publishPlanningSceneFrames() +{ + tf2_ros::TransformBroadcaster broadcaster; + geometry_msgs::TransformStamped transform; + ros::Rate rate(rate_); + + while (keep_running_) + { + { + ros::Time stamp = ros::Time::now(); + planning_scene_monitor::LockedPlanningSceneRO locked_planning_scene(context_->planning_scene_monitor_); + collision_detection::WorldConstPtr world = locked_planning_scene->getWorld(); + std::string planning_frame = locked_planning_scene->getPlanningFrame(); + + for (const auto& obj : *world) + { + std::string object_frame = prefix_ + obj.second->id_; + transform = tf2::eigenToTransform(obj.second->shape_poses_[0]); + transform.child_frame_id = object_frame; + transform.header.stamp = stamp; + transform.header.frame_id = planning_frame; + broadcaster.sendTransform(transform); + + const moveit::core::FixedTransformsMap& subframes = obj.second->subframe_poses_; + publishSubframes(broadcaster, subframes, object_frame, planning_frame, stamp); + } + + const moveit::core::RobotState& rs = locked_planning_scene->getCurrentState(); + std::vector attached_collision_objects; + rs.getAttachedBodies(attached_collision_objects); + for (const moveit::core::AttachedBody* attached_body : attached_collision_objects) + { + std::string object_frame = prefix_ + attached_body->getName(); + transform = tf2::eigenToTransform(attached_body->getFixedTransforms()[0]); + transform.child_frame_id = object_frame; + transform.header.stamp = stamp; + transform.header.frame_id = attached_body->getAttachedLinkName(); + broadcaster.sendTransform(transform); + + const moveit::core::FixedTransformsMap& subframes = attached_body->getSubframeTransforms(); + publishSubframes(broadcaster, subframes, object_frame, attached_body->getAttachedLinkName(), stamp); + } + } + + rate.sleep(); + } +} + +void TfPublisher::initialize() +{ + ros::NodeHandle nh = ros::NodeHandle("~"); + + std::string prefix = nh.getNamespace() + "/"; + nh.param("planning_scene_frame_publishing_rate", rate_, 10); + nh.param("planning_scene_tf_prefix", prefix_, prefix); + keep_running_ = true; + + ROS_INFO("Initializing MoveGroupTfPublisher with a frame publishing rate of %d", rate_); + thread_ = std::thread(&TfPublisher::publishPlanningSceneFrames, this); +} +} // namespace move_group + +#include +CLASS_LOADER_REGISTER_CLASS(move_group::TfPublisher, move_group::MoveGroupCapability) diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h new file mode 100644 index 0000000000..7c01174408 --- /dev/null +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h @@ -0,0 +1,59 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Hamburg University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Hamburg University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jonas Tietz */ + +#pragma once + +#include +#include + +namespace move_group +{ +class TfPublisher : public MoveGroupCapability +{ +public: + TfPublisher(); + ~TfPublisher() override; + + void initialize() override; + +private: + void publishPlanningSceneFrames(); + int rate_; + std::string prefix_; + std::thread thread_; + bool keep_running_; +}; +} // namespace move_group diff --git a/moveit_ros/move_group/src/list_capabilities.cpp b/moveit_ros/move_group/src/list_capabilities.cpp index f1a972b1f1..9001ad49c2 100644 --- a/moveit_ros/move_group/src/list_capabilities.cpp +++ b/moveit_ros/move_group/src/list_capabilities.cpp @@ -38,7 +38,7 @@ #include #include -int main(int argc, char** argv) +int main(int /*argc*/, char** /*argv*/) { try { diff --git a/moveit_ros/move_group/src/move_group_capability.cpp b/moveit_ros/move_group/src/move_group_capability.cpp index 2aa6a9cfce..48ee60e0b1 100644 --- a/moveit_ros/move_group/src/move_group_capability.cpp +++ b/moveit_ros/move_group/src/move_group_capability.cpp @@ -57,7 +57,7 @@ void move_group::MoveGroupCapability::convertToMsg(const std::vectorempty()) { - robot_state::robotStateToRobotStateMsg(trajectory[i].trajectory_->getFirstWayPoint(), first_state_msg); + moveit::core::robotStateToRobotStateMsg(trajectory[i].trajectory_->getFirstWayPoint(), first_state_msg); first = false; } trajectory[i].trajectory_->getRobotTrajectoryMsg(trajectory_msg[i]); @@ -72,7 +72,7 @@ void move_group::MoveGroupCapability::convertToMsg(const robot_trajectory::Robot { if (trajectory && !trajectory->empty()) { - robot_state::robotStateToRobotStateMsg(trajectory->getFirstWayPoint(), first_state_msg); + moveit::core::robotStateToRobotStateMsg(trajectory->getFirstWayPoint(), first_state_msg); trajectory->getRobotTrajectoryMsg(trajectory_msg); } } diff --git a/moveit_ros/move_group/test/test_cancel_before_plan_execution.test b/moveit_ros/move_group/test/test_cancel_before_plan_execution.test index 51977ca919..a4f22e456f 100644 --- a/moveit_ros/move_group/test/test_cancel_before_plan_execution.test +++ b/moveit_ros/move_group/test/test_cancel_before_plan_execution.test @@ -1,23 +1,6 @@ - - - - - - - - - [move_group/fake_controller_joint_states] - - - - - - - - - - + + diff --git a/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.py b/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.py new file mode 100755 index 0000000000..784a40ae3c --- /dev/null +++ b/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python + +import sys +import rospy +import unittest +from actionlib import SimpleActionClient +import moveit_commander +from moveit_msgs.msg import Constraints +from moveit_msgs.srv import GetStateValidity + + +class TestCheckStateValidityInEmptyScene(unittest.TestCase): + + def setUp(self): + moveit_commander.roscpp_initialize(sys.argv) + self.robot_commander = moveit_commander.RobotCommander() + self.group_name = self.robot_commander.get_group_names()[0] + self.group_commander = moveit_commander.MoveGroupCommander(self.group_name) + + self._check_state_validity = rospy.ServiceProxy('/check_state_validity', GetStateValidity) + + def test_check_collision_free_state_validity_in_empty_scene(self): + current_robot_state = self.robot_commander.get_current_state() + + validity_report = self._check_state_validity(current_robot_state, self.group_name, Constraints()) + self.assertListEqual(validity_report.contacts, [], "In the default_robot_state, the robot should not collide with itself") + + + def test_check_colliding_state_validity_in_empty_scene(self): + current_robot_state = self.robot_commander.get_current_state() + + # force a colliding state with the Fanuc M-10iA + current_robot_state.joint_state.position = list(current_robot_state.joint_state.position) + current_robot_state.joint_state.position[current_robot_state.joint_state.name.index("joint_3")] = -2 + + validity_report = self._check_state_validity(current_robot_state, self.group_name, Constraints()) + + self.assertNotEqual(len(validity_report.contacts), 0, "When the robot collides with itself, it should have some contacts (with itself)") + + +if __name__ == '__main__': + import rostest + rospy.init_node('check_state_validity_in_empty_scene') + rostest.rosrun('moveit_ros_move_group', 'test_check_state_validity_in_empty_scene', TestCheckStateValidityInEmptyScene) diff --git a/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.test b/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.test new file mode 100644 index 0000000000..b761f4dded --- /dev/null +++ b/moveit_ros/move_group/test/test_check_state_validity_in_empty_scene.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst index d3a8959214..0239bdfeca 100644 --- a/moveit_ros/moveit_ros/CHANGELOG.rst +++ b/moveit_ros/moveit_ros/CHANGELOG.rst @@ -2,6 +2,31 @@ Changelog for package moveit_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + +1.1.0 (2020-09-04) +------------------ +* [maint] Use standard cmake text for metapackages (`#1620 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Jonathan Binney, Robert Haschke, Sean Yen + +1.0.6 (2020-08-19) +------------------ + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ @@ -30,7 +55,7 @@ Changelog for package moveit_ros 0.10.2 (2018-10-24) ------------------- -* Fix references to MoveIt! (`#1020 `_) +* Fix references to MoveIt (`#1020 `_) * Contributors: Mohmmad Ayman 0.10.1 (2018-05-25) diff --git a/moveit_ros/moveit_ros/CMakeLists.txt b/moveit_ros/moveit_ros/CMakeLists.txt index 75cf534418..5425d0c2e5 100644 --- a/moveit_ros/moveit_ros/CMakeLists.txt +++ b/moveit_ros/moveit_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index a64adef04d..75a0f30e15 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -1,7 +1,7 @@ moveit_ros - 1.0.1 - Components of MoveIt! that use ROS + 1.1.1 + Components of MoveIt that use ROS Ioan Sucan Sachin Chitta Acorn Pooley @@ -9,7 +9,7 @@ Michael Ferguson Michael GΓΆrner Dave Coleman - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/moveit_servo/CHANGELOG.rst b/moveit_ros/moveit_servo/CHANGELOG.rst new file mode 100644 index 0000000000..5868a7b118 --- /dev/null +++ b/moveit_ros/moveit_servo/CHANGELOG.rst @@ -0,0 +1,199 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_servo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.1 (2020-10-13) +------------------ +* [feature] A library for servoing toward a moving pose (`#2203 `_) +* [feature] Refactor velocity limit enforcement and add a unit test (`#2260 `_) +* [fix] Servo thread interruption (`#2314 `_) +* [fix] Servo heap-buffer-overflow bug (`#2307 `_) +* [maint] Cleanup MSA includes (`#2351 `_) +* Contributors: AndyZe, Robert Haschke, Tyler Weaver + +1.1.0 (2020-09-04) +------------------ +* [feature] Update last_sent_command\_ at ServoCalcs start (`#2249 `_) +* [feature] Add a utility to print collision pairs (`#2275 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [maint] add soname version to moveit_servo (`#2266 `_) +* [maint] delete python integration tests (`#2186 `_) +* Contributors: AdamPettinger, AndyZe, Robert Haschke, Ruofan Xu, Tyler Weaver, v4hn + +1.0.6 (2020-08-19) +------------------ +* [feature] A ROS service to reset the Servo status (`#2246 `_) +* [feature] Check collisions during joint motions, too (`#2204 `_) +* [fix] Correctly set velocities to zero when stale (`#2255 `_) +* [maint] Remove unused yaml param (`#2232 `_) +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10 +* Contributors: AndyZe, Robert Haschke, Ruofan Xu, Michael GΓΆrner + +1.0.5 (2020-07-08) +------------------ +* [maint] Minor moveit_servo header cleanup (`#2173 `_) +* [maint] Move and rename to moveit_ros/moveit_servo (`#2165 `_) +* [maint] Changes before porting to ROS2 (`#2151 `_) + * throttle warning logs + * ROS1 Basic improvements and changes + * Fixes to drift dimensions, singularity velocity scaling + * tf name changes, const fixes, slight logic changes + * Move ROS_LOG_THROTTLE_PERIOD to cpp files + * Track staleness of joint and twist seperately + * Ensure joint_trajectory output is always populated with something, even when no jog + * Fix joint trajectory redundant points for gazebo pub + * Fix crazy joint jog from bad Eigen init + * Fix variable type in addJointIncrements() + * Initialize last sent command in constructor + * More explicit joint_jog_cmd\ and twist_stamped_cmd\ names + * Add comment clarying transform calculation / use +* [fix] Fix access past end of array bug (`#2155 `_) +* [maint] Remove duplicate line (`#2154 `_) +* [maint] pragma once in jog_arm.h (`#2152 `_) +* [feature] Simplify communication between threads (`#2103 `_) + * get latest joint state c++ api + * throttle warning logs + * publish from jog calcs timer, removing redundant timer and internal messaging to main timer + * outgoing message as pool allocated shared pointer for zero copy + * replace jog_arm shared variables with ros pub/sub + * use built in zero copy message passing instead of spsc_queues + * use ros timers instead of threads in jog_arm +* [feature] Added throttle to jogarm accel limit warning (`#2141 `_) +* [feature] Time-based collision avoidance (`#2100 `_) +* [fix] Fix crash on empty jog msgs (`#2094 `_) +* [feature] Jog arm dimensions (`#1724 `_) +* [maint] Clang-tidy fixes (`#2050 `_) +* [feature] Keep updating joints, even while waiting for a valid command (`#2027 `_) +* [fix] Fix param logic bug for self- and scene-collision proximity thresholds (`#2022 `_) +* [feature] Split collision proximity threshold (`#2008 `_) + * separate proximity threshold values for self-collisions and scene collisions + * increase default value of scene collision proximity threshold + * deprecate old parameters +* [fix] Fix valid command flags (`#2013 `_) + * Rename the 'zero command flag' variables for readability + * Reset flags when incoming commands timeout + * Remove debug line, clang format +* [maint] Use default move constructor + assignment operators for MoveItCpp. (`#2004 `_) +* [fix] Fix low-pass filter initialization (`#1982 `_) + * Pause/stop JogArm threads using shared atomic bool variables + * Add pause/unpause flags for jog thread + * Verify valid joints by filtering for active joint models only + * Remove redundant joint state increments + * Wait for initial jog commands in main loop +* [fix] Remove duplicate collision check in JogArm (`#1986 `_) +* [feature] Add a binary collision check (`#1978 `_) +* [feature] Publish more detailed warnings (`#1915 `_) +* [feature] Use wait_for_service() to fix flaky tests (`#1946 `_) +* [maint] Fix versioning (`#1948 `_) +* [feature] SRDF velocity and acceleration limit enforcement (`#1863 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [fix] JogArm C++ API fixes (`#1911 `_) +* [feature] A ROS service to enable task redundancy (`#1855 `_) +* [fix] Fix segfault with uninitialized JogArm thread (`#1882 `_) +* [feature] Add warnings to moveit_jog_arm low pass filter (`#1872 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [fix] Fix initial end effector transform jump (`#1871 `_) +* [feature] Rework the halt msg functionality (`#1868 `_) +* [fix] Various small fixes (`#1859 `_) +* [maint] Improve formatting in comments +* [fix] Prevent a crash at velocity limit (`#1837 `_) +* [feature] Remove scale/joint parameter (`#1838 `_) +* [feature] Pass planning scene monitor into cpp interface (`#1849 `_) +* [maint] Move attribution below license file, standardize with MoveIt (`#1847 `_) +* [maint] Reduce console output warnings (`#1845 `_) +* [fix] Fix command frame transform computation (`#1842 `_) +* [maint] Fix dependencies + catkin_lint issues +* [feature] Update link transforms before calling checkCollision on robot state in jog_arm (`#1825 `_) +* [feature] Add atomic bool flags for terminating JogArm threads gracefully (`#1816 `_) +* [feature] Get transforms from RobotState instead of TF (`#1803 `_) +* [feature] Add a C++ API (`#1763 `_) +* [maint] Fix unused parameter warnings (`#1773 `_) +* [maint] Update license formatting (`#1764 `_) +* [maint] Unify jog_arm package to be C++14 (`#1762 `_) +* [fix] Fix jog_arm segfault (`#1692 `_) +* [fix] Fix double mutex unlock (`#1672 `_) +* [maint] Rename jog_arm->moveit_jog_arm (`#1663 `_) +* [feature] Do not wait for command msg to start spinning (`#1603 `_) +* [maint] Update jog_arm README with rviz config (`#1614 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Separate moveit_experimental packages (`#1606 `_) +* [feature] Use UR5 example (`#1605 `_) +* [feature] Sudden stop for critical issues, filtered deceleration otherwise (`#1468 `_) +* [feature] Change 2nd order Butterworth low pass filter to 1st order (`#1483 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* [feature] JogArm: Remove dependency on move_group node (`#1569 `_) +* [fix] Fix jog arm CI integration test (`#1466 `_) +* [feature] A jogging PR for Melodic. (`#1360 `_) + * Allow for joints in the msg that are not part of the MoveGroup. + * Switching to the Panda robot model for tests. + * Blacklist the test as I can't get it to pass Travis (fine locally). + * Throttling all warnings. Fix build warning re. unit vs int comparison. + * Continue to publish commands even if stationary + * Scale for 'unitless' commands is not tied to publish_period. + * New function name for checkIfJointsWithinBounds() + * Configure the number of msgs to publish when stationary. + * Run jog_calcs at the same rate as the publishing thread. + * Better comments in config file, add spacenav_node dependency + * Add spacenav_node to CMakeLists. +* Contributors: AdamPettinger, AndyZe, Ayush Garg, Dale Koenig, Dave Coleman, Jonathan Binney, Paul Verhoeckx, Henning Kayser, Jafar Abdi, John Stechschulte, Mike Lautman, Robert Haschke, SansoneG, jschleicher, Tyler Weaver, rfeistenauer + +1.0.1 (2019-03-08) +------------------ + +1.0.0 (2019-02-24) +------------------ + +0.10.8 (2018-12-24) +------------------- + +0.10.5 (2018-11-01) +------------------- + +0.10.4 (2018-10-29 19:44) +------------------------- + +0.10.3 (2018-10-29 04:12) +------------------------- + +0.10.2 (2018-10-24) +------------------- + +0.10.1 (2018-05-25) +------------------- + +0.10.0 (2018-05-22) +------------------- + +0.9.11 (2017-12-25) +------------------- + +0.9.10 (2017-12-09) +------------------- + +0.9.9 (2017-08-06) +------------------ + +0.9.8 (2017-06-21) +------------------ + +0.9.7 (2017-06-05) +------------------ + +0.9.6 (2017-04-12) +------------------ + +0.9.5 (2017-03-08) +------------------ + +0.9.4 (2017-02-06) +------------------ + +0.9.3 (2016-11-16) +------------------ + +0.9.2 (2016-11-05) +------------------ + +0.9.1 (2016-10-21) +------------------ diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt new file mode 100644 index 0000000000..96791050e7 --- /dev/null +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -0,0 +1,197 @@ +cmake_minimum_required(VERSION 3.1.3) +project(moveit_servo) + +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +set(SERVO_LIB_NAME moveit_servo_cpp_api) + +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +find_package(Boost REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(catkin REQUIRED COMPONENTS + control_msgs + control_toolbox + geometry_msgs + moveit_msgs + moveit_ros_planning_interface + rosparam_shortcuts + sensor_msgs + std_msgs + std_srvs + tf2_eigen + trajectory_msgs +) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + pose_tracking + ${SERVO_LIB_NAME} + CATKIN_DEPENDS + control_msgs + control_toolbox + geometry_msgs + moveit_msgs + moveit_ros_planning_interface + rosparam_shortcuts + sensor_msgs + std_msgs + std_srvs + tf2_eigen + trajectory_msgs + DEPENDS + EIGEN3 +) + +include_directories(include) +include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIR} +) + +######################################################### +## Library to process realtime twist or joint commands ## +######################################################### + +# This library provides an interface for sending realtime twist or joint commands to a robot +add_library(${SERVO_LIB_NAME} + # These files are used to produce differential motion + src/collision_check.cpp + src/servo_calcs.cpp + src/servo.cpp + src/low_pass_filter.cpp +) +set_target_properties(${SERVO_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +add_dependencies(${SERVO_LIB_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${SERVO_LIB_NAME} + ${catkin_LIBRARIES} + ${Eigen_LIBRARIES} + ${Boost_LIBRARIES} +) + +# An example of streaming realtime Cartesian and joint commands +add_executable(cpp_interface_example + src/cpp_interface_example/cpp_interface_example.cpp +) +add_dependencies(cpp_interface_example ${catkin_EXPORTED_TARGETS}) +target_link_libraries(cpp_interface_example + ${catkin_LIBRARIES} + ${SERVO_LIB_NAME} +) + +# An example of pose tracking +add_executable(pose_tracking_example + src/cpp_interface_example/pose_tracking_example.cpp +) +add_dependencies(pose_tracking_example ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pose_tracking_example + ${catkin_LIBRARIES} + ${SERVO_LIB_NAME} + pose_tracking +) + +######################################## +## Library for servoing toward a pose ## +######################################## + +add_library(pose_tracking + src/pose_tracking.cpp +) +add_dependencies(pose_tracking ${catkin_EXPORTED_TARGETS}) +target_link_libraries(pose_tracking + ${catkin_LIBRARIES} + ${SERVO_LIB_NAME} +) + +############################ +## ROS message-based node ## +############################ + +add_executable(servo_server + src/servo_server.cpp +) +add_dependencies(servo_server ${catkin_EXPORTED_TARGETS}) +target_link_libraries(servo_server + ${SERVO_LIB_NAME} + ${catkin_LIBRARIES} +) + +################################################ +## An example of converting joystick commands ## +################################################ + +add_executable(spacenav_to_twist + src/teleop_examples/spacenav_to_twist.cpp +) +add_dependencies(spacenav_to_twist ${catkin_EXPORTED_TARGETS}) +target_link_libraries(spacenav_to_twist ${catkin_LIBRARIES}) + +################## +## INSTALLATION ## +################## + +install( + TARGETS + ${SERVO_LIB_NAME} + pose_tracking + pose_tracking_example + servo_server + spacenav_to_twist + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +############# +## TESTING ## +############# + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + # basic functionality + add_rostest_gtest(basic_servo_tests + test/basic_servo_tests.test + test/basic_servo_tests.cpp + ) + target_link_libraries(basic_servo_tests + ${SERVO_LIB_NAME} + ${catkin_LIBRARIES} + ) + + # servo_cpp_interface + add_rostest_gtest(servo_cpp_interface_test + test/servo_cpp_interface_test.test + test/servo_cpp_interface_test.cpp + ) + target_link_libraries(servo_cpp_interface_test + ${SERVO_LIB_NAME} + ${catkin_LIBRARIES} + ) + + # pose_tracking + add_rostest_gtest(pose_tracking_test + test/pose_tracking_test.test + test/pose_tracking_test.cpp + ) + target_link_libraries(pose_tracking_test + pose_tracking + ${catkin_LIBRARIES} + ) +endif() diff --git a/moveit_ros/moveit_servo/MIGRATION.md b/moveit_ros/moveit_servo/MIGRATION.md new file mode 100644 index 0000000000..ae97f6a75a --- /dev/null +++ b/moveit_ros/moveit_servo/MIGRATION.md @@ -0,0 +1,6 @@ +# Migration Notes + +API changes since last Noetic Release + +- Servo::getLatestJointState was removed. To get the latest joint positions of the group servo is working with use the CSM in the PSM. Here is an example of how to get the latest joint positions: + planning_scene_monitor_->getStateMonitor()->getCurrentState()->copyJointGroupPositions(move_group_name, positions); diff --git a/moveit_ros/moveit_servo/README.md b/moveit_ros/moveit_servo/README.md new file mode 100644 index 0000000000..e889244cb8 --- /dev/null +++ b/moveit_ros/moveit_servo/README.md @@ -0,0 +1,58 @@ +## Moveit Servo + +#### Quick Start Guide for UR5 example + +Clone the `universal_robot` repo into your catkin workspace: + + git clone https://github.com/ros-industrial/universal_robot.git + +Run `rosdep install` from the `src` folder to install dependencies. + + rosdep install --from-paths . --ignore-src -y + +Build and subsequently source the catkin workspace. Startup the robot and MoveIt: + + roslaunch ur_gazebo ur5.launch + + roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true + + roslaunch ur5_moveit_config moveit_rviz.launch config:=true + +In RViz, "plan and execute" a motion to a non-singular position (not all zero joint angles) that is not close to a joint limit. + +Switch to a compatible type of `ros-control` controller. It should be a `JointGroupVelocityController` or a `JointGroupPositionController`, not a trajectory controller like MoveIt usually requires. + +```sh +rosservice call /controller_manager/switch_controller "start_controllers: +- 'joint_group_position_controller' +stop_controllers: +- 'arm_controller' +strictness: 2" +``` + +Launch the servo node. This example uses commands from a SpaceNavigator joystick-like device: + + roslaunch moveit_servo spacenav_cpp.launch + +If you dont have a SpaceNavigator, send commands like this: + +```sh +rostopic pub -r 100 /servo_server/delta_twist_cmds geometry_msgs/TwistStamped "header: auto +twist: + linear: + x: 0.0 + y: 0.01 + z: -0.01 + angular: + x: 0.0 + y: 0.0 + z: 0.0" +``` + +If you see a warning about "close to singularity", try changing the direction of motion. + +#### Running Tests + +Run tests from the moveit\_servo folder: + + catkin run_tests --no-deps --this \ No newline at end of file diff --git a/moveit_ros/moveit_servo/config/pose_tracking_settings.yaml b/moveit_ros/moveit_servo/config/pose_tracking_settings.yaml new file mode 100644 index 0000000000..e4b803774d --- /dev/null +++ b/moveit_ros/moveit_servo/config/pose_tracking_settings.yaml @@ -0,0 +1,21 @@ +################################# +# PID parameters for pose seeking +################################# + +# Maximum value of error integral for all PID controllers +windup_limit: 0.05 + +# PID gains +x_proportional_gain: 1.5 +y_proportional_gain: 1.5 +z_proportional_gain: 1.5 +x_integral_gain: 0.0 +y_integral_gain: 0.0 +z_integral_gain: 0.0 +x_derivative_gain: 0.0 +y_derivative_gain: 0.0 +z_derivative_gain: 0.0 + +angular_proportional_gain: 0.5 +angular_integral_gain: 0.0 +angular_derivative_gain: 0.0 diff --git a/moveit_experimental/jog_arm/config/spacenav_via_teleop_tools.yaml b/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml similarity index 93% rename from moveit_experimental/jog_arm/config/spacenav_via_teleop_tools.yaml rename to moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml index cd9e53c9df..cbc6cfa1a4 100644 --- a/moveit_experimental/jog_arm/config/spacenav_via_teleop_tools.yaml +++ b/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml @@ -2,7 +2,7 @@ teleop: cartesian_twist_command: type: topic message_type: geometry_msgs/TwistStamped - topic_name: jog_arm_server/delta_jog_cmds + topic_name: servo_server/delta_twist_cmds axis_mappings: - axis: 0 diff --git a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml new file mode 100644 index 0000000000..21a2c6aecd --- /dev/null +++ b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml @@ -0,0 +1,68 @@ +############################################### +# Modify all parameters related to servoing here +############################################### +use_gazebo: true # Whether the robot is started in a Gazebo simulation environment + +## Properties of incoming commands +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.6 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 0.01 +low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less. + +## Properties of outgoing commands +publish_period: 0.008 # 1/Nominal publish rate [seconds] +low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) +# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) +command_out_type: std_msgs/Float64MultiArray + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: false +publish_joint_accelerations: false + +## MoveIt properties +move_group_name: manipulator # Often 'manipulator' or 'arm' +planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' + +## Other frames +ee_frame_name: ee_link # The name of the end effector link, used to return the EE pose +robot_link_command_frame: world # commands must be given in the frame of a robot link. Usually either the base or end effector + +## Stopping behaviour +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 4 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 17 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 30 # Stop when the condition number hits this +joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + +## Topic names +cartesian_command_in_topic: delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: joint_states +status_topic: status # Publish status to this topic +command_out_topic: /joint_group_position_controller/command # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: threshold_distance +# Parameters for "threshold_distance"-type collision checking +self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h new file mode 100644 index 0000000000..08aadaad46 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -0,0 +1,134 @@ +/******************************************************************************* + * Title : collision_check.h + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +namespace moveit_servo +{ +enum CollisionCheckType +{ + K_THRESHOLD_DISTANCE = 1, + K_STOP_DISTANCE = 2 +}; + +class CollisionCheck +{ +public: + /** \brief Constructor + * \param parameters: common settings of moveit_servo + * \param planning_scene_monitor: PSM should have scene monitor and state monitor + * already started when passed into this class + */ + CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + + ~CollisionCheck() + { + timer_.stop(); + } + + /** \brief start the Timer that regulates collision check rate */ + void start(); + + /** \brief Pause or unpause processing servo commands while keeping the timers alive */ + void setPaused(bool paused); + +private: + /** \brief Run one iteration of collision checking */ + void run(const ros::TimerEvent& timer_event); + + /** \brief Get a read-only copy of the planning scene */ + planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; + + /** \brief Callback for stopping time, from the thread that is aware of velocity and acceleration */ + void worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg); + + ros::NodeHandle nh_; + + // Parameters from yaml + const ServoParameters& parameters_; + + // Pointer to the collision environment + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // Robot state and collision matrix from planning scene + std::shared_ptr current_state_; + collision_detection::AllowedCollisionMatrix acm_; + + // Scale robot velocity according to collision proximity and user-defined thresholds. + // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. + // Proximity decreasing --> decelerate + CollisionCheckType collision_check_type_; + double velocity_scale_ = 1; + double self_collision_distance_ = 0; + double scene_collision_distance_ = 0; + bool collision_detected_ = false; + bool paused_ = false; + + // Variables for stop-distance-based collision checking + double current_collision_distance_ = 0; + double derivative_of_collision_distance_ = 0; + double prev_collision_distance_ = 0; + double est_time_to_collision_ = 0; + double safety_factor_ = 1000; + double worst_case_stop_time_ = std::numeric_limits::max(); + + const double self_velocity_scale_coefficient_; + const double scene_velocity_scale_coefficient_; + + // collision request + collision_detection::CollisionRequest collision_request_; + collision_detection::CollisionResult collision_result_; + + // ROS + ros::Timer timer_; + ros::Duration period_; + ros::Subscriber joint_state_sub_; + ros::Publisher collision_velocity_scale_pub_; + ros::Subscriber worst_case_stop_time_sub_; +}; +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h b/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h new file mode 100644 index 0000000000..b3418e3d7a --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h @@ -0,0 +1,69 @@ +/******************************************************************************* + * Title : low_pass_filter.h + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Andy Zelenak + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +#pragma once + +#include + +namespace moveit_servo +{ +/** + * Class LowPassFilter - Filter a signal to soften jerks. + * This is a first-order Butterworth low-pass filter. + * + * TODO: Use ROS filters package (http://wiki.ros.org/filters, https://github.com/ros/filters) + */ +class LowPassFilter +{ +public: + // Larger filter_coeff-> more smoothing of servo commands, but more lag. + // Rough plot, with cutoff frequency on the y-axis: + // https://www.wolframalpha.com/input/?i=plot+arccot(c) + explicit LowPassFilter(double low_pass_filter_coeff); + double filter(double new_measurement); + void reset(double data); + +private: + static constexpr std::size_t FILTER_LENGTH = 2; + double previous_measurements_[FILTER_LENGTH]; + double previous_filtered_measurement_; + // Scale and feedback term are calculated from supplied filter coefficient + const double scale_term_; + const double feedback_term_; +}; +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h new file mode 100644 index 0000000000..d3931dad37 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h @@ -0,0 +1,57 @@ +/******************************************************************************* + * Title : make_shared_from_pool.h + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Tyler Weaver + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +#pragma once + +#include +#include + +namespace moveit +{ +namespace util +{ +// Useful template for creating messages from a message pool +template +boost::shared_ptr make_shared_from_pool() +{ + using allocator_t = boost::fast_pool_allocator>; + return boost::allocate_shared(allocator_t()); +} + +} // namespace util +} // namespace moveit diff --git a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h new file mode 100644 index 0000000000..5163cc08c8 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h @@ -0,0 +1,196 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/* + Author: Andy Zelenak + Desc: Servoing. Track a pose setpoint in real time. +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +// Conventions: +// Calculations are done in the planning_frame_ unless otherwise noted. + +namespace moveit_servo +{ +struct PIDConfig +{ + // Default values + double dt = 0.001; + double k_p = 1; + double k_i = 0; + double k_d = 0; + double windup_limit = 0.1; +}; + +enum class PoseTrackingStatusCode : int8_t +{ + INVALID = -1, + SUCCESS = 0, + NO_RECENT_TARGET_POSE = 1, + NO_RECENT_END_EFFECTOR_POSE = 2, + STOP_REQUESTED = 3 +}; + +const std::unordered_map POSE_TRACKING_STATUS_CODE_MAP( + { { PoseTrackingStatusCode::INVALID, "Invalid" }, + { PoseTrackingStatusCode::SUCCESS, "Success" }, + { PoseTrackingStatusCode::NO_RECENT_TARGET_POSE, "No recent target pose" }, + { PoseTrackingStatusCode::NO_RECENT_END_EFFECTOR_POSE, "No recent end effector pose" }, + { PoseTrackingStatusCode::STOP_REQUESTED, "Stop requested" } }); + +/** + * Class PoseTracking - subscribe to a target pose. + * Servo toward the target pose. + */ +class PoseTracking +{ +public: + /** \brief Constructor. Loads ROS parameters under the given namespace. */ + PoseTracking(const ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + + PoseTrackingStatusCode moveToPose(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance, + const double target_pose_timeout); + + /** \brief A method for a different thread to stop motion and return early from control loop */ + void stopMotion() + { + stop_requested_ = true; + } + + /** \brief Change PID parameters. Motion is stopped before the udpate */ + void updatePIDConfig(const double x_proportional_gain, const double x_integral_gain, const double x_derivative_gain, + const double y_proportional_gain, const double y_integral_gain, const double y_derivative_gain, + const double z_proportional_gain, const double z_integral_gain, const double z_derivative_gain, + const double angular_proportional_gain, const double angular_integral_gain, + const double angular_derivative_gain); + + void getPIDErrors(double& x_error, double& y_error, double& z_error, double& orientation_error); + + /** + * Get the End Effector link transform. + * The transform from the MoveIt planning frame to EE link + * + * @param transform the transform that will be calculated + * @return true if a valid transform was available + */ + bool getCommandFrameTransform(geometry_msgs::TransformStamped& transform); + + /** \brief Re-initialize the target pose to an empty message. Can be used to reset motion between waypoints. */ + void resetTargetPose(); + + // moveit_servo::Servo instance. Public so we can access member functions like setPaused() + std::unique_ptr servo_; + +private: + /** \brief Load ROS parameters for controller settings. */ + void readROSParams(); + + /** \brief Initialize a PID controller and add it to vector of controllers */ + void initializePID(const PIDConfig& pid_config, std::vector& pid_vector); + + /** \brief Return true if a target pose has been received within timeout [seconds] */ + bool haveRecentTargetPose(const double timeout); + + /** \brief Return true if an end effector pose has been received within timeout [seconds] */ + bool haveRecentEndEffectorPose(const double timeout); + + /** \brief Check if XYZ, roll/pitch/yaw tolerances are satisfied */ + bool satisfiesPoseTolerance(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance); + + /** \brief Subscribe to the target pose on this topic */ + void targetPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg); + + /** \brief Update PID controller target positions & orientations */ + void updateControllerSetpoints(); + + /** \brief Update PID controller states (positions & orientations) */ + void updateControllerStateMeasurements(); + + /** \brief Use PID controllers to calculate a full spatial velocity toward a pose */ + geometry_msgs::TwistStampedConstPtr calculateTwistCommand(); + + /** \brief Reset flags and PID controllers after a motion completes */ + void doPostMotionReset(); + + ros::NodeHandle nh_; + + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + robot_model::RobotModelConstPtr robot_model_; + const moveit::core::JointModelGroup* joint_model_group_; + // Joint group used for controlling the motions + std::string move_group_name_; + + ros::Rate loop_rate_; + + // ROS interface to Servo + ros::Publisher twist_stamped_pub_; + + std::vector cartesian_position_pids_; + std::vector cartesian_orientation_pids_; + // Cartesian PID configs + PIDConfig x_pid_config_, y_pid_config_, z_pid_config_, angular_pid_config_; + + // Transforms w.r.t. planning_frame_ + Eigen::Isometry3d command_frame_transform_; + ros::Time command_frame_transform_stamp_; + geometry_msgs::PoseStamped target_pose_; + mutable std::mutex target_pose_mtx_; + + // Subscribe to target pose + ros::Subscriber target_pose_sub_; + + tf2_ros::Buffer transform_buffer_; + tf2_ros::TransformListener transform_listener_; + + // Expected frame name, for error checking and transforms + std::string planning_frame_; + + // Flag that a different thread has requested a stop. + std::atomic stop_requested_; + + boost::optional angular_error_; +}; + +// using alias +using PoseTrackingPtr = std::shared_ptr; +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h new file mode 100644 index 0000000000..8258c72923 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -0,0 +1,122 @@ +/******************************************************************************* + * Title : servo.h + * Project : moveit_servo + * Created : 3/9/2017 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +#pragma once + +// System +#include + +// MoveIt +#include +#include +#include + +// testing +#include + +namespace moveit_servo +{ +/** + * Class Servo - Jacobian based robot control with collision avoidance. + */ +class Servo +{ +public: + Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + + ~Servo(); + + /** \brief start servo node */ + void start(); + + /** \brief Pause or unpause processing servo commands while keeping the timers alive */ + void setPaused(bool paused); + + /** + * Get the MoveIt planning link transform. + * The transform from the MoveIt planning frame to robot_link_command_frame + * + * @param transform the transform that will be calculated + * @return true if a valid transform was available + */ + bool getCommandFrameTransform(Eigen::Isometry3d& transform); + bool getCommandFrameTransform(geometry_msgs::TransformStamped& transform); + + /** + * Get the End Effector link transform. + * The transform from the MoveIt planning frame to EE link + * + * @param transform the transform that will be calculated + * @return true if a valid transform was available + */ + bool getEEFrameTransform(Eigen::Isometry3d& transform); + bool getEEFrameTransform(geometry_msgs::TransformStamped& transform); + + /** \brief Get the parameters used by servo node. */ + const ServoParameters& getParameters() const; + + /** \brief Change the controlled link. Often, this is the end effector + * This must be a link on the robot since MoveIt tracks the transform (not tf) + */ + void changeRobotLinkCommandFrame(const std::string& new_command_frame) + { + servo_calcs_->changeRobotLinkCommandFrame(new_command_frame); + } + + // Give test access to private/protected methods + friend class ServoFixture; + +private: + bool readParameters(); + + ros::NodeHandle nh_; + + // Pointer to the collision environment + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // Store the parameters that were read from ROS server + ServoParameters parameters_; + + std::unique_ptr servo_calcs_; + std::unique_ptr collision_checker_; +}; + +// ServoPtr using alias +using ServoPtr = std::shared_ptr; + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h new file mode 100644 index 0000000000..437700c4d5 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -0,0 +1,320 @@ +/******************************************************************************* + * Title : servo_calcs.h + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +#pragma once + +// C++ +#include +#include +#include + +// ROS +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// moveit_servo +#include +#include +#include + +namespace moveit_servo +{ +class ServoCalcs +{ +public: + ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + + ~ServoCalcs(); + + /** \brief Start the timer where we do work and publish outputs */ + void start(); + + /** + * Get the MoveIt planning link transform. + * The transform from the MoveIt planning frame to robot_link_command_frame + * + * @param transform the transform that will be calculated + * @return true if a valid transform was available + */ + bool getCommandFrameTransform(Eigen::Isometry3d& transform); + bool getCommandFrameTransform(geometry_msgs::TransformStamped& transform); + + /** + * Get the End Effector link transform. + * The transform from the MoveIt planning frame to EE link + * + * @param transform the transform that will be calculated + * @return true if a valid transform was available + */ + bool getEEFrameTransform(Eigen::Isometry3d& transform); + bool getEEFrameTransform(geometry_msgs::TransformStamped& transform); + + /** \brief Pause or unpause processing servo commands while keeping the timers alive */ + void setPaused(bool paused); + + /** \brief Change the controlled link. Often, this is the end effector + * This must be a link on the robot since MoveIt tracks the transform (not tf) + */ + void changeRobotLinkCommandFrame(const std::string& new_command_frame); + + // Give test access to private/protected methods + friend class ServoFixture; + +private: + /** \brief Run the main calculation loop */ + void mainCalcLoop(); + + /** \brief Do calculations for a single iteration. Publish one outgoing command */ + void calculateSingleIteration(); + + /** \brief Stop the currently running thread */ + void stop(); + + /** \brief Do servoing calculations for Cartesian twist commands. */ + bool cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); + + /** \brief Do servoing calculations for direct commands to a joint. */ + bool jointServoCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); + + /** \brief Parse the incoming joint msg for the joints of our MoveGroup */ + void updateJoints(); + + /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. + * Also, multiply by timestep to calculate a position change. + */ + Eigen::VectorXd scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const; + + /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. + * Also, multiply by timestep to calculate a position change. + */ + Eigen::VectorXd scaleJointCommand(const control_msgs::JointJog& command) const; + + bool addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const; + + /** \brief Suddenly halt for a joint limit or other critical issue. + * Is handled differently for position vs. velocity control. + */ + void suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory); + + /** \brief Scale the delta theta to match joint velocity/acceleration limits */ + void enforceVelLimits(Eigen::ArrayXd& delta_theta); + + /** \brief Avoid overshooting joint limits */ + bool enforcePositionLimits(); + + /** \brief Possibly calculate a velocity scaling factor, due to proximity of + * singularity and direction of motion + */ + double velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, + const Eigen::JacobiSVD& svd, + const Eigen::MatrixXd& pseudo_inverse); + + /** + * Slow motion down if close to singularity or collision. + * @param delta_theta motion command, used in calculating new_joint_tray + * @param singularity_scale tells how close we are to a singularity + */ + void applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singularity_scale); + + /** \brief Compose the outgoing JointTrajectory message */ + void composeJointTrajMessage(const sensor_msgs::JointState& joint_state, + trajectory_msgs::JointTrajectory& joint_trajectory) const; + + /** \brief Smooth position commands with a lowpass filter */ + void lowPassFilterPositions(sensor_msgs::JointState& joint_state); + + /** \brief Set the filters to the specified values */ + void resetLowPassFilters(const sensor_msgs::JointState& joint_state); + + /** \brief Convert an incremental position command to joint velocity message */ + void calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta); + + /** \brief Convert joint deltas to an outgoing JointTrajectory command. + * This happens for joint commands and Cartesian commands. + */ + bool convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& joint_trajectory); + + /** \brief Gazebo simulations have very strict message timestamp requirements. + * Satisfy Gazebo by stuffing multiple messages into one. + */ + void insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& joint_trajectory, int count) const; + + /** + * Remove the Jacobian row and the delta-x element of one Cartesian dimension, to take advantage of task redundancy + * + * @param matrix The Jacobian matrix. + * @param delta_x Vector of Cartesian delta commands, should be the same size as matrix.rows() + * @param row_to_remove Dimension that will be allowed to drift, e.g. row_to_remove = 2 allows z-translation drift. + */ + void removeDimension(Eigen::MatrixXd& matrix, Eigen::VectorXd& delta_x, unsigned int row_to_remove); + + /* \brief Callback for joint subsription */ + void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); + + /* \brief Command callbacks */ + void twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg); + void jointCmdCB(const control_msgs::JointJogConstPtr& msg); + void collisionVelocityScaleCB(const std_msgs::Float64ConstPtr& msg); + + /** + * Allow drift in certain dimensions. For example, may allow the wrist to rotate freely. + * This can help avoid singularities. + * + * @param request the service request + * @param response the service response + * @return true if the adjustment was made + */ + bool changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, + moveit_msgs::ChangeDriftDimensions::Response& res); + + /** \brief Service callback for changing servoing dimensions (e.g. ignore rotation about X) */ + bool changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, + moveit_msgs::ChangeControlDimensions::Response& res); + + /** \brief Service callback to reset Servo status, e.g. so the arm can move again after a collision */ + bool resetServoStatus(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + + ros::NodeHandle nh_; + + // Parameters from yaml + ServoParameters& parameters_; + + // Pointer to the collision environment + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // Track the number of cycles during which motion has not occurred. + // Will avoid re-publishing zero velocities endlessly. + int zero_velocity_count_ = 0; + + // Flag for staying inactive while there are no incoming commands + bool wait_for_servo_commands_ = true; + + // Flag saying if the filters were updated during the timer callback + bool updated_filters_ = false; + + // Nonzero status flags + bool have_nonzero_twist_stamped_ = false; + bool have_nonzero_joint_command_ = false; + bool have_nonzero_command_ = false; + + // Incoming command messages + geometry_msgs::TwistStamped twist_stamped_cmd_; + control_msgs::JointJog joint_servo_cmd_; + + const moveit::core::JointModelGroup* joint_model_group_; + + moveit::core::RobotStatePtr current_state_; + + // incoming_joint_state_ is the incoming message. It may contain passive joints or other joints we don't care about. + // (mutex protected below) + // internal_joint_state_ is used in servo calculations. It shouldn't be relied on to be accurate. + // original_joint_state_ is the same as incoming_joint_state_ except it only contains the joints the servo node acts + // on. + sensor_msgs::JointState internal_joint_state_, original_joint_state_; + std::map joint_state_name_map_; + + std::vector position_filters_; + + trajectory_msgs::JointTrajectoryConstPtr last_sent_command_; + + // ROS + ros::Subscriber joint_state_sub_; + ros::Subscriber twist_stamped_sub_; + ros::Subscriber joint_cmd_sub_; + ros::Subscriber collision_velocity_scale_sub_; + ros::Publisher status_pub_; + ros::Publisher worst_case_stop_time_pub_; + ros::Publisher outgoing_cmd_pub_; + ros::ServiceServer drift_dimensions_server_; + ros::ServiceServer control_dimensions_server_; + ros::ServiceServer reset_servo_status_; + + // Main tracking / result publisher loop + std::thread thread_; + bool stop_requested_; + + // Status + StatusCode status_ = StatusCode::NO_WARNING; + std::atomic paused_; + bool twist_command_is_stale_ = false; + bool joint_command_is_stale_ = false; + bool ok_to_publish_ = false; + double collision_velocity_scale_ = 1.0; + + // Use ArrayXd type to enable more coefficient-wise operations + Eigen::ArrayXd delta_theta_; + Eigen::ArrayXd prev_joint_velocity_; + + const int gazebo_redundant_message_count_ = 30; + + uint num_joints_; + + // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw] + std::array drift_dimensions_ = { { false, false, false, false, false, false } }; + + // The dimesions to control. In the command frame. [x, y, z, roll, pitch, yaw] + std::array control_dimensions_ = { { true, true, true, true, true, true } }; + + // input_mutex_ is used to protect the state below it + mutable std::mutex input_mutex_; + Eigen::Isometry3d tf_moveit_to_robot_cmd_frame_; + Eigen::Isometry3d tf_moveit_to_ee_frame_; + geometry_msgs::TwistStampedConstPtr latest_twist_stamped_; + control_msgs::JointJogConstPtr latest_joint_cmd_; + ros::Time latest_twist_command_stamp_ = ros::Time(0.); + ros::Time latest_joint_command_stamp_ = ros::Time(0.); + bool latest_nonzero_twist_stamped_ = false; + bool latest_nonzero_joint_cmd_ = false; + + // input condition variable used for low latency mode + std::condition_variable input_cv_; + bool new_input_cmd_ = false; +}; +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h new file mode 100644 index 0000000000..14d993cc71 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h @@ -0,0 +1,85 @@ +/******************************************************************************* + * Title : servo_parameters.h + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +#pragma once + +namespace moveit_servo +{ +// Size of queues used in ros pub/sub/service +constexpr size_t ROS_QUEUE_SIZE = 2; + +// ROS params to be read. See the yaml file in /config for a description of each. +struct ServoParameters +{ + std::string move_group_name; + std::string joint_topic; + std::string cartesian_command_in_topic; + std::string robot_link_command_frame; + std::string command_out_topic; + std::string planning_frame; + std::string ee_frame_name; + std::string status_topic; + std::string joint_command_in_topic; + std::string command_in_type; + std::string command_out_type; + double linear_scale; + double rotational_scale; + double joint_scale; + double lower_singularity_threshold; + double hard_stop_singularity_threshold; + double low_pass_filter_coeff; + double publish_period; + double incoming_command_timeout; + double joint_limit_margin; + int num_outgoing_halt_msgs_to_publish; + bool use_gazebo; + bool publish_joint_positions; + bool publish_joint_velocities; + bool publish_joint_accelerations; + bool low_latency_mode; + // Collision checking + bool check_collisions; + std::string collision_check_type; + double collision_check_rate; + double scene_collision_proximity_threshold; + double self_collision_proximity_threshold; + double collision_distance_safety_factor; + double min_allowable_collision_distance; +}; + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h new file mode 100644 index 0000000000..5164f46a9e --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h @@ -0,0 +1,65 @@ +/******************************************************************************* + * Title : status_codes.h + * Project : moveit_servo + * Created : 2/25/2019 + * Author : Andy Zelenak + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +#pragma once + +#include +#include + +namespace moveit_servo +{ +enum class StatusCode : int8_t +{ + INVALID = -1, + NO_WARNING = 0, + DECELERATE_FOR_SINGULARITY = 1, + HALT_FOR_SINGULARITY = 2, + DECELERATE_FOR_COLLISION = 3, + HALT_FOR_COLLISION = 4, + JOINT_BOUND = 5 +}; + +const std::unordered_map + SERVO_STATUS_CODE_MAP({ { StatusCode::INVALID, "Invalid" }, + { StatusCode::NO_WARNING, "No warnings" }, + { StatusCode::DECELERATE_FOR_SINGULARITY, "Close to a singularity, decelerating" }, + { StatusCode::HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, + { StatusCode::DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, + { StatusCode::HALT_FOR_COLLISION, "Collision detected, emergency stop" }, + { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }); +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/launch/cpp_interface_example.launch b/moveit_ros/moveit_servo/launch/cpp_interface_example.launch new file mode 100644 index 0000000000..7498446ff8 --- /dev/null +++ b/moveit_ros/moveit_servo/launch/cpp_interface_example.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch new file mode 100644 index 0000000000..b3fcc374d2 --- /dev/null +++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/moveit_ros/moveit_servo/launch/spacenav_cpp.launch b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch new file mode 100644 index 0000000000..b60db1fa0e --- /dev/null +++ b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + diff --git a/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch b/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch new file mode 100644 index 0000000000..e3834b5e16 --- /dev/null +++ b/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml new file mode 100644 index 0000000000..086a7b60fd --- /dev/null +++ b/moveit_ros/moveit_servo/package.xml @@ -0,0 +1,44 @@ + + + moveit_servo + 1.1.1 + + Provides real-time manipulator Cartesian and joint servoing. + + Blake Anderson + Andy Zelenak + + BSD 3-Clause + + https://ros-planning.github.io/moveit_tutorials + + Brian O'Neil + Andy Zelenak + Blake Anderson + Alexander RΓΆssler + Tyler Weaver + + catkin + + control_msgs + control_toolbox + geometry_msgs + moveit_msgs + moveit_ros_planning_interface + rosparam_shortcuts + sensor_msgs + std_msgs + std_srvs + tf2_eigen + trajectory_msgs + + joy_teleop + spacenav_node + + rostest + moveit_resources_panda_moveit_config + + + + + diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp new file mode 100644 index 0000000000..85aa7d4810 --- /dev/null +++ b/moveit_ros/moveit_servo/src/collision_check.cpp @@ -0,0 +1,210 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* Title : collision_check.cpp + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + */ + +#include + +#include +#include + +static const char LOGNAME[] = "collision_check"; +static const double MIN_RECOMMENDED_COLLISION_RATE = 10; +constexpr double EPSILON = 1e-6; // For very small numeric comparisons +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30; // Seconds to throttle logs inside loops + +namespace moveit_servo +{ +// Constructor for the class that handles collision checking +CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : nh_(nh) + , parameters_(parameters) + , planning_scene_monitor_(planning_scene_monitor) + , self_velocity_scale_coefficient_(-log(0.001) / parameters.self_collision_proximity_threshold) + , scene_velocity_scale_coefficient_(-log(0.001) / parameters.scene_collision_proximity_threshold) + , period_(1. / parameters_.collision_check_rate) +{ + // Init collision request + collision_request_.group_name = parameters_.move_group_name; + collision_request_.distance = true; // enable distance-based collision checking + collision_request_.contacts = true; // Record the names of collision pairs + + if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE) + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "Collision check rate is low, increase it in yaml file if CPU allows"); + + collision_check_type_ = + (parameters_.collision_check_type == "threshold_distance" ? K_THRESHOLD_DISTANCE : K_STOP_DISTANCE); + safety_factor_ = parameters_.collision_distance_safety_factor; + + // Internal namespace + ros::NodeHandle internal_nh(nh_, "internal"); + collision_velocity_scale_pub_ = internal_nh.advertise("collision_velocity_scale", ROS_QUEUE_SIZE); + worst_case_stop_time_sub_ = + internal_nh.subscribe("worst_case_stop_time", ROS_QUEUE_SIZE, &CollisionCheck::worstCaseStopTimeCB, this); + + current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + acm_ = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); +} + +planning_scene_monitor::LockedPlanningSceneRO CollisionCheck::getLockedPlanningSceneRO() const +{ + return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); +} + +void CollisionCheck::start() +{ + timer_ = nh_.createTimer(period_, &CollisionCheck::run, this); +} + +void CollisionCheck::run(const ros::TimerEvent& timer_event) +{ + // Log warning when the last loop duration was longer than the period + if (timer_event.profile.last_duration.toSec() > period_.toSec()) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "last_duration: " << timer_event.profile.last_duration.toSec() << " (" + << period_.toSec() << ")"); + } + + if (paused_) + { + return; + } + + // Update to the latest current state + current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + current_state_->updateCollisionBodyTransforms(); + collision_detected_ = false; + + // Do a timer-safe distance-based collision detection + collision_result_.clear(); + getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, + *current_state_); + scene_collision_distance_ = collision_result_.distance; + collision_detected_ |= collision_result_.collision; + collision_result_.print(); + + collision_result_.clear(); + // Self-collisions and scene collisions are checked separately so different thresholds can be used + getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision(collision_request_, collision_result_, + *current_state_, acm_); + self_collision_distance_ = collision_result_.distance; + collision_detected_ |= collision_result_.collision; + collision_result_.print(); + + velocity_scale_ = 1; + // If we're definitely in collision, stop immediately + if (collision_detected_) + { + velocity_scale_ = 0; + } + // If threshold distances were specified + else if (collision_check_type_ == K_THRESHOLD_DISTANCE) + { + // If we are far from a collision, velocity_scale should be 1. + // If we are very close to a collision, velocity_scale should be ~zero. + // When scene_collision_proximity_threshold is breached, start decelerating exponentially. + if (scene_collision_distance_ < parameters_.scene_collision_proximity_threshold) + { + // velocity_scale = e ^ k * (collision_distance - threshold) + // k = - ln(0.001) / collision_proximity_threshold + // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. + // velocity_scale should equal 0.001 when collision_distance is at zero. + velocity_scale_ = + std::min(velocity_scale_, exp(scene_velocity_scale_coefficient_ * + (scene_collision_distance_ - parameters_.scene_collision_proximity_threshold))); + } + + if (self_collision_distance_ < parameters_.self_collision_proximity_threshold) + { + velocity_scale_ = + std::min(velocity_scale_, exp(self_velocity_scale_coefficient_ * + (self_collision_distance_ - parameters_.self_collision_proximity_threshold))); + } + } + // Else, we stop based on worst-case stopping distance + else + { + // Retrieve the worst-case time to stop, based on current joint velocities + + // Calculate rate of change of distance to nearest collision + current_collision_distance_ = std::min(scene_collision_distance_, self_collision_distance_); + derivative_of_collision_distance_ = (current_collision_distance_ - prev_collision_distance_) / period_.toSec(); + + if (current_collision_distance_ < parameters_.min_allowable_collision_distance && + derivative_of_collision_distance_ <= 0) + { + velocity_scale_ = 0; + } + // Only bother doing calculations if we are moving toward the nearest collision + else if (derivative_of_collision_distance_ < -EPSILON) + { + // At the rate the collision distance is decreasing, how long until we collide? + est_time_to_collision_ = fabs(current_collision_distance_ / derivative_of_collision_distance_); + + // halt if we can't stop fast enough (including the safety factor) + if (est_time_to_collision_ < (safety_factor_ * worst_case_stop_time_)) + { + velocity_scale_ = 0; + } + } + + // Update for the next iteration + prev_collision_distance_ = current_collision_distance_; + } + + // publish message + { + auto msg = moveit::util::make_shared_from_pool(); + msg->data = velocity_scale_; + collision_velocity_scale_pub_.publish(msg); + } +} + +void CollisionCheck::worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg) +{ + worst_case_stop_time_ = msg->data; +} + +void CollisionCheck::setPaused(bool paused) +{ + paused_ = paused; +} + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp new file mode 100644 index 0000000000..d51ce11b19 --- /dev/null +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp @@ -0,0 +1,155 @@ +/******************************************************************************* + * Title : cpp_interface_example.cpp + * Project : moveit_servo + * Created : 11/20/2019 + * Author : Andy Zelenak + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +#include + +#include +#include +#include + +static const std::string LOGNAME = "cpp_interface_example"; + +// Class for monitoring status of moveit_servo +class StatusMonitor +{ +public: + StatusMonitor(ros::NodeHandle& nh, const std::string& topic) + { + sub_ = nh.subscribe(topic, 1, &StatusMonitor::statusCB, this); + } + +private: + void statusCB(const std_msgs::Int8ConstPtr& msg) + { + moveit_servo::StatusCode latest_status = static_cast(msg->data); + if (latest_status != status_) + { + status_ = latest_status; + const auto& status_str = moveit_servo::SERVO_STATUS_CODE_MAP.at(status_); + ROS_INFO_STREAM_NAMED(LOGNAME, "Servo status: " << status_str); + } + } + moveit_servo::StatusCode status_ = moveit_servo::StatusCode::INVALID; + ros::Subscriber sub_; +}; + +/** + * Instantiate the C++ servo node interface. + * Send some Cartesian commands, then some joint commands. + */ +int main(int argc, char** argv) +{ + ros::init(argc, argv, LOGNAME); + ros::NodeHandle nh("~"); + ros::AsyncSpinner spinner(8); + spinner.start(); + + // Load the planning scene monitor + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; + planning_scene_monitor = std::make_shared("robot_description"); + if (!planning_scene_monitor->getPlanningScene()) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); + exit(EXIT_FAILURE); + } + + planning_scene_monitor->startSceneMonitor(); + planning_scene_monitor->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + planning_scene_monitor->startStateMonitor(); + + // Run the servo node C++ interface in a new timer to ensure a constant outgoing message rate. + moveit_servo::Servo servo(nh, planning_scene_monitor); + servo.start(); + + // Subscribe to servo status (and log it when it changes) + StatusMonitor status_monitor(nh, servo.getParameters().status_topic); + + // Create publishers to send servo commands + auto twist_stamped_pub = + nh.advertise(servo.getParameters().cartesian_command_in_topic, 1); + auto joint_servo_pub = nh.advertise(servo.getParameters().joint_command_in_topic, 1); + + ros::Rate cmd_rate(100); + uint num_commands = 0; + + // Send a few Cartesian velocity commands + while (ros::ok() && num_commands < 200) + { + // Make a Cartesian velocity message + // Messages are sent to servo node as boost::shared_ptr to enable zero-copy message_passing. + // Because this message is not copied we should not modify it after we send it. + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "base_link"; + msg->twist.linear.y = 0.01; + msg->twist.linear.z = -0.01; + + // Send the message + twist_stamped_pub.publish(msg); + cmd_rate.sleep(); + ++num_commands; + } + + // Leave plenty of time for the servo node to halt its previous motion. + // For a faster response, adjust the incoming_command_timeout yaml parameter + ros::Duration(2).sleep(); + + // Send a few joint commands + num_commands = 0; + while (ros::ok() && num_commands < 200) + { + // Make a joint command + // Messages are sent to servo node as boost::shared_ptr to enable zero-copy message_passing. + // Because this message is not copied we should not modify it after we send it. + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->joint_names.push_back("elbow_joint"); + msg->velocities.push_back(0.2); + + // Send the message + joint_servo_pub.publish(msg); + cmd_rate.sleep(); + ++num_commands; + } + + servo.setPaused(true); + return 0; +} diff --git a/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp new file mode 100644 index 0000000000..3657d52864 --- /dev/null +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/pose_tracking_example.cpp @@ -0,0 +1,159 @@ +/******************************************************************************* + * Title : pose_tracking_example.cpp + * Project : moveit_servo + * Created : 09/04/2020 + * Author : Adam Pettinger + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include + +static const std::string LOGNAME = "cpp_interface_example"; + +// Class for monitoring status of moveit_servo +class StatusMonitor +{ +public: + StatusMonitor(ros::NodeHandle& nh, const std::string& topic) + { + sub_ = nh.subscribe(topic, 1, &StatusMonitor::statusCB, this); + } + +private: + void statusCB(const std_msgs::Int8ConstPtr& msg) + { + moveit_servo::StatusCode latest_status = static_cast(msg->data); + if (latest_status != status_) + { + status_ = latest_status; + const auto& status_str = moveit_servo::SERVO_STATUS_CODE_MAP.at(status_); + ROS_INFO_STREAM_NAMED(LOGNAME, "Servo status: " << status_str); + } + } + moveit_servo::StatusCode status_ = moveit_servo::StatusCode::INVALID; + ros::Subscriber sub_; +}; + +/** + * Instantiate the pose tracking interface. + * Send a pose slightly different from the starting pose + * Then keep updating the target pose a little bit + */ +int main(int argc, char** argv) +{ + ros::init(argc, argv, LOGNAME); + ros::NodeHandle nh("~"); + ros::AsyncSpinner spinner(8); + spinner.start(); + + // Load the planning scene monitor + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; + planning_scene_monitor = std::make_shared("robot_description"); + if (!planning_scene_monitor->getPlanningScene()) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); + exit(EXIT_FAILURE); + } + + planning_scene_monitor->startSceneMonitor(); + planning_scene_monitor->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + planning_scene_monitor->startStateMonitor(); + + // Create the pose tracker + moveit_servo::PoseTracking tracker(nh, planning_scene_monitor); + + // Make a publisher for sending pose commands + ros::Publisher target_pose_pub = + nh.advertise("target_pose", 1 /* queue */, true /* latch */); + + // Subscribe to servo status (and log it when it changes) + StatusMonitor status_monitor(nh, "status"); + + Eigen::Vector3d lin_tol{ 0.01, 0.01, 0.01 }; + double rot_tol = 0.1; + + // Get the current EE transform + geometry_msgs::TransformStamped current_ee_tf; + tracker.getCommandFrameTransform(current_ee_tf); + + // Convert it to a Pose + geometry_msgs::PoseStamped target_pose; + target_pose.header.frame_id = current_ee_tf.header.frame_id; + target_pose.pose.position.x = current_ee_tf.transform.translation.x; + target_pose.pose.position.y = current_ee_tf.transform.translation.y; + target_pose.pose.position.z = current_ee_tf.transform.translation.z; + target_pose.pose.orientation = current_ee_tf.transform.rotation; + + // Modify it a little bit + target_pose.pose.position.x += 0.1; + + // resetTargetPose() can be used to clear the target pose and wait for a new one, e.g. when moving between multiple + // waypoints + tracker.resetTargetPose(); + + // Publish target pose + target_pose.header.stamp = ros::Time::now(); + target_pose_pub.publish(target_pose); + + // Run the pose tracking in a new thread + std::thread move_to_pose_thread( + [&tracker, &lin_tol, &rot_tol] { tracker.moveToPose(lin_tol, rot_tol, 0.1 /* target pose timeout */); }); + + ros::Rate loop_rate(50); + for (size_t i = 0; i < 500; ++i) + { + // Modify the pose target a little bit each cycle + // This is a dynamic pose target + target_pose.pose.position.z += 0.0004; + target_pose.header.stamp = ros::Time::now(); + target_pose_pub.publish(target_pose); + + loop_rate.sleep(); + } + + // Make sure the tracker is stopped and clean up + tracker.stopMotion(); + move_to_pose_thread.join(); + + return EXIT_SUCCESS; +} diff --git a/moveit_ros/moveit_servo/src/low_pass_filter.cpp b/moveit_ros/moveit_servo/src/low_pass_filter.cpp new file mode 100644 index 0000000000..da04cd2716 --- /dev/null +++ b/moveit_ros/moveit_servo/src/low_pass_filter.cpp @@ -0,0 +1,100 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* Title : low_pass_filter.cpp + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Andy Zelenak + */ + +#include +#include +#include +#include + +namespace moveit_servo +{ +namespace +{ +constexpr char LOGNAME[] = "low_pass_filter"; +constexpr double EPSILON = 1e-9; +} // namespace + +LowPassFilter::LowPassFilter(double low_pass_filter_coeff) + : previous_measurements_{ 0., 0. } + , previous_filtered_measurement_(0.) + , scale_term_(1. / (1. + low_pass_filter_coeff)) + , feedback_term_(1. - low_pass_filter_coeff) +{ + // guarantee this doesn't change because the logic below depends on this length implicity + static_assert(LowPassFilter::FILTER_LENGTH == 2, "moveit_servo::LowPassFilter::FILTER_LENGTH should be 2"); + + ROS_ASSERT_MSG(!std::isinf(feedback_term_), "%s: outputs from filter will be inf because feedback term is inf", + LOGNAME); + ROS_ASSERT_MSG(!std::isinf(scale_term_), "%s: outputs from filter will be inf because denominator of scale is 0", + LOGNAME); + ROS_ASSERT_MSG(low_pass_filter_coeff >= 1., "%s: Filter coefficient < 1. makes the lowpass filter unstable", LOGNAME); + + if (std::abs(feedback_term_) < EPSILON) + { + ROS_WARN_STREAM_NAMED(LOGNAME, "Filter coefficient value of " << low_pass_filter_coeff + << " resulted in feedback term of 0. " + " This results in a window averaging Finite " + "Impulse Response (FIR) filter with a gain of " + << scale_term_ * LowPassFilter::FILTER_LENGTH); + } +} + +void LowPassFilter::reset(double data) +{ + previous_measurements_[0] = data; + previous_measurements_[1] = data; + + previous_filtered_measurement_ = data; +} + +double LowPassFilter::filter(double new_measurement) +{ + // Push in the new measurement + previous_measurements_[1] = previous_measurements_[0]; + previous_measurements_[0] = new_measurement; + + double new_filtered_measurement = scale_term_ * (previous_measurements_[1] + previous_measurements_[0] - + feedback_term_ * previous_filtered_measurement_); + + // Store the new filtered measurement + previous_filtered_measurement_ = new_filtered_measurement; + + return new_filtered_measurement; +} +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp new file mode 100644 index 0000000000..723fe2d8f7 --- /dev/null +++ b/moveit_ros/moveit_servo/src/pose_tracking.cpp @@ -0,0 +1,375 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "moveit_servo/pose_tracking.h" + +namespace +{ +constexpr char LOGNAME[] = "pose_tracking"; +constexpr double DEFAULT_LOOP_RATE = 100; // Hz +constexpr double ROS_STARTUP_WAIT = 10; // sec +} // namespace + +namespace moveit_servo +{ +PoseTracking::PoseTracking(const ros::NodeHandle& nh, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : nh_(nh) + , planning_scene_monitor_(planning_scene_monitor) + , loop_rate_(DEFAULT_LOOP_RATE) + , transform_listener_(transform_buffer_) + , stop_requested_(false) + , angular_error_(boost::none) +{ + readROSParams(); + + robot_model_ = planning_scene_monitor_->getRobotModel(); + joint_model_group_ = robot_model_->getJointModelGroup(move_group_name_); + + // Initialize PID controllers + initializePID(x_pid_config_, cartesian_position_pids_); + initializePID(y_pid_config_, cartesian_position_pids_); + initializePID(z_pid_config_, cartesian_position_pids_); + initializePID(angular_pid_config_, cartesian_orientation_pids_); + + // Use the C++ interface that Servo provides + servo_ = std::make_unique(nh_, planning_scene_monitor_); + servo_->start(); + + // Connect to Servo ROS interfaces + target_pose_sub_ = + nh_.subscribe("target_pose", 1, &PoseTracking::targetPoseCallback, this); + + // Publish outgoing twist commands to the Servo object + twist_stamped_pub_ = + nh_.advertise(servo_->getParameters().cartesian_command_in_topic, 1); +} + +PoseTrackingStatusCode PoseTracking::moveToPose(const Eigen::Vector3d& positional_tolerance, + const double angular_tolerance, const double target_pose_timeout) +{ + // Wait a bit for a target pose message to arrive. + // The target pose may get updated by new messages as the robot moves (in a callback function). + const ros::Time start_time = ros::Time::now(); + while ((!haveRecentTargetPose(target_pose_timeout) || !haveRecentEndEffectorPose(target_pose_timeout)) && + ((ros::Time::now() - start_time).toSec() < target_pose_timeout)) + { + if (servo_->getCommandFrameTransform(command_frame_transform_)) + { + command_frame_transform_stamp_ = ros::Time::now(); + } + ros::Duration(0.001).sleep(); + } + + if (!haveRecentTargetPose(target_pose_timeout)) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "The target pose was not updated recently. Aborting."); + return PoseTrackingStatusCode::NO_RECENT_TARGET_POSE; + } + + // Continue sending PID controller output to Servo until one of the following conditions is met: + // - Goal tolerance is satisfied + // - Target pose becomes outdated + // - Command frame transform becomes outdated + // - Another thread requested a stop + while (ros::ok() && !satisfiesPoseTolerance(positional_tolerance, angular_tolerance)) + { + // Attempt to update robot pose + if (servo_->getCommandFrameTransform(command_frame_transform_)) + { + command_frame_transform_stamp_ = ros::Time::now(); + } + + // Check that end-effector pose (command frame transform) is recent enough. + if (!haveRecentEndEffectorPose(target_pose_timeout)) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "The end effector pose was not updated in time. Aborting."); + doPostMotionReset(); + return PoseTrackingStatusCode::NO_RECENT_END_EFFECTOR_POSE; + } + + if (stop_requested_) + { + ROS_INFO_STREAM_NAMED(LOGNAME, "Halting servo motion, a stop was requested."); + doPostMotionReset(); + return PoseTrackingStatusCode::STOP_REQUESTED; + } + + // Compute servo command from PID controller output and send it to the Servo object, for execution + twist_stamped_pub_.publish(calculateTwistCommand()); + + if (!loop_rate_.sleep()) + { + ROS_WARN_STREAM_THROTTLE_NAMED(1, LOGNAME, "Target control rate was missed"); + } + } + + doPostMotionReset(); + return PoseTrackingStatusCode::SUCCESS; +} + +void PoseTracking::readROSParams() +{ + // Optional parameter sub-namespace specified in the launch file. All other parameters will be read from this namespace. + std::string parameter_ns; + ros::param::get("~parameter_ns", parameter_ns); + + // If parameters have been loaded into sub-namespace within the node namespace, append the parameter namespace + // to load the parameters correctly. + ros::NodeHandle nh = parameter_ns.empty() ? nh_ : ros::NodeHandle(nh_, parameter_ns); + + // Wait for ROS parameters to load + ros::Time begin = ros::Time::now(); + while (ros::ok() && !nh.hasParam("planning_frame") && ((ros::Time::now() - begin).toSec() < ROS_STARTUP_WAIT)) + { + ROS_WARN_STREAM_NAMED(LOGNAME, "Waiting for parameter: " + << "planning_frame"); + ros::Duration(0.1).sleep(); + } + + std::size_t error = 0; + + error += !rosparam_shortcuts::get(LOGNAME, nh, "planning_frame", planning_frame_); + error += !rosparam_shortcuts::get(LOGNAME, nh, "move_group_name", move_group_name_); + if (!planning_scene_monitor_->getRobotModel()->hasJointModelGroup(move_group_name_)) + { + ++error; + ROS_ERROR_STREAM_NAMED(LOGNAME, "Unable to find the specified joint model group: " << move_group_name_); + } + + double publish_period; + error += !rosparam_shortcuts::get(LOGNAME, nh, "publish_period", publish_period); + loop_rate_ = ros::Rate(1 / publish_period); + + x_pid_config_.dt = publish_period; + y_pid_config_.dt = publish_period; + z_pid_config_.dt = publish_period; + angular_pid_config_.dt = publish_period; + + double windup_limit; + error += !rosparam_shortcuts::get(LOGNAME, nh, "windup_limit", windup_limit); + x_pid_config_.windup_limit = windup_limit; + y_pid_config_.windup_limit = windup_limit; + z_pid_config_.windup_limit = windup_limit; + angular_pid_config_.windup_limit = windup_limit; + + error += !rosparam_shortcuts::get(LOGNAME, nh, "x_proportional_gain", x_pid_config_.k_p); + error += !rosparam_shortcuts::get(LOGNAME, nh, "y_proportional_gain", y_pid_config_.k_p); + error += !rosparam_shortcuts::get(LOGNAME, nh, "z_proportional_gain", z_pid_config_.k_p); + error += !rosparam_shortcuts::get(LOGNAME, nh, "x_integral_gain", x_pid_config_.k_i); + error += !rosparam_shortcuts::get(LOGNAME, nh, "y_integral_gain", y_pid_config_.k_i); + error += !rosparam_shortcuts::get(LOGNAME, nh, "z_integral_gain", z_pid_config_.k_i); + error += !rosparam_shortcuts::get(LOGNAME, nh, "x_derivative_gain", x_pid_config_.k_d); + error += !rosparam_shortcuts::get(LOGNAME, nh, "y_derivative_gain", y_pid_config_.k_d); + error += !rosparam_shortcuts::get(LOGNAME, nh, "z_derivative_gain", z_pid_config_.k_d); + + error += !rosparam_shortcuts::get(LOGNAME, nh, "angular_proportional_gain", angular_pid_config_.k_p); + error += !rosparam_shortcuts::get(LOGNAME, nh, "angular_integral_gain", angular_pid_config_.k_i); + error += !rosparam_shortcuts::get(LOGNAME, nh, "angular_derivative_gain", angular_pid_config_.k_d); + + rosparam_shortcuts::shutdownIfError(ros::this_node::getName(), error); +} + +void PoseTracking::initializePID(const PIDConfig& pid_config, std::vector& pid_vector) +{ + bool use_anti_windup = true; + pid_vector.push_back(control_toolbox::Pid(pid_config.k_p, pid_config.k_i, pid_config.k_d, pid_config.windup_limit, + -pid_config.windup_limit, use_anti_windup)); +} + +bool PoseTracking::haveRecentTargetPose(const double timespan) +{ + std::lock_guard lock(target_pose_mtx_); + return ((ros::Time::now() - target_pose_.header.stamp).toSec() < timespan); +} + +bool PoseTracking::haveRecentEndEffectorPose(const double timespan) +{ + return ((ros::Time::now() - command_frame_transform_stamp_).toSec() < timespan); +} + +bool PoseTracking::satisfiesPoseTolerance(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance) +{ + std::lock_guard lock(target_pose_mtx_); + double x_error = target_pose_.pose.position.x - command_frame_transform_.translation()(0); + double y_error = target_pose_.pose.position.y - command_frame_transform_.translation()(1); + double z_error = target_pose_.pose.position.z - command_frame_transform_.translation()(2); + + // If uninitialized, likely haven't received the target pose yet. + if (!angular_error_) + return false; + + return ((std::abs(x_error) < positional_tolerance(0)) && (std::abs(y_error) < positional_tolerance(1)) && + (std::abs(z_error) < positional_tolerance(2)) && (std::abs(*angular_error_) < angular_tolerance)); +} + +void PoseTracking::targetPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg) +{ + std::lock_guard lock(target_pose_mtx_); + target_pose_ = *msg; + + // If the target pose is not defined in planning frame, transform the target pose. + if (target_pose_.header.frame_id != planning_frame_) + { + try + { + geometry_msgs::TransformStamped target_to_planning_frame = transform_buffer_.lookupTransform( + planning_frame_, target_pose_.header.frame_id, ros::Time(0), ros::Duration(0.1)); + tf2::doTransform(target_pose_, target_pose_, target_to_planning_frame); + + // Prevent doTransform from copying a stamp of 0, which will cause the haveRecentTargetPose check to fail servo motions + target_pose_.header.stamp = ros::Time::now(); + } + catch (const tf2::TransformException& ex) + { + ROS_WARN_STREAM_NAMED(LOGNAME, ex.what()); + return; + } + } +} + +geometry_msgs::TwistStampedConstPtr PoseTracking::calculateTwistCommand() +{ + // use the shared pool to create a message more efficiently + auto msg = moveit::util::make_shared_from_pool(); + + // Get twist components from PID controllers + geometry_msgs::Twist& twist = msg->twist; + Eigen::Quaterniond q_desired; + + // Scope mutex locking only to operations which require access to target pose. + { + std::lock_guard lock(target_pose_mtx_); + msg->header.frame_id = target_pose_.header.frame_id; + + // Position + twist.linear.x = cartesian_position_pids_[0].computeCommand( + target_pose_.pose.position.x - command_frame_transform_.translation()(0), loop_rate_.expectedCycleTime()); + twist.linear.y = cartesian_position_pids_[1].computeCommand( + target_pose_.pose.position.y - command_frame_transform_.translation()(1), loop_rate_.expectedCycleTime()); + twist.linear.z = cartesian_position_pids_[2].computeCommand( + target_pose_.pose.position.z - command_frame_transform_.translation()(2), loop_rate_.expectedCycleTime()); + + // Orientation algorithm: + // - Find the orientation error as a quaternion: q_error = q_desired * q_current ^ -1 + // - Use the angle-axis PID controller to calculate an angular rate + // - Convert to angular velocity for the TwistStamped message + q_desired = Eigen::Quaterniond(target_pose_.pose.orientation.w, target_pose_.pose.orientation.x, + target_pose_.pose.orientation.y, target_pose_.pose.orientation.z); + } + + Eigen::Quaterniond q_current(command_frame_transform_.rotation()); + Eigen::Quaterniond q_error = q_desired * q_current.inverse(); + + // Convert axis-angle to angular velocity + Eigen::AngleAxisd axis_angle(q_error); + // Cache the angular error, for rotation tolerance checking + angular_error_ = axis_angle.angle(); + double ang_vel_magnitude = + cartesian_orientation_pids_[0].computeCommand(*angular_error_, loop_rate_.expectedCycleTime()); + twist.angular.x = ang_vel_magnitude * axis_angle.axis()[0]; + twist.angular.y = ang_vel_magnitude * axis_angle.axis()[1]; + twist.angular.z = ang_vel_magnitude * axis_angle.axis()[2]; + + msg->header.stamp = ros::Time::now(); + + return msg; +} + +void PoseTracking::doPostMotionReset() +{ + stop_requested_ = false; + angular_error_ = boost::none; + + // Reset error integrals and previous errors of PID controllers + cartesian_position_pids_[0].reset(); + cartesian_position_pids_[1].reset(); + cartesian_position_pids_[2].reset(); + cartesian_orientation_pids_[0].reset(); +} + +void PoseTracking::updatePIDConfig(const double x_proportional_gain, const double x_integral_gain, + const double x_derivative_gain, const double y_proportional_gain, + const double y_integral_gain, const double y_derivative_gain, + const double z_proportional_gain, const double z_integral_gain, + const double z_derivative_gain, const double angular_proportional_gain, + const double angular_integral_gain, const double angular_derivative_gain) +{ + stopMotion(); + + x_pid_config_.k_p = x_proportional_gain; + x_pid_config_.k_i = x_integral_gain; + x_pid_config_.k_d = x_derivative_gain; + y_pid_config_.k_p = y_proportional_gain; + y_pid_config_.k_i = y_integral_gain; + y_pid_config_.k_d = y_derivative_gain; + z_pid_config_.k_p = z_proportional_gain; + z_pid_config_.k_i = z_integral_gain; + z_pid_config_.k_d = z_derivative_gain; + + angular_pid_config_.k_p = angular_proportional_gain; + angular_pid_config_.k_i = angular_integral_gain; + angular_pid_config_.k_d = angular_derivative_gain; + + cartesian_position_pids_.clear(); + cartesian_orientation_pids_.clear(); + initializePID(x_pid_config_, cartesian_position_pids_); + initializePID(y_pid_config_, cartesian_position_pids_); + initializePID(z_pid_config_, cartesian_position_pids_); + initializePID(angular_pid_config_, cartesian_orientation_pids_); + + doPostMotionReset(); +} + +void PoseTracking::getPIDErrors(double& x_error, double& y_error, double& z_error, double& orientation_error) +{ + double dummy1, dummy2; + cartesian_position_pids_.at(0).getCurrentPIDErrors(&x_error, &dummy1, &dummy2); + cartesian_position_pids_.at(1).getCurrentPIDErrors(&y_error, &dummy1, &dummy2); + cartesian_position_pids_.at(2).getCurrentPIDErrors(&z_error, &dummy1, &dummy2); + cartesian_orientation_pids_.at(0).getCurrentPIDErrors(&orientation_error, &dummy1, &dummy2); +} + +void PoseTracking::resetTargetPose() +{ + std::lock_guard lock(target_pose_mtx_); + target_pose_ = geometry_msgs::PoseStamped(); + target_pose_.header.stamp = ros::Time(0); +} + +bool PoseTracking::getCommandFrameTransform(geometry_msgs::TransformStamped& transform) +{ + return servo_->getCommandFrameTransform(transform); +} +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp new file mode 100644 index 0000000000..aba61fc1fe --- /dev/null +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -0,0 +1,348 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* Title : servo.cpp + * Project : moveit_servo + * Created : 3/9/2017 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + */ + +#include + +#include +#include + +static const std::string LOGNAME = "servo_node"; + +namespace moveit_servo +{ +namespace +{ +constexpr double ROBOT_STATE_WAIT_TIME = 10.0; // seconds +} // namespace + +Servo::Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : nh_(nh), planning_scene_monitor_(planning_scene_monitor) +{ + // Read ROS parameters, typically from YAML file + if (!readParameters()) + exit(EXIT_FAILURE); + + // Async spinner is needed to receive messages to wait for the robot state to be complete + ros::AsyncSpinner spinner(1); + spinner.start(); + + // Confirm the planning scene monitor is ready to be used + if (!planning_scene_monitor_->getStateMonitor()) + { + planning_scene_monitor_->startStateMonitor(parameters_.joint_topic); + } + planning_scene_monitor->getStateMonitor()->enableCopyDynamics(true); + + if (!planning_scene_monitor_->getStateMonitor()->waitForCompleteState(parameters_.move_group_name, + ROBOT_STATE_WAIT_TIME)) + { + ROS_FATAL_NAMED(LOGNAME, "Timeout waiting for current state"); + exit(EXIT_FAILURE); + } + + servo_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); + + collision_checker_ = std::make_unique(nh_, parameters_, planning_scene_monitor_); +} + +// Read ROS parameters, typically from YAML file +bool Servo::readParameters() +{ + std::size_t error = 0; + + // Optional parameter sub-namespace specified in the launch file. All other parameters will be read from this namespace. + std::string parameter_ns; + ros::param::get("~parameter_ns", parameter_ns); + + // If parameters have been loaded into sub-namespace within the node namespace, append the parameter namespace + // to load the parameters correctly. + ros::NodeHandle nh = parameter_ns.empty() ? nh_ : ros::NodeHandle(nh_, parameter_ns); + + error += !rosparam_shortcuts::get(LOGNAME, nh, "publish_period", parameters_.publish_period); + error += !rosparam_shortcuts::get(LOGNAME, nh, "collision_check_rate", parameters_.collision_check_rate); + error += !rosparam_shortcuts::get(LOGNAME, nh, "num_outgoing_halt_msgs_to_publish", + parameters_.num_outgoing_halt_msgs_to_publish); + error += !rosparam_shortcuts::get(LOGNAME, nh, "scale/linear", parameters_.linear_scale); + error += !rosparam_shortcuts::get(LOGNAME, nh, "scale/rotational", parameters_.rotational_scale); + error += !rosparam_shortcuts::get(LOGNAME, nh, "scale/joint", parameters_.joint_scale); + error += !rosparam_shortcuts::get(LOGNAME, nh, "low_pass_filter_coeff", parameters_.low_pass_filter_coeff); + error += !rosparam_shortcuts::get(LOGNAME, nh, "joint_topic", parameters_.joint_topic); + error += !rosparam_shortcuts::get(LOGNAME, nh, "command_in_type", parameters_.command_in_type); + error += !rosparam_shortcuts::get(LOGNAME, nh, "cartesian_command_in_topic", parameters_.cartesian_command_in_topic); + error += !rosparam_shortcuts::get(LOGNAME, nh, "joint_command_in_topic", parameters_.joint_command_in_topic); + error += !rosparam_shortcuts::get(LOGNAME, nh, "robot_link_command_frame", parameters_.robot_link_command_frame); + error += !rosparam_shortcuts::get(LOGNAME, nh, "incoming_command_timeout", parameters_.incoming_command_timeout); + error += + !rosparam_shortcuts::get(LOGNAME, nh, "lower_singularity_threshold", parameters_.lower_singularity_threshold); + error += !rosparam_shortcuts::get(LOGNAME, nh, "hard_stop_singularity_threshold", + parameters_.hard_stop_singularity_threshold); + error += !rosparam_shortcuts::get(LOGNAME, nh, "move_group_name", parameters_.move_group_name); + error += !rosparam_shortcuts::get(LOGNAME, nh, "planning_frame", parameters_.planning_frame); + error += !rosparam_shortcuts::get(LOGNAME, nh, "ee_frame_name", parameters_.ee_frame_name); + error += !rosparam_shortcuts::get(LOGNAME, nh, "use_gazebo", parameters_.use_gazebo); + error += !rosparam_shortcuts::get(LOGNAME, nh, "joint_limit_margin", parameters_.joint_limit_margin); + error += !rosparam_shortcuts::get(LOGNAME, nh, "command_out_topic", parameters_.command_out_topic); + error += !rosparam_shortcuts::get(LOGNAME, nh, "command_out_type", parameters_.command_out_type); + error += !rosparam_shortcuts::get(LOGNAME, nh, "publish_joint_positions", parameters_.publish_joint_positions); + error += !rosparam_shortcuts::get(LOGNAME, nh, "publish_joint_velocities", parameters_.publish_joint_velocities); + error += + !rosparam_shortcuts::get(LOGNAME, nh, "publish_joint_accelerations", parameters_.publish_joint_accelerations); + + // Parameters for collision checking + error += !rosparam_shortcuts::get(LOGNAME, nh, "check_collisions", parameters_.check_collisions); + error += !rosparam_shortcuts::get(LOGNAME, nh, "collision_check_type", parameters_.collision_check_type); + bool have_self_collision_proximity_threshold = rosparam_shortcuts::get( + LOGNAME, nh, "self_collision_proximity_threshold", parameters_.self_collision_proximity_threshold); + bool have_scene_collision_proximity_threshold = rosparam_shortcuts::get( + LOGNAME, nh, "scene_collision_proximity_threshold", parameters_.scene_collision_proximity_threshold); + + double collision_proximity_threshold; + // 'collision_proximity_threshold' parameter was removed, replaced with separate self- and scene-collision proximity + // thresholds + // TODO(JStech): remove this deprecation warning in ROS Noetic; simplify error case handling + if (nh.hasParam("collision_proximity_threshold") && + rosparam_shortcuts::get(LOGNAME, nh, "collision_proximity_threshold", collision_proximity_threshold)) + { + ROS_WARN_NAMED(LOGNAME, "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate" + "'self_collision_proximity_threshold' and 'scene_collision_proximity_threshold' " + "parameters. Please update the servoing yaml file."); + if (!have_self_collision_proximity_threshold) + { + parameters_.self_collision_proximity_threshold = collision_proximity_threshold; + have_self_collision_proximity_threshold = true; + } + if (!have_scene_collision_proximity_threshold) + { + parameters_.scene_collision_proximity_threshold = collision_proximity_threshold; + have_scene_collision_proximity_threshold = true; + } + } + error += !have_self_collision_proximity_threshold; + error += !have_scene_collision_proximity_threshold; + error += !rosparam_shortcuts::get(LOGNAME, nh, "collision_distance_safety_factor", + parameters_.collision_distance_safety_factor); + error += !rosparam_shortcuts::get(LOGNAME, nh, "min_allowable_collision_distance", + parameters_.min_allowable_collision_distance); + + // This parameter name was changed recently. + // Try retrieving from the correct name. If it fails, then try the deprecated name. + // TODO(andyz): remove this deprecation warning in ROS Noetic + if (!rosparam_shortcuts::get(LOGNAME, nh, "status_topic", parameters_.status_topic)) + { + ROS_WARN_NAMED(LOGNAME, "'status_topic' parameter is missing. Recently renamed from 'warning_topic'. Please update " + "the servoing yaml file."); + error += !rosparam_shortcuts::get(LOGNAME, nh, "warning_topic", parameters_.status_topic); + } + + if (nh.hasParam("low_latency_mode")) + { + error += !rosparam_shortcuts::get(LOGNAME, nh, "low_latency_mode", parameters_.low_latency_mode); + } + else + { + ROS_WARN_NAMED(LOGNAME, "'low_latency_mode' is a new parameter that runs servo calc immediately after receiving " + "input. Setting to the default value of false."); + parameters_.low_latency_mode = false; + } + + rosparam_shortcuts::shutdownIfError(LOGNAME, error); + + // Input checking + if (parameters_.publish_period <= 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'publish_period' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.num_outgoing_halt_msgs_to_publish < 0) + { + ROS_WARN_NAMED(LOGNAME, + "Parameter 'num_outgoing_halt_msgs_to_publish' should be greater than zero. Check yaml file."); + return false; + } + if (parameters_.hard_stop_singularity_threshold < parameters_.lower_singularity_threshold) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'hard_stop_singularity_threshold' " + "should be greater than 'lower_singularity_threshold.' " + "Check yaml file."); + return false; + } + if ((parameters_.hard_stop_singularity_threshold < 0.) || (parameters_.lower_singularity_threshold < 0.)) + { + ROS_WARN_NAMED(LOGNAME, "Parameters 'hard_stop_singularity_threshold' " + "and 'lower_singularity_threshold' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.low_pass_filter_coeff < 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'low_pass_filter_coeff' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.joint_limit_margin < 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'joint_limit_margin' should be " + "greater than or equal to zero. Check yaml file."); + return false; + } + if (parameters_.command_in_type != "unitless" && parameters_.command_in_type != "speed_units") + { + ROS_WARN_NAMED(LOGNAME, "command_in_type should be 'unitless' or " + "'speed_units'. Check yaml file."); + return false; + } + if (parameters_.command_out_type != "trajectory_msgs/JointTrajectory" && + parameters_.command_out_type != "std_msgs/Float64MultiArray") + { + ROS_WARN_NAMED(LOGNAME, "Parameter command_out_type should be " + "'trajectory_msgs/JointTrajectory' or " + "'std_msgs/Float64MultiArray'. Check yaml file."); + return false; + } + if (!parameters_.publish_joint_positions && !parameters_.publish_joint_velocities && + !parameters_.publish_joint_accelerations) + { + ROS_WARN_NAMED(LOGNAME, "At least one of publish_joint_positions / " + "publish_joint_velocities / " + "publish_joint_accelerations must be true. Check " + "yaml file."); + return false; + } + if ((parameters_.command_out_type == "std_msgs/Float64MultiArray") && parameters_.publish_joint_positions && + parameters_.publish_joint_velocities) + { + ROS_WARN_NAMED(LOGNAME, "When publishing a std_msgs/Float64MultiArray, " + "you must select positions OR velocities."); + return false; + } + // Collision checking + if (parameters_.collision_check_type != "threshold_distance" && parameters_.collision_check_type != "stop_distance") + { + ROS_WARN_NAMED(LOGNAME, "collision_check_type must be 'threshold_distance' or 'stop_distance'"); + return false; + } + if (parameters_.self_collision_proximity_threshold < 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.scene_collision_proximity_threshold < 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'scene_collision_proximity_threshold' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.scene_collision_proximity_threshold < parameters_.self_collision_proximity_threshold) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should probably be less " + "than or equal to 'scene_collision_proximity_threshold'. Check yaml file."); + } + if (parameters_.collision_check_rate < 0) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_check_rate' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.collision_distance_safety_factor < 1) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_distance_safety_factor' should be " + "greater than or equal to 1. Check yaml file."); + return false; + } + if (parameters_.min_allowable_collision_distance < 0) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'min_allowable_collision_distance' should be " + "greater than zero. Check yaml file."); + return false; + } + + return true; +} + +void Servo::start() +{ + setPaused(false); + + // Crunch the numbers in this timer + servo_calcs_->start(); + + // Check collisions in this timer + if (parameters_.check_collisions) + collision_checker_->start(); +} + +Servo::~Servo() +{ + setPaused(true); +} + +void Servo::setPaused(bool paused) +{ + servo_calcs_->setPaused(paused); + collision_checker_->setPaused(paused); +} + +bool Servo::getCommandFrameTransform(Eigen::Isometry3d& transform) +{ + return servo_calcs_->getCommandFrameTransform(transform); +} + +bool Servo::getCommandFrameTransform(geometry_msgs::TransformStamped& transform) +{ + return servo_calcs_->getEEFrameTransform(transform); +} + +bool Servo::getEEFrameTransform(Eigen::Isometry3d& transform) +{ + return servo_calcs_->getEEFrameTransform(transform); +} + +bool Servo::getEEFrameTransform(geometry_msgs::TransformStamped& transform) +{ + return servo_calcs_->getEEFrameTransform(transform); +} + +const ServoParameters& Servo::getParameters() const +{ + return parameters_; +} + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp new file mode 100644 index 0000000000..6270c507eb --- /dev/null +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -0,0 +1,1136 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* Title : servo_calcs.cpp + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + */ + +#include + +#include +#include + +#include +#include + +static const std::string LOGNAME = "servo_calcs"; +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30; // Seconds to throttle logs inside loops + +namespace moveit_servo +{ +namespace +{ +// Helper function for detecting zeroed message +bool isNonZero(const geometry_msgs::TwistStamped& msg) +{ + return msg.twist.linear.x != 0.0 || msg.twist.linear.y != 0.0 || msg.twist.linear.z != 0.0 || + msg.twist.angular.x != 0.0 || msg.twist.angular.y != 0.0 || msg.twist.angular.z != 0.0; +} + +// Helper function for detecting zeroed message +bool isNonZero(const control_msgs::JointJog& msg) +{ + bool all_zeros = true; + for (double delta : msg.velocities) + { + all_zeros &= (delta == 0.0); + }; + return !all_zeros; +} + +// Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped +geometry_msgs::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, + const std::string& parent_frame, + const std::string& child_frame) +{ + geometry_msgs::TransformStamped output = tf2::eigenToTransform(eigen_tf); + output.header.frame_id = parent_frame; + output.child_frame_id = child_frame; + + return output; +} +} // namespace + +// Constructor for the class that handles servoing calculations +ServoCalcs::ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : nh_(nh) + , parameters_(parameters) + , planning_scene_monitor_(planning_scene_monitor) + , stop_requested_(true) + , paused_(false) +{ + // MoveIt Setup + current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + joint_model_group_ = current_state_->getJointModelGroup(parameters_.move_group_name); + prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getActiveJointModels().size()); + + // Subscribe to command topics + twist_stamped_sub_ = + nh_.subscribe(parameters_.cartesian_command_in_topic, ROS_QUEUE_SIZE, &ServoCalcs::twistStampedCB, this); + joint_cmd_sub_ = nh_.subscribe(parameters_.joint_command_in_topic, ROS_QUEUE_SIZE, &ServoCalcs::jointCmdCB, this); + + // ROS Server for allowing drift in some dimensions + drift_dimensions_server_ = nh_.advertiseService(ros::names::append(nh_.getNamespace(), "change_drift_dimensions"), + &ServoCalcs::changeDriftDimensions, this); + + // ROS Server for changing the control dimensions + control_dimensions_server_ = nh_.advertiseService(ros::names::append(nh_.getNamespace(), "change_control_dimensions"), + &ServoCalcs::changeControlDimensions, this); + + // ROS Server to reset the status, e.g. so the arm can move again after a collision + reset_servo_status_ = nh_.advertiseService(ros::names::append(nh_.getNamespace(), "reset_servo_status"), + &ServoCalcs::resetServoStatus, this); + + // Publish and Subscribe to internal namespace topics + ros::NodeHandle internal_nh(nh_, "internal"); + collision_velocity_scale_sub_ = + internal_nh.subscribe("collision_velocity_scale", ROS_QUEUE_SIZE, &ServoCalcs::collisionVelocityScaleCB, this); + worst_case_stop_time_pub_ = internal_nh.advertise("worst_case_stop_time", ROS_QUEUE_SIZE); + + // Publish freshly-calculated joints to the robot. + // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). + if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") + outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, ROS_QUEUE_SIZE); + else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") + outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, ROS_QUEUE_SIZE); + + // Publish status + status_pub_ = nh_.advertise(parameters_.status_topic, ROS_QUEUE_SIZE); + + internal_joint_state_.name = joint_model_group_->getActiveJointModelNames(); + num_joints_ = internal_joint_state_.name.size(); + internal_joint_state_.position.resize(num_joints_); + internal_joint_state_.velocity.resize(num_joints_); + + // A map for the indices of incoming joint commands + for (std::size_t i = 0; i < num_joints_; ++i) + { + joint_state_name_map_[internal_joint_state_.name[i]] = i; + } + + // Low-pass filters for the joint positions + for (size_t i = 0; i < num_joints_; ++i) + { + position_filters_.emplace_back(parameters_.low_pass_filter_coeff); + } + + // A matrix of all zeros is used to check whether matrices have been initialized + Eigen::Matrix3d empty_matrix; + empty_matrix.setZero(); + tf_moveit_to_ee_frame_ = empty_matrix; + tf_moveit_to_robot_cmd_frame_ = empty_matrix; +} + +ServoCalcs::~ServoCalcs() +{ + stop(); +} + +void ServoCalcs::start() +{ + // Stop the thread if we are currently running + stop(); + + // We will update last_sent_command_ every time we start servo + auto initial_joint_trajectory = moveit::util::make_shared_from_pool(); + + // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately" + // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement + initial_joint_trajectory->header.stamp = ros::Time(0); + initial_joint_trajectory->header.frame_id = parameters_.planning_frame; + initial_joint_trajectory->joint_names = internal_joint_state_.name; + trajectory_msgs::JointTrajectoryPoint point; + point.time_from_start = ros::Duration(parameters_.publish_period); + + if (parameters_.publish_joint_positions) + planning_scene_monitor_->getStateMonitor()->getCurrentState()->copyJointGroupPositions(joint_model_group_, + point.positions); + if (parameters_.publish_joint_velocities) + { + std::vector velocity(num_joints_); + point.velocities = velocity; + } + if (parameters_.publish_joint_accelerations) + { + // I do not know of a robot that takes acceleration commands. + // However, some controllers check that this data is non-empty. + // Send all zeros, for now. + std::vector acceleration(num_joints_); + point.accelerations = acceleration; + } + initial_joint_trajectory->points.push_back(point); + last_sent_command_ = initial_joint_trajectory; + + current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + tf_moveit_to_ee_frame_ = current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + current_state_->getGlobalLinkTransform(parameters_.ee_frame_name); + tf_moveit_to_robot_cmd_frame_ = current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + current_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); + + stop_requested_ = false; + thread_ = std::thread([this] { mainCalcLoop(); }); + new_input_cmd_ = false; +} + +void ServoCalcs::stop() +{ + // Request stop + stop_requested_ = true; + + // Notify condition variable in case the thread is blocked on it + { + // scope so the mutex is unlocked after so the thread can continue + // and therefore be joinable + const std::lock_guard lock(input_mutex_); + new_input_cmd_ = false; + input_cv_.notify_all(); + } + + // Join the thread + if (thread_.joinable()) + { + thread_.join(); + } +} + +void ServoCalcs::mainCalcLoop() +{ + ros::Rate rate(1.0 / parameters_.publish_period); + + while (ros::ok() && !stop_requested_) + { + // lock the input state mutex + std::unique_lock input_lock(input_mutex_); + + // low latency mode -- begin calculations as soon as a new command is received. + if (parameters_.low_latency_mode) + { + input_cv_.wait(input_lock, [this] { return (new_input_cmd_ || stop_requested_); }); + } + + // reset new_input_cmd_ flag + new_input_cmd_ = false; + + // run servo calcs + const auto start_time = ros::Time::now(); + calculateSingleIteration(); + const auto run_duration = ros::Time::now() - start_time; + + // Log warning when the run duration was longer than the period + if (run_duration.toSec() > parameters_.publish_period) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "run_duration: " << run_duration.toSec() << " (" << parameters_.publish_period + << ")"); + } + + // normal mode, unlock input mutex and wait for the period of the loop + if (!parameters_.low_latency_mode) + { + input_lock.unlock(); + rate.sleep(); + } + } +} + +void ServoCalcs::calculateSingleIteration() +{ + // Publish status each loop iteration + auto status_msg = moveit::util::make_shared_from_pool(); + status_msg->data = static_cast(status_); + status_pub_.publish(status_msg); + + // Always update the joints and end-effector transform for 2 reasons: + // 1) in case the getCommandFrameTransform() method is being used + // 2) so the low-pass filters are up to date and don't cause a jump + updateJoints(); + + // Update from latest state + current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + + if (latest_twist_stamped_) + twist_stamped_cmd_ = *latest_twist_stamped_; + if (latest_joint_cmd_) + joint_servo_cmd_ = *latest_joint_cmd_; + + // Check for stale cmds + twist_command_is_stale_ = + ((ros::Time::now() - latest_twist_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); + joint_command_is_stale_ = + ((ros::Time::now() - latest_joint_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); + + have_nonzero_twist_stamped_ = latest_nonzero_twist_stamped_; + have_nonzero_joint_command_ = latest_nonzero_joint_cmd_; + + // Get the transform from MoveIt planning frame to servoing command frame + // Calculate this transform to ensure it is available via C++ API + // We solve (planning_frame -> base -> robot_link_command_frame) + // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) + tf_moveit_to_robot_cmd_frame_ = current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + current_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); + + // Calculate the transform from MoveIt planning frame to End Effector frame + // Calculate this transform to ensure it is available via C++ API + tf_moveit_to_ee_frame_ = current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + current_state_->getGlobalLinkTransform(parameters_.ee_frame_name); + + have_nonzero_command_ = have_nonzero_twist_stamped_ || have_nonzero_joint_command_; + + // Don't end this function without updating the filters + updated_filters_ = false; + + // If paused or while waiting for initial servo commands, just keep the low-pass filters up to date with current + // joints so a jump doesn't occur when restarting + if (wait_for_servo_commands_ || paused_) + { + resetLowPassFilters(original_joint_state_); + + // Check if there are any new commands with valid timestamp + wait_for_servo_commands_ = + twist_stamped_cmd_.header.stamp == ros::Time(0.) && joint_servo_cmd_.header.stamp == ros::Time(0.); + + // Early exit + return; + } + + // If not waiting for initial command, and not paused. + // Do servoing calculations only if the robot should move, for efficiency + // Create new outgoing joint trajectory command message + auto joint_trajectory = moveit::util::make_shared_from_pool(); + + // Prioritize cartesian servoing above joint servoing + // Only run commands if not stale and nonzero + if (have_nonzero_twist_stamped_ && !twist_command_is_stale_) + { + if (!cartesianServoCalcs(twist_stamped_cmd_, *joint_trajectory)) + { + resetLowPassFilters(original_joint_state_); + return; + } + } + else if (have_nonzero_joint_command_ && !joint_command_is_stale_) + { + if (!jointServoCalcs(joint_servo_cmd_, *joint_trajectory)) + { + resetLowPassFilters(original_joint_state_); + return; + } + } + else + { + // Joint trajectory is not populated with anything, so set it to the last positions and 0 velocity + *joint_trajectory = *last_sent_command_; + for (auto& point : joint_trajectory->points) + { + point.velocities.assign(point.velocities.size(), 0); + } + } + + // Print a warning to the user if both are stale + if (!twist_command_is_stale_ || !joint_command_is_stale_) + { + ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, + "Stale command. " + "Try a larger 'incoming_command_timeout' parameter?"); + } + + // If we should halt + if (!have_nonzero_command_) + { + suddenHalt(*joint_trajectory); + have_nonzero_twist_stamped_ = false; + have_nonzero_joint_command_ = false; + } + + // Skip the servoing publication if all inputs have been zero for several cycles in a row. + // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. + if (!have_nonzero_command_ && (parameters_.num_outgoing_halt_msgs_to_publish != 0) && + (zero_velocity_count_ > parameters_.num_outgoing_halt_msgs_to_publish)) + { + ok_to_publish_ = false; + ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "All-zero command. Doing nothing."); + } + else + { + ok_to_publish_ = true; + } + + // Store last zero-velocity message flag to prevent superfluous warnings. + // Cartesian and joint commands must both be zero. + if (!have_nonzero_command_) + { + // Avoid overflow + if (zero_velocity_count_ < std::numeric_limits::max()) + ++zero_velocity_count_; + } + else + { + zero_velocity_count_ = 0; + } + + if (ok_to_publish_ && !paused_) + { + // Put the outgoing msg in the right format + // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). + if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") + { + // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately" + // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement + joint_trajectory->header.stamp = ros::Time(0); + outgoing_cmd_pub_.publish(joint_trajectory); + } + else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") + { + auto joints = moveit::util::make_shared_from_pool(); + if (parameters_.publish_joint_positions && !joint_trajectory->points.empty()) + joints->data = joint_trajectory->points[0].positions; + else if (parameters_.publish_joint_velocities && !joint_trajectory->points.empty()) + joints->data = joint_trajectory->points[0].velocities; + outgoing_cmd_pub_.publish(joints); + } + + last_sent_command_ = joint_trajectory; + } + + // Update the filters if we haven't yet + if (!updated_filters_) + resetLowPassFilters(original_joint_state_); +} +// Perform the servoing calculations +bool ServoCalcs::cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, + trajectory_msgs::JointTrajectory& joint_trajectory) +{ + // Check for nan's in the incoming command + if (std::isnan(cmd.twist.linear.x) || std::isnan(cmd.twist.linear.y) || std::isnan(cmd.twist.linear.z) || + std::isnan(cmd.twist.angular.x) || std::isnan(cmd.twist.angular.y) || std::isnan(cmd.twist.angular.z)) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "nan in incoming command. Skipping this datapoint."); + return false; + } + + // If incoming commands should be in the range [-1:1], check for |delta|>1 + if (parameters_.command_in_type == "unitless") + { + if ((fabs(cmd.twist.linear.x) > 1) || (fabs(cmd.twist.linear.y) > 1) || (fabs(cmd.twist.linear.z) > 1) || + (fabs(cmd.twist.angular.x) > 1) || (fabs(cmd.twist.angular.y) > 1) || (fabs(cmd.twist.angular.z) > 1)) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "Component of incoming command is >1. Skipping this datapoint."); + return false; + } + } + + // Set uncontrolled dimensions to 0 in command frame + if (!control_dimensions_[0]) + cmd.twist.linear.x = 0; + if (!control_dimensions_[1]) + cmd.twist.linear.y = 0; + if (!control_dimensions_[2]) + cmd.twist.linear.z = 0; + if (!control_dimensions_[3]) + cmd.twist.angular.x = 0; + if (!control_dimensions_[4]) + cmd.twist.angular.y = 0; + if (!control_dimensions_[5]) + cmd.twist.angular.z = 0; + + // Transform the command to the MoveGroup planning frame + if (cmd.header.frame_id != parameters_.planning_frame) + { + Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z); + Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z); + + // If the incoming frame is empty or is the command frame, we use the previously calculated tf + if (cmd.header.frame_id.empty() || cmd.header.frame_id == parameters_.robot_link_command_frame) + { + translation_vector = tf_moveit_to_robot_cmd_frame_.linear() * translation_vector; + angular_vector = tf_moveit_to_robot_cmd_frame_.linear() * angular_vector; + } + else if (cmd.header.frame_id == parameters_.ee_frame_name) + { + // If the frame is the EE frame, we already have that transform as well + translation_vector = tf_moveit_to_ee_frame_.linear() * translation_vector; + angular_vector = tf_moveit_to_ee_frame_.linear() * angular_vector; + } + else + { + // We solve (planning_frame -> base -> cmd.header.frame_id) + // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id) + const auto tf_moveit_to_incoming_cmd_frame = + current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + current_state_->getGlobalLinkTransform(cmd.header.frame_id); + + translation_vector = tf_moveit_to_incoming_cmd_frame.linear() * translation_vector; + angular_vector = tf_moveit_to_incoming_cmd_frame.linear() * angular_vector; + } + + // Put these components back into a TwistStamped + cmd.header.frame_id = parameters_.planning_frame; + cmd.twist.linear.x = translation_vector(0); + cmd.twist.linear.y = translation_vector(1); + cmd.twist.linear.z = translation_vector(2); + cmd.twist.angular.x = angular_vector(0); + cmd.twist.angular.y = angular_vector(1); + cmd.twist.angular.z = angular_vector(2); + } + + Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); + + // Convert from cartesian commands to joint commands + Eigen::MatrixXd jacobian = current_state_->getJacobian(joint_model_group_); + + // May allow some dimensions to drift, based on drift_dimensions + // i.e. take advantage of task redundancy. + // Remove the Jacobian rows corresponding to True in the vector drift_dimensions + // Work backwards through the 6-vector so indices don't get out of order + for (auto dimension = jacobian.rows() - 1; dimension >= 0; --dimension) + { + if (drift_dimensions_[dimension] && jacobian.rows() > 1) + { + removeDimension(jacobian, delta_x, dimension); + } + } + + Eigen::JacobiSVD svd = + Eigen::JacobiSVD(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal(); + Eigen::MatrixXd pseudo_inverse = svd.matrixV() * matrix_s.inverse() * svd.matrixU().transpose(); + + delta_theta_ = pseudo_inverse * delta_x; + + enforceVelLimits(delta_theta_); + + // If close to a collision or a singularity, decelerate + applyVelocityScaling(delta_theta_, velocityScalingFactorForSingularity(delta_x, svd, pseudo_inverse)); + + prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; + + return convertDeltasToOutgoingCmd(joint_trajectory); +} + +bool ServoCalcs::jointServoCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory) +{ + // Check for nan's + for (double velocity : cmd.velocities) + { + if (std::isnan(velocity)) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "nan in incoming command. Skipping this datapoint."); + return false; + } + } + + // Apply user-defined scaling + delta_theta_ = scaleJointCommand(cmd); + + enforceVelLimits(delta_theta_); + + // If close to a collision, decelerate + applyVelocityScaling(delta_theta_, 1.0 /* scaling for singularities -- ignore for joint motions */); + + prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; + + return convertDeltasToOutgoingCmd(joint_trajectory); +} + +bool ServoCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& joint_trajectory) +{ + internal_joint_state_ = original_joint_state_; + if (!addJointIncrements(internal_joint_state_, delta_theta_)) + return false; + + lowPassFilterPositions(internal_joint_state_); + + // Calculate joint velocities here so that positions are filtered and SRDF bounds still get checked + calculateJointVelocities(internal_joint_state_, delta_theta_); + + composeJointTrajMessage(internal_joint_state_, joint_trajectory); + + if (!enforcePositionLimits()) + { + suddenHalt(joint_trajectory); + status_ = StatusCode::JOINT_BOUND; + } + + // done with calculations + if (parameters_.use_gazebo) + { + insertRedundantPointsIntoTrajectory(joint_trajectory, gazebo_redundant_message_count_); + } + + return true; +} + +// Spam several redundant points into the trajectory. The first few may be skipped if the +// time stamp is in the past when it reaches the client. Needed for gazebo simulation. +// Start from 2 because the first point's timestamp is already 1*parameters_.publish_period +void ServoCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& joint_trajectory, int count) const +{ + joint_trajectory.points.resize(count); + auto point = joint_trajectory.points[0]; + // Start from 2 because we already have the first point. End at count+1 so (total #) == count + for (int i = 2; i < count; ++i) + { + point.time_from_start = ros::Duration(i * parameters_.publish_period); + joint_trajectory.points[i] = point; + } +} + +void ServoCalcs::lowPassFilterPositions(sensor_msgs::JointState& joint_state) +{ + for (size_t i = 0; i < position_filters_.size(); ++i) + { + joint_state.position[i] = position_filters_[i].filter(joint_state.position[i]); + } + + updated_filters_ = true; +} + +void ServoCalcs::resetLowPassFilters(const sensor_msgs::JointState& joint_state) +{ + for (std::size_t i = 0; i < position_filters_.size(); ++i) + { + position_filters_[i].reset(joint_state.position[i]); + } + + updated_filters_ = true; +} + +void ServoCalcs::calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta) +{ + for (int i = 0; i < delta_theta.size(); ++i) + { + joint_state.velocity[i] = delta_theta[i] / parameters_.publish_period; + } +} + +void ServoCalcs::composeJointTrajMessage(const sensor_msgs::JointState& joint_state, + trajectory_msgs::JointTrajectory& joint_trajectory) const +{ + // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately" + // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement + joint_trajectory.header.stamp = ros::Time(0); + joint_trajectory.header.frame_id = parameters_.planning_frame; + joint_trajectory.joint_names = joint_state.name; + + trajectory_msgs::JointTrajectoryPoint point; + point.time_from_start = ros::Duration(parameters_.publish_period); + if (parameters_.publish_joint_positions) + point.positions = joint_state.position; + if (parameters_.publish_joint_velocities) + point.velocities = joint_state.velocity; + if (parameters_.publish_joint_accelerations) + { + // I do not know of a robot that takes acceleration commands. + // However, some controllers check that this data is non-empty. + // Send all zeros, for now. + std::vector acceleration(num_joints_); + point.accelerations = acceleration; + } + joint_trajectory.points.push_back(point); +} + +// Apply velocity scaling for proximity of collisions and singularities. +void ServoCalcs::applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singularity_scale) +{ + double collision_scale = collision_velocity_scale_; + + if (collision_scale > 0 && collision_scale < 1) + { + status_ = StatusCode::DECELERATE_FOR_COLLISION; + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, SERVO_STATUS_CODE_MAP.at(status_)); + } + else if (collision_scale == 0) + { + status_ = StatusCode::HALT_FOR_COLLISION; + } + + delta_theta = collision_scale * singularity_scale * delta_theta; + + if (status_ == StatusCode::HALT_FOR_COLLISION) + { + ROS_WARN_STREAM_THROTTLE_NAMED(3, LOGNAME, "Halting for collision!"); + delta_theta_.setZero(); + } +} + +// Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion +double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, + const Eigen::JacobiSVD& svd, + const Eigen::MatrixXd& pseudo_inverse) +{ + double velocity_scale = 1; + std::size_t num_dimensions = commanded_velocity.size(); + + // Find the direction away from nearest singularity. + // The last column of U from the SVD of the Jacobian points directly toward or away from the singularity. + // The sign can flip at any time, so we have to do some extra checking. + // Look ahead to see if the Jacobian's condition will decrease. + Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(num_dimensions - 1); + + double ini_condition = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); + + // This singular vector tends to flip direction unpredictably. See R. Bro, + // "Resolving the Sign Ambiguity in the Singular Value Decomposition". + // Look ahead to see if the Jacobian's condition will decrease in this + // direction. Start with a scaled version of the singular vector + Eigen::VectorXd delta_x(num_dimensions); + double scale = 100; + delta_x = vector_toward_singularity / scale; + + // Calculate a small change in joints + Eigen::VectorXd new_theta; + current_state_->copyJointGroupPositions(joint_model_group_, new_theta); + new_theta += pseudo_inverse * delta_x; + current_state_->setJointGroupPositions(joint_model_group_, new_theta); + Eigen::MatrixXd new_jacobian = current_state_->getJacobian(joint_model_group_); + + Eigen::JacobiSVD new_svd(new_jacobian); + double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1); + // If new_condition < ini_condition, the singular vector does point towards a + // singularity. Otherwise, flip its direction. + if (ini_condition >= new_condition) + { + vector_toward_singularity *= -1; + } + + // If this dot product is positive, we're moving toward singularity ==> decelerate + double dot = vector_toward_singularity.dot(commanded_velocity); + if (dot > 0) + { + // Ramp velocity down linearly when the Jacobian condition is between lower_singularity_threshold and + // hard_stop_singularity_threshold, and we're moving towards the singularity + if ((ini_condition > parameters_.lower_singularity_threshold) && + (ini_condition < parameters_.hard_stop_singularity_threshold)) + { + velocity_scale = 1. - (ini_condition - parameters_.lower_singularity_threshold) / + (parameters_.hard_stop_singularity_threshold - parameters_.lower_singularity_threshold); + status_ = StatusCode::DECELERATE_FOR_SINGULARITY; + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, SERVO_STATUS_CODE_MAP.at(status_)); + } + + // Very close to singularity, so halt. + else if (ini_condition > parameters_.hard_stop_singularity_threshold) + { + velocity_scale = 0; + status_ = StatusCode::HALT_FOR_SINGULARITY; + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, SERVO_STATUS_CODE_MAP.at(status_)); + } + } + + return velocity_scale; +} + +void ServoCalcs::enforceVelLimits(Eigen::ArrayXd& delta_theta) +{ + // Convert to joint angle velocities for checking and applying joint specific velocity limits. + Eigen::ArrayXd velocity = delta_theta / parameters_.publish_period; + + std::size_t joint_delta_index{ 0 }; + double velocity_scaling_factor{ 1.0 }; + for (const moveit::core::JointModel* joint : joint_model_group_->getActiveJointModels()) + { + const auto& bounds = joint->getVariableBounds(joint->getName()); + if (bounds.velocity_bounded_ && velocity(joint_delta_index) != 0.0) + { + const double unbounded_velocity = velocity(joint_delta_index); + // Clamp each joint velocity to a joint specific [min_velocity, max_velocity] range. + const auto bounded_velocity = std::min(std::max(unbounded_velocity, bounds.min_velocity_), bounds.max_velocity_); + velocity_scaling_factor = std::min(velocity_scaling_factor, bounded_velocity / unbounded_velocity); + } + ++joint_delta_index; + } + + // Convert back to joint angle increments. + delta_theta = velocity_scaling_factor * velocity * parameters_.publish_period; +} + +bool ServoCalcs::enforcePositionLimits() +{ + bool halting = false; + + for (auto joint : joint_model_group_->getActiveJointModels()) + { + // Halt if we're past a joint margin and joint velocity is moving even farther past + double joint_angle = 0; + for (std::size_t c = 0; c < original_joint_state_.name.size(); ++c) + { + if (original_joint_state_.name[c] == joint->getName()) + { + joint_angle = original_joint_state_.position.at(c); + break; + } + } + if (!current_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin)) + { + const std::vector limits = joint->getVariableBoundsMsg(); + + // Joint limits are not defined for some joints. Skip them. + if (!limits.empty()) + { + if ((current_state_->getJointVelocities(joint)[0] < 0 && + (joint_angle < (limits[0].min_position + parameters_.joint_limit_margin))) || + (current_state_->getJointVelocities(joint)[0] > 0 && + (joint_angle > (limits[0].max_position - parameters_.joint_limit_margin)))) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + ros::this_node::getName() << " " << joint->getName() + << " close to a " + " position limit. Halting."); + halting = true; + } + } + } + } + return !halting; +} + +// Suddenly halt for a joint limit or other critical issue. +// Is handled differently for position vs. velocity control. +void ServoCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory) +{ + // Prepare the joint trajectory message to stop the robot + joint_trajectory.points.clear(); + joint_trajectory.points.emplace_back(); + trajectory_msgs::JointTrajectoryPoint& point = joint_trajectory.points.front(); + + // When sending out trajectory_msgs/JointTrajectory type messages, the "trajectory" is just a single point. + // That point cannot have the same timestamp as the start of trajectory execution since that would mean the + // arm has to reach the first trajectory point the moment execution begins. To prevent errors about points + // being 0 seconds in the past, the smallest supported timestep is added as time from start to the trajectory point. + point.time_from_start.fromNSec(1); + + point.positions.resize(num_joints_); + point.velocities.resize(num_joints_); + + // Assert the following loop is safe to execute + assert(original_joint_state_.position.size() >= num_joints_); + + // Set the positions and velocities vectors + for (std::size_t i = 0; i < num_joints_; ++i) + { + // For position-controlled robots, can reset the joints to a known, good state + if (parameters_.publish_joint_positions) + point.positions[i] = original_joint_state_.position[i]; + + // For velocity-controlled robots, stop + if (parameters_.publish_joint_velocities) + point.velocities[i] = 0; + } +} + +// Parse the incoming joint msg for the joints of our MoveGroup +void ServoCalcs::updateJoints() +{ + // Get the latest joint group positions + current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + current_state_->copyJointGroupPositions(joint_model_group_, internal_joint_state_.position); + current_state_->copyJointGroupVelocities(joint_model_group_, internal_joint_state_.velocity); + + // Cache the original joints in case they need to be reset + original_joint_state_ = internal_joint_state_; + + // Calculate worst case joint stop time, for collision checking + std::string joint_name = ""; + moveit::core::JointModel::Bounds kinematic_bounds; + double accel_limit = 0; + double joint_velocity = 0; + double worst_case_stop_time = 0; + for (size_t jt_state_idx = 0; jt_state_idx < internal_joint_state_.velocity.size(); ++jt_state_idx) + { + joint_name = internal_joint_state_.name[jt_state_idx]; + + // Get acceleration limit for this joint + for (auto joint_model : joint_model_group_->getActiveJointModels()) + { + if (joint_model->getName() == joint_name) + { + kinematic_bounds = joint_model->getVariableBounds(); + // Some joints do not have acceleration limits + if (kinematic_bounds[0].acceleration_bounded_) + { + // Be conservative when calculating overall acceleration limit from min and max limits + accel_limit = + std::min(fabs(kinematic_bounds[0].min_acceleration_), fabs(kinematic_bounds[0].max_acceleration_)); + } + else + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "An acceleration limit is not defined for this joint; minimum stop distance " + "should not be used for collision checking"); + } + break; + } + } + + // Get the current joint velocity + joint_velocity = internal_joint_state_.velocity[jt_state_idx]; + + // Calculate worst case stop time + worst_case_stop_time = std::max(worst_case_stop_time, fabs(joint_velocity / accel_limit)); + } + + // publish message + { + auto msg = moveit::util::make_shared_from_pool(); + msg->data = worst_case_stop_time; + worst_case_stop_time_pub_.publish(msg); + } +} + +// Scale the incoming servo command +Eigen::VectorXd ServoCalcs::scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const +{ + Eigen::VectorXd result(6); + + // Apply user-defined scaling if inputs are unitless [-1:1] + if (parameters_.command_in_type == "unitless") + { + result[0] = parameters_.linear_scale * parameters_.publish_period * command.twist.linear.x; + result[1] = parameters_.linear_scale * parameters_.publish_period * command.twist.linear.y; + result[2] = parameters_.linear_scale * parameters_.publish_period * command.twist.linear.z; + result[3] = parameters_.rotational_scale * parameters_.publish_period * command.twist.angular.x; + result[4] = parameters_.rotational_scale * parameters_.publish_period * command.twist.angular.y; + result[5] = parameters_.rotational_scale * parameters_.publish_period * command.twist.angular.z; + } + // Otherwise, commands are in m/s and rad/s + else if (parameters_.command_in_type == "speed_units") + { + result[0] = command.twist.linear.x * parameters_.publish_period; + result[1] = command.twist.linear.y * parameters_.publish_period; + result[2] = command.twist.linear.z * parameters_.publish_period; + result[3] = command.twist.angular.x * parameters_.publish_period; + result[4] = command.twist.angular.y * parameters_.publish_period; + result[5] = command.twist.angular.z * parameters_.publish_period; + } + else + ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Unexpected command_in_type"); + + return result; +} + +Eigen::VectorXd ServoCalcs::scaleJointCommand(const control_msgs::JointJog& command) const +{ + Eigen::VectorXd result(num_joints_); + result.setZero(); + + std::size_t c; + for (std::size_t m = 0; m < command.joint_names.size(); ++m) + { + try + { + c = joint_state_name_map_.at(command.joint_names[m]); + } + catch (const std::out_of_range& e) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Ignoring joint " << command.joint_names[m]); + continue; + } + // Apply user-defined scaling if inputs are unitless [-1:1] + if (parameters_.command_in_type == "unitless") + result[c] = command.velocities[m] * parameters_.joint_scale * parameters_.publish_period; + // Otherwise, commands are in m/s and rad/s + else if (parameters_.command_in_type == "speed_units") + result[c] = command.velocities[m] * parameters_.publish_period; + else + ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Unexpected command_in_type, check yaml file."); + } + + return result; +} + +// Add the deltas to each joint +bool ServoCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const +{ + for (std::size_t i = 0, size = static_cast(increments.size()); i < size; ++i) + { + try + { + output.position[i] += increments[i]; + } + catch (const std::out_of_range& e) + { + ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + ros::this_node::getName() << " Lengths of output and " + "increments do not match."); + return false; + } + } + + return true; +} + +void ServoCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta_x, unsigned int row_to_remove) +{ + unsigned int num_rows = jacobian.rows() - 1; + unsigned int num_cols = jacobian.cols(); + + if (row_to_remove < num_rows) + { + jacobian.block(row_to_remove, 0, num_rows - row_to_remove, num_cols) = + jacobian.block(row_to_remove + 1, 0, num_rows - row_to_remove, num_cols); + delta_x.segment(row_to_remove, num_rows - row_to_remove) = + delta_x.segment(row_to_remove + 1, num_rows - row_to_remove); + } + jacobian.conservativeResize(num_rows, num_cols); + delta_x.conservativeResize(num_rows); +} + +bool ServoCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) +{ + const std::lock_guard lock(input_mutex_); + transform = tf_moveit_to_robot_cmd_frame_; + + // All zeros means the transform wasn't initialized, so return false + return !transform.matrix().isZero(0); +} + +bool ServoCalcs::getCommandFrameTransform(geometry_msgs::TransformStamped& transform) +{ + const std::lock_guard lock(input_mutex_); + // All zeros means the transform wasn't initialized, so return false + if (tf_moveit_to_robot_cmd_frame_.matrix().isZero(0)) + { + return false; + } + + transform = convertIsometryToTransform(tf_moveit_to_robot_cmd_frame_, parameters_.planning_frame, + parameters_.robot_link_command_frame); + return true; +} + +bool ServoCalcs::getEEFrameTransform(Eigen::Isometry3d& transform) +{ + const std::lock_guard lock(input_mutex_); + transform = tf_moveit_to_ee_frame_; + + // All zeros means the transform wasn't initialized, so return false + return !transform.matrix().isZero(0); +} + +bool ServoCalcs::getEEFrameTransform(geometry_msgs::TransformStamped& transform) +{ + const std::lock_guard lock(input_mutex_); + // All zeros means the transform wasn't initialized, so return false + if (tf_moveit_to_ee_frame_.matrix().isZero(0)) + { + return false; + } + + transform = convertIsometryToTransform(tf_moveit_to_ee_frame_, parameters_.planning_frame, parameters_.ee_frame_name); + return true; +} + +void ServoCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg) +{ + const std::lock_guard lock(input_mutex_); + latest_twist_stamped_ = msg; + latest_nonzero_twist_stamped_ = isNonZero(*latest_twist_stamped_); + + if (msg->header.stamp != ros::Time(0.)) + latest_twist_command_stamp_ = msg->header.stamp; + + // notify that we have a new input + new_input_cmd_ = true; + input_cv_.notify_all(); +} + +void ServoCalcs::jointCmdCB(const control_msgs::JointJogConstPtr& msg) +{ + const std::lock_guard lock(input_mutex_); + latest_joint_cmd_ = msg; + latest_nonzero_joint_cmd_ = isNonZero(*latest_joint_cmd_); + + if (msg->header.stamp != ros::Time(0.)) + latest_joint_command_stamp_ = msg->header.stamp; + + // notify that we have a new input + new_input_cmd_ = true; + input_cv_.notify_all(); +} + +void ServoCalcs::collisionVelocityScaleCB(const std_msgs::Float64ConstPtr& msg) +{ + collision_velocity_scale_ = msg->data; +} + +bool ServoCalcs::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, + moveit_msgs::ChangeDriftDimensions::Response& res) +{ + drift_dimensions_[0] = req.drift_x_translation; + drift_dimensions_[1] = req.drift_y_translation; + drift_dimensions_[2] = req.drift_z_translation; + drift_dimensions_[3] = req.drift_x_rotation; + drift_dimensions_[4] = req.drift_y_rotation; + drift_dimensions_[5] = req.drift_z_rotation; + + res.success = true; + return true; +} + +bool ServoCalcs::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, + moveit_msgs::ChangeControlDimensions::Response& res) +{ + control_dimensions_[0] = req.control_x_translation; + control_dimensions_[1] = req.control_y_translation; + control_dimensions_[2] = req.control_z_translation; + control_dimensions_[3] = req.control_x_rotation; + control_dimensions_[4] = req.control_y_rotation; + control_dimensions_[5] = req.control_z_rotation; + + res.success = true; + return true; +} + +bool ServoCalcs::resetServoStatus(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/) +{ + status_ = StatusCode::NO_WARNING; + return true; +} + +void ServoCalcs::setPaused(bool paused) +{ + paused_ = paused; +} + +void ServoCalcs::changeRobotLinkCommandFrame(const std::string& new_command_frame) +{ + parameters_.robot_link_command_frame = new_command_frame; +} + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_server.cpp b/moveit_ros/moveit_servo/src/servo_server.cpp new file mode 100644 index 0000000000..5c630f3082 --- /dev/null +++ b/moveit_ros/moveit_servo/src/servo_server.cpp @@ -0,0 +1,86 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* Title : servo_server.cpp + * Project : moveit_servo + * Created : 12/31/2018 + * Author : Andy Zelenak + */ + +#include + +namespace +{ +constexpr char LOGNAME[] = "servo_server"; +constexpr char ROS_THREADS = 8; + +} // namespace + +int main(int argc, char** argv) +{ + ros::init(argc, argv, LOGNAME); + ros::AsyncSpinner spinner(ROS_THREADS); + spinner.start(); + + ros::NodeHandle nh("~"); + + // Load the planning scene monitor + auto planning_scene_monitor = std::make_shared("robot_description"); + if (!planning_scene_monitor->getPlanningScene()) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); + exit(EXIT_FAILURE); + } + + // Start the planning scene monitor + planning_scene_monitor->startSceneMonitor(); + planning_scene_monitor->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + planning_scene_monitor->startStateMonitor(); + + // Create the servo server + moveit_servo::Servo servo(nh, planning_scene_monitor); + + // Start the servo server (runs in the ros spinner) + servo.start(); + + // Wait for ros to shutdown + ros::waitForShutdown(); + + // Stop the servo server + servo.setPaused(true); + + return 0; +} diff --git a/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp new file mode 100644 index 0000000000..cec304711a --- /dev/null +++ b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp @@ -0,0 +1,107 @@ +/******************************************************************************* + * Title : spacenav_to_twist.cpp + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +#include "geometry_msgs/TwistStamped.h" +#include "control_msgs/JointJog.h" +#include "ros/ros.h" +#include "sensor_msgs/Joy.h" + +namespace moveit_servo +{ +static const int NUM_SPINNERS = 1; +static const int QUEUE_LENGTH = 1; + +class SpaceNavToTwist +{ +public: + SpaceNavToTwist() : spinner_(NUM_SPINNERS) + { + joy_sub_ = n_.subscribe("spacenav/joy", QUEUE_LENGTH, &SpaceNavToTwist::joyCallback, this); + twist_pub_ = n_.advertise("servo_server/delta_twist_cmds", QUEUE_LENGTH); + joint_delta_pub_ = n_.advertise("servo_server/delta_joint_cmds", QUEUE_LENGTH); + + spinner_.start(); + ros::waitForShutdown(); + }; + +private: + // Convert incoming joy commands to TwistStamped commands for servoing. + // The TwistStamped component goes to servoing, while buttons 0 & 1 control + // joints directly. + void joyCallback(const sensor_msgs::Joy::ConstPtr& msg) + { + // Cartesian servoing with the axes + geometry_msgs::TwistStamped twist; + twist.header.stamp = ros::Time::now(); + twist.twist.linear.x = msg->axes[0]; + twist.twist.linear.y = msg->axes[1]; + twist.twist.linear.z = msg->axes[2]; + + twist.twist.angular.x = msg->axes[3]; + twist.twist.angular.y = msg->axes[4]; + twist.twist.angular.z = msg->axes[5]; + + // Joint servoing with the buttons + control_msgs::JointJog joint_deltas; + // This example is for a UR5. + joint_deltas.joint_names.push_back("shoulder_pan_joint"); + + // Button 0: positive on the wrist joint + // Button 1: negative on the wrist joint + joint_deltas.velocities.push_back(msg->buttons[0] - msg->buttons[1]); + joint_deltas.header.stamp = ros::Time::now(); + + twist_pub_.publish(twist); + joint_delta_pub_.publish(joint_deltas); + } + + ros::NodeHandle n_; + ros::Subscriber joy_sub_; + ros::Publisher twist_pub_, joint_delta_pub_; + ros::AsyncSpinner spinner_; +}; +} // namespace moveit_servo + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "spacenav_to_twist"); + + moveit_servo::SpaceNavToTwist to_twist; + + return 0; +} \ No newline at end of file diff --git a/moveit_ros/moveit_servo/test/basic_servo_tests.cpp b/moveit_ros/moveit_servo/test/basic_servo_tests.cpp new file mode 100644 index 0000000000..b73e59c5c6 --- /dev/null +++ b/moveit_ros/moveit_servo/test/basic_servo_tests.cpp @@ -0,0 +1,255 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Tyler Weaver, Andy Zelenak + Desc: Basic functionality tests +*/ + +// System +#include + +// ROS +#include + +// Testing +#include + +// Servo +#include +#include + +static const std::string LOGNAME = "basic_servo_tests"; + +namespace moveit_servo +{ +class ServoFixture : public ::testing::Test +{ +public: + void SetUp() override + { + // Wait for several key topics / parameters + ros::topic::waitForMessage("/joint_states"); + while (!nh_.hasParam("/robot_description") && ros::ok()) + { + ros::Duration(0.1).sleep(); + } + + // Load the planning scene monitor + planning_scene_monitor_ = std::make_shared("robot_description"); + planning_scene_monitor_->startSceneMonitor(); + planning_scene_monitor_->startStateMonitor(); + planning_scene_monitor_->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + + // Create moveit_servo + servo_ = std::make_shared(nh_, planning_scene_monitor_); + } + void TearDown() override + { + } + +protected: + void enforceVelLimits(Eigen::ArrayXd& delta_theta) + { + servo_->servo_calcs_->enforceVelLimits(delta_theta); + } + ros::NodeHandle nh_{ "~" }; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + moveit_servo::ServoPtr servo_; +}; // class ServoFixture + +TEST_F(ServoFixture, SendTwistStampedTest) +{ + servo_->start(); + + auto parameters = servo_->getParameters(); + + // count trajectory messages sent by servo + size_t received_count = 0; + boost::function traj_callback = + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& /*msg*/) { ++received_count; }; + auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); + + // Create publisher to send servo commands + auto twist_stamped_pub = nh_.advertise(parameters.cartesian_command_in_topic, 1); + + constexpr double test_duration = 1.0; + const double publish_period = parameters.publish_period; + const size_t num_commands = static_cast(test_duration / publish_period); + + // Set the rate differently from the publish period from the parameters to show that + // the number of outputs is set by the number of commands sent and not the rate they are sent. + ros::Rate publish_rate(2. / publish_period); + + // Send a few Cartesian velocity commands + for (size_t i = 0; i < num_commands && ros::ok(); ++i) + { + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "panda_link0"; + msg->twist.angular.y = 1.0; + + // Send the message + twist_stamped_pub.publish(msg); + publish_rate.sleep(); + } + + EXPECT_GT(received_count, num_commands - 20); + EXPECT_GT(received_count, (unsigned)0); + EXPECT_LT(received_count, num_commands + 20); + servo_->setPaused(true); +} + +TEST_F(ServoFixture, SendJointServoTest) +{ + servo_->start(); + + auto parameters = servo_->getParameters(); + + // count trajectory messages sent by servo + size_t received_count = 0; + boost::function traj_callback = + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& /*msg*/) { ++received_count; }; + auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); + + // Create publisher to send servo commands + auto joint_servo_pub = nh_.advertise(parameters.joint_command_in_topic, 1); + + constexpr double test_duration = 1.0; + const double publish_period = parameters.publish_period; + const size_t num_commands = static_cast(test_duration / publish_period); + + // Set the rate differently from the publish period from the parameters to show that + // the number of outputs is set by the number of commands sent and not the rate they are sent. + ros::Rate publish_rate(2. / publish_period); + + // Send a few joint velocity commands + for (size_t i = 0; i < num_commands && ros::ok(); ++i) + { + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "panda_link3"; + msg->velocities.push_back(0.1); + + // Send the message + joint_servo_pub.publish(msg); + publish_rate.sleep(); + } + + EXPECT_GT(received_count, num_commands - 20); + EXPECT_GT(received_count, (unsigned)0); + EXPECT_LT(received_count, num_commands + 20); + servo_->setPaused(true); +} + +// This a friend test of a private member function +TEST_F(ServoFixture, EnforceVelLimitsTest) +{ + auto parameters = servo_->getParameters(); + const double publish_period = parameters.publish_period; + + // Request joint angle changes that are too fast, given the control period in servo settings YAML file. + Eigen::ArrayXd delta_theta(7); + delta_theta[0] = 0; // rad + delta_theta[1] = 0.01; + delta_theta[2] = 0.02; + delta_theta[3] = 0.03; + delta_theta[4] = 0.04; + delta_theta[5] = 0.05; + delta_theta[6] = 0.06; + + // Store the original joint commands for comparison before applying velocity scaling. + Eigen::ArrayXd orig_delta_theta = delta_theta; + enforceVelLimits(delta_theta); + + // From Panda arm MoveIt joint_limits.yaml. The largest velocity limits for a joint. + const double panda_max_joint_vel = 2.610; // rad/s + const double velocity_scaling_factor = panda_max_joint_vel / (orig_delta_theta.maxCoeff() / publish_period); + const double tolerance = 5e-3; + for (int i = 0; i < 7; ++i) + { + EXPECT_NEAR(orig_delta_theta(i) * velocity_scaling_factor, delta_theta(i), tolerance); + } + + // Now, negative joint angle deltas. Some will result to velocities + // greater than the arm joint velocity limits. + delta_theta[0] = 0; // rad + delta_theta[1] = -0.01; + delta_theta[2] = -0.02; + delta_theta[3] = -0.03; + delta_theta[4] = -0.04; + delta_theta[5] = -0.05; + delta_theta[6] = -0.06; + + // Store the original joint commands for comparison before applying velocity scaling. + orig_delta_theta = delta_theta; + enforceVelLimits(delta_theta); + for (int i = 0; i < 7; ++i) + { + EXPECT_NEAR(orig_delta_theta(i) * velocity_scaling_factor, delta_theta(i), tolerance); + } + + // Final test with joint angle deltas that will result in velocities + // below the lowest Panda arm joint velocity limit. + delta_theta[0] = 0; // rad + delta_theta[1] = -0.013; + delta_theta[2] = 0.023; + delta_theta[3] = -0.004; + delta_theta[4] = 0.021; + delta_theta[5] = 0.012; + delta_theta[6] = 0.0075; + + // Store the original joint commands for comparison before applying velocity scaling. + orig_delta_theta = delta_theta; + enforceVelLimits(delta_theta); + for (int i = 0; i < 7; ++i) + { + EXPECT_NEAR(orig_delta_theta(i), delta_theta(i), tolerance); + } +} +} // namespace moveit_servo + +int main(int argc, char** argv) +{ + ros::init(argc, argv, LOGNAME); + testing::InitGoogleTest(&argc, argv); + + ros::AsyncSpinner spinner(8); + spinner.start(); + + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/moveit_ros/moveit_servo/test/basic_servo_tests.test b/moveit_ros/moveit_servo/test/basic_servo_tests.test new file mode 100644 index 0000000000..930d0d0e41 --- /dev/null +++ b/moveit_ros/moveit_servo/test/basic_servo_tests.test @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/moveit_experimental/jog_arm/test/arm_setup/config/initial_position.yaml b/moveit_ros/moveit_servo/test/config/initial_position.yaml similarity index 71% rename from moveit_experimental/jog_arm/test/arm_setup/config/initial_position.yaml rename to moveit_ros/moveit_servo/test/config/initial_position.yaml index a296398243..7a86cc0aa7 100644 --- a/moveit_experimental/jog_arm/test/arm_setup/config/initial_position.yaml +++ b/moveit_ros/moveit_servo/test/config/initial_position.yaml @@ -1,3 +1,4 @@ +# This is a good initial position, not near joint limits or singularity zeros: panda_joint1: 0 panda_joint2: -0.785 diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml new file mode 100644 index 0000000000..b081f28c73 --- /dev/null +++ b/moveit_ros/moveit_servo/test/config/servo_settings.yaml @@ -0,0 +1,68 @@ +############################################### +# Modify all parameters related to servoing here +############################################### +use_gazebo: false # Whether the robot is started in a Gazebo simulation environment + +## Properties of incoming commands +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.003 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.006 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 0.01 +low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less. + +## Properties of outgoing commands +low_latency_mode: false # Set this to true to tie the output rate to the input rate +publish_period: 0.01 # 1/Nominal publish rate [seconds] + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) +# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) +command_out_type: trajectory_msgs/JointTrajectory + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: false +publish_joint_accelerations: false + +## MoveIt properties +move_group_name: panda_arm # Often 'manipulator' or 'arm' +planning_frame: panda_link0 # The MoveIt planning frame. Often 'panda_link0' or 'world' + +## Other frames +ee_frame_name: panda_link7 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector + +## Stopping behaviour +incoming_command_timeout: 1 # Stop servoing if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 1 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 30 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 45 # Stop when the condition number hits this +joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + +## Topic names +cartesian_command_in_topic: servo_server/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: servo_server/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: joint_states +status_topic: servo_server/status # Publish status to this topic +command_out_topic: servo_server/command # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: stop_distance +# Parameters for "threshold_distance"-type collision checking +self_collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m] +scene_collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] diff --git a/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml b/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml new file mode 100644 index 0000000000..b470e02ac2 --- /dev/null +++ b/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml @@ -0,0 +1,68 @@ +############################################### +# Modify all parameters related to servoing here +############################################### +use_gazebo: false # Whether the robot is started in a Gazebo simulation environment + +## Properties of incoming commands +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.003 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.006 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 0.01 +low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less. + +## Properties of outgoing commands +low_latency_mode: true # Set this to true to tie the output rate to the input rate +publish_period: 0.01 # 1/Nominal publish rate [seconds] + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) +# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) +command_out_type: trajectory_msgs/JointTrajectory + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: false +publish_joint_accelerations: false + +## MoveIt properties +move_group_name: panda_arm # Often 'manipulator' or 'arm' +planning_frame: panda_link0 # The MoveIt planning frame. Often 'panda_link0' or 'world' + +## Other frames +ee_frame_name: panda_link7 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector + +## Stopping behaviour +incoming_command_timeout: 1 # Stop servoing if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 1 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 30 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 45 # Stop when the condition number hits this +joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + +## Topic names +cartesian_command_in_topic: servo_server/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: servo_server/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: joint_states +status_topic: servo_server/status # Publish status to this topic +command_out_topic: servo_server/command # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: stop_distance +# Parameters for "threshold_distance"-type collision checking +self_collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m] +scene_collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] diff --git a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp new file mode 100644 index 0000000000..5dab02713f --- /dev/null +++ b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp @@ -0,0 +1,166 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Andy Zelenak + Desc: Test of tracking toward a pose +*/ + +// C++ +#include +#include + +// ROS +#include + +// Testing +#include + +// Servo +#include +#include + +static const std::string LOGNAME = "servo_cpp_interface_test"; +static constexpr double TRANSLATION_TOLERANCE = 0.01; // meters +static constexpr double ROTATION_TOLERANCE = 0.1; // quaternion +static constexpr double ROS_PUB_SUB_DELAY = 4; // allow for subscribers to initialize + +namespace moveit_servo +{ +class PoseTrackingFixture : public ::testing::Test +{ +public: + void SetUp() override + { + // Wait for several key topics / parameters + ros::topic::waitForMessage("/joint_states"); + while (!nh_.hasParam("/robot_description") && ros::ok()) + { + ros::Duration(0.1).sleep(); + } + + // Load the planning scene monitor + planning_scene_monitor_ = std::make_shared("robot_description"); + planning_scene_monitor_->startSceneMonitor(); + planning_scene_monitor_->startStateMonitor(); + planning_scene_monitor_->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + + tracker_ = std::make_shared(nh_, planning_scene_monitor_); + + target_pose_pub_ = nh_.advertise("target_pose", 1 /* queue */, true /* latch */); + + // Tolerance for pose seeking + translation_tolerance_ << TRANSLATION_TOLERANCE, TRANSLATION_TOLERANCE, TRANSLATION_TOLERANCE; + } + void TearDown() override + { + } + +protected: + ros::NodeHandle nh_{ "~" }; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + Eigen::Vector3d translation_tolerance_; + moveit_servo::PoseTrackingPtr tracker_; + ros::Publisher target_pose_pub_; +}; // class PoseTrackingFixture + +// Check for commands going out to ros_control +TEST_F(PoseTrackingFixture, OutgoingMsgTest) +{ + // halt Servoing when first msg to ros_control is received + // and test some conditions + trajectory_msgs::JointTrajectory last_received_msg; + boost::function traj_callback = + [&/* this */](const trajectory_msgs::JointTrajectoryConstPtr& msg) { + EXPECT_EQ(msg->header.frame_id, "panda_link0"); + // Check for an expected joint position command + // As of now, the robot doesn't actually move because there are no controllers enabled. + double angle_tolerance = 0.08; // rad + EXPECT_NEAR(msg->points[0].positions[0], 0, angle_tolerance); + EXPECT_NEAR(msg->points[0].positions[1], -0.785, angle_tolerance); + EXPECT_NEAR(msg->points[0].positions[2], 0, angle_tolerance); + EXPECT_NEAR(msg->points[0].positions[3], -2.360, angle_tolerance); + EXPECT_NEAR(msg->points[0].positions[4], 0, angle_tolerance); + EXPECT_NEAR(msg->points[0].positions[5], 1.571, angle_tolerance); + EXPECT_NEAR(msg->points[0].positions[6], 0.785, angle_tolerance); + + this->tracker_->stopMotion(); + return; + }; + auto traj_sub = nh_.subscribe("servo_server/command", 1, traj_callback); + + geometry_msgs::PoseStamped target_pose; + target_pose.header.frame_id = "panda_link4"; + target_pose.header.stamp = ros::Time::now(); + target_pose.pose.position.x = 0.2; + target_pose.pose.position.y = 0.2; + target_pose.pose.position.z = 0.2; + target_pose.pose.orientation.w = 1; + + // Republish the target pose in a new thread, as if the target is moving + std::thread target_pub_thread([&] { + size_t msg_count = 0; + while (++msg_count < 100) + { + target_pose_pub_.publish(target_pose); + ros::Duration(0.01).sleep(); + } + }); + + ros::Duration(ROS_PUB_SUB_DELAY).sleep(); + + // resetTargetPose() can be used to clear the target pose and wait for a new one, e.g. when moving between multiple + // waypoints + tracker_->resetTargetPose(); + + tracker_->moveToPose(translation_tolerance_, ROTATION_TOLERANCE, 1 /* target pose timeout */); + + target_pub_thread.join(); +} + +} // namespace moveit_servo + +int main(int argc, char** argv) +{ + ros::init(argc, argv, LOGNAME); + testing::InitGoogleTest(&argc, argv); + + ros::AsyncSpinner spinner(8); + spinner.start(); + + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/moveit_ros/moveit_servo/test/pose_tracking_test.test b/moveit_ros/moveit_servo/test/pose_tracking_test.test new file mode 100644 index 0000000000..f149e48c32 --- /dev/null +++ b/moveit_ros/moveit_servo/test/pose_tracking_test.test @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp new file mode 100644 index 0000000000..fa61c32e0a --- /dev/null +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp @@ -0,0 +1,269 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman and Tyler Weaver + Desc: Test for the C++ interface to moveit_servo +*/ + +// C++ +#include + +// ROS +#include + +// Testing +#include + +// Servo +#include +#include + +static const std::string LOGNAME = "servo_cpp_interface_test"; +static constexpr double LARGEST_ALLOWABLE_PANDA_VEL = 2.8710; // to test joint velocity limit enforcement + +namespace moveit_servo +{ +class ServoFixture : public ::testing::Test +{ +public: + void SetUp() override + { + // Wait for several key topics / parameters + ros::topic::waitForMessage("/joint_states"); + while (!nh_.hasParam("/robot_description") && ros::ok()) + { + ros::Duration(0.1).sleep(); + } + + // Load the planning scene monitor + planning_scene_monitor_ = std::make_shared("robot_description"); + planning_scene_monitor_->startSceneMonitor(); + planning_scene_monitor_->startStateMonitor(); + planning_scene_monitor_->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + + // Create moveit_servo + servo_ = std::make_shared(nh_, planning_scene_monitor_); + } + void TearDown() override + { + } + + bool waitForFirstStatus() + { + auto msg = ros::topic::waitForMessage(servo_->getParameters().status_topic, nh_, ros::Duration(1)); + return static_cast(msg); + } + +protected: + ros::NodeHandle nh_{ "~" }; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + moveit_servo::ServoPtr servo_; +}; // class ServoFixture + +TEST_F(ServoFixture, StartStopTest) +{ + servo_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + servo_->setPaused(true); + + ros::Duration(1.0).sleep(); + + // Start and stop again + servo_->setPaused(false); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + servo_->setPaused(true); +} + +TEST_F(ServoFixture, SendTwistStampedTest) +{ + servo_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + + auto parameters = servo_->getParameters(); + + // count trajectory messages sent by servo + size_t received_count = 0; + boost::function traj_callback = + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& /*msg*/) { ++received_count; }; + auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); + + // Create publisher to send servo commands + auto twist_stamped_pub = nh_.advertise(parameters.cartesian_command_in_topic, 1); + + constexpr double test_duration = 1.0; + const double publish_period = parameters.publish_period; + const size_t num_commands = static_cast(test_duration / publish_period); + + ros::Rate publish_rate(1. / publish_period); + + // Send a few Cartesian velocity commands + for (size_t i = 0; i < num_commands && ros::ok(); ++i) + { + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "panda_link0"; + msg->twist.angular.y = 1.0; + + // Send the message + twist_stamped_pub.publish(msg); + publish_rate.sleep(); + } + + EXPECT_GT(received_count, num_commands - 20); + EXPECT_GT(received_count, (unsigned)0); + EXPECT_LT(received_count, num_commands + 20); + servo_->setPaused(true); +} + +TEST_F(ServoFixture, SendJointServoTest) +{ + servo_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + + auto parameters = servo_->getParameters(); + + // count trajectory messages sent by servo + size_t received_count = 0; + boost::function traj_callback = + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& /*msg*/) { ++received_count; }; + auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); + + // Create publisher to send servo commands + auto joint_servo_pub = nh_.advertise(parameters.joint_command_in_topic, 1); + + constexpr double test_duration = 1.0; + const double publish_period = parameters.publish_period; + const size_t num_commands = static_cast(test_duration / publish_period); + + ros::Rate publish_rate(1. / publish_period); + + // Send a few joint velocity commands + for (size_t i = 0; i < num_commands && ros::ok(); ++i) + { + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "panda_link3"; + msg->velocities.push_back(0.1); + + // Send the message + joint_servo_pub.publish(msg); + publish_rate.sleep(); + } + + EXPECT_GT(received_count, num_commands - 20); + EXPECT_LT(received_count, num_commands + 20); + servo_->setPaused(true); +} + +TEST_F(ServoFixture, JointVelocityEnforcementTest) +{ + servo_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + + auto parameters = servo_->getParameters(); + double publish_period = parameters.publish_period; + + // count trajectory messages sent by servo + size_t received_count = 0; + trajectory_msgs::JointTrajectory joint_command_from_servo; + trajectory_msgs::JointTrajectory prev_joint_command_from_servo; + boost::function traj_callback = + [&](const trajectory_msgs::JointTrajectoryConstPtr& msg) { + ++received_count; + // Store a series of two commands so we can calculate velocities + // from positions + prev_joint_command_from_servo = joint_command_from_servo; + joint_command_from_servo = *msg; + + // Start running checks when we have at least two datapoints + if (received_count > 1) + { + // Need a sequence of two commands to calculate a velocity + EXPECT_GT(joint_command_from_servo.points.size(), (unsigned)0); + EXPECT_GT(prev_joint_command_from_servo.points.size(), (unsigned)0); + EXPECT_EQ(prev_joint_command_from_servo.points.size(), joint_command_from_servo.points.size()); + // No velocities larger than the largest allowable Panda velocity + for (size_t joint_index = 0; joint_index < joint_command_from_servo.points[0].positions.size(); ++joint_index) + { + double joint_velocity = (joint_command_from_servo.points[0].positions[joint_index] - + prev_joint_command_from_servo.points[0].positions[joint_index]) / + publish_period; + EXPECT_LE(joint_velocity, LARGEST_ALLOWABLE_PANDA_VEL); + } + } + }; + auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); + + // Create publisher to send servo commands + auto twist_stamped_pub = nh_.advertise(parameters.cartesian_command_in_topic, 1); + + constexpr double test_duration = 1.0; + const size_t num_commands = static_cast(test_duration / publish_period); + + ros::Rate publish_rate(1. / publish_period); + + // Send a few Cartesian commands with very high velocity + for (size_t i = 0; i < num_commands && ros::ok(); ++i) + { + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "panda_link0"; + msg->twist.linear.x = 10.0; + msg->twist.angular.y = 5 * LARGEST_ALLOWABLE_PANDA_VEL; + + // Send the message + twist_stamped_pub.publish(msg); + publish_rate.sleep(); + } + + EXPECT_GT(received_count, num_commands - 20); + EXPECT_LT(received_count, num_commands + 20); + servo_->setPaused(true); +} +} // namespace moveit_servo + +int main(int argc, char** argv) +{ + ros::init(argc, argv, LOGNAME); + testing::InitGoogleTest(&argc, argv); + + ros::AsyncSpinner spinner(8); + spinner.start(); + + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test new file mode 100644 index 0000000000..94c6a2a7f4 --- /dev/null +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst new file mode 100644 index 0000000000..6307a04d83 --- /dev/null +++ b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst @@ -0,0 +1,40 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_ros_occupancy_map_monitor +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.1 (2020-10-13) +------------------ +* [fix] clang-tidy issues on Travis (`#2337 `_) + * Replace typedefs with using declarations + * Move default destructor definitions to headers + * Silent spurious clang-tidy warning + * Move variable definitions to their usage location +* [maint] Cleanup MSA includes (`#2351 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, Robert Haschke + +1.1.0 (2020-09-04) +------------------ + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* Contributors: Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [fix] Add error message on failure to initialize occupancy map monitor (`#1873 `_) +* [fix] Update occupancy grid when loaded from file (`#1594 `_) +* [maint] Apply clang-tidy fix (`#1394 `_) +* [maint] Windows build fixes + * Fix header inclusion and other MSVC build errors (`#1636 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [maint] move occupancy_map_monitor into its own package (`#1533 `_) +* Contributors: Bjar Ne, Dale Koenig, Raphael Druon, Robert Haschke, Sean Yen, Simon Schmeisser, Yu, Yan, jschleicher diff --git a/moveit_ros/occupancy_map_monitor/CMakeLists.txt b/moveit_ros/occupancy_map_monitor/CMakeLists.txt index 218148cca1..c62d9c5551 100644 --- a/moveit_ros/occupancy_map_monitor/CMakeLists.txt +++ b/moveit_ros/occupancy_map_monitor/CMakeLists.txt @@ -1,8 +1,12 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_occupancy_map_monitor) set(MOVEIT_LIB_NAME ${PROJECT_NAME}) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -17,7 +21,9 @@ endif(APPLE) find_package(catkin REQUIRED COMPONENTS moveit_core moveit_msgs + geometric_shapes pluginlib + tf2_ros ) find_package(Eigen3 REQUIRED) @@ -31,16 +37,17 @@ catkin_package( CATKIN_DEPENDS moveit_core moveit_msgs + geometric_shapes + tf2_ros DEPENDS EIGEN3 OCTOMAP ) -include_directories(include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ) +include_directories(include) include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${X11_INCLUDE_DIR} ) @@ -55,8 +62,13 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_executable(moveit_ros_occupancy_map_server src/occupancy_map_server.cpp) target_link_libraries(moveit_ros_occupancy_map_server ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} moveit_ros_occupancy_map_server - LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION} +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(TARGETS moveit_ros_occupancy_map_server RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h index e1cff913dd..35a08b69d0 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h @@ -34,10 +34,10 @@ /* Author: Ioan Sucan, Jon Binney */ -#ifndef MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_ -#define MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_ +#pragma once #include +#include #include #include #include @@ -83,8 +83,8 @@ class OccMapTree : public octomap::OcTree tree_mutex_.unlock(); } - typedef boost::shared_lock ReadLock; - typedef boost::unique_lock WriteLock; + using ReadLock = boost::shared_lock; + using WriteLock = boost::unique_lock; ReadLock reading() { @@ -96,7 +96,7 @@ class OccMapTree : public octomap::OcTree return WriteLock(tree_mutex_); } - void triggerUpdateCallback(void) + void triggerUpdateCallback() { if (update_callback_) update_callback_(); @@ -113,8 +113,6 @@ class OccMapTree : public octomap::OcTree boost::function update_callback_; }; -typedef std::shared_ptr OccMapTreePtr; -typedef std::shared_ptr OccMapTreeConstPtr; -} - -#endif +using OccMapTreePtr = std::shared_ptr; +using OccMapTreeConstPtr = std::shared_ptr; +} // namespace occupancy_map_monitor diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h index 1558c434ae..32da5da893 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Jon Binney */ -#ifndef MOVEIT_PERCEPTION_OCCUPANCY_MAP_MONITOR_ -#define MOVEIT_PERCEPTION_OCCUPANCY_MAP_MONITOR_ +#pragma once #include #include @@ -159,6 +158,4 @@ class OccupancyMapMonitor bool active_; }; -} - -#endif +} // namespace occupancy_map_monitor diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h index cd91709567..408a6f7c32 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Jon Binney */ -#ifndef MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_UPDATER_ -#define MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_UPDATER_ +#pragma once #include #include @@ -45,16 +44,14 @@ namespace occupancy_map_monitor { -typedef unsigned int ShapeHandle; -typedef std::map, - Eigen::aligned_allocator > > - ShapeTransformCache; -typedef boost::function - TransformCacheProvider; +using ShapeHandle = unsigned int; +using ShapeTransformCache = std::map, + Eigen::aligned_allocator > >; +using TransformCacheProvider = boost::function; class OccupancyMapMonitor; -MOVEIT_CLASS_FORWARD(OccupancyMapUpdater); +MOVEIT_CLASS_FORWARD(OccupancyMapUpdater); // Defines OccupancyMapUpdaterPtr, ConstPtr, WeakPtr... etc /** \brief Base class for classes which update the occupancy map. */ @@ -111,6 +108,4 @@ class OccupancyMapUpdater static void readXmlParam(XmlRpc::XmlRpcValue& params, const std::string& param_name, double* value); static void readXmlParam(XmlRpc::XmlRpcValue& params, const std::string& param_name, unsigned int* value); }; -} - -#endif +} // namespace occupancy_map_monitor diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml index 8c1e95689d..595f0cbba6 100644 --- a/moveit_ros/occupancy_map_monitor/package.xml +++ b/moveit_ros/occupancy_map_monitor/package.xml @@ -1,15 +1,15 @@ moveit_ros_occupancy_map_monitor - 1.0.1 - Components of MoveIt! connecting to occupancy map + 1.1.1 + Components of MoveIt connecting to occupancy map Ioan Sucan Jon Binney Suat Gedikli Michael Ferguson - MoveIt! Release Team + MoveIt Release Team BSD @@ -23,6 +23,8 @@ moveit_msgs octomap pluginlib + tf2_ros + geometric_shapes eigen diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index b2200d936c..79ec097a8a 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -43,6 +43,8 @@ namespace occupancy_map_monitor { +static const std::string LOGNAME = "occupancy_map_monitor"; + OccupancyMapMonitor::OccupancyMapMonitor(double map_resolution) : map_resolution_(map_resolution), debug_info_(false), mesh_handle_count_(0), nh_("~"), active_(false) { @@ -77,20 +79,25 @@ void OccupancyMapMonitor::initialize() { /* load params from param server */ if (map_resolution_ <= std::numeric_limits::epsilon()) + { if (!nh_.getParam("octomap_resolution", map_resolution_)) { map_resolution_ = 0.1; - ROS_WARN("Resolution not specified for Octomap. Assuming resolution = %g instead", map_resolution_); + ROS_WARN_NAMED(LOGNAME, "Resolution not specified for Octomap. Assuming resolution = %g instead", map_resolution_); } - ROS_DEBUG("Using resolution = %lf m for building octomap", map_resolution_); + } + ROS_DEBUG_NAMED(LOGNAME, "Using resolution = %lf m for building octomap", map_resolution_); if (map_frame_.empty()) if (!nh_.getParam("octomap_frame", map_frame_)) if (tf_buffer_) - ROS_WARN("No target frame specified for Octomap. No transforms will be applied to received data."); + ROS_WARN_NAMED(LOGNAME, "No target frame specified for Octomap. " + "No transforms will be applied to received data."); if (!tf_buffer_ && !map_frame_.empty()) - ROS_WARN("Target frame specified but no TF instance specified. No transforms will be applied to received data."); + ROS_WARN_STREAM_NAMED(LOGNAME, "Target frame \"" << map_frame_ + << "\" specified but no TF instance (buffer) specified. " + "No transforms will be applied to received data."); tree_.reset(new OccMapTree(map_resolution_)); tree_const_ = tree_; @@ -105,20 +112,20 @@ void OccupancyMapMonitor::initialize() { if (sensor_list[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("Params for octomap updater %d not a struct; ignoring.", i); + ROS_ERROR_NAMED(LOGNAME, "Params for octomap updater %d not a struct; ignoring.", i); continue; } if (!sensor_list[i].hasMember("sensor_plugin")) { - ROS_ERROR("No sensor plugin specified for octomap updater %d; ignoring.", i); + ROS_ERROR_NAMED(LOGNAME, "No sensor plugin specified for octomap updater %d; ignoring.", i); continue; } std::string sensor_plugin = std::string(sensor_list[i]["sensor_plugin"]); if (sensor_plugin.empty() || sensor_plugin[0] == '~') { - ROS_INFO("Skipping octomap updater plugin '%s'", sensor_plugin.c_str()); + ROS_INFO_STREAM_NAMED(LOGNAME, "Skipping octomap updater plugin '" << sensor_plugin << "'"); continue; } @@ -131,7 +138,7 @@ void OccupancyMapMonitor::initialize() } catch (pluginlib::PluginlibException& ex) { - ROS_FATAL_STREAM("Exception while creating octomap updater plugin loader " << ex.what()); + ROS_FATAL_STREAM_NAMED(LOGNAME, "Exception while creating octomap updater plugin loader " << ex.what()); } } @@ -143,22 +150,22 @@ void OccupancyMapMonitor::initialize() } catch (pluginlib::PluginlibException& ex) { - ROS_ERROR_STREAM("Exception while loading octomap updater '" << sensor_plugin << "': " << ex.what() - << std::endl); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Exception while loading octomap updater '" + << sensor_plugin << "': " << ex.what() << std::endl); } if (up) { /* pass the params struct directly in to the updater */ if (!up->setParams(sensor_list[i])) { - ROS_ERROR("Failed to configure updater of type %s", up->getType().c_str()); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to configure updater of type " << up->getType()); continue; } if (!up->initialize()) { - ROS_ERROR("Unable to initialize map updater of type %s (plugin %s)", up->getType().c_str(), - sensor_plugin.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Unable to initialize map updater of type %s (plugin %s)", up->getType().c_str(), + sensor_plugin.c_str()); continue; } @@ -166,13 +173,15 @@ void OccupancyMapMonitor::initialize() } } else - ROS_ERROR("List of sensors must be an array!"); + ROS_ERROR_NAMED(LOGNAME, "List of sensors must be an array!"); } catch (XmlRpc::XmlRpcException& ex) { - ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str()); + ROS_ERROR_STREAM_NAMED(LOGNAME, "XmlRpc Exception: " << ex.getMessage()); } } + else + ROS_ERROR_NAMED(LOGNAME, "Failed to find 3D sensor plugin parameters for octomap generation"); /* advertise a service for loading octomaps from disk */ save_map_srv_ = nh_.advertiseService("save_map", &OccupancyMapMonitor::saveMapCallback, this); @@ -188,8 +197,8 @@ void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater) if (map_updaters_.size() > 1) { mesh_handles_.resize(map_updaters_.size()); - if (map_updaters_.size() == - 2) // when we had one updater only, we passed direcly the transform cache callback to that updater + // when we had one updater only, we passed direcly the transform cache callback to that updater + if (map_updaters_.size() == 2) { map_updaters_[0]->setTransformCacheCallback( boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 0, _1, _2, _3)); @@ -204,7 +213,7 @@ void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater) updater->setTransformCacheCallback(transform_cache_callback_); } else - ROS_ERROR("NULL updater was specified"); + ROS_ERROR_NAMED(LOGNAME, "NULL updater was specified"); } void OccupancyMapMonitor::publishDebugInformation(bool flag) @@ -280,7 +289,7 @@ bool OccupancyMapMonitor::getShapeTransformCache(std::size_t index, const std::s std::map::const_iterator jt = mesh_handles_[index].find(it.first); if (jt == mesh_handles_[index].end()) { - ROS_ERROR_THROTTLE(1, "Incorrect mapping of mesh handles"); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Incorrect mapping of mesh handles"); return false; } else @@ -298,7 +307,7 @@ bool OccupancyMapMonitor::getShapeTransformCache(std::size_t index, const std::s bool OccupancyMapMonitor::saveMapCallback(moveit_msgs::SaveMap::Request& request, moveit_msgs::SaveMap::Response& response) { - ROS_INFO("Writing map to %s", request.filename.c_str()); + ROS_INFO_STREAM_NAMED(LOGNAME, "Writing map to " << request.filename); tree_->lockRead(); try { @@ -315,7 +324,7 @@ bool OccupancyMapMonitor::saveMapCallback(moveit_msgs::SaveMap::Request& request bool OccupancyMapMonitor::loadMapCallback(moveit_msgs::LoadMap::Request& request, moveit_msgs::LoadMap::Response& response) { - ROS_INFO("Reading map from %s", request.filename.c_str()); + ROS_INFO_STREAM_NAMED(LOGNAME, "Reading map from " << request.filename); /* load the octree from disk */ tree_->lockWrite(); @@ -325,11 +334,14 @@ bool OccupancyMapMonitor::loadMapCallback(moveit_msgs::LoadMap::Request& request } catch (...) { - ROS_ERROR("Failed to load map from file"); + ROS_ERROR_NAMED(LOGNAME, "Failed to load map from file"); response.success = false; } tree_->unlockWrite(); + if (response.success) + tree_->triggerUpdateCallback(); + return true; } diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp index 4453583fc1..b91b591d07 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp @@ -40,6 +40,8 @@ #include #include +static const std::string LOGNAME = "occupancy_map_server"; + static void publishOctomap(ros::Publisher* octree_binary_pub, occupancy_map_monitor::OccupancyMapMonitor* server) { octomap_msgs::Octomap map; @@ -51,11 +53,11 @@ static void publishOctomap(ros::Publisher* octree_binary_pub, occupancy_map_moni try { if (!octomap_msgs::binaryMapToMsgData(*server->getOcTreePtr(), map.data)) - ROS_ERROR_THROTTLE(1, "Could not generate OctoMap message"); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Could not generate OctoMap message"); } catch (...) { - ROS_ERROR_THROTTLE(1, "Exception thrown while generating OctoMap message"); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Exception thrown while generating OctoMap message"); } server->getOcTreePtr()->unlockRead(); diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp index bc7a78e095..0e44f7d587 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp @@ -39,6 +39,8 @@ namespace occupancy_map_monitor { +static const std::string LOGNAME = "occupancy_map_monitor"; + OccupancyMapUpdater::OccupancyMapUpdater(const std::string& type) : type_(type) { } @@ -75,7 +77,7 @@ bool OccupancyMapUpdater::updateTransformCache(const std::string& target_frame, return transform_provider_callback_(target_frame, target_time, transform_cache_); else { - ROS_WARN_THROTTLE(1, "No callback provided for updating the transform cache for octomap updaters"); + ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "No callback provided for updating the transform cache for octomap updaters"); return false; } } diff --git a/moveit_ros/perception/CHANGELOG.rst b/moveit_ros/perception/CHANGELOG.rst index 9df15123be..9f6dddd4f8 100644 --- a/moveit_ros/perception/CHANGELOG.rst +++ b/moveit_ros/perception/CHANGELOG.rst @@ -2,6 +2,65 @@ Changelog for package moveit_ros_perception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [fix] various issues with Noetic build (`#2327 `_) +* [maint] Cleanup MSA includes (`#2351 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, G.A. vd. Hoorn, Robert Haschke + +1.1.0 (2020-09-04) +------------------ +* [feature] Utilize new geometric_shapes functions to improve performance (`#2038 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix getTransform() (`#2113 `_) +* [fix] depth_image_octomap_updater: correctly set properties of debug images (`#1653 `_) +* [maint] Optional cpp version setting (`#2166 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#2004 `_, `#1419 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] NAMED logging for moveit_ros_perception (`#1897 `_) +* [maint] Fix various build issues on Windows (`#1880 `_) +* [maint] Fix compiler warnings (`#1773 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Bjar Ne, Dale Koenig, Dave Coleman, Henning Kayser, Jonathan Binney, Mahmoud Ahmed Selim, Markus Vieth, Martin Pecka, Matthias Nieuwenhuisen, Michael GΓΆrner, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan, jschleicher + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* [maint] Further increase acceptance threshold for mesh-filter test +* [maint] Prefer vendor-specific OpenGL library +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ +* [maint] Fix mesh_filter test (`#2044 `_) +* Contributors: Bjar Ne + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build fixes + * Fix header inclusion and other MSVC build errors (`#1636 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [maint] Allow subclassing of point_containment_filter::ShapeMask. (`#1457 `_) +* [fix] `depth_image_octomap_updater`: reset depth transfer function to standard values (`#1661 `_) +* [fix] `depth_image_octomap_updater`: correctly set properties of debug images (`#1652 `_) +* [maint] Move `occupancy_map_monitor` into its own package (`#1533 `_) +* Contributors: Martin Pecka, Matthias Nieuwenhuisen, Robert Haschke, Sean Yen, Yu, Yan, jschleicher + +1.0.2 (2019-06-28) +------------------ +* [maintenance] Removed unnecessary null pointer checks on deletion (`#1410 `_) +* Contributors: Mahmoud Ahmed Selim + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -79,7 +138,7 @@ Changelog for package moveit_ros_perception 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_ros/perception/CMakeLists.txt b/moveit_ros/perception/CMakeLists.txt index 894fa3d28b..73f692e687 100644 --- a/moveit_ros/perception/CMakeLists.txt +++ b/moveit_ros/perception/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_perception) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) option(WITH_OPENGL "Build the parts that depend on OpenGL" ON) @@ -12,6 +16,10 @@ endif() find_package(Boost REQUIRED thread) if(WITH_OPENGL) + # Prefer newer vendor-specific OpenGL library + if (POLICY CMP0072) + cmake_policy(SET CMP0072 NEW) + endif() find_package(OpenGL REQUIRED) find_package(GLEW REQUIRED) find_package(GLUT REQUIRED) @@ -65,6 +73,7 @@ catkin_package( moveit_msgs moveit_ros_occupancy_map_monitor object_recognition_msgs + roscpp sensor_msgs tf2_geometry_msgs DEPENDS @@ -76,12 +85,12 @@ include_directories(lazy_free_space_updater/include pointcloud_octomap_updater/include semantic_world/include ${perception_GL_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ) include_directories(SYSTEM - ${OpenCV_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ${SYSTEM_GL_INCLUDE_DIR} ${X11_INCLUDE_DIR} ) diff --git a/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt b/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt index 81485c633d..4222cd346a 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt +++ b/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt @@ -10,5 +10,8 @@ add_library(${MOVEIT_LIB_NAME} src/updater_plugin.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h index d223e8d4b4..6779854111 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h +++ b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OCCUPANCY_MAP_DEPTH_IMAGE_OCCUPANCY_MAP_UPDATER_ -#define MOVEIT_OCCUPANCY_MAP_DEPTH_IMAGE_OCCUPANCY_MAP_UPDATER_ +#pragma once #include #include @@ -106,6 +105,4 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater std::vector filtered_labels_; ros::WallTime last_depth_callback_start_; }; -} - -#endif +} // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index 4a33d7c40a..4ae40c00a8 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -48,6 +48,8 @@ namespace occupancy_map_monitor { +static const std::string LOGNAME = "depth_image_octomap_updater"; + DepthImageOctomapUpdater::DepthImageOctomapUpdater() : OccupancyMapUpdater("DepthImageUpdater") , nh_("~") @@ -106,7 +108,7 @@ bool DepthImageOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params) } catch (XmlRpc::XmlRpcException& ex) { - ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str()); + ROS_ERROR_STREAM_NAMED(LOGNAME, "XmlRpc Exception: " << ex.getMessage()); return false; } @@ -171,7 +173,7 @@ mesh_filter::MeshHandle DepthImageOctomapUpdater::excludeShape(const shapes::Sha } } else - ROS_ERROR("Mesh filter not yet initialized!"); + ROS_ERROR_NAMED(LOGNAME, "Mesh filter not yet initialized!"); return h; } @@ -186,7 +188,7 @@ bool DepthImageOctomapUpdater::getShapeTransform(mesh_filter::MeshHandle h, Eige ShapeTransformCache::const_iterator it = transform_cache_.find(h); if (it == transform_cache_.end()) { - ROS_ERROR("Internal error. Mesh filter handle %u not found", h); + ROS_ERROR_NAMED(LOGNAME, "Internal error. Mesh filter handle %u not found", h); return false; } transform = it->second; @@ -211,8 +213,8 @@ static const bool HOST_IS_BIG_ENDIAN = host_is_big_endian(); void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::CameraInfoConstPtr& info_msg) { - ROS_DEBUG("Received a new depth image message (frame = '%s', encoding='%s')", depth_msg->header.frame_id.c_str(), - depth_msg->encoding.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "Received a new depth image message (frame = '%s', encoding='%s')", + depth_msg->header.frame_id.c_str(), depth_msg->encoding.c_str()); ros::WallTime start = ros::WallTime::now(); if (max_update_rate_ > 0) @@ -262,9 +264,9 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP for (int t = 0; t < nt; ++t) try { - tf2::fromMsg( - tf_buffer_->lookupTransform(monitor_->getMapFrame(), depth_msg->header.frame_id, depth_msg->header.stamp), - map_h_sensor); + tf2::fromMsg(tf_buffer_->lookupTransform(monitor_->getMapFrame(), depth_msg->header.frame_id, + depth_msg->header.stamp), + map_h_sensor); found = true; break; } @@ -289,11 +291,12 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP { failed_tf_++; if (failed_tf_ > good_tf_) - ROS_WARN_THROTTLE(1, "More than half of the image messages discared due to TF being unavailable (%u%%). " - "Transform error of sensor data: %s; quitting callback.", - (100 * failed_tf_) / (good_tf_ + failed_tf_), err.c_str()); + ROS_WARN_THROTTLE_NAMED(1, LOGNAME, + "More than half of the image messages discared due to TF being unavailable (%u%%). " + "Transform error of sensor data: %s; quitting callback.", + (100 * failed_tf_) / (good_tf_ + failed_tf_), err.c_str()); else - ROS_DEBUG_THROTTLE(1, "Transform error of sensor data: %s; quitting callback", err.c_str()); + ROS_DEBUG_THROTTLE_NAMED(1, LOGNAME, "Transform error of sensor data: %s; quitting callback", err.c_str()); if (failed_tf_ > MAX_TF_COUNTER) { const unsigned int div = MAX_TF_COUNTER / 10; @@ -309,12 +312,12 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP if (!updateTransformCache(depth_msg->header.frame_id, depth_msg->header.stamp)) { - ROS_ERROR_THROTTLE(1, "Transform cache was not updated. Self-filtering may fail."); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform cache was not updated. Self-filtering may fail."); return; } if (depth_msg->is_bigendian && !HOST_IS_BIG_ENDIAN) - ROS_ERROR_THROTTLE(1, "endian problem: received image data does not match host"); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "endian problem: received image data does not match host"); const int w = depth_msg->width; const int h = depth_msg->height; @@ -331,7 +334,8 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP { if (depth_msg->encoding != sensor_msgs::image_encodings::TYPE_32FC1) { - ROS_ERROR_THROTTLE(1, "Unexpected encoding type: '%s'. Ignoring input.", depth_msg->encoding.c_str()); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Unexpected encoding type: '%s'. Ignoring input.", + depth_msg->encoding.c_str()); return; } mesh_filter_->filter(&depth_msg->data[0], GL_FLOAT); @@ -394,33 +398,32 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP { sensor_msgs::Image debug_msg; debug_msg.header = depth_msg->header; - debug_msg.height = depth_msg->height; - debug_msg.width = depth_msg->width; + debug_msg.height = h; + debug_msg.width = w; + debug_msg.is_bigendian = HOST_IS_BIG_ENDIAN; debug_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - debug_msg.is_bigendian = depth_msg->is_bigendian; - debug_msg.step = depth_msg->step; + debug_msg.step = w * sizeof(float); debug_msg.data.resize(img_size * sizeof(float)); mesh_filter_->getModelDepth(reinterpret_cast(&debug_msg.data[0])); pub_model_depth_image_.publish(debug_msg, *info_msg); sensor_msgs::Image filtered_depth_msg; filtered_depth_msg.header = depth_msg->header; - filtered_depth_msg.height = depth_msg->height; - filtered_depth_msg.width = depth_msg->width; + filtered_depth_msg.height = h; + filtered_depth_msg.width = w; + filtered_depth_msg.is_bigendian = HOST_IS_BIG_ENDIAN; filtered_depth_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - filtered_depth_msg.is_bigendian = depth_msg->is_bigendian; - filtered_depth_msg.step = depth_msg->step; + filtered_depth_msg.step = w * sizeof(float); filtered_depth_msg.data.resize(img_size * sizeof(float)); - mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_depth_msg.data[0])); pub_filtered_depth_image_.publish(filtered_depth_msg, *info_msg); sensor_msgs::Image label_msg; label_msg.header = depth_msg->header; - label_msg.height = depth_msg->height; - label_msg.width = depth_msg->width; + label_msg.height = h; + label_msg.width = w; + label_msg.is_bigendian = HOST_IS_BIG_ENDIAN; label_msg.encoding = sensor_msgs::image_encodings::RGBA8; - label_msg.is_bigendian = depth_msg->is_bigendian; label_msg.step = w * sizeof(unsigned int); label_msg.data.resize(img_size * sizeof(unsigned int)); mesh_filter_->getFilteredLabels(reinterpret_cast(&label_msg.data[0])); @@ -430,22 +433,26 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP if (!filtered_cloud_topic_.empty()) { - static std::vector filtered_data; sensor_msgs::Image filtered_msg; filtered_msg.header = depth_msg->header; - filtered_msg.height = depth_msg->height; - filtered_msg.width = depth_msg->width; + filtered_msg.height = h; + filtered_msg.width = w; + filtered_msg.is_bigendian = HOST_IS_BIG_ENDIAN; filtered_msg.encoding = sensor_msgs::image_encodings::TYPE_16UC1; - filtered_msg.is_bigendian = depth_msg->is_bigendian; - filtered_msg.step = depth_msg->step; + filtered_msg.step = w * sizeof(unsigned short); filtered_msg.data.resize(img_size * sizeof(unsigned short)); + + // reuse float buffer across callbacks + static std::vector filtered_data; if (filtered_data.size() < img_size) filtered_data.resize(img_size); + mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_data[0])); - unsigned short* tmp_ptr = (unsigned short*)&filtered_msg.data[0]; + unsigned short* msg_data = reinterpret_cast(&filtered_msg.data[0]); for (std::size_t i = 0; i < img_size; ++i) { - tmp_ptr[i] = (unsigned short)(filtered_data[i] * 1000 + 0.5); + // rescale depth to millimeter to work with `unsigned short` + msg_data[i] = static_cast(filtered_data[i] * 1000 + 0.5); } pub_filtered_depth_image_.publish(filtered_msg, *info_msg); } @@ -466,7 +473,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP for (int x = skip_horizontal_pixels_; x < w_bound; ++x) { // not filtered - if (labels_row[x] == mesh_filter::MeshFilterBase::Background) + if (labels_row[x] == mesh_filter::MeshFilterBase::BACKGROUND) { float zz = (float)input_row[x] * 1e-3; // scale from mm to m float yy = y_cache_[y] * zz; @@ -476,7 +483,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); } // on far plane or a model point -> remove - else if (labels_row[x] >= mesh_filter::MeshFilterBase::FarClip) + else if (labels_row[x] >= mesh_filter::MeshFilterBase::FAR_CLIP) { float zz = input_row[x] * 1e-3; float yy = y_cache_[y] * zz; @@ -495,7 +502,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP for (int y = skip_vertical_pixels_; y < h_bound; ++y, labels_row += w, input_row += w) for (int x = skip_horizontal_pixels_; x < w_bound; ++x) { - if (labels_row[x] == mesh_filter::MeshFilterBase::Background) + if (labels_row[x] == mesh_filter::MeshFilterBase::BACKGROUND) { float zz = input_row[x]; float yy = y_cache_[y] * zz; @@ -504,7 +511,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(xx, yy, zz); occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ())); } - else if (labels_row[x] >= mesh_filter::MeshFilterBase::FarClip) + else if (labels_row[x] >= mesh_filter::MeshFilterBase::FAR_CLIP) { float zz = input_row[x]; float yy = y_cache_[y] * zz; @@ -520,7 +527,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP catch (...) { tree_->unlockRead(); - ROS_ERROR("Internal error while parsing depth data"); + ROS_ERROR_NAMED(LOGNAME, "Internal error while parsing depth data"); return; } tree_->unlockRead(); @@ -539,7 +546,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP } catch (...) { - ROS_ERROR("Internal error while updating octree"); + ROS_ERROR_NAMED(LOGNAME, "Internal error while updating octree"); } tree_->unlockWrite(); tree_->triggerUpdateCallback(); @@ -547,6 +554,6 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP // at this point we still have not freed the space free_space_updater_->pushLazyUpdate(occupied_cells_ptr, model_cells_ptr, sensor_origin); - ROS_DEBUG("Processed depth image in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); + ROS_DEBUG_NAMED(LOGNAME, "Processed depth image in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); } } // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/updater_plugin.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/updater_plugin.cpp index ab76ac2076..83d955c803 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/updater_plugin.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/updater_plugin.cpp @@ -37,5 +37,4 @@ #include #include -CLASS_LOADER_REGISTER_CLASS(occupancy_map_monitor::DepthImageOctomapUpdater, - occupancy_map_monitor::OccupancyMapUpdater); +CLASS_LOADER_REGISTER_CLASS(occupancy_map_monitor::DepthImageOctomapUpdater, occupancy_map_monitor::OccupancyMapUpdater); diff --git a/moveit_ros/perception/lazy_free_space_updater/CMakeLists.txt b/moveit_ros/perception/lazy_free_space_updater/CMakeLists.txt index ce935b1f36..e9092875c1 100644 --- a/moveit_ros/perception/lazy_free_space_updater/CMakeLists.txt +++ b/moveit_ros/perception/lazy_free_space_updater/CMakeLists.txt @@ -8,5 +8,8 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${sensor_msgs_EXPORTED_TARGETS}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h b/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h index cad8b0a48c..64e93612bd 100644 --- a/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h +++ b/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_OCCUPANCY_MAP_MONITOR_LAZY_FREE_SPACE_UPDATER_ -#define MOVEIT_OCCUPANCY_MAP_MONITOR_LAZY_FREE_SPACE_UPDATER_ +#pragma once #include #include @@ -88,6 +87,4 @@ class LazyFreeSpaceUpdater boost::thread update_thread_; boost::thread process_thread_; }; -} - -#endif /* MOVEIT_OCCUPANCY_MAP_UPDATER_H_ */ +} // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp index 5b9cde2bc5..300da502fb 100644 --- a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp +++ b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp @@ -39,13 +39,14 @@ namespace occupancy_map_monitor { +static const std::string LOGNAME = "lazy_free_space_updater"; + LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const OccMapTreePtr& tree, unsigned int max_batch_size) : tree_(tree) , running_(true) , max_batch_size_(max_batch_size) - , max_sensor_delta_(1e-3) - , // 1mm - process_occupied_cells_set_(nullptr) + , max_sensor_delta_(1e-3) // 1mm + , process_occupied_cells_set_(nullptr) , process_model_cells_set_(nullptr) , update_thread_(boost::bind(&LazyFreeSpaceUpdater::lazyUpdateThread, this)) , process_thread_(boost::bind(&LazyFreeSpaceUpdater::processThread, this)) @@ -70,8 +71,8 @@ LazyFreeSpaceUpdater::~LazyFreeSpaceUpdater() void LazyFreeSpaceUpdater::pushLazyUpdate(octomap::KeySet* occupied_cells, octomap::KeySet* model_cells, const octomap::point3d& sensor_origin) { - ROS_DEBUG("Pushing %lu occupied cells and %lu model cells for lazy updating...", - (long unsigned int)occupied_cells->size(), (long unsigned int)model_cells->size()); + ROS_DEBUG_NAMED(LOGNAME, "Pushing %lu occupied cells and %lu model cells for lazy updating...", + (long unsigned int)occupied_cells->size(), (long unsigned int)model_cells->size()); boost::mutex::scoped_lock _(update_cell_sets_lock_); occupied_cells_sets_.push_back(occupied_cells); model_cells_sets_.push_back(model_cells); @@ -95,7 +96,7 @@ void LazyFreeSpaceUpdater::pushBatchToProcess(OcTreeKeyCountMap* occupied_cells, } else { - ROS_WARN("Previous batch update did not complete. Ignoring set of cells to be freed."); + ROS_WARN_NAMED(LOGNAME, "Previous batch update did not complete. Ignoring set of cells to be freed."); delete occupied_cells; delete model_cells; } @@ -121,9 +122,10 @@ void LazyFreeSpaceUpdater::processThread() if (!running_) break; - ROS_DEBUG("Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells", - (long unsigned int)process_occupied_cells_set_->size(), - (long unsigned int)process_model_cells_set_->size()); + ROS_DEBUG_NAMED(LOGNAME, + "Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells", + (long unsigned int)process_occupied_cells_set_->size(), + (long unsigned int)process_model_cells_set_->size()); ros::WallTime start = ros::WallTime::now(); tree_->lockRead(); @@ -162,7 +164,8 @@ void LazyFreeSpaceUpdater::processThread() free_cells1.erase(it); free_cells2.erase(it); } - ROS_DEBUG("Marking %lu cells as free...", (long unsigned int)(free_cells1.size() + free_cells2.size())); + ROS_DEBUG_NAMED(LOGNAME, "Marking %lu cells as free...", + (long unsigned int)(free_cells1.size() + free_cells2.size())); tree_->lockWrite(); @@ -180,12 +183,12 @@ void LazyFreeSpaceUpdater::processThread() } catch (...) { - ROS_ERROR("Internal error while updating octree"); + ROS_ERROR_NAMED(LOGNAME, "Internal error while updating octree"); } tree_->unlockWrite(); tree_->triggerUpdateCallback(); - ROS_DEBUG("Marked free cells in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); + ROS_DEBUG_NAMED(LOGNAME, "Marked free cells in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); delete process_occupied_cells_set_; process_occupied_cells_set_ = nullptr; @@ -229,7 +232,8 @@ void LazyFreeSpaceUpdater::lazyUpdateThread() { if ((sensor_origins_.front() - sensor_origin).norm() > max_sensor_delta_) { - ROS_DEBUG("Pushing %u sets of occupied/model cells to free cells update thread (origin changed)", batch_size); + ROS_DEBUG_NAMED(LOGNAME, "Pushing %u sets of occupied/model cells to free cells update thread (origin changed)", + batch_size); pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin); batch_size = 0; break; @@ -250,7 +254,7 @@ void LazyFreeSpaceUpdater::lazyUpdateThread() if (batch_size >= max_batch_size_) { - ROS_DEBUG("Pushing %u sets of occupied/model cells to free cells update thread", batch_size); + ROS_DEBUG_NAMED(LOGNAME, "Pushing %u sets of occupied/model cells to free cells update thread", batch_size); pushBatchToProcess(occupied_cells_set, model_cells_set, sensor_origin); occupied_cells_set = nullptr; batch_size = 0; diff --git a/moveit_ros/perception/mesh_filter/CMakeLists.txt b/moveit_ros/perception/mesh_filter/CMakeLists.txt index 940772f8da..100350e472 100644 --- a/moveit_ros/perception/mesh_filter/CMakeLists.txt +++ b/moveit_ros/perception/mesh_filter/CMakeLists.txt @@ -9,7 +9,7 @@ add_library(${MOVEIT_LIB_NAME} ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${gl_LIBS} glut GLEW) +target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${gl_LIBS} GLUT::GLUT ${GLEW_LIBRARIES}) if (CATKIN_ENABLE_TESTING) #catkin_lint: ignore_once env_var @@ -17,10 +17,15 @@ if (CATKIN_ENABLE_TESTING) if (DEFINED ENV{DISPLAY} AND NOT $ENV{DISPLAY} STREQUAL "") catkin_add_gtest(mesh_filter_test test/mesh_filter_test.cpp) target_link_libraries(mesh_filter_test ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_mesh_filter) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(mesh_filter_test PRIVATE -Wno-deprecated-declarations) else() message("No display, will not configure tests for moveit_ros_perception/mesh_filter") endif() endif() -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h index 6ab4d6ab37..323bc03326 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_DEPTH_SELF_FILTER_NODELET_ -#define MOVEIT_DEPTH_SELF_FILTER_NODELET_ +#pragma once #include #include @@ -132,5 +131,3 @@ class DepthSelfFiltering : public nodelet::Nodelet }; } // namespace mesh_filter - -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h index a1f098f244..eb97306f79 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_FILTER_JOB_ -#define MOVEIT_MESH_FILTER_FILTER_JOB_ +#pragma once #include #include @@ -44,7 +43,7 @@ namespace mesh_filter { -MOVEIT_CLASS_FORWARD(Job); +MOVEIT_CLASS_FORWARD(Job); // Defines JobPtr, ConstPtr, WeakPtr... etc /** * \brief This class is used to execute functions within the thread that holds the OpenGL context. @@ -141,5 +140,4 @@ class FilterJob : public Job private: boost::function exec_; }; -} -#endif +} // namespace mesh_filter diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h index 2ba940d7f7..969f4e268d 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h @@ -34,17 +34,15 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_GLMESH_ -#define MOVEIT_MESH_FILTER_GLMESH_ +#pragma once -#include +#include // for Isometry3d #include #ifdef __APPLE__ #include #else #include #endif -#include namespace shapes { @@ -85,4 +83,3 @@ class GLMesh unsigned int mesh_label_; }; } // namespace mesh_filter -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h index 67fa5abb1b..2c280c9e13 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_GLRENDERER_ -#define MOVEIT_MESH_FILTER_GLRENDERER_ +#pragma once #include #include @@ -49,7 +48,7 @@ namespace mesh_filter { -MOVEIT_CLASS_FORWARD(GLRenderer); +MOVEIT_CLASS_FORWARD(GLRenderer); // Defines GLRendererPtr, ConstPtr, WeakPtr... etc /** \brief Abstracts the OpenGL frame buffer objects, and provides an interface to render meshes, and retrieve the color * and depth ap from opengl. @@ -303,4 +302,3 @@ class GLRenderer static bool glutInitialized_; }; } // namespace mesh_filter -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h index e488aa2883..3872aac0f5 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h @@ -34,15 +34,13 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_MESHFILTER_ -#define MOVEIT_MESH_FILTER_MESHFILTER_ +#pragma once #include #include #include #include #include -#include // forward declarations namespace shapes @@ -112,4 +110,3 @@ const typename SensorType::Parameters& MeshFilter::parameters() cons } } // namespace mesh_filter -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h index 86f85270e0..5c02d52cec 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_MESH_FILTER_BASE_ -#define MOVEIT_MESH_FILTER_MESH_FILTER_BASE_ +#pragma once #include #include @@ -43,7 +42,7 @@ #include #include #include -#include +#include // for Isometry3d #include // forward declarations @@ -54,8 +53,8 @@ class Mesh; namespace mesh_filter { -MOVEIT_CLASS_FORWARD(Job); -MOVEIT_CLASS_FORWARD(GLMesh); +MOVEIT_CLASS_FORWARD(Job); // Defines JobPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(GLMesh); // Defines GLMeshPtr, ConstPtr, WeakPtr... etc typedef unsigned int MeshHandle; typedef uint32_t LabelType; @@ -70,11 +69,11 @@ class MeshFilterBase // this would allow me to do a single comparison instead of 3, in the code i write enum { - Background = 0, - Shadow = 1, - NearClip = 2, - FarClip = 3, - FirstLabel = 16 + BACKGROUND = 0, + SHADOW = 1, + NEAR_CLIP = 2, + FAR_CLIP = 3, + FIRST_LABEL = 16 }; public: @@ -294,5 +293,3 @@ class MeshFilterBase float shadow_threshold_; }; } // namespace mesh_filter - -#endif /* __MESH_FILTER_MESH_FILTER_BASE_H__ */ diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h index 87239b0bd2..607fb663c0 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h @@ -34,11 +34,10 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_SENSOR_MODEL_ -#define MOVEIT_MESH_FILTER_SENSOR_MODEL_ +#pragma once #include -#include +#include // for Vector3f namespace mesh_filter { @@ -52,7 +51,7 @@ class GLRenderer; class SensorModel { public: - MOVEIT_CLASS_FORWARD(Parameters); + MOVEIT_CLASS_FORWARD(Parameters); // Defines ParametersPtr, ConstPtr, WeakPtr... etc /** * \brief Abstract Interface defining Sensor Parameters. @@ -170,5 +169,3 @@ class SensorModel virtual ~SensorModel(); }; } // namespace mesh_filter - -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h index 2dfac68ad6..5ffd8174d3 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_STEREO_CAMERA_MODEL_ -#define MOVEIT_MESH_FILTER_STEREO_CAMERA_MODEL_ +#pragma once #include #include @@ -149,7 +148,7 @@ class StereoCameraModel : public SensorModel }; /** \brief predefined sensor model for OpenNI compatible devices (e.g., PrimeSense, Kinect, Asus Xtion) */ - static const StereoCameraModel::Parameters& REGISTERED_PSDK_PARAMS; + static const StereoCameraModel::Parameters& REGISTERED_PSDK_PARAMS; // NOLINT(readability-identifier-naming) /** \brief source code of the vertex shader used to render the meshes*/ static const std::string RENDER_VERTEX_SHADER_SOURCE; @@ -164,4 +163,3 @@ class StereoCameraModel : public SensorModel static const std::string FILTER_FRAGMENT_SHADER_SOURCE; }; } // namespace mesh_filter -#endif diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h index fe5547edad..8f23a34970 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h @@ -34,8 +34,7 @@ /* Author: Suat Gedikli */ -#ifndef MOVEIT_MESH_FILTER_TRANSFORM_PROVIDER_ -#define MOVEIT_MESH_FILTER_TRANSFORM_PROVIDER_ +#pragma once #include #include @@ -49,7 +48,7 @@ namespace tf2_ros { class TransformListener; class Buffer; -} +} // namespace tf2_ros /** * \brief Class that caches and updates transformations for given frames. @@ -121,7 +120,7 @@ class TransformProvider */ void updateTransforms(); - MOVEIT_CLASS_FORWARD(TransformContext); + MOVEIT_CLASS_FORWARD(TransformContext); // Defines TransformContextPtr, ConstPtr, WeakPtr... etc /** * \brief Context Object for registered frames @@ -167,4 +166,3 @@ class TransformProvider /** \brief update interval in micro seconds*/ unsigned long interval_us_; }; -#endif diff --git a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp index 511e2cca6d..ecd1830dff 100644 --- a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp +++ b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp @@ -42,7 +42,6 @@ #include #include #include -#include #include namespace enc = sensor_msgs::image_encodings; @@ -176,8 +175,8 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d void mesh_filter::DepthSelfFiltering::addMeshes(MeshFilter& mesh_filter) { robot_model_loader::RobotModelLoader robotModelLoader("robot_description"); - robot_model::RobotModelConstPtr robotModel = robotModelLoader.getModel(); - const vector& links = robotModel->getLinkModelsWithCollisionGeometry(); + moveit::core::RobotModelConstPtr robotModel = robotModelLoader.getModel(); + const vector& links = robotModel->getLinkModelsWithCollisionGeometry(); for (size_t i = 0; i < links.size(); ++i) { shapes::ShapeConstPtr shape = links[i]->getShape(); diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index 03d19a6b5d..4f351b53e6 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -47,7 +47,6 @@ #include #include #include -#include #include #include @@ -182,7 +181,7 @@ void mesh_filter::GLRenderer::deleteFrameBuffers() void mesh_filter::GLRenderer::begin() const { - glPushAttrib(GL_VIEWPORT_BIT | GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_POLYGON_BIT); + glPushAttrib(GL_VIEWPORT_BIT | GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_POLYGON_BIT | GL_PIXEL_MODE_BIT); glBindFramebuffer(GL_FRAMEBUFFER, fbo_id_); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glViewport(0, 0, width_, height_); diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp index 4dd045381d..98ed797ae4 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp @@ -52,8 +52,7 @@ using namespace std; using namespace Eigen; using shapes::Mesh; -mesh_filter::MeshFilter::MeshFilter( - const boost::function& transform_callback) +mesh_filter::MeshFilter::MeshFilter(const boost::function& transform_callback) : mesh_renderer_(640, 480, 0.3, 10) // some default values for buffer sizes and clipping planes , depth_filter_(640, 480, 0.3, 10) , next_handle_(FirstLabel) // 0 and 1 are reserved! @@ -163,7 +162,7 @@ inline bool isAligned16(const void* pointer) { return (((uintptr_t)pointer & 15) == 0); } -} +} // namespace void mesh_filter::MeshFilter::getModelDepth(float* depth) const { diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp index b8808d13c7..27d5627f2c 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp @@ -43,7 +43,6 @@ #include #include #include -#include #include @@ -59,8 +58,8 @@ mesh_filter::MeshFilterBase::MeshFilterBase(const TransformCallback& transform_c const std::string& filter_vertex_shader, const std::string& filter_fragment_shader) : sensor_parameters_(sensor_parameters.clone()) - , next_handle_(FirstLabel) // 0 and 1 are reserved! - , min_handle_(FirstLabel) + , next_handle_(FIRST_LABEL) // 0 and 1 are reserved! + , min_handle_(FIRST_LABEL) , stop_(false) , transform_callback_(transform_callback) , padding_scale_(1.0) @@ -256,8 +255,7 @@ void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const void mesh_filter::MeshFilterBase::getFilteredLabels(LabelType* labels) const { - JobPtr job( - new FilterJob(boost::bind(&GLRenderer::getColorBuffer, depth_filter_.get(), (unsigned char*)labels))); + JobPtr job(new FilterJob(boost::bind(&GLRenderer::getColorBuffer, depth_filter_.get(), (unsigned char*)labels))); addJob(job); job->wait(); } @@ -324,7 +322,7 @@ void mesh_filter::MeshFilterBase::doFilter(const void* sensor_data, const int en glUniform3f(padding_coefficients_id, padding_coefficients[0], padding_coefficients[1], padding_coefficients[2]); Eigen::Isometry3d transform; - for (const std::pair& mesh : meshes_) + for (const std::pair& mesh : meshes_) if (transform_callback_(mesh.first, transform)) mesh.second->render(transform); diff --git a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp index d09cb649d9..df29ec2b21 100644 --- a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp +++ b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp @@ -35,7 +35,6 @@ /* Author: Suat Gedikli */ #include -#include #include mesh_filter::SensorModel::~SensorModel() = default; @@ -91,6 +90,7 @@ float mesh_filter::SensorModel::Parameters::getFarClippingPlaneDistance() const namespace { +#if HAVE_SSE_EXTENSIONS inline unsigned alignment16(const void* pointer) { return ((uintptr_t)pointer & 15); @@ -99,6 +99,7 @@ inline bool isAligned16(const void* pointer) { return (((uintptr_t)pointer & 15) == 0); } +#endif } // namespace void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(float* depth) const diff --git a/moveit_ros/perception/mesh_filter/src/transform_provider.cpp b/moveit_ros/perception/mesh_filter/src/transform_provider.cpp index cc5e8bda85..527776a207 100644 --- a/moveit_ros/perception/mesh_filter/src/transform_provider.cpp +++ b/moveit_ros/perception/mesh_filter/src/transform_provider.cpp @@ -129,7 +129,7 @@ void TransformProvider::setUpdateInterval(unsigned long usecs) void TransformProvider::updateTransforms() { static tf2::Stamped input_transform, output_transform; - static robot_state::RobotStatePtr robot_state; + static moveit::core::RobotStatePtr robot_state; robot_state = psm_->getStateMonitor()->getCurrentState(); try { diff --git a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp index a452eb14f3..e8c90782bc 100644 --- a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp +++ b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp @@ -231,7 +231,7 @@ void MeshFilterTest::test() float sensor_depth = sensor_data_[idx] * FilterTraits::ToMetricScale; if (fabs(sensor_depth - distance_ - shadow_) > epsilon_ && fabs(sensor_depth - distance_) > epsilon_) { - ASSERT_FLOAT_EQ(filtered_depth[idx], gt_depth[idx]); + ASSERT_NEAR(filtered_depth[idx], gt_depth[idx], 1e-4); ASSERT_EQ(filtered_labels[idx], gt_labels[idx]); } } @@ -251,11 +251,11 @@ void MeshFilterTest::getGroundTruth(unsigned int* labels, float* depth) co { depth[idx] = double(sensor_data_[idx]) * scale; if (depth[idx] < near_) - labels[idx] = MeshFilterBase::NearClip; + labels[idx] = MeshFilterBase::NEAR_CLIP; else if (depth[idx] >= far_) - labels[idx] = MeshFilterBase::FarClip; + labels[idx] = MeshFilterBase::FAR_CLIP; else - labels[idx] = MeshFilterBase::Background; + labels[idx] = MeshFilterBase::BACKGROUND; if (depth[idx] <= near_ || depth[idx] >= far_) depth[idx] = 0; @@ -272,21 +272,21 @@ void MeshFilterTest::getGroundTruth(unsigned int* labels, float* depth) co if (depth[idx] < near_) { - labels[idx] = MeshFilterBase::NearClip; + labels[idx] = MeshFilterBase::NEAR_CLIP; depth[idx] = 0; } else { double diff = depth[idx] - distance_; if (diff < 0 && depth[idx] < far_) - labels[idx] = MeshFilterBase::Background; + labels[idx] = MeshFilterBase::BACKGROUND; else if (diff > shadow_) - labels[idx] = MeshFilterBase::Shadow; + labels[idx] = MeshFilterBase::SHADOW; else if (depth[idx] >= far_) - labels[idx] = MeshFilterBase::FarClip; + labels[idx] = MeshFilterBase::FAR_CLIP; else { - labels[idx] = MeshFilterBase::FirstLabel; + labels[idx] = MeshFilterBase::FIRST_LABEL; depth[idx] = 0; } diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml index 61033ec542..7ace55ed69 100644 --- a/moveit_ros/perception/package.xml +++ b/moveit_ros/perception/package.xml @@ -1,14 +1,14 @@ moveit_ros_perception - 1.0.1 - Components of MoveIt! connecting to perception + 1.1.1 + Components of MoveIt connecting to perception Ioan Sucan Jon Binney Suat Gedikli Michael Ferguson - MoveIt! Release Team + MoveIt Release Team BSD @@ -27,6 +27,7 @@ image_transport glut libglew-dev + libomp-dev opengl cv_bridge sensor_msgs diff --git a/moveit_ros/perception/point_containment_filter/CMakeLists.txt b/moveit_ros/perception/point_containment_filter/CMakeLists.txt index ed76980c0a..a672692acb 100644 --- a/moveit_ros/perception/point_containment_filter/CMakeLists.txt +++ b/moveit_ros/perception/point_containment_filter/CMakeLists.txt @@ -6,5 +6,8 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_F set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h index 2b6ee820bd..f504acb9f5 100644 --- a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h +++ b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h @@ -34,13 +34,11 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_POINT_CONTAINMENT_FILTER_SELF_MASK_ -#define MOVEIT_POINT_CONTAINMENT_FILTER_SELF_MASK_ +#pragma once #include #include #include -#include #include #include #include @@ -69,7 +67,7 @@ class ShapeMask ShapeMask(const TransformCallback& transform_callback = TransformCallback()); /** \brief Destructor to clean up */ - ~ShapeMask(); + virtual ~ShapeMask(); ShapeHandle addShape(const shapes::ShapeConstPtr& shape, double scale = 1.0, double padding = 0.0); void removeShape(ShapeHandle handle); @@ -91,12 +89,12 @@ class ShapeMask It is assumed the point is in the frame corresponding to the TransformCallback */ int getMaskContainment(const Eigen::Vector3d& pt) const; -private: +protected: struct SeeShape { SeeShape() { - body = NULL; + body = nullptr; } bodies::Body* body; @@ -106,7 +104,7 @@ class ShapeMask struct SortBodies { - bool operator()(const SeeShape& b1, const SeeShape& b2) + bool operator()(const SeeShape& b1, const SeeShape& b2) const { if (b1.volume > b2.volume) return true; @@ -116,18 +114,19 @@ class ShapeMask } }; + TransformCallback transform_callback_; + + /** \brief Protects, bodies_ and bspheres_. All public methods acquire this mutex for their whole duration. */ + mutable boost::mutex shapes_lock_; + std::set bodies_; + std::vector bspheres_; + +private: /** \brief Free memory. */ void freeMemory(); - TransformCallback transform_callback_; ShapeHandle next_handle_; ShapeHandle min_handle_; - - mutable boost::mutex shapes_lock_; - std::set bodies_; std::map::iterator> used_handles_; - std::vector bspheres_; }; -} - -#endif +} // namespace point_containment_filter diff --git a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp index 7c6c33f07f..2d9da2b231 100644 --- a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp +++ b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp @@ -39,6 +39,8 @@ #include #include +static const std::string LOGNAME = "shape_mask"; + point_containment_filter::ShapeMask::ShapeMask(const TransformCallback& transform_callback) : transform_callback_(transform_callback), next_handle_(1), min_handle_(1) { @@ -67,16 +69,18 @@ point_containment_filter::ShapeHandle point_containment_filter::ShapeMask::addSh { boost::mutex::scoped_lock _(shapes_lock_); SeeShape ss; - ss.body = bodies::createBodyFromShape(shape.get()); + ss.body = bodies::createEmptyBodyFromShapeType(shape->type); if (ss.body) { - ss.body->setScale(scale); - ss.body->setPadding(padding); + ss.body->setDimensionsDirty(shape.get()); + ss.body->setScaleDirty(scale); + ss.body->setPaddingDirty(padding); + ss.body->updateInternalData(); ss.volume = ss.body->computeVolume(); ss.handle = next_handle_; std::pair::iterator, bool> insert_op = bodies_.insert(ss); if (!insert_op.second) - ROS_ERROR("Internal error in management of bodies in ShapeMask. This is a serious error."); + ROS_ERROR_NAMED(LOGNAME, "Internal error in management of bodies in ShapeMask. This is a serious error."); used_handles_[next_handle_] = insert_op.first; } else @@ -107,11 +111,11 @@ void point_containment_filter::ShapeMask::removeShape(ShapeHandle handle) min_handle_ = handle; } else - ROS_ERROR("Unable to remove shape handle %u", handle); + ROS_ERROR_NAMED(LOGNAME, "Unable to remove shape handle %u", handle); } void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::PointCloud2& data_in, - const Eigen::Vector3d& sensor_origin, + const Eigen::Vector3d& /*sensor_origin*/, const double min_sensor_dist, const double max_sensor_dist, std::vector& mask) { @@ -131,11 +135,10 @@ void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::Poi if (!transform_callback_(it->handle, tmp)) { if (!it->body) - ROS_ERROR_STREAM_NAMED("shape_mask", "Missing transform for shape with handle " << it->handle - << " without a body"); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Missing transform for shape with handle " << it->handle << " without a body"); else - ROS_ERROR_STREAM_NAMED("shape_mask", "Missing transform for shape " << it->body->getType() << " with handle " - << it->handle); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Missing transform for shape " << it->body->getType() << " with handle " + << it->handle); } else { diff --git a/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt b/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt index 503c46fb2f..425a12beed 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt +++ b/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt @@ -14,4 +14,5 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h index cf56d68dfe..69cd73f047 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h @@ -34,8 +34,7 @@ /* Author: Jon Binney, Ioan Sucan */ -#ifndef MOVEIT_PERCEPTION_POINTCLOUD_OCTOMAP_UPDATER_ -#define MOVEIT_PERCEPTION_POINTCLOUD_OCTOMAP_UPDATER_ +#pragma once #include #include @@ -100,6 +99,4 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater std::unique_ptr shape_mask_; std::vector mask_; }; -} - -#endif +} // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index af47bd5b9a..38953339ca 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -47,6 +47,7 @@ namespace occupancy_map_monitor { +static const std::string LOGNAME = "occupancy_map_monitor"; PointCloudOctomapUpdater::PointCloudOctomapUpdater() : OccupancyMapUpdater("PointCloudUpdater") , private_nh_("~") @@ -84,7 +85,7 @@ bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params) } catch (XmlRpc::XmlRpcException& ex) { - ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str()); + ROS_ERROR_STREAM_NAMED(LOGNAME, "XmlRpc Exception: " << ex.getMessage()); return false; } @@ -113,13 +114,13 @@ void PointCloudOctomapUpdater::start() point_cloud_filter_ = new tf2_ros::MessageFilter(*point_cloud_subscriber_, *tf_buffer_, monitor_->getMapFrame(), 5, root_nh_); point_cloud_filter_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1)); - ROS_INFO("Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), - point_cloud_filter_->getTargetFramesString().c_str()); + ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), + point_cloud_filter_->getTargetFramesString().c_str()); } else { point_cloud_subscriber_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1)); - ROS_INFO("Listening to '%s'", point_cloud_topic_.c_str()); + ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", point_cloud_topic_.c_str()); } } @@ -142,7 +143,7 @@ ShapeHandle PointCloudOctomapUpdater::excludeShape(const shapes::ShapeConstPtr& if (shape_mask_) h = shape_mask_->addShape(shape, scale_, padding_); else - ROS_ERROR("Shape filter not yet initialized!"); + ROS_ERROR_NAMED(LOGNAME, "Shape filter not yet initialized!"); return h; } @@ -162,14 +163,14 @@ bool PointCloudOctomapUpdater::getShapeTransform(ShapeHandle h, Eigen::Isometry3 return it != transform_cache_.end(); } -void PointCloudOctomapUpdater::updateMask(const sensor_msgs::PointCloud2& cloud, const Eigen::Vector3d& sensor_origin, - std::vector& mask) +void PointCloudOctomapUpdater::updateMask(const sensor_msgs::PointCloud2& /*cloud*/, + const Eigen::Vector3d& /*sensor_origin*/, std::vector& /*mask*/) { } void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) { - ROS_DEBUG("Received a new point cloud message"); + ROS_DEBUG_NAMED(LOGNAME, "Received a new point cloud message"); ros::WallTime start = ros::WallTime::now(); if (max_update_rate_ > 0) @@ -193,13 +194,13 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2:: { try { - tf2::fromMsg( - tf_buffer_->lookupTransform(monitor_->getMapFrame(), cloud_msg->header.frame_id, cloud_msg->header.stamp), - map_h_sensor); + tf2::fromMsg(tf_buffer_->lookupTransform(monitor_->getMapFrame(), cloud_msg->header.frame_id, + cloud_msg->header.stamp), + map_h_sensor); } catch (tf2::TransformException& ex) { - ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << "; quitting callback"); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Transform error of sensor data: " << ex.what() << "; quitting callback"); return; } } @@ -214,7 +215,7 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2:: if (!updateTransformCache(cloud_msg->header.frame_id, cloud_msg->header.stamp)) { - ROS_ERROR_THROTTLE(1, "Transform cache was not updated. Self-filtering may fail."); + ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform cache was not updated. Self-filtering may fail."); return; } @@ -346,10 +347,10 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2:: } catch (...) { - ROS_ERROR("Internal error while updating octree"); + ROS_ERROR_NAMED(LOGNAME, "Internal error while updating octree"); } tree_->unlockWrite(); - ROS_DEBUG("Processed point cloud in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); + ROS_DEBUG_NAMED(LOGNAME, "Processed point cloud in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0); tree_->triggerUpdateCallback(); if (filtered_cloud) diff --git a/moveit_ros/perception/semantic_world/CMakeLists.txt b/moveit_ros/perception/semantic_world/CMakeLists.txt index 6ed3c99610..0c6ef4f3f0 100644 --- a/moveit_ros/perception/semantic_world/CMakeLists.txt +++ b/moveit_ros/perception/semantic_world/CMakeLists.txt @@ -9,4 +9,5 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h index bed034f0c9..a87c6ccb74 100644 --- a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h +++ b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h @@ -34,8 +34,7 @@ /* Author: Sachin Chitta */ -#ifndef MOVEIT_SEMANTIC_WORLD_ -#define MOVEIT_SEMANTIC_WORLD_ +#pragma once #include #include @@ -47,14 +46,14 @@ namespace shapes { -MOVEIT_CLASS_FORWARD(Shape); +MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc } namespace moveit { namespace semantic_world { -MOVEIT_CLASS_FORWARD(SemanticWorld); +MOVEIT_CLASS_FORWARD(SemanticWorld); // Defines SemanticWorldPtr, ConstPtr, WeakPtr... etc /** * @brief A (simple) semantic world representation for pick and place and other tasks. @@ -162,7 +161,5 @@ class SemanticWorld ros::Publisher planning_scene_diff_publisher_; }; -} -} - -#endif +} // namespace semantic_world +} // namespace moveit diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp index aa6d99403d..f1eb076184 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -38,7 +38,7 @@ #include #include -// MoveIt! +// MoveIt #include #include #include @@ -54,6 +54,8 @@ namespace moveit { namespace semantic_world { +static const std::string LOGNAME = "semantic_world"; + SemanticWorld::SemanticWorld(const planning_scene::PlanningSceneConstPtr& planning_scene) : planning_scene_(planning_scene) { @@ -66,7 +68,7 @@ SemanticWorld::SemanticWorld(const planning_scene::PlanningSceneConstPtr& planni visualization_msgs::MarkerArray SemanticWorld::getPlaceLocationsMarker(const std::vector& poses) const { - ROS_DEBUG("Visualizing: %d place poses", (int)poses.size()); + ROS_DEBUG_NAMED(LOGNAME, "Visualizing: %d place poses", (int)poses.size()); visualization_msgs::MarkerArray marker; for (std::size_t i = 0; i < poses.size(); ++i) { @@ -224,7 +226,7 @@ SemanticWorld::generatePlacePoses(const std::string& table_name, const shapes::S } std::vector place_poses; - ROS_ERROR("Did not find table %s to place on", table_name.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Did not find table %s to place on", table_name.c_str()); return place_poses; } @@ -347,7 +349,7 @@ std::vector SemanticWorld::generatePlacePoses(const unsigned int num_x = fabs(x_max - x_min) / resolution + 1; unsigned int num_y = fabs(y_max - y_min) / resolution + 1; - ROS_DEBUG("Num points for possible place operations: %d %d", num_x, num_y); + ROS_DEBUG_NAMED(LOGNAME, "Num points for possible place operations: %d %d", num_x, num_y); std::vector > contours; std::vector hierarchy; @@ -434,7 +436,7 @@ bool SemanticWorld::isInsideTableContour(const geometry_msgs::Pose& pose, const // Assuming Z axis points upwards for the table if (point.z() < -fabs(min_vertical_offset)) { - ROS_ERROR("Object is not above table"); + ROS_ERROR_NAMED(LOGNAME, "Object is not above table"); return false; } @@ -442,7 +444,7 @@ bool SemanticWorld::isInsideTableContour(const geometry_msgs::Pose& pose, const int point_y = (point.y() - y_min) * scale_factor; cv::Point2f point2f(point_x, point_y); double result = cv::pointPolygonTest(contours[0], point2f, true); - ROS_DEBUG("table distance: %f", result); + ROS_DEBUG_NAMED(LOGNAME, "table distance: %f", result); return (int)result >= (int)(min_distance_from_edge * scale_factor); } @@ -453,7 +455,7 @@ std::string SemanticWorld::findObjectTable(const geometry_msgs::Pose& pose, doub std::map::const_iterator it; for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it) { - ROS_DEBUG("Testing table: %s", it->first.c_str()); + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Testing table: " << it->first); if (isInsideTableContour(pose, it->second, min_distance_from_edge, min_vertical_offset)) return it->first; } @@ -463,12 +465,12 @@ std::string SemanticWorld::findObjectTable(const geometry_msgs::Pose& pose, doub void SemanticWorld::tableCallback(const object_recognition_msgs::TableArrayPtr& msg) { table_array_ = *msg; - ROS_INFO("Table callback with %d tables", (int)table_array_.tables.size()); + ROS_INFO_NAMED(LOGNAME, "Table callback with %d tables", (int)table_array_.tables.size()); transformTableArray(table_array_); // Callback on an update if (table_callback_) { - ROS_INFO("Calling table callback"); + ROS_INFO_NAMED(LOGNAME, "Calling table callback"); table_callback_(); } } @@ -480,18 +482,19 @@ void SemanticWorld::transformTableArray(object_recognition_msgs::TableArray& tab std::string original_frame = table.header.frame_id; if (table.convex_hull.empty()) continue; - ROS_INFO_STREAM("Original pose: " << table.pose.position.x << "," << table.pose.position.y << "," - << table.pose.position.z); + ROS_INFO_STREAM_NAMED(LOGNAME, "Original pose: " << table.pose.position.x << "," << table.pose.position.y << "," + << table.pose.position.z); std::string error_text; - const Eigen::Isometry3d& original_transform = planning_scene_->getTransforms().getTransform(original_frame); + const Eigen::Isometry3d& original_transform = planning_scene_->getFrameTransform(original_frame); Eigen::Isometry3d original_pose; tf2::fromMsg(table.pose, original_pose); original_pose = original_transform * original_pose; table.pose = tf2::toMsg(original_pose); table.header.frame_id = planning_scene_->getTransforms().getTargetFrame(); - ROS_INFO_STREAM("Successfully transformed table array from " << original_frame << "to " << table.header.frame_id); - ROS_INFO_STREAM("Transformed pose: " << table.pose.position.x << "," << table.pose.position.y << "," - << table.pose.position.z); + ROS_INFO_STREAM_NAMED(LOGNAME, "Successfully transformed table array from " << original_frame << "to " + << table.header.frame_id); + ROS_INFO_STREAM_NAMED(LOGNAME, "Transformed pose: " << table.pose.position.x << "," << table.pose.position.y << "," + << table.pose.position.z); } } diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst index e2bd85fd4d..101db3f469 100644 --- a/moveit_ros/planning/CHANGELOG.rst +++ b/moveit_ros/planning/CHANGELOG.rst @@ -2,6 +2,92 @@ Changelog for package moveit_ros_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [fix] some clang-tidy issues on Travis (`#2337 `_) +* [fix] various issues with Noetic build (`#2327 `_) +* [fix] "Clear Octomap" button, disable when no octomap is published (`#2320 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, Robert Haschke + +1.1.0 (2020-09-04) +------------------ +* [feature] Use Eigen::Transform::linear() instead of rotation() (`#1964 `_) +* [feature] Bullet collision detection (`#1839 `_) (`#1504 `_) +* [feature] Allow different controllers for execution (`#1832 `_) +* [feature] Adding continuous collision detection to Bullet (`#1551 `_) +* [feature] plan_execution: refine logging for invalid paths (`#1705 `_) +* [feature] Unified Collision Environment Integration (`#1584 `_) +* [feature] Allow ROS namespaces for planning request adapters (`#1530 `_) +* [feature] Add named frames to CollisionObjects (`#1439 `_) +* [feature] get_planning_scene_service: return full scene when nothing was requested (`#1424 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Initialize zero dynamics in CurrentStateMonitor (`#1883 `_) +* [fix] memory leak (`#1526 `_) +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] partially transition unused test bin to rostest (`#2158 `_) +* [maint] Optional cpp version setting (`#2166 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#2004 `_, `#1419 `_) +* [maint] Fix usage of panda_moveit_config (`#1904 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Adapt cmake for Bullet (`#1744 `_) +* [maint] Readme for speed benchmark (`#1648 `_) +* [maint] Fix compiler warnings (`#1773 `_) +* [maint] Improve variable naming in RobotModelLoader (`#1759 `_) +* [maint] Move isEmpty() test functions to moveit_core/utils (`#1627 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Bianca Homberg, Dave Coleman, Felix von Drigalski, Henning Kayser, Jens P, Jonathan Binney, Markus Vieth, Martin Pecka, Max Krichenbauer, Michael GΓΆrner, Robert Haschke, Sean Yen, Simon Schmeisser, Tyler Weaver, Yu, Yan, jschleicher, livanov93, llach + +1.0.6 (2020-08-19) +------------------ +* [fix] Fix segfault in PSM::clearOctomap() (`#2193 `_) +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Henning Kayser, Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ +* [feature] Trajectory Execution: fix check for start state position (`#2157 `_) +* [feature] Improve responsiveness of PlanningSceneDisplay (`#2049 `_) + - PlanningSceneMonitor: increate update frequency from 10Hz to 30Hz + - send RobotState diff if only position changed +* Contributors: Michael GΓΆrner, Robert Haschke, Simon Schmeisser + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [fix] `CurrentStateMonitor`: Initialize velocity/effort with unset dynamics +* [fix] Fix spurious warning message (# IK attempts) (`#1876 `_) +* [maint] Move `get_planning_scene` service into `PlanningSceneMonitor` for reusability (`#1854 `_) +* [feature] Forward controller names to TrajectoryExecutionManager +* [fix] Always copy dynamics if enabled in CurrentStateMonitor (`#1676 `_) +* [feature] TrajectoryMonitor: zero sampling frequency disables trajectory recording (`#1542 `_) +* [feature] Add user warning when planning fails with multiple constraints (`#1443 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build fixes + * Fix header inclusion and other MSVC build errors (`#1636 `_) + * Fix binary artifact install locations. (`#1575 `_) + * Favor ros::Duration.sleep over sleep. (`#1634 `_) + * Remove GCC extensions (`#1583 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] Fix potential memory leak in `RDFLoader` (`#1828 `_) + [maint] Use smart pointers to avoid explicit new/delete +* [fix] `TrajectoryExecutionManager`: fix race condition (`#1709 `_) +* [fix] Correctly propagate error if time parameterization fails (`#1562 `_) +* [maint] move `occupancy_map_monitor` into its own package (`#1533 `_) +* [feature] `PlanExecution`: return executed trajectory (`#1538 `_) +* Contributors: Felix von Drigalski, Henning Kayser, Max Krichenbauer, Michael GΓΆrner, Robert Haschke, Sean Yen, Yu, Yan, jschleicher, livanov93, Luca Lach + +1.0.2 (2019-06-28) +------------------ +* [fix] Removed MessageFilter for /collision_object messages (`#1406 `_) +* Contributors: Robert Haschke + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -91,7 +177,7 @@ Changelog for package moveit_ros_planning 0.9.9 (2017-08-06) ------------------ * [fix] Change getCurrentExpectedTrajectory index so collision detection is still performed even if the path timing is not known (`#550 `_) -* [fix] Support for MultiDoF only trajectories `#553 `_ +* [fix] Support for MultiDoF only trajectories `#553 `_ * [fix] ros_error macro name (`#544 `_) * [fix] check plan size for plan length=0 `#535 `_ * Contributors: Cyrille Morin, Michael GΓΆrner, Mikael Arguedas, Notou, Unknown diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 8f9b64aa03..eeab8f238d 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_planning) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -61,23 +65,24 @@ catkin_package( INCLUDE_DIRS ${THIS_PACKAGE_INCLUDE_DIRS} CATKIN_DEPENDS - pluginlib + actionlib + dynamic_reconfigure moveit_core moveit_ros_occupancy_map_monitor moveit_msgs + pluginlib + roscpp tf2_msgs tf2_geometry_msgs DEPENDS EIGEN3 ) -include_directories(${THIS_PACKAGE_INCLUDE_DIRS} +include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} -) -include_directories(SYSTEM - ${EIGEN3_INCLUDE_DIRS} -) + ${EIGEN3_INCLUDE_DIRS}) add_subdirectory(rdf_loader) add_subdirectory(collision_plugin_loader) diff --git a/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt b/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt index 86eea90717..6ad2115652 100644 --- a/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt +++ b/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt @@ -4,5 +4,8 @@ add_library(${MOVEIT_LIB_NAME} src/collision_plugin_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h b/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h index 4527ae450e..d498bac512 100644 --- a/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h +++ b/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h @@ -32,8 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MOVEIT_COLLISION_PLUGIN_LOADER_COLLISION_PLUGIN_LOADER_H -#define MOVEIT_COLLISION_PLUGIN_LOADER_COLLISION_PLUGIN_LOADER_H +#pragma once #include #include @@ -63,10 +62,8 @@ class CollisionPluginLoader bool activate(const std::string& name, const planning_scene::PlanningScenePtr& scene, bool exclusive); private: - MOVEIT_CLASS_FORWARD(CollisionPluginLoaderImpl); + MOVEIT_CLASS_FORWARD(CollisionPluginLoaderImpl); // Defines CollisionPluginLoaderImplPtr, ConstPtr, WeakPtr... etc CollisionPluginLoaderImplPtr loader_; }; } // namespace collision_detection - -#endif // MOVEIT_COLLISION_PLUGIN_LOADER_COLLISION_PLUGIN_LOADER_H diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt b/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt index 8e0dd2dc28..8e11bfaafa 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt +++ b/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt @@ -4,5 +4,8 @@ add_library(${MOVEIT_LIB_NAME} src/constraint_sampler_manager_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h b/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h index 5d24813893..74d808b59d 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h +++ b/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h @@ -34,15 +34,15 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_CONSTRAINT_SAMPLER_MANAGER_LOADER_ -#define MOVEIT_CONSTRAINT_SAMPLER_MANAGER_LOADER_ +#pragma once #include #include namespace constraint_sampler_manager_loader { -MOVEIT_CLASS_FORWARD(ConstraintSamplerManagerLoader); +MOVEIT_CLASS_FORWARD( + ConstraintSamplerManagerLoader); // Defines ConstraintSamplerManagerLoaderPtr, ConstPtr, WeakPtr... etc class ConstraintSamplerManagerLoader { @@ -61,6 +61,4 @@ class ConstraintSamplerManagerLoader MOVEIT_CLASS_FORWARD(Helper) HelperPtr impl_; }; -} - -#endif +} // namespace constraint_sampler_manager_loader diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp index a03d06eff7..d3a12b0b6c 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp +++ b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp @@ -88,8 +88,9 @@ class ConstraintSamplerManagerLoader::Helper ConstraintSamplerManagerLoader::ConstraintSamplerManagerLoader( const constraint_samplers::ConstraintSamplerManagerPtr& csm) - : constraint_sampler_manager_(csm ? csm : constraint_samplers::ConstraintSamplerManagerPtr( - new constraint_samplers::ConstraintSamplerManager())) + : constraint_sampler_manager_( + csm ? csm : + constraint_samplers::ConstraintSamplerManagerPtr(new constraint_samplers::ConstraintSamplerManager())) , impl_(new Helper(constraint_sampler_manager_)) { } diff --git a/moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt b/moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt index ebcc035b60..48ce472264 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt +++ b/moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt @@ -4,5 +4,8 @@ add_library(${MOVEIT_LIB_NAME} src/kinematics_plugin_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} moveit_rdf_loader ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h index 9bc06594e3..c3e47affc6 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h +++ b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Dave Coleman */ -#ifndef MOVEIT_KINEMATICS_PLUGIN_LOADER_ -#define MOVEIT_KINEMATICS_PLUGIN_LOADER_ +#pragma once #include #include @@ -44,7 +43,7 @@ namespace kinematics_plugin_loader { -MOVEIT_CLASS_FORWARD(KinematicsPluginLoader); +MOVEIT_CLASS_FORWARD(KinematicsPluginLoader); // Defines KinematicsPluginLoaderPtr, ConstPtr, WeakPtr... etc /** \brief Helper class for loading kinematics solvers */ class KinematicsPluginLoader @@ -68,7 +67,7 @@ class KinematicsPluginLoader parameter under which the robot description can be found. This is passed to the kinematics solver initialization as well as used to read the SRDF document when needed. */ - KinematicsPluginLoader(const std::string& solver_plugin, double solve_timeout, unsigned int ik_attempts, + KinematicsPluginLoader(const std::string& solver_plugin, double solve_timeout, unsigned int /*ik_attempts*/, const std::string& robot_description = "robot_description", double default_search_resolution = 0.0) : robot_description_(robot_description) @@ -80,11 +79,11 @@ class KinematicsPluginLoader /** \brief Get a function pointer that allocates and initializes a kinematics solver. If not previously called, this * function reads the SRDF and calls the variant below. */ - robot_model::SolverAllocatorFn getLoaderFunction(); + moveit::core::SolverAllocatorFn getLoaderFunction(); /** \brief Get a function pointer that allocates and initializes a kinematics solver. If not previously called, this * function reads ROS parameters for the groups defined in the SRDF. */ - robot_model::SolverAllocatorFn getLoaderFunction(const srdf::ModelSharedPtr& srdf_model); + moveit::core::SolverAllocatorFn getLoaderFunction(const srdf::ModelSharedPtr& srdf_model); /** \brief Get the groups for which the function pointer returned by getLoaderFunction() can allocate a solver */ const std::vector& getKnownGroups() const @@ -104,7 +103,7 @@ class KinematicsPluginLoader std::string robot_description_; double default_search_resolution_; - MOVEIT_CLASS_FORWARD(KinematicsLoaderImpl); + MOVEIT_CLASS_FORWARD(KinematicsLoaderImpl); // Defines KinematicsLoaderImplPtr, ConstPtr, WeakPtr... etc KinematicsLoaderImplPtr loader_; std::vector groups_; @@ -114,6 +113,4 @@ class KinematicsPluginLoader std::string default_solver_plugin_; double default_solver_timeout_; }; -} - -#endif +} // namespace kinematics_plugin_loader diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index 6756b71f90..a5c75bc2a4 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -84,7 +84,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl * \param jmg - joint model group pointer * \return tips - list of valid links in a planning group to plan for */ - std::vector chooseTipFrames(const robot_model::JointModelGroup* jmg) + std::vector chooseTipFrames(const moveit::core::JointModelGroup* jmg) { std::vector tips; std::map >::const_iterator ik_it = @@ -124,7 +124,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl return tips; } - kinematics::KinematicsBasePtr allocKinematicsSolver(const robot_model::JointModelGroup* jmg) + kinematics::KinematicsBasePtr allocKinematicsSolver(const moveit::core::JointModelGroup* jmg) { kinematics::KinematicsBasePtr result; if (!kinematics_loader_) @@ -137,7 +137,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl ROS_ERROR_NAMED(LOGNAME, "Specified group is NULL. Cannot allocate kinematics solver."); return result; } - const std::vector& links = jmg->getLinkModels(); + const std::vector& links = jmg->getLinkModels(); if (links.empty()) { ROS_ERROR_NAMED(LOGNAME, "No links specified for group '%s'. Cannot allocate kinematics solver.", @@ -211,7 +211,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl // cache solver between two consecutive calls // first call in RobotModelLoader::loadKinematicsSolvers() is just to check suitability for jmg // second call in JointModelGroup::setSolverAllocators() is to actually retrieve the instance for use - kinematics::KinematicsBasePtr allocKinematicsSolverWithCache(const robot_model::JointModelGroup* jmg) + kinematics::KinematicsBasePtr allocKinematicsSolverWithCache(const moveit::core::JointModelGroup* jmg) { boost::mutex::scoped_lock slock(cache_lock_); kinematics::KinematicsBasePtr& cached = instances_[jmg]; @@ -239,7 +239,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl std::map > iksolver_to_tip_links_; // a map between each ik solver and a vector // of custom-specified tip link(s) std::shared_ptr > kinematics_loader_; - std::map instances_; + std::map instances_; boost::mutex lock_; boost::mutex cache_lock_; }; @@ -252,7 +252,7 @@ void KinematicsPluginLoader::status() const ROS_INFO_NAMED(LOGNAME, "Loader function was never required"); } -robot_model::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction() +moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction() { moveit::tools::Profiler::ScopedStart prof_start; moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction"); @@ -265,7 +265,7 @@ robot_model::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction() return getLoaderFunction(rml.getSRDF()); } -robot_model::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const srdf::ModelSharedPtr& srdf_model) +moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const srdf::ModelSharedPtr& srdf_model) { moveit::tools::Profiler::ScopedStart prof_start; moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction(SRDF)"); @@ -346,10 +346,12 @@ robot_model::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const s // TODO: Remove in future release (deprecated in PR #1288, Jan-2019, Melodic) std::string ksolver_attempts_param_name; - if (nh.searchParam(base_param_name + "/kinematics_solver_attempts", ksolver_attempts_param_name)) + if (nh.searchParam(base_param_name + "/kinematics_solver_attempts", ksolver_attempts_param_name) && + nh.hasParam(ksolver_attempts_param_name)) { - ROS_WARN_ONCE_NAMED(LOGNAME, "Kinematics solver doesn't support #attempts anymore, but only a timeout.\n" - "Please remove the parameter '%s' from your configuration.", + ROS_WARN_ONCE_NAMED(LOGNAME, + "Kinematics solver doesn't support #attempts anymore, but only a timeout.\n" + "Please remove the parameter '%s' from your configuration.", ksolver_attempts_param_name.c_str()); } @@ -405,8 +407,9 @@ robot_model::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const s { if (ksolver_ik_links.getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_WARN_STREAM_NAMED(LOGNAME, "rosparam '" << ksolver_ik_links_param_name << "' should be an XmlRpc " - "value type 'Array'"); + ROS_WARN_STREAM_NAMED(LOGNAME, "rosparam '" << ksolver_ik_links_param_name + << "' should be an XmlRpc " + "value type 'Array'"); } else { diff --git a/moveit_ros/planning/launch/collision_checker_compare.launch b/moveit_ros/planning/launch/collision_checker_compare.launch new file mode 100644 index 0000000000..1c180c13e6 --- /dev/null +++ b/moveit_ros/planning/launch/collision_checker_compare.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index ee9e4f923e..9eb394c6f0 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -1,14 +1,14 @@ moveit_ros_planning - 1.0.1 - Planning components of MoveIt! that use ROS + 1.1.1 + Planning components of MoveIt that use ROS Ioan Sucan Sachin Chitta Michael Ferguson Michael GΓΆrner - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/planning/plan_execution/CMakeLists.txt b/moveit_ros/planning/plan_execution/CMakeLists.txt index 6d2a0c24fe..6b579cfb11 100644 --- a/moveit_ros/planning/plan_execution/CMakeLists.txt +++ b/moveit_ros/planning/plan_execution/CMakeLists.txt @@ -11,5 +11,8 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/plan_execution/cfg/PlanExecutionDynamicReconfigure.cfg b/moveit_ros/planning/plan_execution/cfg/PlanExecutionDynamicReconfigure.cfg index fd8be081d6..cfc09ea25a 100755 --- a/moveit_ros/planning/plan_execution/cfg/PlanExecutionDynamicReconfigure.cfg +++ b/moveit_ros/planning/plan_execution/cfg/PlanExecutionDynamicReconfigure.cfg @@ -4,6 +4,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("max_replan_attempts", int_t, 1, "Set the maximum number of times a sensor can be pointed to parts of the environment doring a motion plan", 5, 0, 1000) -gen.add("record_trajectory_state_frequency", double_t, 6, "The frequency at which to record states when monitoring trajectories", 10.0, 1.0, 1000.0) +gen.add("record_trajectory_state_frequency", double_t, 6, "The frequency at which to record states when monitoring trajectories", 0.0, 0.0, 1000.0) exit(gen.generate(PACKAGE, PACKAGE, "PlanExecutionDynamicReconfigure")) diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h index e673c04031..c428071beb 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLAN_EXECUTION_PLAN_EXECUTION_ -#define MOVEIT_PLAN_EXECUTION_PLAN_EXECUTION_ +#pragma once #include #include @@ -48,7 +47,7 @@ /** \brief This namespace includes functionality specific to the execution and monitoring of motion plans */ namespace plan_execution { -MOVEIT_CLASS_FORWARD(PlanExecution); +MOVEIT_CLASS_FORWARD(PlanExecution); // Defines PlanExecutionPtr, ConstPtr, WeakPtr... etc class PlanExecution { @@ -142,7 +141,6 @@ class PlanExecution private: void planAndExecuteHelper(ExecutableMotionPlan& plan, const Options& opt); - bool isRemainingPathValid(const ExecutableMotionPlan& plan); bool isRemainingPathValid(const ExecutableMotionPlan& plan, const std::pair& path_segment); void planningSceneUpdatedCallback(const planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type); @@ -165,5 +163,4 @@ class PlanExecution class DynamicReconfigureImpl; DynamicReconfigureImpl* reconfigure_impl_; }; -} -#endif +} // namespace plan_execution diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h index c400874990..8e2c728ccb 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLAN_EXECUTION_PLAN_REPRESENTATION_ -#define MOVEIT_PLAN_EXECUTION_PLAN_REPRESENTATION_ +#pragma once #include #include @@ -53,8 +52,12 @@ struct ExecutableTrajectory { } - ExecutableTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, const std::string& description) - : trajectory_(trajectory), description_(description), trajectory_monitoring_(true) + ExecutableTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, const std::string& description, + std::vector controller_names = {}) + : trajectory_(trajectory) + , description_(description) + , trajectory_monitoring_(true) + , controller_names_(std::move(controller_names)) { } @@ -63,6 +66,7 @@ struct ExecutableTrajectory bool trajectory_monitoring_; collision_detection::AllowedCollisionMatrixConstPtr allowed_collision_matrix_; boost::function effect_on_success_; + std::vector controller_names_; }; /// A generic representation on what a computed motion plan looks like @@ -81,6 +85,5 @@ struct ExecutableMotionPlan }; /// The signature of a function that can compute a motion plan -typedef boost::function ExecutableMotionPlanComputationFn; -} -#endif +using ExecutableMotionPlanComputationFn = boost::function; +} // namespace plan_execution diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h index 55a7d57f8a..d05760afa0 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLAN_EXECUTION_PLAN_WITH_SENSING_ -#define MOVEIT_PLAN_EXECUTION_PLAN_WITH_SENSING_ +#pragma once #include #include @@ -49,7 +48,7 @@ namespace plan_execution { -MOVEIT_CLASS_FORWARD(PlanWithSensing); +MOVEIT_CLASS_FORWARD(PlanWithSensing); // Defines PlanWithSensingPtr, ConstPtr, WeakPtr... etc class PlanWithSensing { @@ -134,5 +133,4 @@ class PlanWithSensing class DynamicReconfigureImpl; DynamicReconfigureImpl* reconfigure_impl_; }; -} -#endif +} // namespace plan_execution diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index afd1694f00..f4ef67a931 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -58,7 +59,7 @@ class PlanExecution::DynamicReconfigureImpl } private: - void dynamicReconfigureCallback(PlanExecutionDynamicReconfigureConfig& config, uint32_t level) + void dynamicReconfigureCallback(PlanExecutionDynamicReconfigureConfig& config, uint32_t /*level*/) { owner_->setMaxReplanAttempts(config.max_replan_attempts); owner_->setTrajectoryStateRecordingFrequency(config.record_trajectory_state_frequency); @@ -141,7 +142,7 @@ void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan& plan, c void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan& plan, const moveit_msgs::PlanningScene& scene_diff, const Options& opt) { - if (planning_scene::PlanningScene::isEmpty(scene_diff)) + if (moveit::core::isEmpty(scene_diff)) planAndExecute(plan, opt); else { @@ -197,8 +198,7 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p plan.error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN || plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA) { - if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA && - opt.replan_delay_ > 0.0) + if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA && opt.replan_delay_ > 0.0) { ros::WallDuration d(opt.replan_delay_); d.sleep(); @@ -220,7 +220,7 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p if (preempt_requested_) break; - // execute the trajectory, and monitor its executionm + // execute the trajectory, and monitor its execution plan.error_code_ = executeAndMonitor(plan); } @@ -261,13 +261,6 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p getErrorCodeString(plan.error_code_).c_str()); } -bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionPlan& plan) -{ - // check the validity of the currently executed path segment only, since there could be - // changes in the world in between path segments - return isRemainingPathValid(plan, trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex()); -} - bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionPlan& plan, const std::pair& path_segment) { @@ -295,10 +288,6 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP if (res.collision || !plan.planning_scene_->isStateFeasible(t.getWayPoint(i), false)) { - // Dave's debacle - ROS_INFO_NAMED("plan_execution", "Trajectory component '%s' is invalid", - plan.plan_components_[path_segment.first].description_.c_str()); - // call the same functions again, in verbose mode, to show what issues have been detected plan.planning_scene_->isStateFeasible(t.getWayPoint(i), true); req.verbose = true; @@ -380,7 +369,7 @@ moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(E // convert to message, pass along moveit_msgs::RobotTrajectory msg; plan.plan_components_[i].trajectory_->getRobotTrajectoryMsg(msg); - if (!trajectory_execution_manager_->push(msg)) + if (!trajectory_execution_manager_->push(msg, plan.plan_components_[i].controller_names_)) { trajectory_execution_manager_->clear(); ROS_ERROR_STREAM_NAMED("plan_execution", "Apparently trajectory initialization failed"); @@ -391,17 +380,22 @@ moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(E } if (!trajectory_monitor_ && planning_scene_monitor_->getStateMonitor()) - trajectory_monitor_.reset( - new planning_scene_monitor::TrajectoryMonitor(planning_scene_monitor_->getStateMonitor())); + { + // Pass current value of reconfigurable parameter plan_execution/record_trajectory_state_frequency + double sampling_frequency = 0.0; + node_handle_.getParam("plan_execution/record_trajectory_state_frequency", sampling_frequency); + trajectory_monitor_ = std::make_shared( + planning_scene_monitor_->getStateMonitor(), sampling_frequency); + } // start recording trajectory states if (trajectory_monitor_) trajectory_monitor_->startTrajectoryMonitor(); // start a trajectory execution thread - trajectory_execution_manager_->execute( - boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1), - boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, _1)); + trajectory_execution_manager_->execute(boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1), + boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, + _1)); // wait for path to be done, while checking that the path does not become invalid ros::Rate r(100); path_became_invalid_ = false; @@ -412,8 +406,11 @@ moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(E if (new_scene_update_) { new_scene_update_ = false; - if (!isRemainingPathValid(plan)) + std::pair current_index = trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex(); + if (!isRemainingPathValid(plan, current_index)) { + ROS_INFO_NAMED("plan_execution", "Trajectory component '%s' is invalid after scene update", + plan.plan_components_[current_index.first].description_.c_str()); path_became_invalid_ = true; break; } @@ -481,7 +478,7 @@ void plan_execution::PlanExecution::planningSceneUpdatedCallback( } void plan_execution::PlanExecution::doneWithTrajectoryExecution( - const moveit_controller_manager::ExecutionStatus& status) + const moveit_controller_manager::ExecutionStatus& /*status*/) { execution_complete_ = true; } @@ -511,7 +508,12 @@ void plan_execution::PlanExecution::successfulTrajectorySegmentExecution(const E if (index < plan->plan_components_.size() && plan->plan_components_[index].trajectory_ && !plan->plan_components_[index].trajectory_->empty()) { - if (!isRemainingPathValid(*plan, std::make_pair(index, 0))) + std::pair next_index(static_cast(index), 0); + if (!isRemainingPathValid(*plan, next_index)) + { + ROS_INFO_NAMED("plan_execution", "Upcoming trajectory component '%s' is invalid", + plan->plan_components_[next_index.first].description_.c_str()); path_became_invalid_ = true; + } } } diff --git a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp index 6c62e6d411..1ef3147279 100644 --- a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp @@ -57,7 +57,7 @@ class PlanWithSensing::DynamicReconfigureImpl } private: - void dynamicReconfigureCallback(SenseForPlanDynamicReconfigureConfig& config, uint32_t level) + void dynamicReconfigureCallback(SenseForPlanDynamicReconfigureConfig& config, uint32_t /*level*/) { owner_->setMaxSafePathCost(config.max_safe_path_cost); owner_->setMaxCostSources(config.max_cost_sources); @@ -150,7 +150,7 @@ bool plan_execution::PlanWithSensing::computePlan(ExecutableMotionPlan& plan, unsigned int look_attempts = 0; // this flag is set to true when all conditions for looking around are met, and the command is sent. - // the intention is for the planning looop not to terminate when having just looked around + // the intention is for the planning loop not to terminate when having just looked around bool just_looked_around = false; // this flag indicates whether the last lookAt() operation failed. If this operation fails once, we assume that @@ -212,7 +212,7 @@ bool plan_execution::PlanWithSensing::computePlan(ExecutableMotionPlan& plan, bool looked_at_result = lookAt(cost_sources, plan.planning_scene_->getPlanningFrame()); if (looked_at_result) - ROS_INFO("Sensor was succesfully actuated. Attempting to recompute a motion plan."); + ROS_INFO("Sensor was successfully actuated. Attempting to recompute a motion plan."); else { if (look_around_failed) @@ -248,7 +248,7 @@ bool plan_execution::PlanWithSensing::lookAt(const std::set + + + +

+ +The results of the speed test are printed to the console. Their unit is collision checks per second. A current overview over the results is given in the table below. Note that absolute values are not relevant because they are depending on the computing power of the local machine but the relative differences between Bullet and FCL should be reproducible. + +| Collision environment | Bullet | FCL | +| :--------------- |------------:| ----:| +| Robot self check, no collision | 220,000 | 110,000 | +| World clutter 100 meshes, no collision | 37,000 | 38,000 | +| World clutter 100 meshes, 4 in collision | 8,000 | 3,000 | diff --git a/moveit_ros/planning/planning_components_tools/images/collision_speed_100_box_collision.png b/moveit_ros/planning/planning_components_tools/images/collision_speed_100_box_collision.png new file mode 100644 index 0000000000..463d4d5f10 Binary files /dev/null and b/moveit_ros/planning/planning_components_tools/images/collision_speed_100_box_collision.png differ diff --git a/moveit_ros/planning/planning_components_tools/images/collision_speed_100_meshes_collision.png b/moveit_ros/planning/planning_components_tools/images/collision_speed_100_meshes_collision.png new file mode 100644 index 0000000000..bc5d8b61fc Binary files /dev/null and b/moveit_ros/planning/planning_components_tools/images/collision_speed_100_meshes_collision.png differ diff --git a/moveit_ros/planning/planning_components_tools/images/collision_speed_100_meshes_no_collision.png b/moveit_ros/planning/planning_components_tools/images/collision_speed_100_meshes_no_collision.png new file mode 100644 index 0000000000..d2f99f939a Binary files /dev/null and b/moveit_ros/planning/planning_components_tools/images/collision_speed_100_meshes_no_collision.png differ diff --git a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp new file mode 100644 index 0000000000..c658792d55 --- /dev/null +++ b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp @@ -0,0 +1,372 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Jens Petit + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jens Petit */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +static const std::string ROBOT_DESCRIPTION = "robot_description"; + +/** \brief Factor to compute the maximum number of trials random clutter generation. */ +static const int MAX_SEARCH_FACTOR_CLUTTER = 3; + +/** \brief Factor to compute the maximum number of trials for random state generation. */ +static const int MAX_SEARCH_FACTOR_STATES = 30; + +/** \brief Defines a random robot state. */ +enum class RobotStateSelector +{ + IN_COLLISION, + NOT_IN_COLLISION, + RANDOM, +}; + +/** \brief Enumerates the available collision detectors. */ +enum class CollisionDetector +{ + FCL, + BULLET, +}; + +/** \brief Enumerates the different types of collision objects. */ +enum class CollisionObjectType +{ + MESH, + BOX, +}; + +/** \brief Clutters the world of the planning scene with random objects in a certain area around the origin. All added + * objects are not in collision with the robot. + * + * \param planning_scene The planning scene + * \param num_objects The number of objects to be cluttered + * \param CollisionObjectType Type of object to clutter (mesh or box) */ +void clutterWorld(const planning_scene::PlanningScenePtr& planning_scene, const size_t num_objects, + CollisionObjectType type) +{ + ROS_INFO("Cluttering scene..."); + + random_numbers::RandomNumberGenerator num_generator = random_numbers::RandomNumberGenerator(123); + + // allow all robot links to be in collision for world check + collision_detection::AllowedCollisionMatrix acm{ collision_detection::AllowedCollisionMatrix( + planning_scene->getRobotModel()->getLinkModelNames(), true) }; + + // set the robot state to home position + moveit::core::RobotState& current_state{ planning_scene->getCurrentStateNonConst() }; + collision_detection::CollisionRequest req; + current_state.setToDefaultValues(current_state.getJointModelGroup("panda_arm"), "home"); + current_state.update(); + + // load panda link5 as world collision object + std::string name; + shapes::ShapeConstPtr shape; + std::string kinect = "package://moveit_resources_panda_description/meshes/collision/link5.stl"; + + Eigen::Quaterniond quat; + Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() }; + + size_t added_objects{ 0 }; + size_t i{ 0 }; + // create random objects until as many added as desired or quit if too many attempts + while (added_objects < num_objects && i < num_objects * MAX_SEARCH_FACTOR_CLUTTER) + { + // add with random size and random position + pos.translation().x() = num_generator.uniformReal(-1.0, 1.0); + pos.translation().y() = num_generator.uniformReal(-1.0, 1.0); + pos.translation().z() = num_generator.uniformReal(0.0, 1.0); + + quat.x() = num_generator.uniformReal(-1.0, 1.0); + quat.y() = num_generator.uniformReal(-1.0, 1.0); + quat.z() = num_generator.uniformReal(-1.0, 1.0); + quat.w() = num_generator.uniformReal(-1.0, 1.0); + quat.normalize(); + pos.rotate(quat); + + switch (type) + { + case CollisionObjectType::MESH: + { + shapes::Mesh* mesh = shapes::createMeshFromResource(kinect); + mesh->scale(num_generator.uniformReal(0.3, 1.0)); + shape.reset(mesh); + name = "mesh"; + break; + } + case CollisionObjectType::BOX: + { + shape.reset(new shapes::Box(num_generator.uniformReal(0.05, 0.2), num_generator.uniformReal(0.05, 0.2), + num_generator.uniformReal(0.05, 0.2))); + name = "box"; + break; + } + } + + name.append(std::to_string(i)); + planning_scene->getWorldNonConst()->addToObject(name, shape, pos); + + // try if it isn't in collision if yes, ok, if no remove. + collision_detection::CollisionResult res; + planning_scene->checkCollision(req, res, current_state, acm); + + if (!res.collision) + { + added_objects++; + } + else + { + ROS_DEBUG_STREAM("Object was in collision, remove"); + planning_scene->getWorldNonConst()->removeObject(name); + } + + i++; + } + ROS_INFO_STREAM("Cluttered the planning scene with " << added_objects << " objects"); +} + +/** \brief Runs a collision detection benchmark and measures the time. + * + * \param trials The number of repeated collision checks for each state + * \param scene The planning scene + * \param CollisionDetector The type of collision detector + * \param only_self Flag for only self collision check performed */ +void runCollisionDetection(unsigned int trials, const planning_scene::PlanningScenePtr& scene, + const std::vector& states, const CollisionDetector col_detector, + bool only_self, bool distance = false) +{ + collision_detection::AllowedCollisionMatrix acm{ collision_detection::AllowedCollisionMatrix( + scene->getRobotModel()->getLinkModelNames(), true) }; + + ROS_INFO_STREAM("Starting detection using " << (col_detector == CollisionDetector::FCL ? "FCL" : "Bullet")); + + if (col_detector == CollisionDetector::FCL) + { + scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); + } + else + { + scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorBullet::create()); + } + + collision_detection::CollisionResult res; + collision_detection::CollisionRequest req; + + req.distance = distance; + // for world collision request detailed information + if (!only_self) + { + req.contacts = true; + req.max_contacts = 99; + req.max_contacts_per_pair = 10; + // If distance is turned on it will slow down the collision checking a lot. Try reducing the + // number of contacts consequently. + // req.distance = true; + } + + ros::WallTime start = ros::WallTime::now(); + for (unsigned int i = 0; i < trials; ++i) + { + for (auto& state : states) + { + res.clear(); + + if (only_self) + { + scene->checkSelfCollision(req, res); + } + else + { + scene->checkCollision(req, res, state); + } + } + } + double duration = (ros::WallTime::now() - start).toSec(); + ROS_INFO("Performed %lf collision checks per second", (double)trials * states.size() / duration); + ROS_INFO_STREAM("Total number was " << trials * states.size() << " checks."); + ROS_INFO_STREAM("We had " << states.size() << " different robot states which were " + << (res.collision ? "in collison " : "not in collision ") << "with " << res.contact_count); + + // color collided objects red + for (auto& contact : res.contacts) + { + ROS_INFO_STREAM("Between: " << contact.first.first << " and " << contact.first.second); + std_msgs::ColorRGBA red; + red.a = 0.8; + red.r = 1; + red.g = 0; + red.b = 0; + scene->setObjectColor(contact.first.first, red); + scene->setObjectColor(contact.first.second, red); + } + + scene->setCurrentState(states.back()); +} + +/** \brief Samples valid states of the robot which can be in collision if desired. + * \param desired_states Specifier for type for desired state + * \param num_states Number of desired states + * \param scene The planning scene + * \param robot_states Result vector */ +void findStates(const RobotStateSelector desired_states, unsigned int num_states, + const planning_scene::PlanningScenePtr& scene, std::vector& robot_states) +{ + moveit::core::RobotState& current_state{ scene->getCurrentStateNonConst() }; + collision_detection::CollisionRequest req; + + size_t i{ 0 }; + while (robot_states.size() < num_states && i < num_states * MAX_SEARCH_FACTOR_STATES) + { + current_state.setToRandomPositions(); + current_state.update(); + collision_detection::CollisionResult res; + scene->checkSelfCollision(req, res); + ROS_INFO_STREAM("Found state " << (res.collision ? "in collision" : "not in collision")); + + switch (desired_states) + { + case RobotStateSelector::IN_COLLISION: + if (res.collision) + robot_states.push_back(current_state); + break; + case RobotStateSelector::NOT_IN_COLLISION: + if (!res.collision) + robot_states.push_back(current_state); + break; + case RobotStateSelector::RANDOM: + robot_states.push_back(current_state); + break; + } + i++; + } + + if (!robot_states.empty()) + { + scene->setCurrentState(robot_states[0]); + } + else + { + ROS_ERROR_STREAM("Did not find any correct states."); + } +} + +int main(int argc, char** argv) +{ + moveit::core::RobotModelPtr robot_model; + ros::init(argc, argv, "compare_collision_checking_speed"); + ros::NodeHandle node_handle; + + ros::Publisher planning_scene_diff_publisher = node_handle.advertise("planning_scene", 1); + + unsigned int trials = 10000; + + ros::AsyncSpinner spinner(1); + spinner.start(); + ros::WallDuration sleep_t(2.5); + + robot_model = moveit::core::loadTestingRobotModel("panda"); + + planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); + planning_scene_monitor::PlanningSceneMonitor psm(planning_scene, ROBOT_DESCRIPTION); + psm.startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); + psm.startSceneMonitor(); + planning_scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorBullet::create()); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + collision_detection::AllowedCollisionMatrix acm{ collision_detection::AllowedCollisionMatrix( + robot_model->getLinkModelNames(), true) }; + planning_scene->checkCollision(req, res, planning_scene->getCurrentState(), acm); + + ROS_INFO("Starting..."); + + if (psm.getPlanningScene()) + { + ros::Duration(0.5).sleep(); + + moveit::core::RobotState& current_state{ planning_scene->getCurrentStateNonConst() }; + current_state.setToDefaultValues(current_state.getJointModelGroup("panda_arm"), "home"); + current_state.update(); + + std::vector sampled_states; + sampled_states.push_back(current_state); + + ROS_INFO("Starting benchmark: Robot in empty world, only self collision check"); + runCollisionDetection(trials, planning_scene, sampled_states, CollisionDetector::BULLET, true); + runCollisionDetection(trials, planning_scene, sampled_states, CollisionDetector::FCL, true); + + planning_scene->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorBullet::create()); + + clutterWorld(planning_scene, 100, CollisionObjectType::MESH); + + ROS_INFO("Starting benchmark: Robot in cluttered world, no collision with world"); + runCollisionDetection(trials, planning_scene, sampled_states, CollisionDetector::BULLET, false); + runCollisionDetection(trials, planning_scene, sampled_states, CollisionDetector::FCL, false); + + // bring the robot into a position which collides with the world clutter + double joint_2 = 1.5; + current_state.setJointPositions("panda_joint2", &joint_2); + current_state.update(); + + std::vector sampled_states_2; + sampled_states_2.push_back(current_state); + + ROS_INFO("Starting benchmark: Robot in cluttered world, in collision with world"); + runCollisionDetection(trials, planning_scene, sampled_states_2, CollisionDetector::BULLET, false); + runCollisionDetection(trials, planning_scene, sampled_states_2, CollisionDetector::FCL, false); + + bool visualize; + node_handle.getParam("/compare_collision_checking_speed/visualization", visualize); + if (visualize) + { + // publishes the planning scene to visualize in rviz if possible + moveit_msgs::PlanningScene planning_scene_msg; + planning_scene->getPlanningSceneMsg(planning_scene_msg); + planning_scene_diff_publisher.publish(planning_scene_msg); + } + } + else + { + ROS_ERROR("Planning scene not configured"); + } + + return 0; +} diff --git a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp index f70c4e0860..ac7bf2c7ec 100644 --- a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp +++ b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp @@ -136,7 +136,7 @@ int main(int argc, char** argv) pub_scene.publish(psmsg); std::cout << psm.getPlanningScene()->getCurrentState() << std::endl; - sleep(1); + ros::Duration(1).sleep(); } } while (nh.ok()); diff --git a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp index c990a30e68..b8530d349e 100644 --- a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp +++ b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp @@ -41,7 +41,7 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; void runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene* scene, - const robot_state::RobotState* state) + const moveit::core::RobotState* state) { ROS_INFO("Starting thread %u", id); collision_detection::CollisionRequest req; @@ -95,12 +95,12 @@ int main(int argc, char** argv) else ros::Duration(0.5).sleep(); - std::vector states; + std::vector states; ROS_INFO("Sampling %u valid states...", nthreads); for (unsigned int i = 0; i < nthreads; ++i) { // sample a valid state - robot_state::RobotState* state = new robot_state::RobotState(psm.getPlanningScene()->getRobotModel()); + moveit::core::RobotState* state = new moveit::core::RobotState(psm.getPlanningScene()->getRobotModel()); collision_detection::CollisionRequest req; do { @@ -111,7 +111,7 @@ int main(int argc, char** argv) if (!res.collision) break; } while (true); - states.push_back(robot_state::RobotStatePtr(state)); + states.push_back(moveit::core::RobotStatePtr(state)); } std::vector threads; diff --git a/moveit_ros/planning/planning_components_tools/src/evaluate_state_operations_speed.cpp b/moveit_ros/planning/planning_components_tools/src/evaluate_state_operations_speed.cpp index d4ab597dcc..d38519c3c7 100644 --- a/moveit_ros/planning/planning_components_tools/src/evaluate_state_operations_speed.cpp +++ b/moveit_ros/planning/planning_components_tools/src/evaluate_state_operations_speed.cpp @@ -51,11 +51,11 @@ int main(int argc, char** argv) robot_model_loader::RobotModelLoader rml(ROBOT_DESCRIPTION); ros::Duration(0.5).sleep(); - robot_model::RobotModelConstPtr robot_model = rml.getModel(); + moveit::core::RobotModelConstPtr robot_model = rml.getModel(); if (robot_model) { static const int N = 10000; - robot_state::RobotState state(robot_model); + moveit::core::RobotState state(robot_model); printf("Evaluating model '%s' using %d trials for each test\n", robot_model->getName().c_str(), N); @@ -80,12 +80,12 @@ int main(int argc, char** argv) moveit::tools::Profiler::End("FK Random"); } - std::vector copies(N, (robot_state::RobotState*)nullptr); + std::vector copies(N, (moveit::core::RobotState*)nullptr); printf("Evaluating Copy State ...\n"); for (int i = 0; i < N; ++i) { moveit::tools::Profiler::Begin("Copy State"); - copies[i] = new robot_state::RobotState(state); + copies[i] = new moveit::core::RobotState(state); moveit::tools::Profiler::End("Copy State"); } @@ -101,7 +101,7 @@ int main(int argc, char** argv) for (const std::string& group : groups) { printf("\n"); - const robot_model::JointModelGroup* jmg = robot_model->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); printf("%s: Evaluating FK Random ...\n", group.c_str()); std::string pname = group + ":FK Random"; diff --git a/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp b/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp index 117136f163..7030c918da 100644 --- a/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp +++ b/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp @@ -56,14 +56,14 @@ int main(int argc, char** argv) std::string group = argv[1]; ROS_INFO_STREAM("Evaluating IK for " << group); - const robot_model::JointModelGroup* jmg = rml.getModel()->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = rml.getModel()->getJointModelGroup(group); if (jmg) { const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); if (solver) { const std::string& tip = solver->getTipFrame(); - robot_state::RobotState state(rml.getModel()); + moveit::core::RobotState state(rml.getModel()); state.setToDefaultValues(); ROS_INFO_STREAM("Tip Frame: " << solver->getTipFrame()); @@ -87,14 +87,16 @@ int main(int argc, char** argv) for (unsigned int i = 0; i < test_count; ++i) { state.setToRandomPositions(jmg); + // getGlobalLinkTransform() returns a valid isometry by contract Eigen::Isometry3d pose = state.getGlobalLinkTransform(tip); state.setToRandomPositions(jmg); moveit::tools::Profiler::Begin("IK"); state.setFromIK(jmg, pose); moveit::tools::Profiler::End("IK"); + // getGlobalLinkTransform() returns a valid isometry by contract const Eigen::Isometry3d& pose_upd = state.getGlobalLinkTransform(tip); - Eigen::Isometry3d diff = pose_upd * pose.inverse(); - double rot_err = (diff.rotation() - Eigen::Matrix3d::Identity()).norm(); + Eigen::Isometry3d diff = pose_upd * pose.inverse(); // valid isometry + double rot_err = (diff.linear() - Eigen::Matrix3d::Identity()).norm(); double trans_err = diff.translation().norm(); moveit::tools::Profiler::Average("Rotation error", rot_err); moveit::tools::Profiler::Average("Translation error", trans_err); diff --git a/moveit_ros/planning/planning_components_tools/src/print_planning_scene_info.cpp b/moveit_ros/planning/planning_components_tools/src/print_planning_scene_info.cpp index 6811f7b4e0..f7f64c761d 100644 --- a/moveit_ros/planning/planning_components_tools/src/print_planning_scene_info.cpp +++ b/moveit_ros/planning/planning_components_tools/src/print_planning_scene_info.cpp @@ -38,7 +38,7 @@ #include -// MoveIt! +// MoveIt #include static const std::string ROBOT_DESCRIPTION = "robot_description"; diff --git a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp index 274e250433..b2f441d5c8 100644 --- a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp +++ b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp @@ -90,7 +90,7 @@ int main(int argc, char** argv) else pub_scene.publish(ps_msg.world); - sleep(1); + ros::Duration(1).sleep(); } } else diff --git a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp index 50d2941091..e162df1bbd 100644 --- a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp +++ b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp @@ -35,7 +35,6 @@ /* Author: Ioan Sucan */ #include -#include #include #include diff --git a/moveit_ros/planning/planning_pipeline/CMakeLists.txt b/moveit_ros/planning/planning_pipeline/CMakeLists.txt index 853d2efb11..abeb034e3a 100644 --- a/moveit_ros/planning/planning_pipeline/CMakeLists.txt +++ b/moveit_ros/planning/planning_pipeline/CMakeLists.txt @@ -4,5 +4,8 @@ add_library(${MOVEIT_LIB_NAME} src/planning_pipeline.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index f08af40a04..543ea0f5b2 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_PIPELINE_PLANNING_PIPELINE_ -#define MOVEIT_PLANNING_PIPELINE_PLANNING_PIPELINE_ +#pragma once #include #include @@ -76,7 +75,7 @@ class PlanningPipeline \param adapter_plugins_param_name The name of the ROS parameter under which the names of the request adapter plugins are specified (plugin names separated by space; order matters) */ - PlanningPipeline(const robot_model::RobotModelConstPtr& model, const ros::NodeHandle& nh = ros::NodeHandle("~"), + PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const ros::NodeHandle& nh = ros::NodeHandle("~"), const std::string& planning_plugin_param_name = "planning_plugin", const std::string& adapter_plugins_param_name = "request_adapters"); @@ -86,7 +85,7 @@ class PlanningPipeline \param planning_plugin_name The name of the planning plugin to load \param adapter_plugins_names The names of the planning request adapter plugins to load */ - PlanningPipeline(const robot_model::RobotModelConstPtr& model, const ros::NodeHandle& nh, + PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const ros::NodeHandle& nh, const std::string& planning_plugin_name, const std::vector& adapter_plugin_names); /** \brief Pass a flag telling the pipeline whether or not to publish the computed motion plans on DISPLAY_PATH_TOPIC. @@ -161,7 +160,7 @@ class PlanningPipeline } /** \brief Get the robot model that this pipeline is using */ - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -188,14 +187,12 @@ class PlanningPipeline std::unique_ptr adapter_chain_; std::vector adapter_plugin_names_; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; /// Flag indicating whether the reported plans should be checked once again, by the planning pipeline itself bool check_solution_paths_; ros::Publisher contacts_publisher_; }; -MOVEIT_CLASS_FORWARD(PlanningPipeline); -} - -#endif +MOVEIT_CLASS_FORWARD(PlanningPipeline); // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc +} // namespace planning_pipeline diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 6bee28fe7e..8f78cbf8b9 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -48,7 +48,7 @@ const std::string planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC = "dis const std::string planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC = "motion_plan_request"; const std::string planning_pipeline::PlanningPipeline::MOTION_CONTACTS_TOPIC = "display_contacts"; -planning_pipeline::PlanningPipeline::PlanningPipeline(const robot_model::RobotModelConstPtr& model, +planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const ros::NodeHandle& nh, const std::string& planner_plugin_param_name, const std::string& adapter_plugins_param_name) @@ -70,7 +70,7 @@ planning_pipeline::PlanningPipeline::PlanningPipeline(const robot_model::RobotMo configure(); } -planning_pipeline::PlanningPipeline::PlanningPipeline(const robot_model::RobotModelConstPtr& model, +planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const ros::NodeHandle& nh, const std::string& planner_plugin_name, const std::vector& adapter_plugin_names) : nh_(nh), planner_plugin_name_(planner_plugin_name), adapter_plugin_names_(adapter_plugin_names), robot_model_(model) @@ -298,7 +298,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla for (std::size_t it : index) { // check validity with verbose on - const robot_state::RobotState& robot_state = res.trajectory_->getWayPoint(it); + const moveit::core::RobotState& robot_state = res.trajectory_->getWayPoint(it); planning_scene->isStateValid(robot_state, req.path_constraints, req.group_name, true); // compute the contacts if any @@ -338,7 +338,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla disp.model_id = robot_model_->getName(); disp.trajectory.resize(1); res.trajectory_->getRobotTrajectoryMsg(disp.trajectory[0]); - robot_state::robotStateToRobotStateMsg(res.trajectory_->getFirstWayPoint(), disp.trajectory_start); + moveit::core::robotStateToRobotStateMsg(res.trajectory_->getFirstWayPoint(), disp.trajectory_start); display_path_publisher_.publish(disp); } @@ -349,7 +349,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla bool stacked_constraints = false; if (req.path_constraints.position_constraints.size() > 1 || req.path_constraints.orientation_constraints.size() > 1) stacked_constraints = true; - for (auto constraint : req.goal_constraints) + for (const auto& constraint : req.goal_constraints) { if (constraint.position_constraints.size() > 1 || constraint.orientation_constraints.size() > 1) stacked_constraints = true; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt index f3a9dbae8d..b32ffe5349 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt +++ b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt @@ -19,6 +19,10 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_executable(moveit_list_request_adapter_plugins src/list.cpp) target_link_libraries(moveit_list_request_adapter_plugins ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} moveit_list_request_adapter_plugins +install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +install(TARGETS moveit_list_request_adapter_plugins RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp index 6904160457..716890f341 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp @@ -49,7 +49,7 @@ class AddIterativeSplineParameterization : public planning_request_adapter::Plan { } - void initialize(const ros::NodeHandle& nh) override + void initialize(const ros::NodeHandle& /*nh*/) override { } @@ -60,7 +60,7 @@ class AddIterativeSplineParameterization : public planning_request_adapter::Plan bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { bool result = planner(planning_scene, req, res); if (result && res.trajectory_) @@ -68,7 +68,10 @@ class AddIterativeSplineParameterization : public planning_request_adapter::Plan ROS_DEBUG("Running '%s'", getDescription().c_str()); if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) - ROS_WARN("Time parametrization for the solution path failed."); + { + ROS_ERROR("Time parametrization for the solution path failed."); + result = false; + } } return result; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp index 805e6b7329..c72f20b854 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -51,7 +51,7 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning { } - void initialize(const ros::NodeHandle& nh) override + void initialize(const ros::NodeHandle& /*nh*/) override { } @@ -62,7 +62,7 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { bool result = planner(planning_scene, req, res); if (result && res.trajectory_) @@ -71,7 +71,10 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning TimeOptimalTrajectoryGeneration totg; if (!totg.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) - ROS_WARN("Time parametrization for the solution path failed."); + { + ROS_ERROR("Time parametrization for the solution path failed."); + result = false; + } } return result; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp index be06624689..dced1a8d87 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp @@ -48,7 +48,7 @@ class AddTimeParameterization : public planning_request_adapter::PlanningRequest { } - void initialize(const ros::NodeHandle& nh) override + void initialize(const ros::NodeHandle& /*nh*/) override { } @@ -59,7 +59,7 @@ class AddTimeParameterization : public planning_request_adapter::PlanningRequest bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { bool result = planner(planning_scene, req, res); if (result && res.trajectory_) @@ -67,7 +67,10 @@ class AddTimeParameterization : public planning_request_adapter::PlanningRequest ROS_DEBUG("Running '%s'", getDescription().c_str()); if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) - ROS_WARN("Time parametrization for the solution path failed."); + { + ROS_ERROR("Time parametrization for the solution path failed."); + result = false; + } } return result; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp index 92a48ca821..7bb0555e76 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp @@ -49,12 +49,12 @@ class Empty : public planning_request_adapter::PlanningRequestAdapter bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { return planner(planning_scene, req, res); } - void initialize(const ros::NodeHandle& node_handle) override + void initialize(const ros::NodeHandle& /*nh*/) override { } }; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp index ea3f09f0ad..83ecdda130 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp @@ -84,10 +84,10 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap ROS_DEBUG("Running '%s'", getDescription().c_str()); // get the specified start state - robot_state::RobotState start_state = planning_scene->getCurrentState(); - robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); + moveit::core::RobotState start_state = planning_scene->getCurrentState(); + moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); - const std::vector& jmodels = + const std::vector& jmodels = planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ? planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() : planning_scene->getRobotModel()->getJointModels(); @@ -101,9 +101,9 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap // how many times the joint was wrapped. Because of this, we remember the offsets for continuous // joints, and we un-do them when the plan comes from the planner - if (jm->getType() == robot_model::JointModel::REVOLUTE) + if (jm->getType() == moveit::core::JointModel::REVOLUTE) { - if (static_cast(jm)->isContinuous()) + if (static_cast(jm)->isContinuous()) { double initial = start_state.getJointPositions(jm)[0]; start_state.enforceBounds(jm); @@ -114,11 +114,11 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } else // Normalize yaw; no offset needs to be remembered - if (jm->getType() == robot_model::JointModel::PLANAR) + if (jm->getType() == moveit::core::JointModel::PLANAR) { const double* p = start_state.getJointPositions(jm); double copy[3] = { p[0], p[1], p[2] }; - if (static_cast(jm)->normalizeRotation(copy)) + if (static_cast(jm)->normalizeRotation(copy)) { start_state.setJointPositions(jm, copy); change_req = true; @@ -126,11 +126,11 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } else // Normalize quaternions - if (jm->getType() == robot_model::JointModel::FLOATING) + if (jm->getType() == moveit::core::JointModel::FLOATING) { const double* p = start_state.getJointPositions(jm); double copy[7] = { p[0], p[1], p[2], p[3], p[4], p[5], p[6] }; - if (static_cast(jm)->normalizeRotation(copy)) + if (static_cast(jm)->normalizeRotation(copy)) { start_state.setJointPositions(jm, copy); change_req = true; @@ -139,7 +139,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } // pointer to a prefix state we could possibly add, if we detect we have to make changes - robot_state::RobotStatePtr prefix_state; + moveit::core::RobotStatePtr prefix_state; for (const moveit::core::JointModel* jmodel : jmodels) { if (!start_state.satisfiesBounds(jmodel)) @@ -147,7 +147,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap if (start_state.satisfiesBounds(jmodel, bounds_dist_)) { if (!prefix_state) - prefix_state.reset(new robot_state::RobotState(start_state)); + prefix_state.reset(new moveit::core::RobotState(start_state)); start_state.enforceBounds(jmodel); change_req = true; ROS_INFO("Starting state is just outside bounds (joint '%s'). Assuming within bounds.", @@ -161,7 +161,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap const double* p = start_state.getJointPositions(jmodel); for (std::size_t k = 0; k < jmodel->getVariableCount(); ++k) joint_values << p[k] << " "; - const robot_model::JointModel::Bounds& b = jmodel->getVariableBounds(); + const moveit::core::JointModel::Bounds& b = jmodel->getVariableBounds(); for (const moveit::core::VariableBounds& variable_bounds : b) { joint_bounds_low << variable_bounds.min_position_ << " "; @@ -181,7 +181,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap if (change_req) { planning_interface::MotionPlanRequest req2 = req; - robot_state::robotStateToRobotStateMsg(start_state, req2.start_state, false); + moveit::core::robotStateToRobotStateMsg(start_state, req2.start_state); solved = planner(planning_scene, req2, res); } else @@ -192,8 +192,8 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap { // heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to // the computed trajectory) - res.trajectory_->setWayPointDurationFromPrevious( - 0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration())); + res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, + res.trajectory_->getAverageSegmentDuration())); res.trajectory_->addPrefixWayPoint(prefix_state, 0.0); // we add a prefix point, so we need to bump any previously added index positions for (std::size_t& added_index : added_path_index) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp index 02768c02e8..754d521260 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp @@ -99,8 +99,8 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA ROS_DEBUG("Running '%s'", getDescription().c_str()); // get the specified start state - robot_state::RobotState start_state = planning_scene->getCurrentState(); - robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); + moveit::core::RobotState start_state = planning_scene->getCurrentState(); + moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); collision_detection::CollisionRequest creq; creq.group_name = req.group_name; @@ -119,10 +119,10 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA else ROS_INFO_STREAM("Start state appears to be in collision with respect to group " << creq.group_name); - robot_state::RobotStatePtr prefix_state(new robot_state::RobotState(start_state)); + moveit::core::RobotStatePtr prefix_state(new moveit::core::RobotState(start_state)); random_numbers::RandomNumberGenerator& rng = prefix_state->getRandomNumberGenerator(); - const std::vector& jmodels = + const std::vector& jmodels = planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ? planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() : planning_scene->getRobotModel()->getJointModels(); @@ -151,14 +151,14 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA if (found) { planning_interface::MotionPlanRequest req2 = req; - robot_state::robotStateToRobotStateMsg(start_state, req2.start_state); + moveit::core::robotStateToRobotStateMsg(start_state, req2.start_state); bool solved = planner(planning_scene, req2, res); if (solved && !res.trajectory_->empty()) { // heuristically decide a duration offset for the trajectory (induced by the additional point added as a // prefix to the computed trajectory) - res.trajectory_->setWayPointDurationFromPrevious( - 0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration())); + res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, + res.trajectory_->getAverageSegmentDuration())); res.trajectory_->addPrefixWayPoint(prefix_state, 0.0); // we add a prefix point, so we need to bump any previously added index positions for (std::size_t& added_index : added_path_index) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp index 3d5f5b3135..c7cdd212bf 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp @@ -50,7 +50,7 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe { } - void initialize(const ros::NodeHandle& nh) override + void initialize(const ros::NodeHandle& /*nh*/) override { } @@ -66,8 +66,8 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe ROS_DEBUG("Running '%s'", getDescription().c_str()); // get the specified start state - robot_state::RobotState start_state = planning_scene->getCurrentState(); - robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); + moveit::core::RobotState start_state = planning_scene->getCurrentState(); + moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); // if the start state is otherwise valid but does not meet path constraints if (planning_scene->isStateValid(start_state, req.group_name) && @@ -95,7 +95,7 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe ROS_INFO("Planned to path constraints. Resuming original planning request."); // extract the last state of the computed motion plan and set it as the new start state - robot_state::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state); + moveit::core::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state); bool solved2 = planner(planning_scene, req3, res); res.planning_time_ += res2.planning_time_; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp index fc36ca06f7..ea7586c583 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp @@ -68,7 +68,7 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { ROS_DEBUG("Running '%s'", getDescription().c_str()); const moveit_msgs::WorkspaceParameters& wparams = req.workspace_parameters; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index a8682a5ce6..0c5cdb3dd2 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -48,7 +48,7 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest { } - void initialize(const ros::NodeHandle& nh) override + void initialize(const ros::NodeHandle& /*nh*/) override { } @@ -59,7 +59,7 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + std::vector& /*added_path_index*/) const override { ROS_DEBUG("Running '%s'", getDescription().c_str()); planning_interface::MotionPlanRequest modified = req; diff --git a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt index 9ad23d6175..6329feef36 100644 --- a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt +++ b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt @@ -14,5 +14,8 @@ target_link_libraries(${MOVEIT_LIB_NAME} add_executable(demo_scene demos/demo_scene.cpp) target_link_libraries(demo_scene ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp b/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp index 681d6af0d2..976f1ba678 100644 --- a/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp +++ b/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp @@ -76,7 +76,7 @@ void sendKnife() co.primitive_poses[0].orientation.w = 1.0; pub_aco.publish(aco); - sleep(1); + ros::Duration(1).sleep(); pub_aco.publish(aco); ROS_INFO("Object published."); ros::Duration(1.5).sleep(); diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h index 709586792e..558a3c8c12 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_SCENE_MONITOR_CURRENT_STATE_MONITOR_ -#define MOVEIT_PLANNING_SCENE_MONITOR_CURRENT_STATE_MONITOR_ +#pragma once #include #include @@ -47,13 +46,13 @@ namespace planning_scene_monitor { -typedef boost::function JointStateUpdateCallback; +using JointStateUpdateCallback = boost::function; /** @class CurrentStateMonitor @brief Monitors the joint_states topic and tf to maintain the current state of the robot. */ class CurrentStateMonitor { - typedef boost::signals2::connection TFConnection; + using TFConnection = boost::signals2::connection; public: /** @@ -61,7 +60,7 @@ class CurrentStateMonitor * @param robot_model The current kinematic model to build on * @param tf_buffer A pointer to the tf2_ros Buffer to use */ - CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, + CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer); /** @brief Constructor. @@ -69,7 +68,7 @@ class CurrentStateMonitor * @param tf_buffer A pointer to the tf2_ros Buffer to use * @param nh A ros::NodeHandle to pass node specific options */ - CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, + CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer, const ros::NodeHandle& nh); ~CurrentStateMonitor(); @@ -87,7 +86,7 @@ class CurrentStateMonitor bool isActive() const; /** @brief Get the RobotModel for which we are monitoring state */ - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -121,17 +120,17 @@ class CurrentStateMonitor /** @brief Get the current state * @return Returns the current state */ - robot_state::RobotStatePtr getCurrentState() const; + moveit::core::RobotStatePtr getCurrentState() const; /** @brief Set the state \e upd to the current state maintained by this class. */ - void setToCurrentState(robot_state::RobotState& upd) const; + void setToCurrentState(moveit::core::RobotState& upd) const; /** @brief Get the time stamp for the current state */ ros::Time getCurrentStateTime() const; /** @brief Get the current state and its time stamp * @return Returns a pair of the current state and its time stamp */ - std::pair getCurrentStateAndTime() const; + std::pair getCurrentStateAndTime() const; /** @brief Get the current state values as a map from joint names to joint state values * @return Returns the map from joint names to joint state values*/ @@ -139,7 +138,7 @@ class CurrentStateMonitor /** @brief Wait for at most \e wait_time seconds (default 1s) for a robot state more recent than t * @return true on success, false if up-to-date robot state wasn't received within \e wait_time - */ + */ bool waitForCurrentState(const ros::Time t = ros::Time::now(), double wait_time = 1.0) const; /** @brief Wait for at most \e wait_time seconds until the complete robot state is known. @@ -194,8 +193,8 @@ class CurrentStateMonitor ros::NodeHandle nh_; std::shared_ptr tf_buffer_; - robot_model::RobotModelConstPtr robot_model_; - robot_state::RobotState robot_state_; + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotState robot_state_; std::map joint_time_; bool state_monitor_started_; bool copy_dynamics_; // Copy velocity and effort from joint_state @@ -211,7 +210,5 @@ class CurrentStateMonitor std::shared_ptr tf_connection_; }; -MOVEIT_CLASS_FORWARD(CurrentStateMonitor); -} - -#endif +MOVEIT_CLASS_FORWARD(CurrentStateMonitor); // Defines CurrentStateMonitorPtr, ConstPtr, WeakPtr... etc +} // namespace planning_scene_monitor diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index d03afd28ec..34591d2ceb 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_ -#define MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_ +#pragma once #include #include @@ -46,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -53,7 +53,7 @@ namespace planning_scene_monitor { -MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); +MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); // Defines PlanningSceneMonitorPtr, ConstPtr, WeakPtr... etc /** * @brief PlanningSceneMonitor @@ -170,7 +170,7 @@ class PlanningSceneMonitor : private boost::noncopyable return rm_loader_; } - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -325,19 +325,29 @@ class PlanningSceneMonitor : private boost::noncopyable return 0.0; } - /** @brief Start the scene monitor + /** @brief Start the scene monitor (ROS topic-based) * @param scene_topic The name of the planning scene topic */ void startSceneMonitor(const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC); - /** @brief Request planning scene state using a service call - * @param service_name The name of the service to use for requesting the - * planning scene. This must be a service of type - * moveit_msgs::GetPlanningScene and is usually called - * "/get_planning_scene". + /** @brief Request a full planning scene state using a service call + * Be careful not to use this in conjunction with providePlanningSceneService(), + * as it will create a pointless feedback loop. + * @param service_name The name of the service to use for requesting the planning scene. + * This must be a service of type moveit_msgs::GetPlanningScene and is usually called + * "/get_planning_scene". */ bool requestPlanningSceneState(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE); + /** @brief Create an optional service for getting the complete planning scene + * This is useful for satisfying the Rviz PlanningScene display's need for a service + * without having to use a move_group node. + * Be careful not to use this in conjunction with requestPlanningSceneState(), + * as it will create a pointless feedback loop. + * @param service_name The topic to provide the service + */ + void providePlanningSceneService(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE); + /** @brief Stop the scene monitor*/ void stopSceneMonitor(); @@ -427,7 +437,7 @@ class PlanningSceneMonitor : private boost::noncopyable void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj); /** @brief Callback for a change for an attached object of the current state of the planning scene */ - void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body, bool just_attached); + void currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody* attached_body, bool just_attached); /** @brief Callback for a change in the world maintained by the planning scene */ void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& object, @@ -443,8 +453,8 @@ class PlanningSceneMonitor : private boost::noncopyable void excludeAttachedBodiesFromOctree(); void includeAttachedBodiesInOctree(); - void excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body); - void includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body); + void excludeAttachedBodyFromOctree(const moveit::core::AttachedBody* attached_body); + void includeAttachedBodyInOctree(const moveit::core::AttachedBody* attached_body); bool getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time, occupancy_map_monitor::ShapeTransformCache& cache) const; @@ -496,20 +506,23 @@ class PlanningSceneMonitor : private boost::noncopyable ros::Subscriber attached_collision_object_subscriber_; ros::Subscriber collision_object_subscriber_; + // provide an optional service to get the full planning scene state + // this is used by MoveGroup and related application nodes + ros::ServiceServer get_scene_service_; + // include a octomap monitor std::unique_ptr octomap_monitor_; // include a current state monitor CurrentStateMonitorPtr current_state_monitor_; - typedef std::map > > LinkShapeHandles; - typedef std::map > > - AttachedBodyShapeHandles; - typedef std::map > > - CollisionBodyShapeHandles; + using AttachedBodyShapeHandles = std::map > >; + using CollisionBodyShapeHandles = + std::map > >; LinkShapeHandles link_shape_handles_; AttachedBodyShapeHandles attached_body_shape_handles_; @@ -536,6 +549,10 @@ class PlanningSceneMonitor : private boost::noncopyable // Callback for a new planning scene msg void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene); + // Callback for requesting the full planning scene via service + bool getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req, + moveit_msgs::GetPlanningScene::Response& res); + // Lock for state_update_pending_ and dt_state_update_ boost::mutex state_pending_mutex_; @@ -562,7 +579,7 @@ class PlanningSceneMonitor : private boost::noncopyable ros::WallTime last_robot_state_update_wall_time_; robot_model_loader::RobotModelLoaderPtr rm_loader_; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; collision_detection::CollisionPluginLoader collision_loader_; @@ -578,7 +595,7 @@ class PlanningSceneMonitor : private boost::noncopyable * "operator->" functions. Therefore you will often see code like this: * @code * planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); - * robot_model::RobotModelConstPtr model = ls->getRobotModel(); + * moveit::core::RobotModelConstPtr model = ls->getRobotModel(); * @endcode * The function "getRobotModel()" is a member of PlanningScene and not @@ -610,7 +627,7 @@ class LockedPlanningSceneRO return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene(); } - operator const planning_scene::PlanningSceneConstPtr&() const + operator const planning_scene::PlanningSceneConstPtr &() const { return static_cast(planning_scene_monitor_.get())->getPlanningScene(); } @@ -670,7 +687,7 @@ class LockedPlanningSceneRO * "operator->" functions. Therefore you will often see code like this: * @code * planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor); - * robot_model::RobotModelConstPtr model = ls->getRobotModel(); + * moveit::core::RobotModelConstPtr model = ls->getRobotModel(); * @endcode * The function "getRobotModel()" is a member of PlanningScene and not @@ -691,7 +708,7 @@ class LockedPlanningSceneRW : public LockedPlanningSceneRO { } - operator const planning_scene::PlanningScenePtr&() + operator const planning_scene::PlanningScenePtr &() { return planning_scene_monitor_->getPlanningScene(); } @@ -701,6 +718,4 @@ class LockedPlanningSceneRW : public LockedPlanningSceneRO return planning_scene_monitor_->getPlanningScene(); } }; -} - -#endif +} // namespace planning_scene_monitor diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h index 3c753cffe6..4d30d4ec7d 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_SCENE_MONITOR_TRAJECTORY_MONITOR_ -#define MOVEIT_PLANNING_SCENE_MONITOR_TRAJECTORY_MONITOR_ +#pragma once #include #include @@ -45,10 +44,9 @@ namespace planning_scene_monitor { -typedef boost::function - TrajectoryStateAddedCallback; +using TrajectoryStateAddedCallback = boost::function; -MOVEIT_CLASS_FORWARD(TrajectoryMonitor); +MOVEIT_CLASS_FORWARD(TrajectoryMonitor); // Defines TrajectoryMonitorPtr, ConstPtr, WeakPtr... etc /** @class TrajectoryMonitor @brief Monitors the joint_states topic and tf to record the trajectory of the robot. */ @@ -57,7 +55,7 @@ class TrajectoryMonitor public: /** @brief Constructor. */ - TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor, double sampling_frequency = 5.0); + TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor, double sampling_frequency = 0.0); ~TrajectoryMonitor(); @@ -106,6 +104,4 @@ class TrajectoryMonitor std::unique_ptr record_states_thread_; TrajectoryStateAddedCallback state_add_callback_; }; -} - -#endif +} // namespace planning_scene_monitor diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index 43f1da2158..d9262aedbb 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -41,13 +41,13 @@ #include -planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, +planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer) : CurrentStateMonitor(robot_model, tf_buffer, ros::NodeHandle()) { } -planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, +planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer, const ros::NodeHandle& nh) : nh_(nh) @@ -66,11 +66,11 @@ planning_scene_monitor::CurrentStateMonitor::~CurrentStateMonitor() stopStateMonitor(); } -robot_state::RobotStatePtr planning_scene_monitor::CurrentStateMonitor::getCurrentState() const +moveit::core::RobotStatePtr planning_scene_monitor::CurrentStateMonitor::getCurrentState() const { boost::mutex::scoped_lock slock(state_update_lock_); - robot_state::RobotState* result = new robot_state::RobotState(robot_state_); - return robot_state::RobotStatePtr(result); + moveit::core::RobotState* result = new moveit::core::RobotState(robot_state_); + return moveit::core::RobotStatePtr(result); } ros::Time planning_scene_monitor::CurrentStateMonitor::getCurrentStateTime() const @@ -79,12 +79,12 @@ ros::Time planning_scene_monitor::CurrentStateMonitor::getCurrentStateTime() con return current_state_time_; } -std::pair +std::pair planning_scene_monitor::CurrentStateMonitor::getCurrentStateAndTime() const { boost::mutex::scoped_lock slock(state_update_lock_); - robot_state::RobotState* result = new robot_state::RobotState(robot_state_); - return std::make_pair(robot_state::RobotStatePtr(result), current_state_time_); + moveit::core::RobotState* result = new moveit::core::RobotState(robot_state_); + return std::make_pair(moveit::core::RobotStatePtr(result), current_state_time_); } std::map planning_scene_monitor::CurrentStateMonitor::getCurrentStateValues() const @@ -98,7 +98,7 @@ std::map planning_scene_monitor::CurrentStateMonitor::getCu return m; } -void planning_scene_monitor::CurrentStateMonitor::setToCurrentState(robot_state::RobotState& upd) const +void planning_scene_monitor::CurrentStateMonitor::setToCurrentState(moveit::core::RobotState& upd) const { boost::mutex::scoped_lock slock(state_update_lock_); const double* pos = robot_state_.getVariablePositions(); @@ -364,28 +364,12 @@ void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const senso update = true; robot_state_.setJointPositions(jm, &(joint_state->position[i])); - // optionally copy velocities and effort - if (copy_dynamics_) - { - // check if velocities exist - if (joint_state->name.size() == joint_state->velocity.size()) - { - robot_state_.setJointVelocities(jm, &(joint_state->velocity[i])); - - // check if effort exist. assume they are not useful if no velocities were passed in - if (joint_state->name.size() == joint_state->effort.size()) - { - robot_state_.setJointEfforts(jm, &(joint_state->effort[i])); - } - } - } - // continuous joints wrap, so we don't modify them (even if they are outside bounds!) if (jm->getType() == moveit::core::JointModel::REVOLUTE) if (static_cast(jm)->isContinuous()) continue; - const robot_model::VariableBounds& b = + const moveit::core::VariableBounds& b = jm->getVariableBounds()[0]; // only one variable in the joint, so we get its bounds // if the read variable is 'almost' within bounds (up to error_ difference), then consider it to be within @@ -395,6 +379,26 @@ void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const senso else if (joint_state->position[i] > b.max_position_ && joint_state->position[i] <= b.max_position_ + error_) robot_state_.setJointPositions(jm, &b.max_position_); } + + // optionally copy velocities and effort + if (copy_dynamics_) + { + // update joint velocities + if (joint_state->name.size() == joint_state->velocity.size() && + (!robot_state_.hasVelocities() || robot_state_.getJointVelocities(jm)[0] != joint_state->velocity[i])) + { + update = true; + robot_state_.setJointVelocities(jm, &(joint_state->velocity[i])); + } + + // update joint efforts + if (joint_state->name.size() == joint_state->effort.size() && + (!robot_state_.hasEffort() || robot_state_.getJointEffort(jm)[0] != joint_state->effort[i])) + { + update = true; + robot_state_.setJointEfforts(jm, &(joint_state->effort[i])); + } + } } } @@ -443,20 +447,20 @@ void planning_scene_monitor::CurrentStateMonitor::tfCallback() continue; joint_time_[joint] = latest_common_time; - double new_values[joint->getStateSpaceDimension()]; - const robot_model::LinkModel* link = joint->getChildLinkModel(); + std::vector new_values(joint->getStateSpaceDimension()); + const moveit::core::LinkModel* link = joint->getChildLinkModel(); if (link->jointOriginTransformIsIdentity()) - joint->computeVariablePositions(tf2::transformToEigen(transf), new_values); + joint->computeVariablePositions(tf2::transformToEigen(transf), new_values.data()); else joint->computeVariablePositions(link->getJointOriginTransform().inverse() * tf2::transformToEigen(transf), - new_values); + new_values.data()); - if (joint->distance(new_values, robot_state_.getJointPositions(joint)) > 1e-5) + if (joint->distance(new_values.data(), robot_state_.getJointPositions(joint)) > 1e-5) { changes = true; } - robot_state_.setJointPositions(joint, new_values); + robot_state_.setJointPositions(joint, new_values.data()); update = true; } } diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 23a0ca3f28..799c5df0f2 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -83,7 +84,7 @@ class PlanningSceneMonitor::DynamicReconfigureImpl return ns; } - void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig& config, uint32_t level) + void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig& config, uint32_t /*level*/) { PlanningSceneMonitor::SceneUpdateType event = PlanningSceneMonitor::UPDATE_NONE; if (config.publish_geometry_updates) @@ -161,7 +162,7 @@ PlanningSceneMonitor::~PlanningSceneMonitor() if (scene_) { scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); - scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback()); + scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback()); } stopPublishingPlanningScene(); stopStateMonitor(); @@ -202,15 +203,15 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc configureCollisionMatrix(scene_); configureDefaultPadding(); - scene_->getCollisionRobotNonConst()->setPadding(default_robot_padd_); - scene_->getCollisionRobotNonConst()->setScale(default_robot_scale_); + scene_->getCollisionEnvNonConst()->setPadding(default_robot_padd_); + scene_->getCollisionEnvNonConst()->setScale(default_robot_scale_); for (const std::pair& it : default_robot_link_padd_) { - scene_->getCollisionRobotNonConst()->setLinkPadding(it.first, it.second); + scene_->getCollisionEnvNonConst()->setLinkPadding(it.first, it.second); } for (const std::pair& it : default_robot_link_scale_) { - scene_->getCollisionRobotNonConst()->setLinkScale(it.first, it.second); + scene_->getCollisionEnvNonConst()->setLinkScale(it.first, it.second); } scene_->propogateRobotPadding(); } @@ -239,7 +240,7 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc last_update_time_ = last_robot_motion_time_ = ros::Time::now(); last_robot_state_update_wall_time_ = ros::WallTime::now(); - dt_state_update_ = ros::WallDuration(0.1); + dt_state_update_ = ros::WallDuration(0.03); double temp_wait_time = 0.05; @@ -265,7 +266,7 @@ void PlanningSceneMonitor::monitorDiffs(bool flag) boost::unique_lock ulock(scene_update_mutex_); if (scene_) { - scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback()); + scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback()); scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); scene_->decoupleParent(); parent_scene_ = scene_; @@ -369,13 +370,18 @@ void PlanningSceneMonitor::scenePublishingThread() if (octomap_monitor_) lock = octomap_monitor_->getOcTreePtr()->reading(); scene_->getPlanningSceneDiffMsg(msg); + if (new_scene_update_ == UPDATE_STATE) + { + msg.robot_state.attached_collision_objects.clear(); + msg.robot_state.is_diff = true; + } } boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the // transform cache to // update while we are // potentially changing // attached bodies - scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback()); + scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback()); scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); scene_->pushDiffs(parent_scene_); scene_->clearDiffs(); @@ -404,7 +410,6 @@ void PlanningSceneMonitor::scenePublishingThread() } if (publish_msg) { - rate.reset(); planning_scene_publisher_.publish(msg); if (is_full) ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str()); @@ -466,6 +471,11 @@ void PlanningSceneMonitor::triggerSceneUpdateEvent(SceneUpdateType update_type) bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_name) { + if (get_scene_service_.getService() == service_name) + { + ROS_FATAL_STREAM_NAMED(LOGNAME, "requestPlanningSceneState() to self-provided service '" << service_name << "'"); + throw std::runtime_error("requestPlanningSceneState() to self-provided service: " + service_name); + } // use global namespace for service ros::ServiceClient client = ros::NodeHandle().serviceClient(service_name); // all scene components are returned if none are specified @@ -484,13 +494,36 @@ bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_ } else { - ROS_INFO_NAMED(LOGNAME, "Failed to call service %s, have you launched move_group? at %s:%d", service_name.c_str(), - __FILE__, __LINE__); + ROS_INFO_NAMED( + LOGNAME, "Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?", + service_name.c_str()); return false; } return true; } +void PlanningSceneMonitor::providePlanningSceneService(const std::string& service_name) +{ + // Load the service + get_scene_service_ = + root_nh_.advertiseService(service_name, &PlanningSceneMonitor::getPlanningSceneServiceCallback, this); +} + +bool PlanningSceneMonitor::getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req, + moveit_msgs::GetPlanningScene::Response& res) +{ + if (req.components.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS) + updateFrameTransforms(); + + moveit_msgs::PlanningSceneComponents all_components; + all_components.components = UINT_MAX; // Return all scene components if nothing is specified. + + boost::unique_lock ulock(scene_update_mutex_); + scene_->getPlanningSceneMsg(res.scene, req.components.components ? req.components : all_components); + + return true; +} + void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene) { newPlanningSceneMessage(*scene); @@ -498,9 +531,18 @@ void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningS void PlanningSceneMonitor::clearOctomap() { - octomap_monitor_->getOcTreePtr()->lockWrite(); - octomap_monitor_->getOcTreePtr()->clear(); - octomap_monitor_->getOcTreePtr()->unlockWrite(); + if (scene_->getWorldNonConst()->removeObject(scene_->OCTOMAP_NS)) + triggerSceneUpdateEvent(UPDATE_SCENE); + if (octomap_monitor_) + { + octomap_monitor_->getOcTreePtr()->lockWrite(); + octomap_monitor_->getOcTreePtr()->clear(); + octomap_monitor_->getOcTreePtr()->unlockWrite(); + } + else + { + ROS_WARN_NAMED(LOGNAME, "Unable to clear octomap since no octomap monitor has been initialized"); + } } bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene) @@ -539,7 +581,7 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningSc if (!scene.is_diff && parent_scene_) { // the scene is now decoupled from the parent, since we just reset it - scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback()); + scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback()); scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); parent_scene_ = scene_; scene_ = parent_scene_->diff(); @@ -565,13 +607,13 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningSc if (no_other_scene_upd) { upd = UPDATE_NONE; - if (!planning_scene::PlanningScene::isEmpty(scene.world)) + if (!moveit::core::isEmpty(scene.world)) upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY); if (!scene.fixed_frame_transforms.empty()) upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS); - if (!planning_scene::PlanningScene::isEmpty(scene.robot_state)) + if (!moveit::core::isEmpty(scene.robot_state)) { upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE); if (!scene.robot_state.attached_collision_objects.empty() || !static_cast(scene.robot_state.is_diff)) @@ -644,7 +686,7 @@ void PlanningSceneMonitor::excludeRobotLinksFromOctree() boost::recursive_mutex::scoped_lock _(shape_handles_lock_); includeRobotLinksInOctree(); - const std::vector& links = getRobotModel()->getLinkModelsWithCollisionGeometry(); + const std::vector& links = getRobotModel()->getLinkModelsWithCollisionGeometry(); ros::WallTime start = ros::WallTime::now(); bool warned = false; for (const moveit::core::LinkModel* link : links) @@ -679,7 +721,7 @@ void PlanningSceneMonitor::includeRobotLinksInOctree() boost::recursive_mutex::scoped_lock _(shape_handles_lock_); - for (std::pair>>& link_shape_handle : link_shape_handles_) for (std::pair& it : link_shape_handle.second) @@ -695,7 +737,7 @@ void PlanningSceneMonitor::includeAttachedBodiesInOctree() boost::recursive_mutex::scoped_lock _(shape_handles_lock_); // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid) - for (std::pair>>& attached_body_shape_handle : attached_body_shape_handles_) for (std::pair& it : attached_body_shape_handle.second) @@ -709,7 +751,7 @@ void PlanningSceneMonitor::excludeAttachedBodiesFromOctree() includeAttachedBodiesInOctree(); // add attached objects again - std::vector ab; + std::vector ab; scene_->getCurrentState().getAttachedBodies(ab); for (const moveit::core::AttachedBody* body : ab) excludeAttachedBodyFromOctree(body); @@ -723,8 +765,7 @@ void PlanningSceneMonitor::includeWorldObjectsInOctree() boost::recursive_mutex::scoped_lock _(shape_handles_lock_); // clear information about any attached object - for (std::pair>>& + for (std::pair>>& collision_body_shape_handle : collision_body_shape_handles_) for (std::pair& it : collision_body_shape_handle.second) @@ -741,7 +782,7 @@ void PlanningSceneMonitor::excludeWorldObjectsFromOctree() excludeWorldObjectFromOctree(it.second); } -void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body) +void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const moveit::core::AttachedBody* attached_body) { if (!octomap_monitor_) return; @@ -763,7 +804,7 @@ void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const robot_state::Atta ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str()); } -void PlanningSceneMonitor::includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body) +void PlanningSceneMonitor::includeAttachedBodyInOctree(const moveit::core::AttachedBody* attached_body) { if (!octomap_monitor_) return; @@ -820,7 +861,7 @@ void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection: } } -void PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body, +void PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody* attached_body, bool just_attached) { if (!octomap_monitor_) @@ -968,7 +1009,7 @@ bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_fram { boost::recursive_mutex::scoped_lock _(shape_handles_lock_); - for (const std::pair>>& link_shape_handle : link_shape_handles_) { @@ -980,7 +1021,7 @@ bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_fram cache[link_shape_handle.second[j].first] = ttr * link_shape_handle.first->getCollisionOriginTransforms()[link_shape_handle.second[j].second]; } - for (const std::pair>>& attached_body_shape_handle : attached_body_shape_handles_) { @@ -1142,7 +1183,7 @@ void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr& updateSceneWithCurrentState(); } -void PlanningSceneMonitor::stateUpdateTimerCallback(const ros::WallTimerEvent& event) +void PlanningSceneMonitor::stateUpdateTimerCallback(const ros::WallTimerEvent& /*unused*/) { if (state_update_pending_) { diff --git a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp index 69b6e53851..157020c9c4 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp @@ -40,10 +40,12 @@ #include #include +static const std::string LOGNAME = "TrajectoryMonitor"; + planning_scene_monitor::TrajectoryMonitor::TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor, double sampling_frequency) : current_state_monitor_(state_monitor) - , sampling_frequency_(5.0) + , sampling_frequency_(sampling_frequency) , trajectory_(current_state_monitor_->getRobotModel(), "") { setSamplingFrequency(sampling_frequency); @@ -56,10 +58,14 @@ planning_scene_monitor::TrajectoryMonitor::~TrajectoryMonitor() void planning_scene_monitor::TrajectoryMonitor::setSamplingFrequency(double sampling_frequency) { + if (sampling_frequency != sampling_frequency_) + return; // silently return if nothing changes + if (sampling_frequency <= std::numeric_limits::epsilon()) - ROS_ERROR("The sampling frequency for trajectory states should be positive"); + ROS_INFO_NAMED(LOGNAME, "Disabling trajectory recording"); else - sampling_frequency_ = sampling_frequency; + ROS_DEBUG_NAMED(LOGNAME, "Setting trajectory sampling frequency to %.1f", sampling_frequency); + sampling_frequency_ = sampling_frequency; } bool planning_scene_monitor::TrajectoryMonitor::isActive() const @@ -69,10 +75,10 @@ bool planning_scene_monitor::TrajectoryMonitor::isActive() const void planning_scene_monitor::TrajectoryMonitor::startTrajectoryMonitor() { - if (!record_states_thread_) + if (sampling_frequency_ > std::numeric_limits::epsilon() && !record_states_thread_) { record_states_thread_.reset(new boost::thread(boost::bind(&TrajectoryMonitor::recordStates, this))); - ROS_DEBUG("Started trajectory monitor"); + ROS_DEBUG_NAMED(LOGNAME, "Started trajectory monitor"); } } @@ -83,7 +89,7 @@ void planning_scene_monitor::TrajectoryMonitor::stopTrajectoryMonitor() std::unique_ptr copy; copy.swap(record_states_thread_); copy->join(); - ROS_DEBUG("Stopped trajectory monitor"); + ROS_DEBUG_NAMED(LOGNAME, "Stopped trajectory monitor"); } } @@ -107,7 +113,7 @@ void planning_scene_monitor::TrajectoryMonitor::recordStates() while (record_states_thread_) { rate.sleep(); - std::pair state = current_state_monitor_->getCurrentStateAndTime(); + std::pair state = current_state_monitor_->getCurrentStateAndTime(); if (trajectory_.empty()) { trajectory_.addSuffixWayPoint(state.first, 0.0); diff --git a/moveit_ros/planning/rdf_loader/CMakeLists.txt b/moveit_ros/planning/rdf_loader/CMakeLists.txt index 0a0dbe5055..0b91cafd81 100644 --- a/moveit_ros/planning/rdf_loader/CMakeLists.txt +++ b/moveit_ros/planning/rdf_loader/CMakeLists.txt @@ -4,5 +4,8 @@ add_library(${MOVEIT_LIB_NAME} src/rdf_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h index 76efd13c8e..40e41a415b 100644 --- a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Mathias LΓΌdtke, Dave Coleman */ -#ifndef MOVEIT_PLANNING_RDF_LOADER_ -#define MOVEIT_PLANNING_RDF_LOADER_ +#pragma once #include #include @@ -43,7 +42,7 @@ namespace rdf_loader { -MOVEIT_CLASS_FORWARD(RDFLoader); +MOVEIT_CLASS_FORWARD(RDFLoader); // Defines RDFLoaderPtr, ConstPtr, WeakPtr... etc /** @class RDFLoader * @brief Default constructor @@ -101,5 +100,4 @@ class RDFLoader srdf::ModelSharedPtr srdf_; urdf::ModelInterfaceSharedPtr urdf_; }; -} -#endif +} // namespace rdf_loader diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp index eb655c1710..3c412ef5ee 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan, Mathias LΓΌdtke, Dave Coleman */ -// MoveIt! +// MoveIt #include #include @@ -50,6 +50,11 @@ #include #include +#ifdef _WIN32 +#define popen _popen +#define pclose _pclose +#endif + rdf_loader::RDFLoader::RDFLoader(const std::string& robot_description) { moveit::tools::Profiler::ScopedStart prof_start; @@ -65,13 +70,13 @@ rdf_loader::RDFLoader::RDFLoader(const std::string& robot_description) return; } - urdf::Model* umodel = new urdf::Model(); - if (!umodel->initString(content)) + std::unique_ptr urdf(new urdf::Model()); + if (!urdf->initString(content)) { ROS_ERROR_NAMED("rdf_loader", "Unable to parse URDF from parameter '%s'", robot_description_.c_str()); return; } - urdf_.reset(umodel); + urdf_ = std::move(urdf); const std::string srdf_description(robot_description_ + "_semantic"); std::string scontent; @@ -82,13 +87,13 @@ rdf_loader::RDFLoader::RDFLoader(const std::string& robot_description) return; } - srdf_.reset(new srdf::Model()); - if (!srdf_->initString(*urdf_, scontent)) + srdf::ModelSharedPtr srdf(new srdf::Model()); + if (!srdf->initString(*urdf_, scontent)) { ROS_ERROR_NAMED("rdf_loader", "Unable to parse SRDF from parameter '%s'", srdf_description.c_str()); - srdf_.reset(); return; } + srdf_ = std::move(srdf); ROS_DEBUG_STREAM_NAMED("rdf", "Loaded robot model in " << (ros::WallTime::now() - start).toSec() << " seconds"); } diff --git a/moveit_ros/planning/robot_model_loader/CMakeLists.txt b/moveit_ros/planning/robot_model_loader/CMakeLists.txt index 13af8cdd78..9790d1bcc3 100644 --- a/moveit_ros/planning/robot_model_loader/CMakeLists.txt +++ b/moveit_ros/planning/robot_model_loader/CMakeLists.txt @@ -8,5 +8,8 @@ add_library(${MOVEIT_LIB_NAME} src/robot_model_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} moveit_rdf_loader moveit_kinematics_plugin_loader ${catkin_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h index def0d10d75..5eb084341f 100644 --- a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h +++ b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_MODELS_LOADER_ROBOT_MODEL_LOADER_ -#define MOVEIT_PLANNING_MODELS_LOADER_ROBOT_MODEL_LOADER_ +#pragma once #include #include @@ -44,7 +43,7 @@ namespace robot_model_loader { -MOVEIT_CLASS_FORWARD(RobotModelLoader); +MOVEIT_CLASS_FORWARD(RobotModelLoader); // Defines RobotModelLoaderPtr, ConstPtr, WeakPtr... etc /** @class RobotModelLoader */ class RobotModelLoader @@ -85,7 +84,7 @@ class RobotModelLoader ~RobotModelLoader(); /** @brief Get the constructed planning_models::RobotModel */ - const robot_model::RobotModelPtr& getModel() const + const moveit::core::RobotModelPtr& getModel() const { return model_; } @@ -129,9 +128,8 @@ class RobotModelLoader private: void configure(const Options& opt); - robot_model::RobotModelPtr model_; + moveit::core::RobotModelPtr model_; rdf_loader::RDFLoaderPtr rdf_loader_; kinematics_plugin_loader::KinematicsPluginLoaderPtr kinematics_loader_; }; -} -#endif +} // namespace robot_model_loader diff --git a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp index 80a8556dc2..50e9667a28 100644 --- a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp +++ b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp @@ -67,15 +67,15 @@ RobotModelLoader::~RobotModelLoader() namespace { -bool canSpecifyPosition(const robot_model::JointModel* jmodel, const unsigned int index) +bool canSpecifyPosition(const moveit::core::JointModel* jmodel, const unsigned int index) { bool ok = false; - if (jmodel->getType() == robot_model::JointModel::PLANAR && index == 2) + if (jmodel->getType() == moveit::core::JointModel::PLANAR && index == 2) ROS_ERROR("Cannot specify position limits for orientation of planar joint '%s'", jmodel->getName().c_str()); - else if (jmodel->getType() == robot_model::JointModel::FLOATING && index > 2) + else if (jmodel->getType() == moveit::core::JointModel::FLOATING && index > 2) ROS_ERROR("Cannot specify position limits for orientation of floating joint '%s'", jmodel->getName().c_str()); - else if (jmodel->getType() == robot_model::JointModel::REVOLUTE && - static_cast(jmodel)->isContinuous()) + else if (jmodel->getType() == moveit::core::JointModel::REVOLUTE && + static_cast(jmodel)->isContinuous()) ROS_ERROR("Cannot specify position limits for continuous joint '%s'", jmodel->getName().c_str()); else ok = true; @@ -97,7 +97,7 @@ void RobotModelLoader::configure(const Options& opt) { const srdf::ModelSharedPtr& srdf = rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); - model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf)); + model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); } if (model_ && !rdf_loader_->getRobotDescription().empty()) @@ -107,60 +107,61 @@ void RobotModelLoader::configure(const Options& opt) // if there are additional joint limits specified in some .yaml file, read those in ros::NodeHandle nh("~"); - for (moveit::core::JointModel* jmodel : model_->getJointModels()) + for (moveit::core::JointModel* joint_model : model_->getJointModels()) { - std::vector jlim = jmodel->getVariableBoundsMsg(); - for (std::size_t j = 0; j < jlim.size(); ++j) + std::vector joint_limit = joint_model->getVariableBoundsMsg(); + for (std::size_t joint_id = 0; joint_id < joint_limit.size(); ++joint_id) { - std::string prefix = rdf_loader_->getRobotDescription() + "_planning/joint_limits/" + jlim[j].joint_name + "/"; + std::string prefix = + rdf_loader_->getRobotDescription() + "_planning/joint_limits/" + joint_limit[joint_id].joint_name + "/"; double max_position; if (nh.getParam(prefix + "max_position", max_position)) { - if (canSpecifyPosition(jmodel, j)) + if (canSpecifyPosition(joint_model, joint_id)) { - jlim[j].has_position_limits = true; - jlim[j].max_position = max_position; + joint_limit[joint_id].has_position_limits = true; + joint_limit[joint_id].max_position = max_position; } } double min_position; if (nh.getParam(prefix + "min_position", min_position)) { - if (canSpecifyPosition(jmodel, j)) + if (canSpecifyPosition(joint_model, joint_id)) { - jlim[j].has_position_limits = true; - jlim[j].min_position = min_position; + joint_limit[joint_id].has_position_limits = true; + joint_limit[joint_id].min_position = min_position; } } double max_velocity; if (nh.getParam(prefix + "max_velocity", max_velocity)) { - jlim[j].has_velocity_limits = true; - jlim[j].max_velocity = max_velocity; + joint_limit[joint_id].has_velocity_limits = true; + joint_limit[joint_id].max_velocity = max_velocity; } bool has_vel_limits; if (nh.getParam(prefix + "has_velocity_limits", has_vel_limits)) - jlim[j].has_velocity_limits = has_vel_limits; + joint_limit[joint_id].has_velocity_limits = has_vel_limits; double max_acc; if (nh.getParam(prefix + "max_acceleration", max_acc)) { - jlim[j].has_acceleration_limits = true; - jlim[j].max_acceleration = max_acc; + joint_limit[joint_id].has_acceleration_limits = true; + joint_limit[joint_id].max_acceleration = max_acc; } bool has_acc_limits; if (nh.getParam(prefix + "has_acceleration_limits", has_acc_limits)) - jlim[j].has_acceleration_limits = has_acc_limits; + joint_limit[joint_id].has_acceleration_limits = has_acc_limits; } - jmodel->setVariableBounds(jlim); + joint_model->setVariableBounds(joint_limit); } } if (model_ && opt.load_kinematics_solvers_) loadKinematicsSolvers(); - ROS_DEBUG_STREAM_NAMED("robot_model_loader", "Loaded kinematic model in " << (ros::WallTime::now() - start).toSec() - << " seconds"); + ROS_DEBUG_STREAM_NAMED("robot_model_loader", + "Loaded kinematic model in " << (ros::WallTime::now() - start).toSec() << " seconds"); } void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::KinematicsPluginLoaderPtr& kloader) @@ -174,9 +175,9 @@ void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::Kin if (kloader) kinematics_loader_ = kloader; else - kinematics_loader_.reset( - new kinematics_plugin_loader::KinematicsPluginLoader(rdf_loader_->getRobotDescription())); - robot_model::SolverAllocatorFn kinematics_allocator = kinematics_loader_->getLoaderFunction(rdf_loader_->getSRDF()); + kinematics_loader_.reset(new kinematics_plugin_loader::KinematicsPluginLoader(rdf_loader_->getRobotDescription())); + moveit::core::SolverAllocatorFn kinematics_allocator = + kinematics_loader_->getLoaderFunction(rdf_loader_->getSRDF()); const std::vector& groups = kinematics_loader_->getKnownGroups(); std::stringstream ss; std::copy(groups.begin(), groups.end(), std::ostream_iterator(ss, " ")); @@ -184,14 +185,14 @@ void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::Kin if (groups.empty() && !model_->getJointModelGroups().empty()) ROS_WARN("No kinematics plugins defined. Fill and load kinematics.yaml!"); - std::map imap; + std::map imap; for (const std::string& group : groups) { // Check if a group in kinematics.yaml exists in the srdf if (!model_->hasJointModelGroup(group)) continue; - const robot_model::JointModelGroup* jmg = model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = model_->getJointModelGroup(group); kinematics::KinematicsBasePtr solver = kinematics_allocator(jmg); if (solver) @@ -220,7 +221,7 @@ void RobotModelLoader::loadKinematicsSolvers(const kinematics_plugin_loader::Kin { if (!model_->hasJointModelGroup(it.first)) continue; - robot_model::JointModelGroup* jmg = model_->getJointModelGroup(it.first); + moveit::core::JointModelGroup* jmg = model_->getJointModelGroup(it.first); jmg->setDefaultIKTimeout(it.second); } } diff --git a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt index 51b7336ac1..280835786e 100644 --- a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt +++ b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt @@ -5,12 +5,22 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} moveit_planning_scene_monitor moveit_robot_model_loader ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${moveit_ros_planning_EXPORTED_TARGETS}) # don't build until necessary msgs are finish -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) -add_library(test_controller_manager_plugin test/test_moveit_controller_manager_plugin.cpp) -set_target_properties(test_controller_manager_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(test_controller_manager_plugin ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +if(CATKIN_ENABLE_TESTING) -add_executable(test_controller_manager test/test_app.cpp) -target_link_libraries(test_controller_manager ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +## This needs further cleanup before it can run +# add_library(test_controller_manager_plugin test/test_moveit_controller_manager_plugin.cpp) +# set_target_properties(test_controller_manager_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +# target_link_libraries(test_controller_manager_plugin ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +# +# find_package(rostest REQUIRED) +# add_rostest_gtest(test_execution_manager +# test/test_execution_manager.test +# test/test_execution_manager.cpp) +# target_link_libraries(test_execution_manager ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +endif(CATKIN_ENABLE_TESTING) diff --git a/moveit_ros/planning/trajectory_execution_manager/dox/trajectory_execution.dox b/moveit_ros/planning/trajectory_execution_manager/dox/trajectory_execution.dox index bb4b9068a4..ab7a5e683e 100644 --- a/moveit_ros/planning/trajectory_execution_manager/dox/trajectory_execution.dox +++ b/moveit_ros/planning/trajectory_execution_manager/dox/trajectory_execution.dox @@ -1,11 +1,11 @@ /** \page trajectory_execution Trajectory Execution Manager -@b MoveIt! includes a library for managing controllers and the execution of trajectories. This code exists in the trajectory_execution_manager namespace. +@b MoveIt includes a library for managing controllers and the execution of trajectories. This code exists in the trajectory_execution_manager namespace. The trajectory_execution_manager::TrajectoryExecutionManager class allows two main operations: - trajectory_execution_manager::TrajectoryExecutionManager::push() adds trajectories specified as a moveit_msgs::RobotTrajectory message type to a queue of trajectories to be executed in sequence. Each trajectory can be specified for any set of joints in the robot. Because controllers may only be available for certain groups of joints, this function may decide to split one trajectory into multiple ones and pass them to corresponding controllers (this time in parallel, using the same time stamp for the trajectory points). This approach assumes that controllers respect the time stamps specified for the waypoints. - trajectory_execution_manager::TrajectoryExecutionManager::execute() passes the appropriate trajectories to different controllers, monitors execution, optionally waits for completion of the execution and, very importantly, switches active controllers as needed (optionally) to be able to execute the specified trajectories. -The functionality of the trajectory execution in MoveIt! usually needs robot-specific interaction with controllers. For this reason, the concept of a controller manager specific to MoveIt! (moveit_controller_manager::MoveItControllerManager) was defined. This is an abstract class that defines the functionality needed by trajectory_execution_manager::TrajectoryExecutionManager and needs to be implemented for each robot type. Often, the implementation of these plugins are quite similar and it is easy to modify existing code to achieve the desired functionality (see for example pr2_moveit_controller_manager::Pr2MoveItControllerManager). +The functionality of the trajectory execution in MoveIt usually needs robot-specific interaction with controllers. For this reason, the concept of a controller manager specific to MoveIt (moveit_controller_manager::MoveItControllerManager) was defined. This is an abstract class that defines the functionality needed by trajectory_execution_manager::TrajectoryExecutionManager and needs to be implemented for each robot type. Often, the implementation of these plugins are quite similar and it is easy to modify existing code to achieve the desired functionality (see for example pr2_moveit_controller_manager::Pr2MoveItControllerManager). */ \ No newline at end of file diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index ea2f1a0d6d..1ec1bcc79a 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_TRAJECTORY_EXECUTION_MANAGER_TRAJECTORY_EXECUTION_MANAGER_ -#define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_TRAJECTORY_EXECUTION_MANAGER_ +#pragma once #include #include @@ -52,7 +51,7 @@ namespace trajectory_execution_manager { -MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); +MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); // Defines TrajectoryExecutionManagerPtr, ConstPtr, WeakPtr... etc // Two modes: // Managed controllers @@ -68,7 +67,7 @@ class TrajectoryExecutionManager /// Definition of the function signature that is called when the execution of a pushed trajectory completes /// successfully. - typedef boost::function PathSegmentCompleteCallback; + using PathSegmentCompleteCallback = boost::function; /// Data structure that represents information necessary to execute a trajectory struct TrajectoryExecutionContext @@ -82,11 +81,11 @@ class TrajectoryExecutionManager }; /// Load the controller manager plugin, start listening for events on a topic. - TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& robot_model, + TrajectoryExecutionManager(const moveit::core::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm); /// Load the controller manager plugin, start listening for events on a topic. - TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& robot_model, + TrajectoryExecutionManager(const moveit::core::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers); /// Destructor. Cancels all running trajectories (if any) @@ -304,7 +303,7 @@ class TrajectoryExecutionManager // Name of this class for logging const std::string name_ = "trajectory_execution_manager"; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; planning_scene_monitor::CurrentStateMonitorPtr csm_; ros::NodeHandle node_handle_; ros::NodeHandle root_node_handle_; @@ -321,6 +320,7 @@ class TrajectoryExecutionManager boost::mutex execution_state_mutex_; boost::mutex continuous_execution_mutex_; + boost::mutex execution_thread_mutex_; boost::condition_variable continuous_execution_condition_; @@ -339,8 +339,7 @@ class TrajectoryExecutionManager std::vector trajectories_; std::deque continuous_execution_queue_; - std::unique_ptr > - controller_manager_loader_; + std::unique_ptr > controller_manager_loader_; moveit_controller_manager::MoveItControllerManagerPtr controller_manager_; bool verbose_; @@ -361,6 +360,4 @@ class TrajectoryExecutionManager double execution_velocity_scaling_; bool wait_for_trajectory_completion_; }; -} - -#endif +} // namespace trajectory_execution_manager diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index feea5e7496..63fd9b5ee7 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -64,7 +65,7 @@ class TrajectoryExecutionManager::DynamicReconfigureImpl } private: - void dynamicReconfigureCallback(TrajectoryExecutionDynamicReconfigureConfig& config, uint32_t level) + void dynamicReconfigureCallback(TrajectoryExecutionDynamicReconfigureConfig& config, uint32_t /*level*/) { owner_->enableExecutionDurationMonitoring(config.execution_duration_monitoring); owner_->setAllowedExecutionDurationScaling(config.allowed_execution_duration_scaling); @@ -78,7 +79,7 @@ class TrajectoryExecutionManager::DynamicReconfigureImpl dynamic_reconfigure::Server dynamic_reconfigure_server_; }; -TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& robot_model, +TrajectoryExecutionManager::TrajectoryExecutionManager(const moveit::core::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm) : robot_model_(robot_model), csm_(csm), node_handle_("~") { @@ -88,7 +89,7 @@ TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotM initialize(); } -TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& robot_model, +TrajectoryExecutionManager::TrajectoryExecutionManager(const moveit::core::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers) : robot_model_(robot_model), csm_(csm), node_handle_("~"), manage_controllers_(manage_controllers) @@ -143,8 +144,9 @@ void TrajectoryExecutionManager::initialize() if (classes.size() == 1) { controller = classes[0]; - ROS_WARN_NAMED(name_, "Parameter '~moveit_controller_manager' is not specified but only one " - "matching plugin was found: '%s'. Using that one.", + ROS_WARN_NAMED(name_, + "Parameter '~moveit_controller_manager' is not specified but only one " + "matching plugin was found: '%s'. Using that one.", controller.c_str()); } else @@ -160,8 +162,8 @@ void TrajectoryExecutionManager::initialize() } catch (pluginlib::PluginlibException& ex) { - ROS_FATAL_STREAM_NAMED(name_, "Exception while loading controller manager '" << controller - << "': " << ex.what()); + ROS_FATAL_STREAM_NAMED(name_, + "Exception while loading controller manager '" << controller << "': " << ex.what()); } } @@ -806,10 +808,10 @@ bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::RobotTr std::set actuated_joints_single; for (const std::string& joint_name : trajectory.joint_trajectory.joint_names) { - const robot_model::JointModel* jm = robot_model_->getJointModel(joint_name); + const moveit::core::JointModel* jm = robot_model_->getJointModel(joint_name); if (jm) { - if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == robot_model::JointModel::FIXED) + if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == moveit::core::JointModel::FIXED) continue; actuated_joints_single.insert(jm->getName()); } @@ -936,7 +938,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont ROS_DEBUG_NAMED(name_, "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_); - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; if (!csm_->waitForCurrentState(ros::Time::now()) || !(current_state = csm_->getCurrentState())) { ROS_WARN_NAMED(name_, "Failed to validate trajectory: couldn't receive full current joint state within 1s"); @@ -959,7 +961,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont for (std::size_t i = 0, end = joint_names.size(); i < end; ++i) { - const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]); + const moveit::core::JointModel* jm = current_state->getJointModel(joint_names[i]); if (!jm) { ROS_ERROR_STREAM_NAMED(name_, "Unknown joint in trajectory: " << joint_names[i]); @@ -971,10 +973,11 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont // normalize positions and compare jm->enforcePositionBounds(&cur_position); jm->enforcePositionBounds(&traj_position); - if (fabs(cur_position - traj_position) > allowed_start_tolerance_) + if (jm->distance(&cur_position, &traj_position) > allowed_start_tolerance_) { - ROS_ERROR_NAMED(name_, "\nInvalid Trajectory: start point deviates from current robot state more than %g" - "\njoint '%s': expected: %g, current: %g", + ROS_ERROR_NAMED(name_, + "\nInvalid Trajectory: start point deviates from current robot state more than %g" + "\njoint '%s': expected: %g, current: %g", allowed_start_tolerance_, joint_names[i].c_str(), traj_position, cur_position); return false; } @@ -995,7 +998,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont for (std::size_t i = 0, end = joint_names.size(); i < end; ++i) { - const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]); + const moveit::core::JointModel* jm = current_state->getJointModel(joint_names[i]); if (!jm) { ROS_ERROR_STREAM_NAMED(name_, "Unknown joint in trajectory: " << joint_names[i]); @@ -1005,11 +1008,13 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont // compute difference (offset vector and rotation angle) between current transform // and start transform in trajectory Eigen::Isometry3d cur_transform, start_transform; + // computeTransform() computes a valid isometry by contract jm->computeTransform(current_state->getJointPositions(jm), cur_transform); start_transform = tf2::transformToEigen(transforms[i]); + ASSERT_ISOMETRY(start_transform) // unsanitized input, could contain a non-isometry Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation(); Eigen::AngleAxisd rotation; - rotation.fromRotationMatrix(cur_transform.rotation().transpose() * start_transform.rotation()); + rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear()); if ((offset.array() > allowed_start_tolerance_).any() || rotation.angle() > allowed_start_tolerance_) { ROS_ERROR_STREAM_NAMED(name_, "\nInvalid Trajectory: start point deviates from current robot state more than " @@ -1036,8 +1041,8 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context, std::set actuated_joints; auto is_actuated = [this](const std::string& joint_name) -> bool { - const robot_model::JointModel* jm = robot_model_->getJointModel(joint_name); - return (jm && !jm->isPassive() && !jm->getMimic() && jm->getType() != robot_model::JointModel::FIXED); + const moveit::core::JointModel* jm = robot_model_->getJointModel(joint_name); + return (jm && !jm->isPassive() && !jm->getMimic() && jm->getType() != moveit::core::JointModel::FIXED); }; for (const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names) if (is_actuated(joint_name)) @@ -1170,8 +1175,12 @@ void TrajectoryExecutionManager::stopExecution(bool auto_clear) ROS_INFO_NAMED(name_, "Stopped trajectory execution."); // wait for the execution thread to finish - execution_thread_->join(); - execution_thread_.reset(); + boost::mutex::scoped_lock lock(execution_thread_mutex_); + if (execution_thread_) + { + execution_thread_->join(); + execution_thread_.reset(); + } if (auto_clear) clear(); @@ -1182,8 +1191,12 @@ void TrajectoryExecutionManager::stopExecution(bool auto_clear) else if (execution_thread_) // just in case we have some thread waiting to be joined from some point in the past, we // join it now { - execution_thread_->join(); - execution_thread_.reset(); + boost::mutex::scoped_lock lock(execution_thread_mutex_); + if (execution_thread_) + { + execution_thread_->join(); + execution_thread_.reset(); + } } } @@ -1467,8 +1480,9 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) if (!handle->waitForExecution(expected_trajectory_duration)) if (!execution_complete_ && ros::Time::now() - current_time > expected_trajectory_duration) { - ROS_ERROR_NAMED(name_, "Controller is taking too long to execute trajectory (the expected upper " - "bound for the trajectory execution was %lf seconds). Stopping trajectory.", + ROS_ERROR_NAMED(name_, + "Controller is taking too long to execute trajectory (the expected upper " + "bound for the trajectory execution was %lf seconds). Stopping trajectory.", expected_trajectory_duration.toSec()); { boost::mutex::scoped_lock slock(execution_state_mutex_); @@ -1530,7 +1544,7 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon ros::WallTime start = ros::WallTime::now(); double time_remaining = wait_time; - robot_state::RobotStatePtr prev_state, cur_state; + moveit::core::RobotStatePtr prev_state, cur_state; prev_state = csm_->getCurrentState(); prev_state->enforceBounds(); @@ -1555,7 +1569,7 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon for (std::size_t i = 0; i < n && !moved; ++i) { - const robot_model::JointModel* jm = cur_state->getJointModel(joint_names[i]); + const moveit::core::JointModel* jm = cur_state->getJointModel(joint_names[i]); if (!jm) continue; // joint vanished from robot state (shouldn't happen), but we don't care @@ -1603,7 +1617,7 @@ moveit_controller_manager::ExecutionStatus TrajectoryExecutionManager::getLastEx bool TrajectoryExecutionManager::ensureActiveControllersForGroup(const std::string& group) { - const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group); if (joint_model_group) return ensureActiveControllersForJoints(joint_model_group->getJointModelNames()); else @@ -1620,10 +1634,10 @@ bool TrajectoryExecutionManager::ensureActiveControllersForJoints(const std::vec std::set jset; for (const std::string& joint : joints) { - const robot_model::JointModel* jm = robot_model_->getJointModel(joint); + const moveit::core::JointModel* jm = robot_model_->getJointModel(joint); if (jm) { - if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == robot_model::JointModel::FIXED) + if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == moveit::core::JointModel::FIXED) continue; jset.insert(joint); } diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_app.cpp b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp similarity index 100% rename from moveit_ros/planning/trajectory_execution_manager/test/test_app.cpp rename to moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.test b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.test new file mode 100644 index 0000000000..ef1cab7521 --- /dev/null +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h index 731d2e65c4..beaf196cd2 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef TEST_MOVEIT_CONTROLLER_MANAGER_ -#define TEST_MOVEIT_CONTROLLER_MANAGER_ +#pragma once #include @@ -48,7 +47,7 @@ class TestMoveItControllerHandle : public moveit_controller_manager::MoveItContr { } - bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory) override + bool sendTrajectory(const moveit_msgs::RobotTrajectory& /*trajectory*/) override { return true; } @@ -60,6 +59,7 @@ class TestMoveItControllerHandle : public moveit_controller_manager::MoveItContr bool waitForExecution(const ros::Duration& timeout = ros::Duration(0)) override { + (void)timeout; return false; } @@ -142,15 +142,15 @@ class TestMoveItControllerManager : public moveit_controller_manager::MoveItCont bool switchControllers(const std::vector& activate, const std::vector& deactivate) override { - for (std::size_t i = 0; i < deactivate.size(); ++i) + for (const std::string& controller : deactivate) { - controllers_[deactivate[i]] &= ~ACTIVE; - std::cout << "Deactivated controller " << deactivate[i] << std::endl; + controllers_[controller] &= ~ACTIVE; + std::cout << "Deactivated controller " << controller << std::endl; } - for (std::size_t i = 0; i < activate.size(); ++i) + for (const std::string& controller : activate) { - controllers_[activate[i]] |= ACTIVE; - std::cout << "Activated controller " << activate[i] << std::endl; + controllers_[controller] |= ACTIVE; + std::cout << "Activated controller " << controller << std::endl; } return true; } @@ -159,5 +159,4 @@ class TestMoveItControllerManager : public moveit_controller_manager::MoveItCont std::map controllers_; std::map > controller_joints_; }; -} -#endif +} // namespace test_moveit_controller_manager diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst index 2e0e72f5f1..fbcb7a2659 100644 --- a/moveit_ros/planning_interface/CHANGELOG.rst +++ b/moveit_ros/planning_interface/CHANGELOG.rst @@ -2,6 +2,100 @@ Changelog for package moveit_ros_planning_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature] moveit_cpp: more informative error message, cover another potential failure condition. (`#2336 `_) +* [fix] Make GILReleaser exception-safe (`#2363 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* [maint] Replace panda_moveit_config -> moveit_resources_panda_moveit_config (`#2300 `_) +* Contributors: AndyZe, Bjar Ne, Felix von Drigalski, Robert Haschke + +1.1.0 (2020-09-04) +------------------ +* [feature] Use Eigen::Transform::linear() instead of rotation() (`#1964 `_) +* [feature] move_group pick place test (`#2031 `_) +* [feature] Check for grasp service - general cleanup MGI (`#2077 `_) +* [feature] Integration test to defend subframe tutorial (`#1757 `_) +* [feature] Release Python GIL for C++ calls (`#1947 `_) +* [feature] Add default velocity/acceleration scaling factors (`#1890 `_) +* [feature] Improve move_group_interface's const correctness (`#1715 `_) +* [feature] Add get_jacobian_matrix to moveit_commander (`#1501 `_) +* [feature] Add named frames to CollisionObjects (`#1439 `_) +* [feature] Added GILRelease to pick and place (`#2272 `_) +* [feature] Add missing variants of place from list of PlaceLocations and Poses in the python interface (`#2231 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Resolve PSI lock-up in RViz display (`#1951 `_) +* [fix] Fix flaky moveit_cpp test (`#1781 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [maint] Fix a parameter mix-up in moveit_cpp loading (`#2187 `_) +* [maint] Optional cpp version setting (`#2166 `_) +* [maint] update dependencies for python3 in noetic (`#2131 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#1586 `_, `#1419 `_) +* [maint] Fix some clang tidy issues (`#2004 `_) +* [maint] export moveit_py_bindings_tools library (`#1970 `_) +* [maint] Fix usage of panda_moveit_config (`#1904 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Fix typo in cmake file (`#1857 `_) +* [maint] Reduce console output warnings (`#1845 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* [maint] improve [get|set]JointValueTarget in python wrapper (`#858 `_) +* [maint] moveit_commander.MoveGroupInterface.plan() to better align with C++ MoveGroup::plan() (`#790 `_) +* Contributors: AndyZe, Ayush Garg, Bence Magyar, Bjar Ne, Dave Coleman, Felix von Drigalski, Gerard Canal, Guilhem Saurel, Henning Kayser, Jafar Abdi, JafarAbdi, Jere Liukkonen, Jonathan Binney, Kunal Tyagi, Luca Rinelli, Mahmoud Ahmed Selim, Markus Vieth, Martin Pecka, Masaki Murooka, Michael Ferguson, Michael GΓΆrner, Niklas Fiedler, Robert Haschke, Ryosuke Tajima, Sean Yen, Tyler Weaver, Yeshwanth, Yu, Yan, mvieth, v4hn + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10, Fix warnings +* [maint] Optimize includes (`#2229 `_) +* [feature] Exposed parameter wait_for_servers and getPlannerId() API in MoveGroup's Python API (`#2201 `_) +* Contributors: Gerard Canal, Markus Vieth, Robert Haschke, Michael GΓΆrner + +1.0.5 (2020-07-08) +------------------ +* [maint] Remove dependency on panda_moveit_config (#2194 `_, #2197 `_) +* [maint] Adapt linking to eigenpy (`#2118 `_) +* [maint] Replace robot_model and robot_state namespaces with moveit::core (`#2135 `_) +* [feature] PlanningComponent: Load plan_request_params (`#2033 `_) +* [feature] MoveItCpp: a high-level C++ planning API (`#1656 `_) +* [fix] Validate action client pointer before access +* [fix] Wait and check for the grasp service +* [maint] Add tests for move_group interface (`#1995 `_) +* Contributors: AndyZe, Henning Kayser, Jafar Abdi, Michael GΓΆrner, Robert Haschke, Tyler Weaver, Yeshwanth + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [feature] `MoveGroupInterface`: Add execution methods for moveit_msgs::RobotTrajectory (`#1955 `_) +* [feature] Allow to instantiate a `PlanningSceneInterface` w/ and w/o a running `move_group` node +* [fix] Release Python `GIL` for C++ calls (`#1947 `_) +* [feature] Expose reference_point_position parameter in getJacobian() (`#1595 `_) +* [feature] `MoveGroupInterface`: Expose `constructPickGoal` and `constructPlaceGoal` (`#1498 `_) +* [feature] `python MoveGroupInterface`: Added custom time limit for `wait_for_servers()` (`#1444 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Improve Python 3 compatibility (`#1870 `_) + * Replaced StringIO with BytesIO for python msg serialization + * Use py_bindings_tools::ByteString as byte-based serialization buffer on C++ side +* [feature] Export moveit_py_bindings_tools library +* [maint] Fix various build issues on Windows + * Use `.pyd` as the output suffix for Python module on Windows. (`#1637 `_) + * Favor ros::Duration.sleep over sleep. (`#1634 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [maint] Updated deprecation method: MOVEIT_DEPRECATED -> [[deprecated]] (`#1748 `_) +* [maint] `eigenpy`: switched to system package (`#1737 `_) +* [featue] `PlanningSceneInterface`: wait for its two services +* [feature] Select time parametrization algorithm in retime_trajectory (`#1508 `_) +* Contributors: Bjar Ne, Felix von Drigalski, Kunal Tyagi, Luca Rinelli, Masaki Murooka, Michael GΓΆrner, Niklas Fiedler, Robert Haschke, Sean Yen, Yu, Yan, mvieth, v4hn + +1.0.2 (2019-06-28) +------------------ +* [maintenance] Removed unnecessary null pointer checks on deletion (`#1410 `_) +* Contributors: Mahmoud Ahmed Selim + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -91,7 +185,7 @@ Changelog for package moveit_ros_planning_interface 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * [enhancement][MoveGroup] Add getLinkNames function (`#440 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 55686b8ff8..bcc6070c88 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_planning_interface) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -18,7 +22,6 @@ find_package(catkin REQUIRED COMPONENTS tf2_eigen tf2_geometry_msgs tf2_ros - eigenpy roscpp actionlib rospy @@ -31,7 +34,7 @@ find_package(PythonLibs "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" REQUIR find_package(Boost REQUIRED) if(Boost_VERSION LESS 106700) set(BOOST_PYTHON_COMPONENT python) -elseif() +else() set(BOOST_PYTHON_COMPONENT python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}) endif() @@ -44,12 +47,14 @@ find_package(Boost REQUIRED COMPONENTS thread ) find_package(Eigen3 REQUIRED) +find_package(eigenpy REQUIRED) set(THIS_PACKAGE_INCLUDE_DIRS py_bindings_tools/include common_planning_interface_objects/include planning_scene_interface/include move_group_interface/include + moveit_cpp/include ) catkin_python_setup() @@ -59,26 +64,29 @@ catkin_package( moveit_common_planning_interface_objects moveit_planning_scene_interface moveit_move_group_interface + moveit_cpp + moveit_py_bindings_tools INCLUDE_DIRS ${THIS_PACKAGE_INCLUDE_DIRS} CATKIN_DEPENDS actionlib + geometry_msgs + moveit_msgs moveit_ros_planning moveit_ros_warehouse moveit_ros_manipulation moveit_ros_move_group - geometry_msgs - moveit_msgs + roscpp tf2_geometry_msgs DEPENDS EIGEN3 ) -include_directories(${THIS_PACKAGE_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS}) +include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS}) @@ -88,3 +96,4 @@ add_subdirectory(planning_scene_interface) add_subdirectory(move_group_interface) add_subdirectory(robot_interface) add_subdirectory(test) +add_subdirectory(moveit_cpp) diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt b/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt index f5b6953406..71b61285a4 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt +++ b/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt @@ -6,6 +6,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${PYTHON_LIBRARIES} install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h index bffa91d836..0359fe677b 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h +++ b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_INTERFACE_COMMON_OBJECTS_ -#define MOVEIT_PLANNING_INTERFACE_COMMON_OBJECTS_ +#pragma once #include @@ -45,10 +44,10 @@ namespace planning_interface { std::shared_ptr getSharedTF(); -robot_model::RobotModelConstPtr getSharedRobotModel(const std::string& robot_description); +moveit::core::RobotModelConstPtr getSharedRobotModel(const std::string& robot_description); /** - @brief getSharedStateMonitor is a simpler version of getSharedStateMonitor(const robot_model::RobotModelConstPtr + @brief getSharedStateMonitor is a simpler version of getSharedStateMonitor(const moveit::core::RobotModelConstPtr &robot_model, const std::shared_ptr& tf_buffer, ros::NodeHandle nh = ros::NodeHandle() ). It calls this function using the default constructed ros::NodeHandle @@ -56,7 +55,7 @@ robot_model::RobotModelConstPtr getSharedRobotModel(const std::string& robot_des @param tf_buffer @return */ -planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model, +planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer); /** @@ -67,11 +66,9 @@ planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot @param nh A ros::NodeHandle to pass node specific configurations, such as callbacks queues. @return */ -planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model, - const std::shared_ptr& tf_buffer, - const ros::NodeHandle& nh); +planning_scene_monitor::CurrentStateMonitorPtr +getSharedStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, + const std::shared_ptr& tf_buffer, const ros::NodeHandle& nh); -} // namespace planning interface +} // namespace planning_interface } // namespace moveit - -#endif // end of MOVEIT_PLANNING_INTERFACE_COMMON_OBJECTS_ diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp b/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp index 2c44c9c0d4..684bd1851f 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp +++ b/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp @@ -58,7 +58,7 @@ struct SharedStorage boost::mutex lock_; std::weak_ptr tf_buffer_; - std::map models_; + std::map models_; std::map state_monitors_; }; @@ -113,31 +113,31 @@ std::shared_ptr getSharedTF() return buffer; } -robot_model::RobotModelConstPtr getSharedRobotModel(const std::string& robot_description) +moveit::core::RobotModelConstPtr getSharedRobotModel(const std::string& robot_description) { SharedStorage& s = getSharedStorage(); boost::mutex::scoped_lock slock(s.lock_); - auto it = s.models_.insert(std::make_pair(robot_description, robot_model::RobotModelWeakPtr())).first; - robot_model::RobotModelPtr model = it->second.lock(); + auto it = s.models_.insert(std::make_pair(robot_description, moveit::core::RobotModelWeakPtr())).first; + moveit::core::RobotModelPtr model = it->second.lock(); if (!model) { RobotModelLoader::Options opt(robot_description); opt.load_kinematics_solvers_ = true; RobotModelLoaderPtr loader(new RobotModelLoader(opt)); // create an aliasing shared_ptr - model = robot_model::RobotModelPtr(loader, loader->getModel().get()); + model = moveit::core::RobotModelPtr(loader, loader->getModel().get()); it->second = model; } return model; } -CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model, +CurrentStateMonitorPtr getSharedStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer) { return getSharedStateMonitor(robot_model, tf_buffer, ros::NodeHandle()); } -CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model, +CurrentStateMonitorPtr getSharedStateMonitor(const moveit::core::RobotModelConstPtr& robot_model, const std::shared_ptr& tf_buffer, const ros::NodeHandle& nh) { diff --git a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt index eacfa2aecb..d822174bf0 100644 --- a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt @@ -6,19 +6,24 @@ target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_object add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) add_library(${MOVEIT_LIB_NAME}_python src/wrap_python_move_group.cpp) -target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) +target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} eigenpy::eigenpy ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) add_dependencies(${MOVEIT_LIB_NAME}_python ${catkin_EXPORTED_TARGETS}) set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_move_group_interface PREFIX "") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") +if(WIN32) + set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES SUFFIX .pyd) +endif(WIN32) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME}_python ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 727b626fbe..862f6d7dd2 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -35,11 +35,9 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_INTERFACE_ -#define MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_INTERFACE_ +#pragma once #include -#include #include #include #include @@ -54,11 +52,12 @@ #include #include #include +#include #include namespace moveit { -/** \brief Simple interface to MoveIt! components */ +/** \brief Simple interface to MoveIt components */ namespace planning_interface { class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes @@ -90,7 +89,7 @@ class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes } }; -MOVEIT_CLASS_FORWARD(MoveGroupInterface); +MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc /** \class MoveGroupInterface move_group_interface.h moveit/planning_interface/move_group_interface.h @@ -106,9 +105,9 @@ class MoveGroupInterface /** \brief Specification of options to use when constructing the MoveGroupInterface class */ struct Options { - Options(const std::string& group_name, const std::string& desc = ROBOT_DESCRIPTION, + Options(std::string group_name, std::string desc = ROBOT_DESCRIPTION, const ros::NodeHandle& node_handle = ros::NodeHandle()) - : group_name_(group_name), robot_description_(desc), node_handle_(node_handle) + : group_name_(std::move(group_name)), robot_description_(std::move(desc)), node_handle_(node_handle) { } @@ -119,7 +118,7 @@ class MoveGroupInterface std::string robot_description_; /// Optionally, an instance of the RobotModel to use can be also specified - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; ros::NodeHandle node_handle_; }; @@ -152,8 +151,8 @@ class MoveGroupInterface MoveGroupInterface(const Options& opt, const std::shared_ptr& tf_buffer = std::shared_ptr(), const ros::WallDuration& wait_for_servers = ros::WallDuration()); - MOVEIT_DEPRECATED MoveGroupInterface(const Options& opt, const std::shared_ptr& tf_buffer, - const ros::Duration& wait_for_servers); + [[deprecated]] MoveGroupInterface(const Options& opt, const std::shared_ptr& tf_buffer, + const ros::Duration& wait_for_servers); /** \brief Construct a client for the MoveGroup action for a particular \e group. @@ -165,8 +164,8 @@ class MoveGroupInterface MoveGroupInterface(const std::string& group, const std::shared_ptr& tf_buffer = std::shared_ptr(), const ros::WallDuration& wait_for_servers = ros::WallDuration()); - MOVEIT_DEPRECATED MoveGroupInterface(const std::string& group, const std::shared_ptr& tf_buffer, - const ros::Duration& wait_for_servers); + [[deprecated]] MoveGroupInterface(const std::string& group, const std::shared_ptr& tf_buffer, + const ros::Duration& wait_for_servers); ~MoveGroupInterface(); @@ -178,18 +177,17 @@ class MoveGroupInterface MoveGroupInterface(const MoveGroupInterface&) = delete; MoveGroupInterface& operator=(const MoveGroupInterface&) = delete; - MoveGroupInterface(MoveGroupInterface&& other); - MoveGroupInterface& operator=(MoveGroupInterface&& other); - + MoveGroupInterface(MoveGroupInterface&& other) noexcept; + MoveGroupInterface& operator=(MoveGroupInterface&& other) noexcept; /** \brief Get the name of the group this instance operates on */ const std::string& getName() const; /** \brief Get the names of the named robot states available as targets, both either remembered states or default * states from srdf */ - const std::vector getNamedTargets(); + const std::vector& getNamedTargets() const; /** \brief Get the RobotModel object. */ - robot_model::RobotModelConstPtr getRobotModel() const; + moveit::core::RobotModelConstPtr getRobotModel() const; /** \brief Get the ROS node handle of this instance operates on */ const ros::NodeHandle& getNodeHandle() const; @@ -201,13 +199,13 @@ class MoveGroupInterface const std::vector& getJointModelGroupNames() const; /** \brief Get vector of names of joints available in move group */ - const std::vector& getJointNames(); + const std::vector& getJointNames() const; /** \brief Get vector of names of links available in move group */ - const std::vector& getLinkNames(); + const std::vector& getLinkNames() const; /** \brief Get the joint angles for targets specified by name */ - std::map getNamedTargetValues(const std::string& name); + std::map getNamedTargetValues(const std::string& name) const; /** \brief Get only the active (actuated) joints this instance operates on */ const std::vector& getActiveJoints() const; @@ -220,10 +218,11 @@ class MoveGroupInterface unsigned int getVariableCount() const; /** \brief Get the description of the planning plugin loaded by the action server */ - bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc); + bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) const; /** \brief Get the planner parameters for given group and planner_id */ - std::map getPlannerParams(const std::string& planner_id, const std::string& group = ""); + std::map getPlannerParams(const std::string& planner_id, + const std::string& group = "") const; /** \brief Set the planner parameters for given group and planner_id */ void setPlannerParams(const std::string& planner_id, const std::string& group, @@ -247,16 +246,16 @@ class MoveGroupInterface /** \brief Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,1]. The maximum joint velocity specified - in the robot model is multiplied by the factor. If outside valid range - (importantly, this includes it being set to 0.0), the factor is set to a - default value of 1.0 internally (i.e. maximum joint velocity) */ + in the robot model is multiplied by the factor. If the value is 0, it is set to + the default value, which is defined in joint_limits.yaml of the moveit_config. + If the value is greater than 1, it is set to 1.0. */ void setMaxVelocityScalingFactor(double max_velocity_scaling_factor); /** \brief Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]. The maximum joint acceleration specified - in the robot model is multiplied by the factor. If outside valid range - (importantly, this includes it being set to 0.0), the factor is set to a - default value of 1.0 internally (i.e. maximum joint acceleration) */ + in the robot model is multiplied by the factor. If the value is 0, it is set to + the default value, which is defined in joint_limits.yaml of the moveit_config. + If the value is greater than 1, it is set to 1.0. */ void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor); /** \brief Get the number of seconds set by setPlanningTime() */ @@ -304,7 +303,7 @@ class MoveGroupInterface /** \brief If a different start state should be considered instead of the current state of the robot, this function * sets that state */ - void setStartState(const robot_state::RobotState& start_state); + void setStartState(const moveit::core::RobotState& start_state); /** \brief Set the starting state for planning to be that reported by the robot's joint state publication */ void setStartStateToCurrentState(); @@ -387,7 +386,7 @@ class MoveGroupInterface If these values are out of bounds then false is returned BUT THE VALUES ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const robot_state::RobotState& robot_state); + bool setJointValueTarget(const moveit::core::RobotState& robot_state); /** \brief Set the JointValueTarget and use it for future planning requests. @@ -517,7 +516,7 @@ class MoveGroupInterface void getJointValueTarget(std::vector& group_variable_values) const; /// Get the currently set joint state goal, replaced by private getTargetRobotState() - MOVEIT_DEPRECATED const robot_state::RobotState& getJointValueTarget() const; + [[deprecated]] const moveit::core::RobotState& getJointValueTarget() const; /**@}*/ @@ -727,12 +726,18 @@ class MoveGroupInterface target. No execution is performed. The resulting plan is stored in \e plan*/ MoveItErrorCode plan(Plan& plan); - /** \brief Given a \e plan, execute it without waiting for completion. Return true on success. */ + /** \brief Given a \e plan, execute it without waiting for completion. */ MoveItErrorCode asyncExecute(const Plan& plan); - /** \brief Given a \e plan, execute it while waiting for completion. Return true on success. */ + /** \brief Given a \e robot trajectory, execute it without waiting for completion. */ + MoveItErrorCode asyncExecute(const moveit_msgs::RobotTrajectory& trajectory); + + /** \brief Given a \e plan, execute it while waiting for completion. */ MoveItErrorCode execute(const Plan& plan); + /** \brief Given a \e robot trajectory, execute it while waiting for completion. */ + MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory); + /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the waypoints is that specified by setPoseReferenceFrame(). No more than \e jump_threshold @@ -744,7 +749,7 @@ class MoveGroupInterface Return -1.0 in case of error. */ double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true, - moveit_msgs::MoveItErrorCodes* error_code = NULL); + moveit_msgs::MoveItErrorCodes* error_code = nullptr); /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the @@ -761,7 +766,7 @@ class MoveGroupInterface double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions = true, - moveit_msgs::MoveItErrorCodes* error_code = NULL); + moveit_msgs::MoveItErrorCodes* error_code = nullptr); /** \brief Stop any trajectory execution, if one is active */ void stop(); @@ -779,14 +784,15 @@ class MoveGroupInterface /** \brief Build a PickupGoal for an object named \e object and store it in \e goal_out */ moveit_msgs::PickupGoal constructPickupGoal(const std::string& object, std::vector grasps, - bool plan_only); + bool plan_only) const; /** \brief Build a PlaceGoal for an object named \e object and store it in \e goal_out */ moveit_msgs::PlaceGoal constructPlaceGoal(const std::string& object, - std::vector locations, bool plan_only); + std::vector locations, bool plan_only) const; /** \brief Convert a vector of PoseStamped to a vector of PlaceLocation */ - std::vector posesToPlaceLocations(const std::vector& poses); + std::vector + posesToPlaceLocations(const std::vector& poses) const; /**@}*/ @@ -903,28 +909,28 @@ class MoveGroupInterface bool startStateMonitor(double wait = 1.0); /** \brief Get the current joint values for the joints planned for by this instance (see getJoints()) */ - std::vector getCurrentJointValues(); + std::vector getCurrentJointValues() const; /** \brief Get the current state of the robot within the duration specified by wait. */ - robot_state::RobotStatePtr getCurrentState(double wait = 1); + moveit::core::RobotStatePtr getCurrentState(double wait = 1) const; /** \brief Get the pose for the end-effector \e end_effector_link. If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed */ - geometry_msgs::PoseStamped getCurrentPose(const std::string& end_effector_link = ""); + geometry_msgs::PoseStamped getCurrentPose(const std::string& end_effector_link = "") const; /** \brief Get the roll-pitch-yaw (XYZ) for the end-effector \e end_effector_link. If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed */ - std::vector getCurrentRPY(const std::string& end_effector_link = ""); + std::vector getCurrentRPY(const std::string& end_effector_link = "") const; /** \brief Get random joint values for the joints planned for by this instance (see getJoints()) */ - std::vector getRandomJointValues(); + std::vector getRandomJointValues() const; /** \brief Get a random reachable pose for the end-effector \e end_effector_link. If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed */ - geometry_msgs::PoseStamped getRandomPose(const std::string& end_effector_link = ""); + geometry_msgs::PoseStamped getRandomPose(const std::string& end_effector_link = "") const; /**@}*/ @@ -994,7 +1000,7 @@ class MoveGroupInterface protected: /** return the full RobotState of the joint-space target, only for internal use */ - const robot_state::RobotState& getTargetRobotState() const; + const moveit::core::RobotState& getTargetRobotState() const; private: std::map > remembered_joint_values_; @@ -1003,5 +1009,3 @@ class MoveGroupInterface }; } // namespace planning_interface } // namespace moveit - -#endif diff --git a/moveit_ros/planning_interface/move_group_interface/src/demo.cpp b/moveit_ros/planning_interface/move_group_interface/src/demo.cpp index 6aba08bcc6..0bd7017fce 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/demo.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/demo.cpp @@ -115,7 +115,7 @@ int main(int argc, char** argv) moveit::planning_interface::MoveGroupInterface group(argc > 1 ? argv[1] : "right_arm"); demoPlace(group); - sleep(2); + ros::Duration(2).sleep(); return 0; } diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 9216668fd4..17c4523c25 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -77,6 +77,8 @@ const std::string MoveGroupInterface::ROBOT_DESCRIPTION = const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps +const std::string LOGNAME = "move_group_interface"; + namespace { enum ActiveTargetType @@ -100,20 +102,20 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { std::string error = "Unable to construct robot model. Please make sure all needed information is on the " "parameter server."; - ROS_FATAL_STREAM_NAMED("move_group_interface", error); + ROS_FATAL_STREAM_NAMED(LOGNAME, error); throw std::runtime_error(error); } if (!getRobotModel()->hasJointModelGroup(opt.group_name_)) { std::string error = "Group '" + opt.group_name_ + "' was not found."; - ROS_FATAL_STREAM_NAMED("move_group_interface", error); + ROS_FATAL_STREAM_NAMED(LOGNAME, error); throw std::runtime_error(error); } joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_); - joint_state_target_.reset(new robot_state::RobotState(getRobotModel())); + joint_state_target_.reset(new moveit::core::RobotState(getRobotModel())); joint_state_target_->setToDefaultValues(); active_target_ = JOINT; can_look_ = false; @@ -124,8 +126,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl goal_orientation_tolerance_ = 1e-3; // ~0.1 deg allowed_planning_time_ = 5.0; num_planning_attempts_ = 1; - max_velocity_scaling_factor_ = 1.0; - max_acceleration_scaling_factor_ = 1.0; + node_handle_.param("robot_description_planning/default_velocity_scaling_factor", + max_velocity_scaling_factor_, 0.1); + node_handle_.param("robot_description_planning/default_acceleration_scaling_factor", + max_acceleration_scaling_factor_, 0.1); initializing_constraints_ = false; if (joint_model_group_->isChain()) @@ -172,14 +176,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl plan_grasps_service_ = node_handle_.serviceClient(GRASP_PLANNING_SERVICE_NAME); - ROS_INFO_STREAM_NAMED("move_group_interface", "Ready to take commands for planning group " << opt.group_name_ - << "."); + ROS_INFO_STREAM_NAMED(LOGNAME, "Ready to take commands for planning group " << opt.group_name_ << "."); } template - void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) + void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) const { - ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "Waiting for move_group action server (%s)...", name.c_str()); // wait for the server (and spin as needed) if (timeout == ros::WallTime()) // wait forever @@ -195,8 +198,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else // in case of nodelets and specific callback queue implementations { - ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " - "handling."); + ROS_WARN_ONCE_NAMED(LOGNAME, "Non-default CallbackQueue: Waiting for external queue " + "handling."); } } } @@ -213,8 +216,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else // in case of nodelets and specific callback queue implementations { - ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " - "handling."); + ROS_WARN_ONCE_NAMED(LOGNAME, "Non-default CallbackQueue: Waiting for external queue " + "handling."); } } } @@ -228,7 +231,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_DEBUG_NAMED("move_group_interface", "Connected to '%s'", name.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "Connected to '%s'", name.c_str()); } } @@ -248,12 +251,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return opt_; } - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } - const robot_model::JointModelGroup* getJointModelGroup() const + const moveit::core::JointModelGroup* getJointModelGroup() const { return joint_model_group_; } @@ -335,24 +338,51 @@ class MoveGroupInterface::MoveGroupInterfaceImpl num_planning_attempts_ = num_planning_attempts; } - void setMaxVelocityScalingFactor(double max_velocity_scaling_factor) + void setMaxVelocityScalingFactor(double value) { - max_velocity_scaling_factor_ = max_velocity_scaling_factor; + setMaxScalingFactor(max_velocity_scaling_factor_, value, "velocity_scaling_factor", 0.1); } - void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) + void setMaxAccelerationScalingFactor(double value) { - max_acceleration_scaling_factor_ = max_acceleration_scaling_factor; + setMaxScalingFactor(max_acceleration_scaling_factor_, value, "acceleration_scaling_factor", 0.1); } - robot_state::RobotState& getTargetRobotState() + void setMaxScalingFactor(double& variable, const double target_value, const char* factor_name, double fallback_value) + { + if (target_value > 1.0) + { + ROS_WARN_NAMED(LOGNAME, "Limiting max_%s (%.2f) to 1.0.", factor_name, target_value); + variable = 1.0; + } + else if (target_value <= 0.0) + { + node_handle_.param(std::string("robot_description_planning/default_") + factor_name, variable, + fallback_value); + if (target_value < 0.0) + { + ROS_WARN_NAMED(LOGNAME, "max_%s < 0.0! Setting to default: %.2f.", factor_name, variable); + } + } + else + { + variable = target_value; + } + } + + moveit::core::RobotState& getTargetRobotState() { return *joint_state_target_; } - void setStartState(const robot_state::RobotState& start_state) + const moveit::core::RobotState& getTargetRobotState() const { - considered_start_state_.reset(new robot_state::RobotState(start_state)); + return *joint_state_target_; + } + + void setStartState(const moveit::core::RobotState& start_state) + { + considered_start_state_.reset(new moveit::core::RobotState(start_state)); } void setStartStateToCurrentState() @@ -360,13 +390,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl considered_start_state_.reset(); } - robot_state::RobotStatePtr getStartState() + moveit::core::RobotStatePtr getStartState() { if (considered_start_state_) return considered_start_state_; else { - robot_state::RobotStatePtr s; + moveit::core::RobotStatePtr s; getCurrentState(s); return s; } @@ -414,7 +444,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_ERROR_NAMED("move_group_interface", "Unable to transform from frame '%s' to frame '%s'", frame.c_str(), + ROS_ERROR_NAMED(LOGNAME, "Unable to transform from frame '%s' to frame '%s'", frame.c_str(), getRobotModel()->getModelFrame().c_str()); return false; } @@ -463,7 +493,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; if (eef.empty()) { - ROS_ERROR_NAMED("move_group_interface", "No end-effector to set the pose for"); + ROS_ERROR_NAMED(LOGNAME, "No end-effector to set the pose for"); return false; } else @@ -495,7 +525,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // or return an error static const geometry_msgs::PoseStamped UNKNOWN; - ROS_ERROR_NAMED("move_group_interface", "Pose for end-effector '%s' not known.", eef.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Pose for end-effector '%s' not known.", eef.c_str()); return UNKNOWN; } @@ -510,7 +540,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // or return an error static const std::vector EMPTY; - ROS_ERROR_NAMED("move_group_interface", "Poses for end-effector '%s' are not known.", eef.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Poses for end-effector '%s' are not known.", eef.c_str()); return EMPTY; } @@ -543,7 +573,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!current_state_monitor_) { - ROS_ERROR_NAMED("move_group_interface", "Unable to monitor current robot state"); + ROS_ERROR_NAMED(LOGNAME, "Unable to monitor current robot state"); return false; } @@ -555,11 +585,11 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return true; } - bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds = 1.0) + bool getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds = 1.0) { if (!current_state_monitor_) { - ROS_ERROR_NAMED("move_group_interface", "Unable to get current robot state"); + ROS_ERROR_NAMED(LOGNAME, "Unable to get current robot state"); return false; } @@ -569,7 +599,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds)) { - ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state"); + ROS_ERROR_NAMED(LOGNAME, "Failed to fetch current robot state"); return false; } @@ -578,7 +608,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } /** \brief Convert a vector of PoseStamped to a vector of PlaceLocation */ - std::vector posesToPlaceLocations(const std::vector& poses) + std::vector + posesToPlaceLocations(const std::vector& poses) const { std::vector locations; for (const geometry_msgs::PoseStamped& pose : poses) @@ -598,8 +629,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl location.place_pose = pose; locations.push_back(location); } - ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations", - (unsigned int)locations.size()); + ROS_DEBUG_NAMED(LOGNAME, "Move group interface has %u place locations", (unsigned int)locations.size()); return locations; } @@ -607,20 +637,20 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!place_action_client_) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found"); + ROS_ERROR_STREAM_NAMED(LOGNAME, "place action client not found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } if (!place_action_client_->isServerConnected()) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + ROS_WARN_STREAM_NAMED(LOGNAME, "place action server not connected"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); } place_action_client_->sendGoal(goal); - ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size()); + ROS_DEBUG_NAMED(LOGNAME, "Sent place goal with %d locations", (int)goal.place_locations.size()); if (!place_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early"); + ROS_INFO_STREAM_NAMED(LOGNAME, "Place action returned early"); } if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { @@ -628,8 +658,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": " - << place_action_client_->getState().getText()); + ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << place_action_client_->getState().toString() << ": " + << place_action_client_->getState().getText()); return MoveItErrorCode(place_action_client_->getResult()->error_code); } } @@ -638,19 +668,19 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!pick_action_client_) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found"); + ROS_ERROR_STREAM_NAMED(LOGNAME, "pick action client not found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } if (!pick_action_client_->isServerConnected()) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + ROS_WARN_STREAM_NAMED(LOGNAME, "pick action server not connected"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); } pick_action_client_->sendGoal(goal); if (!pick_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early"); + ROS_INFO_STREAM_NAMED(LOGNAME, "Pickup action returned early"); } if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { @@ -658,8 +688,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": " - << pick_action_client_->getState().getText()); + ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << pick_action_client_->getState().toString() << ": " + << pick_action_client_->getState().getText()); return MoveItErrorCode(pick_action_client_->getResult()->error_code); } } @@ -676,8 +706,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (objects.empty()) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '" - << object << "', but the object could not be found"); + ROS_ERROR_STREAM_NAMED(LOGNAME, + "Asked for grasps for the object '" << object << "', but the object could not be found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME); } @@ -686,13 +716,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false) { - if (!plan_grasps_service_) + if (!plan_grasps_service_.exists()) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '" - << GRASP_PLANNING_SERVICE_NAME - << "' is not available." - " This has to be implemented and started separately."); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Grasp planning service '" + << GRASP_PLANNING_SERVICE_NAME + << "' is not available." + " This has to be implemented and started separately."); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); } moveit_msgs::GraspPlanning::Request request; @@ -702,11 +732,11 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.target = object; request.support_surfaces.push_back(support_surface_); - ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner..."); + ROS_DEBUG_NAMED(LOGNAME, "Calling grasp planner..."); if (!plan_grasps_service_.call(request, response) || response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { - ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick."); + ROS_ERROR_NAMED(LOGNAME, "Grasp planning failed. Unable to pick."); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } @@ -717,11 +747,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!move_action_client_) { + ROS_ERROR_STREAM_NAMED(LOGNAME, "move action client not found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } if (!move_action_client_->isServerConnected()) { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); } moveit_msgs::MoveGroupGoal goal; @@ -735,7 +767,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl move_action_client_->sendGoal(goal); if (!move_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); + ROS_INFO_STREAM_NAMED(LOGNAME, "MoveGroup action returned early"); } if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { @@ -746,8 +778,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": " - << move_action_client_->getState().getText()); + ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << move_action_client_->getState().toString() << ": " + << move_action_client_->getState().getText()); return MoveItErrorCode(move_action_client_->getResult()->error_code); } } @@ -756,11 +788,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!move_action_client_) { + ROS_ERROR_STREAM_NAMED(LOGNAME, "move action client not found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } if (!move_action_client_->isServerConnected()) { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); } moveit_msgs::MoveGroupGoal goal; @@ -780,7 +814,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!move_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); + ROS_INFO_STREAM_NAMED(LOGNAME, "MoveGroup action returned early"); } if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) @@ -789,21 +823,27 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString() - << ": " << move_action_client_->getState().getText()); + ROS_INFO_STREAM_NAMED(LOGNAME, move_action_client_->getState().toString() + << ": " << move_action_client_->getState().getText()); return MoveItErrorCode(move_action_client_->getResult()->error_code); } } - MoveItErrorCode execute(const Plan& plan, bool wait) + MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait) { - if (!execute_action_client_->isServerConnected()) + if (!execute_action_client_) { + ROS_ERROR_STREAM_NAMED(LOGNAME, "execute action client not found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } + if (!execute_action_client_->isServerConnected()) + { + ROS_WARN_STREAM_NAMED(LOGNAME, "execute action server not connected"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE); + } moveit_msgs::ExecuteTrajectoryGoal goal; - goal.trajectory = plan.trajectory_; + goal.trajectory = trajectory; execute_action_client_->sendGoal(goal); if (!wait) @@ -813,7 +853,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!execute_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "ExecuteTrajectory action returned early"); + ROS_INFO_STREAM_NAMED(LOGNAME, "ExecuteTrajectory action returned early"); } if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) @@ -822,8 +862,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString() - << ": " << execute_action_client_->getState().getText()); + ROS_INFO_STREAM_NAMED(LOGNAME, execute_action_client_->getState().toString() + << ": " << execute_action_client_->getState().getText()); return MoveItErrorCode(execute_action_client_->getResult()->error_code); } } @@ -836,7 +876,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl moveit_msgs::GetCartesianPath::Response res; if (considered_start_state_) - robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state); + moveit::core::robotStateToRobotStateMsg(*considered_start_state_, req.start_state); else req.start_state.is_diff = true; @@ -889,7 +929,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } if (l.empty()) { - ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str()); + ROS_ERROR_NAMED(LOGNAME, "No known link to attach object '%s' to", object.c_str()); return false; } moveit_msgs::AttachedCollisionObject aco; @@ -972,13 +1012,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl void allowLooking(bool flag) { can_look_ = flag; - ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no"); + ROS_INFO_NAMED(LOGNAME, "Looking around: %s", can_look_ ? "yes" : "no"); } void allowReplanning(bool flag) { can_replan_ = flag; - ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no"); + ROS_INFO_NAMED(LOGNAME, "Replanning: %s", can_replan_ ? "yes" : "no"); } void setReplanningDelay(double delay) @@ -992,7 +1032,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return replan_delay_; } - void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request) + void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request) const { request.group_name = opt_.group_name_; request.num_planning_attempts = num_planning_attempts_; @@ -1003,7 +1043,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.workspace_parameters = workspace_parameters_; if (considered_start_state_) - robot_state::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); + moveit::core::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); else request.start_state.is_diff = true; @@ -1017,9 +1057,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { // find out how many goals are specified std::size_t goal_count = 0; - for (std::map >::const_iterator it = pose_targets_.begin(); - it != pose_targets_.end(); ++it) - goal_count = std::max(goal_count, it->second.size()); + for (const auto& pose_target : pose_targets_) + goal_count = std::max(goal_count, pose_target.second.size()); // start filling the goals; // each end effector has a number of possible poses (K) as valid goals @@ -1027,13 +1066,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // to reach the goal that corresponds to the goals of the other end effectors request.goal_constraints.resize(goal_count); - for (std::map >::const_iterator it = pose_targets_.begin(); - it != pose_targets_.end(); ++it) + for (const auto& pose_target : pose_targets_) { - for (std::size_t i = 0; i < it->second.size(); ++i) + for (std::size_t i = 0; i < pose_target.second.size(); ++i) { moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints( - it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_); + pose_target.first, pose_target.second[i], goal_position_tolerance_, goal_orientation_tolerance_); if (active_target_ == ORIENTATION) c.position_constraints.clear(); if (active_target_ == POSITION) @@ -1043,7 +1081,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } } else - ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation"); + ROS_ERROR_NAMED(LOGNAME, "Unable to construct MotionPlanRequest representation"); if (path_constraints_) request.path_constraints = *path_constraints_; @@ -1051,13 +1089,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.trajectory_constraints = *trajectory_constraints_; } - void constructGoal(moveit_msgs::MoveGroupGoal& goal) + void constructGoal(moveit_msgs::MoveGroupGoal& goal) const { constructMotionPlanRequest(goal.request); } moveit_msgs::PickupGoal constructPickupGoal(const std::string& object, std::vector&& grasps, - bool plan_only = false) + bool plan_only = false) const { moveit_msgs::PickupGoal goal; goal.target_name = object; @@ -1084,7 +1122,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } moveit_msgs::PlaceGoal constructPlaceGoal(const std::string& object, - std::vector&& locations, bool plan_only = false) + std::vector&& locations, + bool plan_only = false) const { moveit_msgs::PlaceGoal goal; goal.group_name = opt_.group_name_; @@ -1213,7 +1252,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } catch (std::exception& ex) { - ROS_ERROR_NAMED("move_group_interface", "%s", ex.what()); + ROS_ERROR_NAMED(LOGNAME, "%s", ex.what()); } initializing_constraints_ = false; } @@ -1221,7 +1260,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl Options opt_; ros::NodeHandle node_handle_; std::shared_ptr tf_buffer_; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; std::unique_ptr > move_action_client_; std::unique_ptr > execute_action_client_; @@ -1229,7 +1268,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl std::unique_ptr > place_action_client_; // general planning params - robot_state::RobotStatePtr considered_start_state_; + moveit::core::RobotStatePtr considered_start_state_; moveit_msgs::WorkspaceParameters workspace_parameters_; double allowed_planning_time_; std::string planner_id_; @@ -1244,8 +1283,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl double replan_delay_; // joint state goal - robot_state::RobotStatePtr joint_state_target_; - const robot_model::JointModelGroup* joint_model_group_; + moveit::core::RobotStatePtr joint_state_target_; + const moveit::core::JointModelGroup* joint_model_group_; // pose goal; // for each link we have a set of possible goal locations; @@ -1304,13 +1343,13 @@ MoveGroupInterface::~MoveGroupInterface() delete impl_; } -MoveGroupInterface::MoveGroupInterface(MoveGroupInterface&& other) +MoveGroupInterface::MoveGroupInterface(MoveGroupInterface&& other) noexcept : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_) { other.impl_ = nullptr; } -MoveGroupInterface& MoveGroupInterface::operator=(MoveGroupInterface&& other) +MoveGroupInterface& MoveGroupInterface::operator=(MoveGroupInterface&& other) noexcept { if (this != &other) { @@ -1328,22 +1367,14 @@ const std::string& MoveGroupInterface::getName() const return impl_->getOptions().group_name_; } -const std::vector MoveGroupInterface::getNamedTargets() +const std::vector& MoveGroupInterface::getNamedTargets() const { - const robot_model::RobotModelConstPtr& robot = getRobotModel(); - const std::string& group = getName(); - const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group); - - if (joint_group) - { - return joint_group->getDefaultStateNames(); - } - - std::vector empty; - return empty; + // The pointer returned by getJointModelGroup is guaranteed by the class + // constructor to always be non-null + return impl_->getJointModelGroup()->getDefaultStateNames(); } -robot_model::RobotModelConstPtr MoveGroupInterface::getRobotModel() const +moveit::core::RobotModelConstPtr MoveGroupInterface::getRobotModel() const { return impl_->getRobotModel(); } @@ -1353,13 +1384,13 @@ const ros::NodeHandle& MoveGroupInterface::getNodeHandle() const return impl_->getOptions().node_handle_; } -bool MoveGroupInterface::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) +bool MoveGroupInterface::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) const { return impl_->getInterfaceDescription(desc); } std::map MoveGroupInterface::getPlannerParams(const std::string& planner_id, - const std::string& group) + const std::string& group) const { return impl_->getPlannerParams(planner_id, group); } @@ -1417,12 +1448,22 @@ MoveItErrorCode MoveGroupInterface::move() MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan) { - return impl_->execute(plan, false); + return impl_->execute(plan.trajectory_, false); +} + +MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::RobotTrajectory& trajectory) +{ + return impl_->execute(trajectory, false); } MoveItErrorCode MoveGroupInterface::execute(const Plan& plan) { - return impl_->execute(plan, true); + return impl_->execute(plan.trajectory_, true); +} + +MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::RobotTrajectory& trajectory) +{ + return impl_->execute(trajectory, true); } MoveItErrorCode MoveGroupInterface::plan(Plan& plan) @@ -1432,20 +1473,20 @@ MoveItErrorCode MoveGroupInterface::plan(Plan& plan) moveit_msgs::PickupGoal MoveGroupInterface::constructPickupGoal(const std::string& object, std::vector grasps, - bool plan_only = false) + bool plan_only = false) const { return impl_->constructPickupGoal(object, std::move(grasps), plan_only); } moveit_msgs::PlaceGoal MoveGroupInterface::constructPlaceGoal(const std::string& object, std::vector locations, - bool plan_only = false) + bool plan_only = false) const { return impl_->constructPlaceGoal(object, std::move(locations), plan_only); } std::vector -MoveGroupInterface::posesToPlaceLocations(const std::vector& poses) +MoveGroupInterface::posesToPlaceLocations(const std::vector& poses) const { return impl_->posesToPlaceLocations(poses); } @@ -1504,13 +1545,13 @@ void MoveGroupInterface::stop() void MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state) { - robot_state::RobotStatePtr rs; + moveit::core::RobotStatePtr rs; impl_->getCurrentState(rs); - robot_state::robotStateMsgToRobotState(start_state, *rs); + moveit::core::robotStateMsgToRobotState(start_state, *rs); setStartState(*rs); } -void MoveGroupInterface::setStartState(const robot_state::RobotState& start_state) +void MoveGroupInterface::setStartState(const moveit::core::RobotState& start_state) { impl_->setStartState(start_state); } @@ -1526,22 +1567,22 @@ void MoveGroupInterface::setRandomTarget() impl_->setTargetType(JOINT); } -const std::vector& MoveGroupInterface::getJointNames() +const std::vector& MoveGroupInterface::getJointNames() const { return impl_->getJointModelGroup()->getVariableNames(); } -const std::vector& MoveGroupInterface::getLinkNames() +const std::vector& MoveGroupInterface::getLinkNames() const { return impl_->getJointModelGroup()->getLinkModelNames(); } -std::map MoveGroupInterface::getNamedTargetValues(const std::string& name) +std::map MoveGroupInterface::getNamedTargetValues(const std::string& name) const { std::map >::const_iterator it = remembered_joint_values_.find(name); std::map positions; - if (it != remembered_joint_values_.end()) + if (it != remembered_joint_values_.cend()) { std::vector names = impl_->getJointModelGroup()->getVariableNames(); for (size_t x = 0; x < names.size(); ++x) @@ -1570,7 +1611,7 @@ bool MoveGroupInterface::setNamedTarget(const std::string& name) impl_->setTargetType(JOINT); return true; } - ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str()); + ROS_ERROR_NAMED(LOGNAME, "The requested named target '%s' does not exist", name.c_str()); return false; } } @@ -1626,7 +1667,7 @@ bool MoveGroupInterface::setJointValueTarget(const std::vector& var return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance()); } -bool MoveGroupInterface::setJointValueTarget(const robot_state::RobotState& rstate) +bool MoveGroupInterface::setJointValueTarget(const moveit::core::RobotState& rstate) { impl_->setTargetType(JOINT); impl_->getTargetRobotState() = rstate; @@ -1642,7 +1683,7 @@ bool MoveGroupInterface::setJointValueTarget(const std::string& joint_name, doub bool MoveGroupInterface::setJointValueTarget(const std::string& joint_name, const std::vector& values) { impl_->setTargetType(JOINT); - const robot_model::JointModel* jm = impl_->getJointModelGroup()->getJointModel(joint_name); + const moveit::core::JointModel* jm = impl_->getJointModelGroup()->getJointModel(joint_name); if (jm && jm->getVariableCount() == values.size()) { impl_->getTargetRobotState().setJointPositions(jm, values); @@ -1694,12 +1735,12 @@ bool MoveGroupInterface::setApproximateJointValueTarget(const Eigen::Isometry3d& return setApproximateJointValueTarget(msg, end_effector_link); } -const robot_state::RobotState& MoveGroupInterface::getJointValueTarget() const +const moveit::core::RobotState& MoveGroupInterface::getJointValueTarget() const { return impl_->getTargetRobotState(); } -const robot_state::RobotState& MoveGroupInterface::getTargetRobotState() const +const moveit::core::RobotState& MoveGroupInterface::getTargetRobotState() const { return impl_->getTargetRobotState(); } @@ -1725,7 +1766,7 @@ bool MoveGroupInterface::setEndEffectorLink(const std::string& link_name) bool MoveGroupInterface::setEndEffector(const std::string& eef_name) { - const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name); + const moveit::core::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name); if (jmg) return setEndEffectorLink(jmg->getEndEffectorParentGroup().second); return false; @@ -1799,7 +1840,7 @@ bool MoveGroupInterface::setPoseTargets(const std::vectorstartStateMonitor(wait); } -std::vector MoveGroupInterface::getCurrentJointValues() +std::vector MoveGroupInterface::getCurrentJointValues() const { - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; std::vector values; if (impl_->getCurrentState(current_state)) current_state->copyJointGroupPositions(getName(), values); return values; } -std::vector MoveGroupInterface::getRandomJointValues() +std::vector MoveGroupInterface::getRandomJointValues() const { std::vector r; impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getTargetRobotState().getRandomNumberGenerator(), r); return r; } -geometry_msgs::PoseStamped MoveGroupInterface::getRandomPose(const std::string& end_effector_link) +geometry_msgs::PoseStamped MoveGroupInterface::getRandomPose(const std::string& end_effector_link) const { const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; Eigen::Isometry3d pose; pose.setIdentity(); if (eef.empty()) - ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); + ROS_ERROR_NAMED(LOGNAME, "No end-effector specified"); else { - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; if (impl_->getCurrentState(current_state)) { current_state->setToRandomPositions(impl_->getJointModelGroup()); - const robot_model::LinkModel* lm = current_state->getLinkModel(eef); + const moveit::core::LinkModel* lm = current_state->getLinkModel(eef); if (lm) pose = current_state->getGlobalLinkTransform(lm); } @@ -2007,19 +2048,19 @@ geometry_msgs::PoseStamped MoveGroupInterface::getRandomPose(const std::string& return pose_msg; } -geometry_msgs::PoseStamped MoveGroupInterface::getCurrentPose(const std::string& end_effector_link) +geometry_msgs::PoseStamped MoveGroupInterface::getCurrentPose(const std::string& end_effector_link) const { const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; Eigen::Isometry3d pose; pose.setIdentity(); if (eef.empty()) - ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); + ROS_ERROR_NAMED(LOGNAME, "No end-effector specified"); else { - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; if (impl_->getCurrentState(current_state)) { - const robot_model::LinkModel* lm = current_state->getLinkModel(eef); + const moveit::core::LinkModel* lm = current_state->getLinkModel(eef); if (lm) pose = current_state->getGlobalLinkTransform(lm); } @@ -2031,18 +2072,18 @@ geometry_msgs::PoseStamped MoveGroupInterface::getCurrentPose(const std::string& return pose_msg; } -std::vector MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link) +std::vector MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link) const { std::vector result; const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; if (eef.empty()) - ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); + ROS_ERROR_NAMED(LOGNAME, "No end-effector specified"); else { - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; if (impl_->getCurrentState(current_state)) { - const robot_model::LinkModel* lm = current_state->getLinkModel(eef); + const moveit::core::LinkModel* lm = current_state->getLinkModel(eef); if (lm) { result.resize(3); @@ -2073,9 +2114,9 @@ unsigned int MoveGroupInterface::getVariableCount() const return impl_->getJointModelGroup()->getVariableCount(); } -robot_state::RobotStatePtr MoveGroupInterface::getCurrentState(double wait) +moveit::core::RobotStatePtr MoveGroupInterface::getCurrentState(double wait) const { - robot_state::RobotStatePtr current_state; + moveit::core::RobotStatePtr current_state; impl_->getCurrentState(current_state, wait); return current_state; } diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index c367477343..b063ff1e30 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -57,6 +58,8 @@ namespace bp = boost::python; +using moveit::py_bindings_tools::GILReleaser; + namespace moveit { namespace planning_interface @@ -94,21 +97,23 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return setJointValueTarget(v); } - bool setJointValueTargetFromPosePython(const std::string& pose_str, const std::string& eef, bool approx) + bool setJointValueTargetFromPosePython(const py_bindings_tools::ByteString& pose_str, const std::string& eef, + bool approx) { geometry_msgs::Pose pose_msg; py_bindings_tools::deserializeMsg(pose_str, pose_msg); return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef); } - bool setJointValueTargetFromPoseStampedPython(const std::string& pose_str, const std::string& eef, bool approx) + bool setJointValueTargetFromPoseStampedPython(const py_bindings_tools::ByteString& pose_str, const std::string& eef, + bool approx) { geometry_msgs::PoseStamped pose_msg; py_bindings_tools::deserializeMsg(pose_str, pose_msg); return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef); } - bool setJointValueTargetFromJointStatePython(const std::string& js_str) + bool setJointValueTargetFromJointStatePython(const py_bindings_tools::ByteString& js_str) { sensor_msgs::JointState js_msg; py_bindings_tools::deserializeMsg(js_str, js_msg); @@ -125,10 +130,10 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return l; } - std::string getJointValueTarget() + py_bindings_tools::ByteString getJointValueTarget() { moveit_msgs::RobotState msg; - const robot_state::RobotState state = moveit::planning_interface::MoveGroupInterface::getTargetRobotState(); + const moveit::core::RobotState state = moveit::planning_interface::MoveGroupInterface::getTargetRobotState(); moveit::core::robotStateToRobotStateMsg(state, msg); return py_bindings_tools::serializeMsg(msg); } @@ -143,7 +148,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return getPlanningFrame().c_str(); } - std::string getInterfaceDescriptionPython() + py_bindings_tools::ByteString getInterfaceDescriptionPython() { moveit_msgs::PlannerInterfaceDescription msg; getInterfaceDescription(msg); @@ -257,18 +262,42 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer convertListToPose(pose, msg.pose); msg.header.frame_id = getPoseReferenceFrame(); msg.header.stamp = ros::Time::now(); + GILReleaser gr; return place(object_name, msg, plan_only) == MoveItErrorCode::SUCCESS; } - bool placeLocation(const std::string& object_name, const std::string& location_str, bool plan_only = false) + bool placePoses(const std::string& object_name, const bp::list& poses_list, bool plan_only = false) + { + int l = bp::len(poses_list); + std::vector poses(l); + for (int i = 0; i < l; ++i) + py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(poses_list[i]), poses[i]); + GILReleaser gr; + return place(object_name, poses, plan_only) == MoveItErrorCode::SUCCESS; + } + + bool placeLocation(const std::string& object_name, const py_bindings_tools::ByteString& location_str, + bool plan_only = false) { std::vector locations(1); py_bindings_tools::deserializeMsg(location_str, locations[0]); + GILReleaser gr; + return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS; + } + + bool placeLocations(const std::string& object_name, const bp::list& location_list, bool plan_only = false) + { + int l = bp::len(location_list); + std::vector locations(l); + for (int i = 0; i < l; ++i) + py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(location_list[i]), locations[i]); + GILReleaser gr; return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS; } bool placeAnywhere(const std::string& object_name, bool plan_only = false) { + GILReleaser gr; return place(object_name, plan_only) == MoveItErrorCode::SUCCESS; } @@ -303,17 +332,25 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer bp::dict getCurrentStateBoundedPython() { - robot_state::RobotStatePtr current = getCurrentState(); + moveit::core::RobotStatePtr current = getCurrentState(); current->enforceBounds(); moveit_msgs::RobotState rsmv; - robot_state::robotStateToRobotStateMsg(*current, rsmv); + moveit::core::robotStateToRobotStateMsg(*current, rsmv); bp::dict output; for (size_t x = 0; x < rsmv.joint_state.name.size(); ++x) output[rsmv.joint_state.name[x]] = rsmv.joint_state.position[x]; return output; } - void setStartStatePython(const std::string& msg_str) + py_bindings_tools::ByteString getCurrentStatePython() + { + moveit::core::RobotStatePtr current_state = getCurrentState(); + moveit_msgs::RobotState state_message; + moveit::core::robotStateToRobotStateMsg(*current_state, state_message); + return py_bindings_tools::serializeMsg(state_message); + } + + void setStartStatePython(const py_bindings_tools::ByteString& msg_str) { moveit_msgs::RobotState msg; py_bindings_tools::deserializeMsg(msg_str, msg); @@ -326,7 +363,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer convertListToArrayOfPoses(poses, msg); return setPoseTargets(msg, end_effector_link); } - std::string getPoseTargetPython(const std::string& end_effector_link) + py_bindings_tools::ByteString getPoseTargetPython(const std::string& end_effector_link) { geometry_msgs::PoseStamped pose = moveit::planning_interface::MoveGroupInterface::getPoseTarget(end_effector_link); return py_bindings_tools::serializeMsg(pose); @@ -375,6 +412,11 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return getName().c_str(); } + const char* getPlannerIdCStr() const + { + return getPlannerId().c_str(); + } + bp::dict getNamedTargetValuesPython(const std::string& name) { bp::dict output; @@ -393,6 +435,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer bool movePython() { + GILReleaser gr; return move() == MoveItErrorCode::SUCCESS; } @@ -406,14 +449,15 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return attachObject(object_name, link_name, py_bindings_tools::stringFromList(touch_links)); } - bool executePython(const std::string& plan_str) + bool executePython(const py_bindings_tools::ByteString& plan_str) { MoveGroupInterface::Plan plan; py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); + GILReleaser gr; return execute(plan) == MoveItErrorCode::SUCCESS; } - bool asyncExecutePython(const std::string& plan_str) + bool asyncExecutePython(const py_bindings_tools::ByteString& plan_str) { MoveGroupInterface::Plan plan; py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); @@ -423,7 +467,11 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer bp::tuple planPython() { MoveGroupInterface::Plan plan; - moveit_msgs::MoveItErrorCodes res = MoveGroupInterface::plan(plan); + moveit_msgs::MoveItErrorCodes res; + { + GILReleaser gr; + res = MoveGroupInterface::plan(plan); + } return bp::make_tuple(py_bindings_tools::serializeMsg(res), py_bindings_tools::serializeMsg(plan.trajectory_), plan.planning_time_); } @@ -436,7 +484,8 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer } bp::tuple computeCartesianPathConstrainedPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions, const std::string& path_constraints_str) + bool avoid_collisions, + const py_bindings_tools::ByteString& path_constraints_str) { moveit_msgs::Constraints path_constraints; py_bindings_tools::deserializeMsg(path_constraints_str, path_constraints); @@ -449,15 +498,19 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer std::vector poses; convertListToArrayOfPoses(waypoints, poses); moveit_msgs::RobotTrajectory trajectory; - double fraction = - computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions); + double fraction; + { + GILReleaser gr; + fraction = computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions); + } return bp::make_tuple(py_bindings_tools::serializeMsg(trajectory), fraction); } - int pickGrasp(const std::string& object, const std::string& grasp_str, bool plan_only = false) + int pickGrasp(const std::string& object, const py_bindings_tools::ByteString& grasp_str, bool plan_only = false) { moveit_msgs::Grasp grasp; py_bindings_tools::deserializeMsg(grasp_str, grasp); + GILReleaser gr; return pick(object, grasp, plan_only).val; } @@ -466,27 +519,28 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer int l = bp::len(grasp_list); std::vector grasps(l); for (int i = 0; i < l; ++i) - py_bindings_tools::deserializeMsg(bp::extract(grasp_list[i]), grasps[i]); + py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(grasp_list[i]), grasps[i]); + GILReleaser gr; return pick(object, std::move(grasps), plan_only).val; } - void setPathConstraintsFromMsg(const std::string& constraints_str) + void setPathConstraintsFromMsg(const py_bindings_tools::ByteString& constraints_str) { moveit_msgs::Constraints constraints_msg; py_bindings_tools::deserializeMsg(constraints_str, constraints_msg); setPathConstraints(constraints_msg); } - std::string getPathConstraintsPython() + py_bindings_tools::ByteString getPathConstraintsPython() { moveit_msgs::Constraints constraints_msg(getPathConstraints()); - std::string constraints_str = py_bindings_tools::serializeMsg(constraints_msg); - return constraints_str; + return py_bindings_tools::serializeMsg(constraints_msg); } - std::string retimeTrajectory(const std::string& ref_state_str, const std::string& traj_str, - double velocity_scaling_factor, double acceleration_scaling_factor, - std::string algorithm) + py_bindings_tools::ByteString retimeTrajectory(const py_bindings_tools::ByteString& ref_state_str, + const py_bindings_tools::ByteString& traj_str, + double velocity_scaling_factor, double acceleration_scaling_factor, + const std::string& algorithm) { // Convert reference state message to object moveit_msgs::RobotState ref_state_msg; @@ -497,62 +551,93 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer // Convert trajectory message to object moveit_msgs::RobotTrajectory traj_msg; py_bindings_tools::deserializeMsg(traj_str, traj_msg); - robot_trajectory::RobotTrajectory traj_obj(getRobotModel(), getName()); - traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg); - - // Do the actual retiming - if (algorithm == "iterative_time_parameterization") - { - trajectory_processing::IterativeParabolicTimeParameterization time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); - } - else if (algorithm == "iterative_spline_parameterization") + bool algorithm_found = true; { - trajectory_processing::IterativeSplineParameterization time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); - } - else if (algorithm == "time_optimal_trajectory_generation") - { - trajectory_processing::TimeOptimalTrajectoryGeneration time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); - } - else - { - ROS_ERROR_STREAM_NAMED("move_group_py", "Unknown time parameterization algorithm: " << algorithm); - return py_bindings_tools::serializeMsg(moveit_msgs::RobotTrajectory()); - } + GILReleaser gr; + robot_trajectory::RobotTrajectory traj_obj(getRobotModel(), getName()); + traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg); - // Convert the retimed trajectory back into a message - traj_obj.getRobotTrajectoryMsg(traj_msg); - std::string traj_str = py_bindings_tools::serializeMsg(traj_msg); + // Do the actual retiming + if (algorithm == "iterative_time_parameterization") + { + trajectory_processing::IterativeParabolicTimeParameterization time_param; + time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); + } + else if (algorithm == "iterative_spline_parameterization") + { + trajectory_processing::IterativeSplineParameterization time_param; + time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); + } + else if (algorithm == "time_optimal_trajectory_generation") + { + trajectory_processing::TimeOptimalTrajectoryGeneration time_param; + time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); + } + else + { + ROS_ERROR_STREAM_NAMED("move_group_py", "Unknown time parameterization algorithm: " << algorithm); + algorithm_found = false; + traj_msg = moveit_msgs::RobotTrajectory(); + } - // Return it. - return traj_str; + if (algorithm_found) + // Convert the retimed trajectory back into a message + traj_obj.getRobotTrajectoryMsg(traj_msg); + } + return py_bindings_tools::serializeMsg(traj_msg); } else { ROS_ERROR("Unable to convert RobotState message to RobotState instance."); - return ""; + return py_bindings_tools::ByteString(""); } } - Eigen::MatrixXd getJacobianMatrixPython(bp::list& joint_values) + Eigen::MatrixXd getJacobianMatrixPython(const bp::list& joint_values, const bp::object& reference_point = bp::object()) { - std::vector v = py_bindings_tools::doubleFromList(joint_values); - robot_state::RobotState state(getRobotModel()); + const std::vector v = py_bindings_tools::doubleFromList(joint_values); + std::vector ref; + if (reference_point.is_none()) + ref = { 0.0, 0.0, 0.0 }; + else + ref = py_bindings_tools::doubleFromList(reference_point); + if (ref.size() != 3) + throw std::invalid_argument("reference point needs to have 3 elements, got " + std::to_string(ref.size())); + + moveit::core::RobotState state(getRobotModel()); state.setToDefaultValues(); auto group = state.getJointModelGroup(getName()); state.setJointGroupPositions(group, v); - return state.getJacobian(group); + return state.getJacobian(group, Eigen::Map(&ref[0])); + } + + py_bindings_tools::ByteString enforceBoundsPython(const py_bindings_tools::ByteString& msg_str) + { + moveit_msgs::RobotState state_msg; + py_bindings_tools::deserializeMsg(msg_str, state_msg); + moveit::core::RobotState state(getRobotModel()); + if (moveit::core::robotStateMsgToRobotState(state_msg, state, true)) + { + state.enforceBounds(); + moveit::core::robotStateToRobotStateMsg(state, state_msg); + return py_bindings_tools::serializeMsg(state_msg); + } + else + { + ROS_ERROR("Unable to convert RobotState message to RobotState instance."); + return py_bindings_tools::ByteString(""); + } } }; +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getJacobianMatrixOverloads, getJacobianMatrixPython, 1, 2) + static void wrap_move_group_interface() { eigenpy::enableEigenPy(); bp::class_ move_group_interface_class( - "MoveGroupInterface", bp::init>()); + "MoveGroupInterface", bp::init>()); move_group_interface_class.def("async_move", &MoveGroupInterfaceWrapper::asyncMovePython); move_group_interface_class.def("move", &MoveGroupInterfaceWrapper::movePython); @@ -564,14 +649,15 @@ static void wrap_move_group_interface() move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasp); move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasps); move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placePose); + move_group_interface_class.def("place_poses_list", &MoveGroupInterfaceWrapper::placePoses); move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeLocation); + move_group_interface_class.def("place_locations_list", &MoveGroupInterfaceWrapper::placeLocations); move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeAnywhere); move_group_interface_class.def("stop", &MoveGroupInterfaceWrapper::stop); move_group_interface_class.def("get_name", &MoveGroupInterfaceWrapper::getNameCStr); move_group_interface_class.def("get_planning_frame", &MoveGroupInterfaceWrapper::getPlanningFrameCStr); - move_group_interface_class.def("get_interface_description", - &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython); + move_group_interface_class.def("get_interface_description", &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython); move_group_interface_class.def("get_active_joints", &MoveGroupInterfaceWrapper::getActiveJointsList); move_group_interface_class.def("get_joints", &MoveGroupInterfaceWrapper::getJointsList); @@ -628,8 +714,7 @@ static void wrap_move_group_interface() &MoveGroupInterfaceWrapper::rememberJointValues; move_group_interface_class.def("remember_joint_values", remember_joint_values_2); - move_group_interface_class.def("remember_joint_values", - &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList); + move_group_interface_class.def("remember_joint_values", &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList); move_group_interface_class.def("start_state_monitor", &MoveGroupInterfaceWrapper::startStateMonitor); move_group_interface_class.def("get_current_joint_values", &MoveGroupInterfaceWrapper::getCurrentJointValuesList); @@ -657,8 +742,7 @@ static void wrap_move_group_interface() bool (MoveGroupInterfaceWrapper::*set_path_constraints_1)(const std::string&) = &MoveGroupInterfaceWrapper::setPathConstraints; move_group_interface_class.def("set_path_constraints", set_path_constraints_1); - move_group_interface_class.def("set_path_constraints_from_msg", - &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg); + move_group_interface_class.def("set_path_constraints_from_msg", &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg); move_group_interface_class.def("get_path_constraints", &MoveGroupInterfaceWrapper::getPathConstraintsPython); move_group_interface_class.def("clear_path_constraints", &MoveGroupInterfaceWrapper::clearPathConstraints); move_group_interface_class.def("get_known_constraints", &MoveGroupInterfaceWrapper::getKnownConstraintsList); @@ -671,6 +755,7 @@ static void wrap_move_group_interface() move_group_interface_class.def("set_max_acceleration_scaling_factor", &MoveGroupInterfaceWrapper::setMaxAccelerationScalingFactor); move_group_interface_class.def("set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId); + move_group_interface_class.def("get_planner_id", &MoveGroupInterfaceWrapper::getPlannerIdCStr); move_group_interface_class.def("set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts); move_group_interface_class.def("plan", &MoveGroupInterfaceWrapper::planPython); move_group_interface_class.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython); @@ -683,7 +768,10 @@ static void wrap_move_group_interface() move_group_interface_class.def("get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython); move_group_interface_class.def("get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython); move_group_interface_class.def("get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython); - move_group_interface_class.def("get_jacobian_matrix", &MoveGroupInterfaceWrapper::getJacobianMatrixPython); + move_group_interface_class.def("get_current_state", &MoveGroupInterfaceWrapper::getCurrentStatePython); + move_group_interface_class.def("get_jacobian_matrix", &MoveGroupInterfaceWrapper::getJacobianMatrixPython, + getJacobianMatrixOverloads()); + move_group_interface_class.def("enforce_bounds", &MoveGroupInterfaceWrapper::enforceBoundsPython); } } // namespace planning_interface } // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt new file mode 100644 index 0000000000..a61a60f06f --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -0,0 +1,19 @@ +set(MOVEIT_LIB_NAME moveit_cpp) + +add_library(${MOVEIT_LIB_NAME} + src/moveit_cpp.cpp + src/planning_component.cpp + ) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(${MOVEIT_LIB_NAME} + moveit_common_planning_interface_objects + moveit_planning_scene_interface + ${catkin_LIBRARIES} + ${Boost_LIBRARIES}) +add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) + +install(TARGETS ${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h new file mode 100644 index 0000000000..4badc68fdf --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -0,0 +1,193 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Henning Kayser + Desc: A high level interface that does not require the use of ROS Actions, Services, and Messages to access the + core MoveIt functionality +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(MoveItCpp); // Defines MoveItCppPtr, ConstPtr, WeakPtr... etc + +class MoveItCpp +{ +public: + /// Specification of options to use when constructing the MoveItCpp class + struct PlanningSceneMonitorOptions + { + void load(const ros::NodeHandle& nh) + { + std::string ns = "planning_scene_monitor_options/"; + nh.param(ns + "name", name, "planning_scene_monitor"); + nh.param(ns + "robot_description", robot_description, "robot_description"); + nh.param(ns + "joint_state_topic", joint_state_topic, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC); + nh.param(ns + "attached_collision_object_topic", attached_collision_object_topic, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC); + nh.param(ns + "monitored_planning_scene_topic", monitored_planning_scene_topic, + planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC); + nh.param(ns + "publish_planning_scene_topic", publish_planning_scene_topic, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC); + nh.param(ns + "wait_for_initial_state_timeout", wait_for_initial_state_timeout, 0.0); + } + std::string name; + std::string robot_description; + std::string joint_state_topic; + std::string attached_collision_object_topic; + std::string monitored_planning_scene_topic; + std::string publish_planning_scene_topic; + double wait_for_initial_state_timeout; + }; + + /// struct contains the the variables used for loading the planning pipeline + struct PlanningPipelineOptions + { + void load(const ros::NodeHandle& nh) + { + std::string ns = "planning_pipelines/"; + nh.getParam(ns + "pipeline_names", pipeline_names); + nh.getParam(ns + "namespace", parent_namespace); + } + std::vector pipeline_names; + std::string parent_namespace; + }; + + /// Parameter container for initializing MoveItCpp + struct Options + { + Options(const ros::NodeHandle& nh) + { + planning_scene_monitor_options.load(nh); + planning_pipeline_options.load(nh); + } + + PlanningSceneMonitorOptions planning_scene_monitor_options; + PlanningPipelineOptions planning_pipeline_options; + }; + + /** \brief Constructor */ + MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); + MoveItCpp(const Options& options, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + MoveItCpp(const MoveItCpp&) = delete; + MoveItCpp& operator=(const MoveItCpp&) = delete; + + MoveItCpp(MoveItCpp&& other) = default; + MoveItCpp& operator=(MoveItCpp&& other) = default; + + /** \brief Destructor */ + ~MoveItCpp(); + + /** \brief Get the RobotModel object. */ + moveit::core::RobotModelConstPtr getRobotModel() const; + + /** \brief Get the ROS node handle of this instance operates on */ + const ros::NodeHandle& getNodeHandle() const; + + /** \brief Get the current state queried from the current state monitor + \param wait_seconds the time in seconds for the state monitor to wait for a robot state. */ + bool getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds); + + /** \brief Get the current state queried from the current state monitor + \param wait_seconds the time in seconds for the state monitor to wait for a robot state. */ + moveit::core::RobotStatePtr getCurrentState(double wait_seconds = 0.0); + + /** \brief Get all loaded planning pipeline instances mapped to their reference names */ + const std::map& getPlanningPipelines() const; + + /** \brief Get the names of all loaded planning pipelines. Specify group_name to filter the results by planning group + */ + std::set getPlanningPipelineNames(const std::string& group_name = "") const; + + /** \brief Get the stored instance of the planning scene monitor */ + const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const; + planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst(); + + const std::shared_ptr& getTFBuffer() const; + + /** \brief Get the stored instance of the trajectory execution manager */ + const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const; + trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst(); + + /** \brief Execute a trajectory on the planning group specified by group_name using the trajectory execution manager. + * If blocking is set to false, the execution is run in background and the function returns immediately. */ + bool execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + bool blocking = true); + +protected: + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + +private: + // Core properties and instances + ros::NodeHandle node_handle_; + moveit::core::RobotModelConstPtr robot_model_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // Planning + std::map planning_pipelines_; + std::map> groups_pipelines_map_; + std::map> groups_algorithms_map_; + + // Execution + trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; + + /** \brief Reset all member variables */ + void clearContents(); + + /** \brief Initialize and setup the planning scene monitor */ + bool loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options); + + /** \brief Initialize and setup the planning pipelines */ + bool loadPlanningPipelines(const PlanningPipelineOptions& options); +}; +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h new file mode 100644 index 0000000000..94d4a896fe --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -0,0 +1,229 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Henning Kayser + Desc: API for planning and execution capabilities of a JointModelGroup */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(PlanningComponent); // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc + +class PlanningComponent +{ +public: + MOVEIT_STRUCT_FORWARD(PlanSolution); + + class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes + { + public: + MoveItErrorCode() + { + val = 0; + } + MoveItErrorCode(int code) + { + val = code; + } + MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code) + { + val = code.val; + } + explicit operator bool() const + { + return val == moveit_msgs::MoveItErrorCodes::SUCCESS; + } + bool operator==(const int code) const + { + return val == code; + } + bool operator!=(const int code) const + { + return val != code; + } + }; + + /// The representation of a plan solution + struct PlanSolution + { + /// The full starting state used for planning + moveit_msgs::RobotState start_state; + + /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) + robot_trajectory::RobotTrajectoryPtr trajectory; + + /// Reason why the plan failed + MoveItErrorCode error_code; + + explicit operator bool() const + { + return bool(error_code); + } + }; + + /// Planner parameters provided with the MotionPlanRequest + struct PlanRequestParameters + { + std::string planner_id; + std::string planning_pipeline; + int planning_attempts; + double planning_time; + double max_velocity_scaling_factor; + double max_acceleration_scaling_factor; + + void load(const ros::NodeHandle& nh) + { + std::string ns = "plan_request_params/"; + nh.param(ns + "planner_id", planner_id, std::string("")); + nh.param(ns + "planning_pipeline", planning_pipeline, std::string("")); + nh.param(ns + "planning_time", planning_time, 1.0); + nh.param(ns + "planning_attempts", planning_attempts, 5); + nh.param(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor, 1.0); + nh.param(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor, 1.0); + } + }; + + /** \brief Constructor */ + PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh); + PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + PlanningComponent(const PlanningComponent&) = delete; + PlanningComponent& operator=(const PlanningComponent&) = delete; + + PlanningComponent(PlanningComponent&& other) = default; + PlanningComponent& operator=(PlanningComponent&& other) = delete; + + /** \brief Destructor */ + ~PlanningComponent(); + + /** \brief Get the name of the planning group */ + const std::string& getPlanningGroupName() const; + + /** \brief Get the names of the named robot states available as targets */ + const std::vector getNamedTargetStates(); + + /** \brief Get the joint values for targets specified by name */ + std::map getNamedTargetStateValues(const std::string& name); + + /** \brief Specify the workspace bounding box. + The box is specified in the planning frame (i.e. relative to the robot root link start position). + This is useful when the planning group contains the root joint of the robot -- i.e. when planning motion for the + robot relative to the world. */ + void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz); + + /** \brief Remove the workspace bounding box from planning */ + void unsetWorkspace(); + + /** \brief Get the considered start state if defined, otherwise get the current state */ + moveit::core::RobotStatePtr getStartState(); + + /** \brief Set the robot state that should be considered as start state for planning */ + bool setStartState(const moveit::core::RobotState& start_state); + /** \brief Set the named robot state that should be considered as start state for planning */ + bool setStartState(const std::string& named_state); + + /** \brief Set the start state for planning to be the current state of the robot */ + void setStartStateToCurrentState(); + + /** \brief Set the goal constraints used for planning */ + bool setGoal(const std::vector& goal_constraints); + /** \brief Set the goal constraints generated from a target state */ + bool setGoal(const moveit::core::RobotState& goal_state); + /** \brief Set the goal constraints generated from target pose and robot link */ + bool setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name); + /** \brief Set the goal constraints generated from a named target state */ + bool setGoal(const std::string& named_target); + + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using + * default parameters. */ + PlanSolution plan(); + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the + * provided PlanRequestParameters. */ + PlanSolution plan(const PlanRequestParameters& parameters); + + /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates + * after the execution is complete. The execution can be run in background by setting blocking to false. */ + bool execute(bool blocking = true); + + /** \brief Return the last plan solution*/ + const PlanSolutionPtr getLastPlanSolution(); + +private: + // Core properties and instances + ros::NodeHandle nh_; + MoveItCppPtr moveit_cpp_; + const std::string group_name_; + // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources + const moveit::core::JointModelGroup* joint_model_group_; + + // Planning + std::set planning_pipeline_names_; + // The start state used in the planning motion request + moveit::core::RobotStatePtr considered_start_state_; + std::vector current_goal_constraints_; + PlanRequestParameters plan_request_parameters_; + moveit_msgs::WorkspaceParameters workspace_parameters_; + bool workspace_parameters_set_ = false; + PlanSolutionPtr last_plan_solution_; + + // common properties for goals + // TODO(henningkayser): support goal tolerances + // double goal_joint_tolerance_; + // double goal_position_tolerance_; + // double goal_orientation_tolerance_; + // TODO(henningkayser): implment path/trajectory constraints + // std::unique_ptr path_constraints_; + // std::unique_ptr trajectory_constraints_; + + /** \brief Reset all member variables */ + void clearContents(); +}; +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp new file mode 100644 index 0000000000..40b300fc73 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -0,0 +1,316 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Henning Kayser */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +constexpr char LOGNAME[] = "moveit_cpp"; +constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin"; + +MoveItCpp::MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) + : MoveItCpp(Options(nh), nh, tf_buffer) +{ +} + +MoveItCpp::MoveItCpp(const Options& options, const ros::NodeHandle& nh, + const std::shared_ptr& tf_buffer) + : tf_buffer_(tf_buffer), node_handle_(nh) +{ + if (!tf_buffer_) + tf_buffer_.reset(new tf2_ros::Buffer()); + tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_)); + + // Configure planning scene monitor + if (!loadPlanningSceneMonitor(options.planning_scene_monitor_options)) + { + const std::string error = "Unable to configure planning scene monitor"; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + robot_model_ = planning_scene_monitor_->getRobotModel(); + if (!robot_model_) + { + const std::string error = "Unable to construct robot model. Please make sure all needed information is on the " + "parameter server."; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + bool load_planning_pipelines = true; + if (load_planning_pipelines && !loadPlanningPipelines(options.planning_pipeline_options)) + { + const std::string error = "Failed to load planning pipelines from parameter server"; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + // TODO(henningkayser): configure trajectory execution manager + trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager( + robot_model_, planning_scene_monitor_->getStateMonitor())); + + ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp running"); +} + +MoveItCpp::~MoveItCpp() +{ + ROS_INFO_NAMED(LOGNAME, "Deleting MoveItCpp"); + clearContents(); +} + +bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options) +{ + planning_scene_monitor_.reset( + new planning_scene_monitor::PlanningSceneMonitor(options.robot_description, tf_buffer_, options.name)); + // Allows us to sycronize to Rviz and also publish collision objects to ourselves + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Configuring Planning Scene Monitor"); + if (planning_scene_monitor_->getPlanningScene()) + { + // Start state and scene monitors + ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for joint states", options.joint_state_topic.c_str()); + planning_scene_monitor_->startStateMonitor(options.joint_state_topic, options.attached_collision_object_topic); + planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, + options.publish_planning_scene_topic); + planning_scene_monitor_->startSceneMonitor(options.monitored_planning_scene_topic); + } + else + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning scene not configured"); + return false; + } + + // Wait for complete state to be recieved + if (options.wait_for_initial_state_timeout > 0.0) + { + return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), + options.wait_for_initial_state_timeout); + } + + return true; +} + +bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options) +{ + ros::NodeHandle node_handle(options.parent_namespace.empty() ? "~" : options.parent_namespace); + for (const auto& planning_pipeline_name : options.pipeline_names) + { + if (planning_pipelines_.count(planning_pipeline_name) > 0) + { + ROS_WARN_NAMED(LOGNAME, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); + continue; + } + ROS_INFO_NAMED(LOGNAME, "Loading planning pipeline '%s'", planning_pipeline_name.c_str()); + ros::NodeHandle child_nh(node_handle, planning_pipeline_name); + planning_pipeline::PlanningPipelinePtr pipeline; + pipeline.reset(new planning_pipeline::PlanningPipeline(robot_model_, child_nh, PLANNING_PLUGIN_PARAM)); + + if (!pipeline->getPlannerManager()) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); + continue; + } + planning_pipelines_[planning_pipeline_name] = pipeline; + } + + if (planning_pipelines_.empty()) + { + return false; + ROS_ERROR_NAMED(LOGNAME, "Failed to load any planning pipelines."); + } + + // Retrieve group/pipeline mapping for faster lookup + std::vector group_names = robot_model_->getJointModelGroupNames(); + for (const auto& pipeline_entry : planning_pipelines_) + { + for (const auto& group_name : group_names) + { + groups_pipelines_map_[group_name] = {}; + const auto& pipeline = pipeline_entry.second; + for (const auto& planner_configuration : pipeline->getPlannerManager()->getPlannerConfigurations()) + { + if (planner_configuration.second.group == group_name) + { + groups_pipelines_map_[group_name].insert(pipeline_entry.first); + } + } + } + } + + return true; +} + +moveit::core::RobotModelConstPtr MoveItCpp::getRobotModel() const +{ + return robot_model_; +} + +const ros::NodeHandle& MoveItCpp::getNodeHandle() const +{ + return node_handle_; +} + +bool MoveItCpp::getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds) +{ + if (wait_seconds > 0.0 && + !planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), wait_seconds)) + { + ROS_ERROR_NAMED(LOGNAME, "Did not receive robot state"); + return false; + } + { // Lock planning scene + planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor_); + current_state.reset(new moveit::core::RobotState(scene->getCurrentState())); + } // Unlock planning scene + return true; +} + +moveit::core::RobotStatePtr MoveItCpp::getCurrentState(double wait) +{ + moveit::core::RobotStatePtr current_state; + getCurrentState(current_state, wait); + return current_state; +} + +const std::map& MoveItCpp::getPlanningPipelines() const +{ + return planning_pipelines_; +} + +std::set MoveItCpp::getPlanningPipelineNames(const std::string& group_name) const +{ + std::set result_names; + if (!group_name.empty() && groups_pipelines_map_.count(group_name) == 0) + { + ROS_ERROR_NAMED(LOGNAME, + "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.", + group_name.c_str()); + return result_names; // empty + } + for (const auto& pipeline_entry : planning_pipelines_) + { + const std::string& pipeline_name = pipeline_entry.first; + // If group_name is defined and valid, skip pipelines that don't belong to the planning group + if (!group_name.empty()) + { + const auto& group_pipelines = groups_pipelines_map_.at(group_name); + if (group_pipelines.find(pipeline_name) == group_pipelines.end()) + continue; + } + result_names.insert(pipeline_name); + } + // No valid planning pipelines + if (result_names.empty()) + ROS_ERROR_NAMED(LOGNAME, + "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.", + group_name.c_str()); + return result_names; +} + +const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const +{ + return planning_scene_monitor_; +} + +planning_scene_monitor::PlanningSceneMonitorPtr MoveItCpp::getPlanningSceneMonitorNonConst() +{ + return planning_scene_monitor_; +} + +const trajectory_execution_manager::TrajectoryExecutionManagerPtr& MoveItCpp::getTrajectoryExecutionManager() const +{ + return trajectory_execution_manager_; +} + +trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveItCpp::getTrajectoryExecutionManagerNonConst() +{ + return trajectory_execution_manager_; +} + +bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + bool blocking) +{ + if (!robot_trajectory) + { + ROS_ERROR_NAMED(LOGNAME, "Robot trajectory is undefined"); + return false; + } + + // Check if there are controllers that can handle the execution + if (!trajectory_execution_manager_->ensureActiveControllersForGroup(group_name)) + { + ROS_ERROR_NAMED(LOGNAME, "Execution failed! No active controllers configured for group '%s'", group_name.c_str()); + return false; + } + + // Execute trajectory + moveit_msgs::RobotTrajectory robot_trajectory_msg; + robot_trajectory->getRobotTrajectoryMsg(robot_trajectory_msg); + if (blocking) + { + trajectory_execution_manager_->push(robot_trajectory_msg); + trajectory_execution_manager_->execute(); + return trajectory_execution_manager_->waitForExecution(); + } + trajectory_execution_manager_->pushAndExecute(robot_trajectory_msg); + return true; +} + +const std::shared_ptr& MoveItCpp::getTFBuffer() const +{ + return tf_buffer_; +} + +void MoveItCpp::clearContents() +{ + tf_listener_.reset(); + tf_buffer_.reset(); + planning_scene_monitor_.reset(); + robot_model_.reset(); + planning_pipelines_.clear(); +} +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp new file mode 100644 index 0000000000..8a910e7f63 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -0,0 +1,328 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Henning Kayser */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +constexpr char LOGNAME[] = "planning_component"; + +PlanningComponent::PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp) + : nh_(moveit_cpp->getNodeHandle()), moveit_cpp_(moveit_cpp), group_name_(group_name) +{ + joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name); + if (!joint_model_group_) + { + std::string error = "Could not find joint model group '" + group_name + "'."; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name); + plan_request_parameters_.load(nh_); + ROS_DEBUG_STREAM_NAMED( + LOGNAME, "Plan request parameters loaded with --" + << " planning_pipeline: " << plan_request_parameters_.planning_pipeline << "," + << " planner_id: " << plan_request_parameters_.planner_id << "," + << " planning_time: " << plan_request_parameters_.planning_time << "," + << " planning_attempts: " << plan_request_parameters_.planning_attempts << "," + << " max_velocity_scaling_factor: " << plan_request_parameters_.max_velocity_scaling_factor << "," + << " max_acceleration_scaling_factor: " << plan_request_parameters_.max_acceleration_scaling_factor); +} + +PlanningComponent::PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh) + : PlanningComponent(group_name, std::make_shared(nh)) +{ +} + +PlanningComponent::~PlanningComponent() +{ + ROS_INFO_NAMED(LOGNAME, "Deleting PlanningComponent '%s'", group_name_.c_str()); + clearContents(); +} + +const std::vector PlanningComponent::getNamedTargetStates() +{ + if (joint_model_group_) + { + return joint_model_group_->getDefaultStateNames(); + } + else + { + ROS_WARN_NAMED(LOGNAME, "Unable to find joint group with name '%s'.", group_name_.c_str()); + } + + std::vector empty; + return empty; +} + +const std::string& PlanningComponent::getPlanningGroupName() const +{ + return group_name_; +} + +PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParameters& parameters) +{ + last_plan_solution_.reset(new PlanSolution()); + if (!joint_model_group_) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); + return *last_plan_solution_; + } + + // Clone current planning scene + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = + moveit_cpp_->getPlanningSceneMonitorNonConst(); + planning_scene_monitor->updateFrameTransforms(); + planning_scene_monitor->lockSceneRead(); // LOCK planning scene + planning_scene::PlanningScenePtr planning_scene = + planning_scene::PlanningScene::clone(planning_scene_monitor->getPlanningScene()); + planning_scene_monitor->unlockSceneRead(); // UNLOCK planning scene + planning_scene_monitor.reset(); // release this pointer + + // Init MotionPlanRequest + ::planning_interface::MotionPlanRequest req; + req.group_name = group_name_; + req.planner_id = parameters.planner_id; + req.num_planning_attempts = std::max(1, parameters.planning_attempts); + req.allowed_planning_time = parameters.planning_time; + req.max_velocity_scaling_factor = parameters.max_velocity_scaling_factor; + req.max_acceleration_scaling_factor = parameters.max_acceleration_scaling_factor; + if (workspace_parameters_set_) + req.workspace_parameters = workspace_parameters_; + + // Set start state + moveit::core::RobotStatePtr start_state = considered_start_state_; + if (!start_state) + start_state = moveit_cpp_->getCurrentState(); + start_state->update(); + moveit::core::robotStateToRobotStateMsg(*start_state, req.start_state); + planning_scene->setCurrentState(*start_state); + + // Set goal constraints + if (current_goal_constraints_.empty()) + { + ROS_ERROR_NAMED(LOGNAME, "No goal constraints set for planning request"); + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + return *last_plan_solution_; + } + req.goal_constraints = current_goal_constraints_; + + // Run planning attempt + ::planning_interface::MotionPlanResponse res; + if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end()) + { + ROS_ERROR_NAMED(LOGNAME, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return *last_plan_solution_; + } + const planning_pipeline::PlanningPipelinePtr pipeline = + moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline); + pipeline->generatePlan(planning_scene, req, res); + last_plan_solution_->error_code = res.error_code_.val; + if (res.error_code_.val != res.error_code_.SUCCESS) + { + ROS_ERROR("Could not compute plan successfully"); + return *last_plan_solution_; + } + last_plan_solution_->start_state = req.start_state; + last_plan_solution_->trajectory = res.trajectory_; + // TODO(henningkayser): Visualize trajectory + // std::vector eef_links; + // if (joint_model_group->getEndEffectorTips(eef_links)) + //{ + // for (const auto& eef_link : eef_links) + // { + // ROS_INFO_STREAM("Publishing trajectory for end effector " << eef_link->getName()); + // visual_tools_->publishTrajectoryLine(last_solution_trajectory_, eef_link); + // visual_tools_->publishTrajectoryPath(last_solution_trajectory_, false); + // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT); + // } + //} + return *last_plan_solution_; +} + +PlanningComponent::PlanSolution PlanningComponent::plan() +{ + return plan(plan_request_parameters_); +} + +bool PlanningComponent::setStartState(const moveit::core::RobotState& start_state) +{ + considered_start_state_.reset(new moveit::core::RobotState(start_state)); + return true; +} + +moveit::core::RobotStatePtr PlanningComponent::getStartState() +{ + if (considered_start_state_) + return considered_start_state_; + else + { + moveit::core::RobotStatePtr s; + moveit_cpp_->getCurrentState(s, 1.0); + return s; + } +} + +bool PlanningComponent::setStartState(const std::string& start_state_name) +{ + const auto& named_targets = getNamedTargetStates(); + if (std::find(named_targets.begin(), named_targets.end(), start_state_name) == named_targets.end()) + { + ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", start_state_name.c_str()); + return false; + } + moveit::core::RobotState start_state(moveit_cpp_->getRobotModel()); + start_state.setToDefaultValues(joint_model_group_, start_state_name); + return setStartState(start_state); +} + +void PlanningComponent::setStartStateToCurrentState() +{ + considered_start_state_.reset(); +} + +std::map PlanningComponent::getNamedTargetStateValues(const std::string& name) +{ + // TODO(henningkayser): verify result + std::map positions; + joint_model_group_->getVariableDefaultPositions(name, positions); + return positions; +} + +void PlanningComponent::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) +{ + workspace_parameters_.header.frame_id = moveit_cpp_->getRobotModel()->getModelFrame(); + workspace_parameters_.header.stamp = ros::Time::now(); + workspace_parameters_.min_corner.x = minx; + workspace_parameters_.min_corner.y = miny; + workspace_parameters_.min_corner.z = minz; + workspace_parameters_.max_corner.x = maxx; + workspace_parameters_.max_corner.y = maxy; + workspace_parameters_.max_corner.z = maxz; + workspace_parameters_set_ = true; +} + +void PlanningComponent::unsetWorkspace() +{ + workspace_parameters_set_ = false; +} + +bool PlanningComponent::setGoal(const std::vector& goal_constraints) +{ + current_goal_constraints_ = goal_constraints; + return true; +} + +bool PlanningComponent::setGoal(const moveit::core::RobotState& goal_state) +{ + current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_) }; + return true; +} + +bool PlanningComponent::setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name) +{ + current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(link_name, goal_pose) }; + return true; +} + +bool PlanningComponent::setGoal(const std::string& goal_state_name) +{ + const auto& named_targets = getNamedTargetStates(); + if (std::find(named_targets.begin(), named_targets.end(), goal_state_name) == named_targets.end()) + { + ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", goal_state_name.c_str()); + return false; + } + moveit::core::RobotState goal_state(moveit_cpp_->getRobotModel()); + goal_state.setToDefaultValues(joint_model_group_, goal_state_name); + return setGoal(goal_state); +} + +bool PlanningComponent::execute(bool blocking) +{ + if (!last_plan_solution_) + { + ROS_ERROR_NAMED(LOGNAME, "There is no successfull plan to execute"); + return false; + } + + // TODO(henningkayser): parameterize timestamps if required + // trajectory_processing::TimeOptimalTrajectoryGeneration totg; + // if (!totg.computeTimeStamps(*last_solution_trajectory_, max_velocity_scaling_factor_, + // max_acceleration_scaling_factor_)) + //{ + // ROS_ERROR("Failed to parameterize trajectory"); + // return false; + //} + return moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking); +} + +const PlanningComponent::PlanSolutionPtr PlanningComponent::getLastPlanSolution() +{ + return last_plan_solution_; +} + +void PlanningComponent::clearContents() +{ + considered_start_state_.reset(); + last_plan_solution_.reset(); + current_goal_constraints_.clear(); + moveit_cpp_.reset(); + planning_pipeline_names_.clear(); +} +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index 3690783e3f..95778baf9f 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -1,13 +1,14 @@ - + + moveit_ros_planning_interface - 1.0.1 - Components of MoveIt! that offer simpler interfaces to planning and execution + 1.1.1 + Components of MoveIt that offer simpler interfaces to planning and execution Ioan Sucan Michael Ferguson Michael GΓΆrner - MoveIt! Release Team + MoveIt Release Team BSD @@ -16,7 +17,8 @@ https://github.com/ros-planning/moveit catkin - python-catkin-pkg + python-catkin-pkg + python3-catkin-pkg moveit_ros_planning moveit_ros_warehouse @@ -32,11 +34,15 @@ tf2_eigen tf2_geometry_msgs tf2_ros - python + python + python3 eigenpy eigen - moveit_resources + moveit_resources_fanuc_moveit_config + moveit_resources_panda_moveit_config + eigen_conversions rostest + eigen_conversions diff --git a/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt b/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt index 7658b2583e..6a3b1a3f9b 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt @@ -9,13 +9,16 @@ set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_ target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_planning_scene_interface PREFIX "") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") +if(WIN32) + set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES SUFFIX .pyd) +endif(WIN32) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME}_python - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index d421b5484a..6a468ddbf1 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_INTERFACE_PLANNING_SCENE_INTERFACE_ -#define MOVEIT_PLANNING_INTERFACE_PLANNING_SCENE_INTERFACE_ +#pragma once #include #include @@ -48,12 +47,17 @@ namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(PlanningSceneInterface); +MOVEIT_CLASS_FORWARD(PlanningSceneInterface); // Defines PlanningSceneInterfacePtr, ConstPtr, WeakPtr... etc class PlanningSceneInterface { public: - explicit PlanningSceneInterface(const std::string& ns = ""); + /** + \param ns. Namespace in which all MoveIt related topics and services are discovered + \param wait. Wait for services if they are not announced in ROS. + If this is false, the constructor throws std::runtime_error instead. + */ + explicit PlanningSceneInterface(const std::string& ns = "", bool wait = true); ~PlanningSceneInterface(); /** @@ -144,7 +148,5 @@ class PlanningSceneInterface class PlanningSceneInterfaceImpl; PlanningSceneInterfaceImpl* impl_; }; -} -} - -#endif +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index ea61ff3d02..3c2b4a6db2 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -45,17 +45,32 @@ namespace moveit { namespace planning_interface { +static const std::string LOGNAME = "planning_scene_interface"; + class PlanningSceneInterface::PlanningSceneInterfaceImpl { public: - explicit PlanningSceneInterfaceImpl(const std::string& ns = "") + explicit PlanningSceneInterfaceImpl(const std::string& ns = "", bool wait = true) { node_handle_ = ros::NodeHandle(ns); + planning_scene_diff_publisher_ = node_handle_.advertise("planning_scene", 1); planning_scene_service_ = node_handle_.serviceClient(move_group::GET_PLANNING_SCENE_SERVICE_NAME); apply_planning_scene_service_ = node_handle_.serviceClient(move_group::APPLY_PLANNING_SCENE_SERVICE_NAME); - planning_scene_diff_publisher_ = node_handle_.advertise("planning_scene", 1); + + if (wait) + { + waitForService(planning_scene_service_); + waitForService(apply_planning_scene_service_); + } + else + { + if (!planning_scene_service_.exists() || !apply_planning_scene_service_.exists()) + { + throw std::runtime_error("ROS services not available"); + } + } } std::vector getKnownObjectNames(bool with_type) @@ -89,7 +104,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl request.components.components = request.components.WORLD_OBJECT_GEOMETRY; if (!planning_scene_service_.call(request, response)) { - ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get object names"); + ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get object names"); return result; } @@ -133,7 +148,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl request.components.components = request.components.WORLD_OBJECT_GEOMETRY; if (!planning_scene_service_.call(request, response)) { - ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get object names"); + ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get object names"); return result; } @@ -160,14 +175,13 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl request.components.components = request.components.WORLD_OBJECT_GEOMETRY; if (!planning_scene_service_.call(request, response)) { - ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get object geometries"); + ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get object geometries"); return result; } for (const moveit_msgs::CollisionObject& collision_object : response.scene.world.collision_objects) { - if (object_ids.empty() || - std::find(object_ids.begin(), object_ids.end(), collision_object.id) != object_ids.end()) + if (object_ids.empty() || std::find(object_ids.begin(), object_ids.end(), collision_object.id) != object_ids.end()) { result[collision_object.id] = collision_object; } @@ -184,8 +198,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl request.components.components = request.components.ROBOT_STATE_ATTACHED_OBJECTS; if (!planning_scene_service_.call(request, response)) { - ROS_WARN_NAMED("planning_scene_interface", - "Could not call planning scene service to get attached object geometries"); + ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get attached object geometries"); return result; } @@ -208,7 +221,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl request.scene = planning_scene; if (!apply_planning_scene_service_.call(request, response)) { - ROS_WARN_NAMED("planning_scene_interface", "Failed to call ApplyPlanningScene service"); + ROS_WARN_NAMED(LOGNAME, "Failed to call ApplyPlanningScene service"); return false; } return response.success; @@ -248,16 +261,27 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl } private: + void waitForService(ros::ServiceClient& srv) + { + ros::Duration time_before_warning(5.0); + srv.waitForExistence(time_before_warning); + if (!srv.exists()) + { + ROS_WARN_STREAM_NAMED(LOGNAME, "service '" << srv.getService() << "' not advertised yet. Continue waiting..."); + srv.waitForExistence(); + } + } + ros::NodeHandle node_handle_; ros::ServiceClient planning_scene_service_; ros::ServiceClient apply_planning_scene_service_; ros::Publisher planning_scene_diff_publisher_; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; }; -PlanningSceneInterface::PlanningSceneInterface(const std::string& ns) +PlanningSceneInterface::PlanningSceneInterface(const std::string& ns, bool wait) { - impl_ = new PlanningSceneInterfaceImpl(ns); + impl_ = new PlanningSceneInterfaceImpl(ns, wait); } PlanningSceneInterface::~PlanningSceneInterface() diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp index e1e825b0b0..e0cd8f7818 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp @@ -74,7 +74,7 @@ class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitial bp::dict getObjectPosesPython(const bp::list& object_ids) { std::map ops = getObjectPoses(py_bindings_tools::stringFromList(object_ids)); - std::map ser_ops; + std::map ser_ops; for (std::map::const_iterator it = ops.begin(); it != ops.end(); ++it) ser_ops[it->first] = py_bindings_tools::serializeMsg(it->second); @@ -85,7 +85,7 @@ class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitial { std::map objs = getObjects(py_bindings_tools::stringFromList(object_ids)); - std::map ser_objs; + std::map ser_objs; for (std::map::const_iterator it = objs.begin(); it != objs.end(); ++it) ser_objs[it->first] = py_bindings_tools::serializeMsg(it->second); @@ -96,7 +96,7 @@ class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitial { std::map aobjs = getAttachedObjects(py_bindings_tools::stringFromList(object_ids)); - std::map ser_aobjs; + std::map ser_aobjs; for (std::map::const_iterator it = aobjs.begin(); it != aobjs.end(); ++it) ser_aobjs[it->first] = py_bindings_tools::serializeMsg(it->second); @@ -104,7 +104,7 @@ class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitial return py_bindings_tools::dictFromType(ser_aobjs); } - bool applyPlanningScenePython(const std::string& ps_str) + bool applyPlanningScenePython(const py_bindings_tools::ByteString& ps_str) { moveit_msgs::PlanningScene ps_msg; py_bindings_tools::deserializeMsg(ps_str, ps_msg); diff --git a/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt b/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt index 6d1e6899a6..1f82d7e4dc 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt +++ b/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt @@ -6,16 +6,19 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) add_library(${MOVEIT_LIB_NAME}_python src/wrap_python_roscpp_initializer.cpp) target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_roscpp_initializer PREFIX "") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") +if(WIN32) + set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES SUFFIX .pyd) +endif(WIN32) install(TARGETS ${MOVEIT_LIB_NAME}_python - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h new file mode 100644 index 0000000000..215a470810 --- /dev/null +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h @@ -0,0 +1,96 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, RWTH Aachen University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of RWTH Aachen University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Bjarne von Horn */ + +#pragma once + +#include +#include + +namespace moveit +{ +namespace py_bindings_tools +{ +/** \brief RAII Helper to release the Global Interpreter Lock (GIL) + * + * Use this helper class to release the GIL before doing long computations + * or blocking calls. Note that without the GIL Python-related functions must not be called. + * So, before releasing the GIL all \c boost::python variables have to be converted to e.g. \c std::vector. + * Before converting the result back to e.g. a moveit::py_bindings_tools::ByteString instance, the GIL has to be + * reacquired. + */ +class GILReleaser +{ + PyThreadState* m_thread_state; + +public: + /** \brief Release the GIL on construction */ + GILReleaser() noexcept + { + m_thread_state = PyEval_SaveThread(); + } + /** \brief Reacquire the GIL on destruction */ + ~GILReleaser() noexcept + { + if (m_thread_state) + { + PyEval_RestoreThread(m_thread_state); + m_thread_state = nullptr; + } + } + + GILReleaser(const GILReleaser&) = delete; + GILReleaser(GILReleaser&& other) noexcept + { + m_thread_state = other.m_thread_state; + other.m_thread_state = nullptr; + } + + GILReleaser& operator=(const GILReleaser&) = delete; + GILReleaser& operator=(GILReleaser&& other) noexcept + { + GILReleaser copy(std::move(other)); + this->swap(copy); + return *this; + } + + void swap(GILReleaser& other) noexcept + { + std::swap(other.m_thread_state, m_thread_state); + } +}; + +} // namespace py_bindings_tools +} // namespace moveit diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h index fbf4d191ff..acbbde0d2b 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PY_BINDINGS_TOOLS_PY_CONVERSIONS_ -#define MOVEIT_PY_BINDINGS_TOOLS_PY_CONVERSIONS_ +#pragma once #include #include @@ -93,7 +92,5 @@ boost::python::list listFromString(const std::vector& v) { return listFromType(v); } -} -} - -#endif +} // namespace py_bindings_tools +} // namespace moveit diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h index 3796d64760..060d84164c 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h @@ -34,15 +34,14 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PY_BINDINGS_TOOLS_ROSCPP_INITIALIZER_ -#define MOVEIT_PY_BINDINGS_TOOLS_ROSCPP_INITIALIZER_ +#pragma once #include #include namespace moveit { -/** \brief Tools for creating python bindings for MoveIt! */ +/** \brief Tools for creating python bindings for MoveIt */ namespace py_bindings_tools { /** \brief The constructor of this class ensures that ros::init() has @@ -71,7 +70,5 @@ void roscpp_init(boost::python::list& argv); void roscpp_init(); void roscpp_shutdown(); -} -} - -#endif +} // namespace py_bindings_tools +} // namespace moveit diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h index 5000fccbbe..7f72363231 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h +++ b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h @@ -34,42 +34,104 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PY_BINDINGS_TOOLS_SERIALIZE_MSG_ -#define MOVEIT_PY_BINDINGS_TOOLS_SERIALIZE_MSG_ +#pragma once #include +#include +#include +#include +#include +#include namespace moveit { namespace py_bindings_tools { -/** \brief Convert a ROS message to a string */ -template -std::string serializeMsg(const T& msg) +/** \brief C++ Wrapper class for Python 3 \c Bytes Object */ +class ByteString : public boost::python::object { - // we use the fact char is same size as uint8_t; - assert(sizeof(uint8_t) == sizeof(char)); - std::size_t size = ros::serialization::serializationLength(msg); - std::string result(size, '\0'); - if (size) +public: + // constructors for bp::handle and friends + BOOST_PYTHON_FORWARD_OBJECT_CONSTRUCTORS(ByteString, boost::python::object) + ByteString() : boost::python::object(boost::python::handle<>(PyBytes_FromString(""))) + { + } + explicit ByteString(const char* s) : boost::python::object(boost::python::handle<>(PyBytes_FromString(s))) + { + } + explicit ByteString(const std::string& s) + : boost::python::object(boost::python::handle<>(PyBytes_FromStringAndSize(s.c_str(), s.size()))) + { + } + // bp::list[] returns a proxy which has to be converted to an object first + template + explicit ByteString(const boost::python::api::proxy& proxy) : boost::python::object(proxy) + { + } + /** \brief Serializes a ROS message into a Python Bytes object + * The second template parameter ensures that this overload is only chosen with a ROS message argument + */ + template ::value, int>::type = 0> + explicit ByteString(const T& msg) + : boost::python::object( + boost::python::handle<>(PyBytes_FromStringAndSize(nullptr, ros::serialization::serializationLength(msg)))) { - // we convert the message into a string because that is easy to sent back & forth with Python - // This is fine since C0x because &string[0] is guaranteed to point to a contiguous block of memory - ros::serialization::OStream stream_arg(reinterpret_cast(&result[0]), size); + ros::serialization::OStream stream_arg(reinterpret_cast(PyBytes_AS_STRING(ptr())), + PyBytes_GET_SIZE(ptr())); ros::serialization::serialize(stream_arg, msg); } - return result; -} -/** \brief Convert a string to a ROS message */ + /** \brief Convert content to a ROS message */ + template + void deserialize(T& msg) const + { + static_assert(sizeof(uint8_t) == sizeof(char), "ros/python buffer layout mismatch"); + char* buf = PyBytes_AsString(ptr()); + // buf == NULL on error + if (!buf) + { + throw std::runtime_error("Underlying python object is not a Bytes/String instance"); + } + // unfortunately no constructor with const uint8_t + ros::serialization::IStream stream_arg(reinterpret_cast(buf), PyBytes_GET_SIZE(ptr())); + ros::serialization::deserialize(stream_arg, msg); + } +}; + +/** \brief Convert a ROS message to a Python Bytestring */ template -void deserializeMsg(const std::string& data, T& msg) +ByteString serializeMsg(const T& msg) { - assert(sizeof(uint8_t) == sizeof(char)); - ros::serialization::IStream stream_arg(const_cast(reinterpret_cast(&data[0])), data.size()); - ros::serialization::deserialize(stream_arg, msg); -} + return ByteString(msg); } + +/** \brief Convert a Python Bytestring to a ROS message */ +template +void deserializeMsg(const ByteString& data, T& msg) +{ + data.deserialize(msg); } +} // namespace py_bindings_tools +} // namespace moveit + +namespace boost +{ +namespace python +{ +namespace converter +{ +// only accept Python 3 Bytes / Python 2 String instance when used as C++ function parameter +template <> +struct object_manager_traits +#if PY_VERSION_HEX >= 0x03000000 + : pytype_object_manager_traits<&PyBytes_Type, moveit::py_bindings_tools::ByteString> +#else + : pytype_object_manager_traits<&PyString_Type, moveit::py_bindings_tools::ByteString> #endif + +{ +}; +} // namespace converter +} // namespace python +} // namespace boost diff --git a/moveit_ros/planning_interface/robot_interface/CMakeLists.txt b/moveit_ros/planning_interface/robot_interface/CMakeLists.txt index 56cb7b4098..3944155fbf 100644 --- a/moveit_ros/planning_interface/robot_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/robot_interface/CMakeLists.txt @@ -6,7 +6,9 @@ target_link_libraries(${MOVEIT_LIB_NAME}_python ${PYTHON_LIBRARIES} ${catkin_LIB set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_robot_interface PREFIX "") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") +if(WIN32) + set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES SUFFIX .pyd) +endif(WIN32) install(TARGETS ${MOVEIT_LIB_NAME}_python - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) diff --git a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp index daa49825da..f6e3d40061 100644 --- a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp +++ b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -49,6 +50,7 @@ /** @cond IGNORE */ namespace bp = boost::python; +using moveit::py_bindings_tools::GILReleaser; namespace moveit { @@ -77,7 +79,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list getGroupJointNames(const std::string& group) const { - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (jmg) return py_bindings_tools::listFromString(jmg->getJointModelNames()); else @@ -86,7 +88,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list getGroupJointTips(const std::string& group) const { - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (jmg) { std::vector tips; @@ -104,7 +106,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list getGroupLinkNames(const std::string& group) const { - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (jmg) return py_bindings_tools::listFromString(jmg->getLinkModelNames()); else @@ -119,7 +121,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list getJointLimits(const std::string& name) const { bp::list result; - const robot_model::JointModel* jm = robot_model_->getJointModel(name); + const moveit::core::JointModel* jm = robot_model_->getJointModel(name); if (jm) { const std::vector& lim = jm->getVariableBoundsMsg(); @@ -144,16 +146,17 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list l; if (!ensureCurrentState()) return l; - robot_state::RobotStatePtr state = current_state_monitor_->getCurrentState(); - const robot_model::LinkModel* lm = state->getLinkModel(name); + moveit::core::RobotStatePtr state = current_state_monitor_->getCurrentState(); + const moveit::core::LinkModel* lm = state->getLinkModel(name); if (lm) { + // getGlobalLinkTransform() returns a valid isometry by contract const Eigen::Isometry3d& t = state->getGlobalLinkTransform(lm); std::vector v(7); v[0] = t.translation().x(); v[1] = t.translation().y(); v[2] = t.translation().z(); - Eigen::Quaterniond q(t.rotation()); + Eigen::Quaterniond q(t.linear()); v[3] = q.x(); v[4] = q.y(); v[5] = q.z(); @@ -166,7 +169,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list getDefaultStateNames(const std::string& group) { bp::list l; - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (jmg) { for (auto& known_state : jmg->getDefaultStateNames()) @@ -182,8 +185,8 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::list l; if (!ensureCurrentState()) return l; - robot_state::RobotStatePtr state = current_state_monitor_->getCurrentState(); - const robot_model::JointModel* jm = state->getJointModel(name); + moveit::core::RobotStatePtr state = current_state_monitor_->getCurrentState(); + const moveit::core::JointModel* jm = state->getJointModel(name); if (jm) { const double* pos = state->getJointPositions(jm); @@ -197,7 +200,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer bp::dict getJointValues(const std::string& group, const std::string& named_state) { - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg) return boost::python::dict(); std::map values; @@ -216,6 +219,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer // if needed, start the monitor and wait up to 1 second for a full robot state if (!current_state_monitor_->isActive()) { + GILReleaser gr; current_state_monitor_->startStateMonitor(); if (!current_state_monitor_->waitForCompleteState(wait)) ROS_WARN("Joint values for monitored state are requested but the full state is not known"); @@ -223,13 +227,13 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return true; } - std::string getCurrentState() + py_bindings_tools::ByteString getCurrentState() { if (!ensureCurrentState()) - return ""; - robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); + return py_bindings_tools::ByteString(""); + moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); moveit_msgs::RobotState msg; - robot_state::robotStateToRobotStateMsg(*s, msg); + moveit::core::robotStateToRobotStateMsg(*s, msg); return py_bindings_tools::serializeMsg(msg); } @@ -237,23 +241,23 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer { // name of the group that is parent to this end-effector group; // Second: the link this in the parent group that this group attaches to - const robot_state::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg) return boost::python::make_tuple("", ""); std::pair parent_group = jmg->getEndEffectorParentGroup(); return boost::python::make_tuple(parent_group.first, parent_group.second); } - std::string getRobotMarkersPythonDictList(bp::dict& values, bp::list& links) + py_bindings_tools::ByteString getRobotMarkersPythonDictList(bp::dict& values, bp::list& links) { - robot_state::RobotStatePtr state; + moveit::core::RobotStatePtr state; if (ensureCurrentState()) { state = current_state_monitor_->getCurrentState(); } else { - state.reset(new robot_state::RobotState(robot_model_)); + state.reset(new moveit::core::RobotState(robot_model_)); } bp::list k = values.keys(); @@ -273,16 +277,16 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return py_bindings_tools::serializeMsg(msg); } - std::string getRobotMarkersPythonDict(bp::dict& values) + py_bindings_tools::ByteString getRobotMarkersPythonDict(bp::dict& values) { bp::list links = py_bindings_tools::listFromString(robot_model_->getLinkModelNames()); return getRobotMarkersPythonDictList(values, links); } - std::string getRobotMarkersFromMsg(const std::string& state_str) + py_bindings_tools::ByteString getRobotMarkersFromMsg(const py_bindings_tools::ByteString& state_str) { moveit_msgs::RobotState state_msg; - robot_state::RobotState state(robot_model_); + moveit::core::RobotState state(robot_model_); py_bindings_tools::deserializeMsg(state_str, state_msg); moveit::core::robotStateMsgToRobotState(state_msg, state); @@ -292,34 +296,34 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return py_bindings_tools::serializeMsg(msg); } - std::string getRobotMarkers() + py_bindings_tools::ByteString getRobotMarkers() { if (!ensureCurrentState()) - return ""; - robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); + return py_bindings_tools::ByteString(); + moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); visualization_msgs::MarkerArray msg; s->getRobotMarkers(msg, s->getRobotModel()->getLinkModelNames()); return py_bindings_tools::serializeMsg(msg); } - std::string getRobotMarkersPythonList(const bp::list& links) + py_bindings_tools::ByteString getRobotMarkersPythonList(const bp::list& links) { if (!ensureCurrentState()) - return ""; - robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); + return py_bindings_tools::ByteString(""); + moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); visualization_msgs::MarkerArray msg; s->getRobotMarkers(msg, py_bindings_tools::stringFromList(links)); return py_bindings_tools::serializeMsg(msg); } - std::string getRobotMarkersGroup(const std::string& group) + py_bindings_tools::ByteString getRobotMarkersGroup(const std::string& group) { if (!ensureCurrentState()) - return ""; - robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + return py_bindings_tools::ByteString(""); + moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); visualization_msgs::MarkerArray msg; if (jmg) { @@ -329,11 +333,11 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return py_bindings_tools::serializeMsg(msg); } - std::string getRobotMarkersGroupPythonDict(const std::string& group, bp::dict& values) + py_bindings_tools::ByteString getRobotMarkersGroupPythonDict(const std::string& group, bp::dict& values) { - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg) - return ""; + return py_bindings_tools::ByteString(""); bp::list links = py_bindings_tools::listFromString(jmg->getLinkModelNames()); return getRobotMarkersPythonDictList(values, links); } @@ -363,7 +367,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer } private: - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; ros::NodeHandle nh_; }; diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 979b4e1f92..e964d528d2 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -1,12 +1,37 @@ if (CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) find_package(rostest REQUIRED) + find_package(eigen_conversions REQUIRED) add_executable(test_cleanup test_cleanup.cpp) target_link_libraries(test_cleanup moveit_move_group_interface) + add_rostest_gtest(move_group_interface_cpp_test move_group_interface_cpp_test.test move_group_interface_cpp_test.cpp) + target_link_libraries(move_group_interface_cpp_test moveit_move_group_interface ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) + + add_rostest_gtest(move_group_pick_place_test move_group_pick_place_test.test move_group_pick_place_test.cpp) + target_link_libraries(move_group_pick_place_test moveit_move_group_interface ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) + + add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) + target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) + + add_rostest_gtest(subframes_test subframes_test.test subframes_test.cpp) + target_link_libraries(subframes_test moveit_move_group_interface + ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) + add_rostest(python_move_group.test) add_rostest(python_move_group_ns.test) add_rostest(robot_state_update.test) add_rostest(cleanup.test) + + SET(HELPER_LIB moveit_planning_interface_test_serialize_msg_cpp_helper) + add_library(${HELPER_LIB} serialize_msg_python_cpp_helpers.cpp) + set_target_properties(${HELPER_LIB} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + target_link_libraries(${HELPER_LIB} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) + set_target_properties(${HELPER_LIB} PROPERTIES OUTPUT_NAME "_${HELPER_LIB}" PREFIX "") + set_target_properties(${HELPER_LIB} PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") + if(WIN32) + set_target_properties(${HELPER_LIB} PROPERTIES SUFFIX .pyd) + endif(WIN32) + + catkin_add_nosetests(serialize_msg.py) endif() diff --git a/moveit_ros/planning_interface/test/cleanup.test b/moveit_ros/planning_interface/test/cleanup.test index b1a2064369..775ea18666 100644 --- a/moveit_ros/planning_interface/test/cleanup.test +++ b/moveit_ros/planning_interface/test/cleanup.test @@ -1,5 +1,5 @@ - + diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp new file mode 100644 index 0000000000..3789ed631e --- /dev/null +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -0,0 +1,323 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Tyler Weaver + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Robotics nor the + * names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Tyler Weaver, Boston Cleek */ + +/* These integration tests are based on the tutorials for using move_group: + * https://ros-planning.github.io/moveit_tutorials/doc/move_group_interface/move_group_interface_tutorial.html + */ + +// C++ +#include +#include +#include + +// ROS +#include + +// The Testing Framework and Utils +#include + +// MoveIt +#include +#include + +// TF2 +#include +#include + +// 10um acuracy tested for position and orientation +constexpr double EPSILON = 1e-5; + +static const std::string PLANNING_GROUP = "panda_arm"; +constexpr double PLANNING_TIME_S = 30.0; +constexpr double MAX_VELOCITY_SCALE = 1.0; +constexpr double MAX_ACCELERATION_SCALE = 1.0; +constexpr double GOAL_TOLERANCE = 1e-6; + +class MoveGroupTestFixture : public ::testing::Test +{ +public: + void SetUp() override + { + nh_ = ros::NodeHandle("/move_group_interface_cpp_test"); + move_group_ = std::make_shared(PLANNING_GROUP); + + // set velocity and acceleration scaling factors (full speed) + move_group_->setMaxVelocityScalingFactor(MAX_VELOCITY_SCALE); + move_group_->setMaxAccelerationScalingFactor(MAX_ACCELERATION_SCALE); + + // allow more time for planning + move_group_->setPlanningTime(PLANNING_TIME_S); + + // set the tolerance for the goals to be smaller than epsilon + move_group_->setGoalTolerance(GOAL_TOLERANCE); + } + + void planAndMoveToPose(const geometry_msgs::Pose& pose) + { + SCOPED_TRACE("planAndMoveToPose"); + ASSERT_TRUE(move_group_->setJointValueTarget(pose)); + planAndMove(); + } + + void planAndMove() + { + SCOPED_TRACE("planAndMove"); + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + ASSERT_EQ(move_group_->plan(my_plan), moveit::planning_interface::MoveItErrorCode::SUCCESS); + ASSERT_EQ(move_group_->move(), moveit::planning_interface::MoveItErrorCode::SUCCESS); + } + + void testEigenPose(const Eigen::Isometry3d& expected, const Eigen::Isometry3d& actual) + { + SCOPED_TRACE("testEigenPose"); + std::stringstream ss; + ss << "expected: \n" << expected.matrix() << "\nactual: \n" << actual.matrix(); + EXPECT_TRUE(actual.isApprox(expected, EPSILON)) << ss.str(); + } + + void testPose(const Eigen::Isometry3d& expected_pose) + { + SCOPED_TRACE("testPose(const Eigen::Isometry3d&)"); + // get the pose of the end effector link after the movement + geometry_msgs::PoseStamped actual_pose_stamped = move_group_->getCurrentPose(); + Eigen::Isometry3d actual_pose; + tf::poseMsgToEigen(actual_pose_stamped.pose, actual_pose); + + // compare to planned pose + testEigenPose(expected_pose, actual_pose); + } + + void testPose(const geometry_msgs::Pose& expected_pose_msg) + { + SCOPED_TRACE("testPose(const geometry_msgs::Pose&)"); + Eigen::Isometry3d expected_pose; + tf::poseMsgToEigen(expected_pose_msg, expected_pose); + testPose(expected_pose); + } + + void testJointPositions(const std::vector& expected) + { + SCOPED_TRACE("testJointPositions"); + const moveit::core::JointModelGroup* joint_model_group = + move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP); + std::vector actual; + move_group_->getCurrentState()->copyJointGroupPositions(joint_model_group, actual); + ASSERT_EQ(expected.size(), actual.size()); + for (size_t i = 0; i < actual.size(); ++i) + { + double delta = std::abs(expected[i] - actual[i]); + EXPECT_LT(delta, EPSILON) << "joint index: " << i << ", plan: " << expected[i] << ", result: " << actual[i]; + } + } + +protected: + ros::NodeHandle nh_; + moveit::planning_interface::MoveGroupInterfacePtr move_group_; + moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; +}; + +TEST_F(MoveGroupTestFixture, PathConstraintCollisionTest) +{ + SCOPED_TRACE("PathConstraintCollisionTest"); + + //////////////////////////////////////////////////////////////////// + // set a custom start state + // this simplifies planning for the orientation constraint bellow + geometry_msgs::Pose start_pose; + start_pose.orientation.w = 1.0; + start_pose.position.x = 0.3; + start_pose.position.y = 0.0; + start_pose.position.z = 0.6; + planAndMoveToPose(start_pose); + + //////////////////////////////////////////////////////////////////// + // Test setting target pose with eigen and with geometry_msgs + geometry_msgs::Pose target_pose; + target_pose.orientation.w = 1.0; + target_pose.position.x = 0.3; + target_pose.position.y = -0.3; + target_pose.position.z = 0.6; + + // convert to eigen + Eigen::Isometry3d eigen_target_pose; + tf::poseMsgToEigen(target_pose, eigen_target_pose); + + // set with eigen, get ros message representation + move_group_->setPoseTarget(eigen_target_pose); + geometry_msgs::PoseStamped set_target_pose = move_group_->getPoseTarget(); + Eigen::Isometry3d eigen_set_target_pose; + tf::poseMsgToEigen(set_target_pose.pose, eigen_set_target_pose); + + // expect that they are identical + testEigenPose(eigen_target_pose, eigen_set_target_pose); + + //////////////////////////////////////////////////////////////////// + // create an orientation constraint + moveit_msgs::OrientationConstraint ocm; + ocm.link_name = move_group_->getEndEffectorLink(); + ocm.header.frame_id = move_group_->getPlanningFrame(); + ocm.orientation.w = 1.0; + ocm.absolute_x_axis_tolerance = 0.1; + ocm.absolute_y_axis_tolerance = 0.1; + ocm.absolute_z_axis_tolerance = 0.1; + ocm.weight = 1.0; + moveit_msgs::Constraints test_constraints; + test_constraints.orientation_constraints.push_back(ocm); + move_group_->setPathConstraints(test_constraints); + + //////////////////////////////////////////////////////////////////// + // Define a collision object ROS message. + moveit_msgs::CollisionObject collision_object; + collision_object.header.frame_id = move_group_->getPlanningFrame(); + + // The id of the object is used to identify it. + collision_object.id = "box1"; + + // Define a box to add to the world. + shape_msgs::SolidPrimitive primitive; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[0] = 0.1; + primitive.dimensions[1] = 1.0; + primitive.dimensions[2] = 1.0; + + // Define a pose for the box (specified relative to frame_id) + geometry_msgs::Pose box_pose; + box_pose.orientation.w = 1.0; + box_pose.position.x = 0.5; + box_pose.position.y = 0.0; + box_pose.position.z = 0.5; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + collision_object.operation = collision_object.ADD; + + std::vector collision_objects; + collision_objects.push_back(collision_object); + + // Now, let's add the collision object into the world + planning_scene_interface_.addCollisionObjects(collision_objects); + + //////////////////////////////////////////////////////////////////// + // plan and move + planAndMove(); + + // get the pose after the movement + testPose(eigen_target_pose); + + // clear path constraints + move_group_->clearPathConstraints(); + + // attach and detach collision object + EXPECT_TRUE(move_group_->attachObject(collision_object.id)); + EXPECT_EQ(planning_scene_interface_.getAttachedObjects().size(), std::size_t(1)); + EXPECT_TRUE(move_group_->detachObject(collision_object.id)); + EXPECT_EQ(planning_scene_interface_.getAttachedObjects().size(), std::size_t(0)); + + // remove object from world + const std::vector object_ids = { collision_object.id }; + EXPECT_EQ(planning_scene_interface_.getObjects().size(), std::size_t(1)); + planning_scene_interface_.removeCollisionObjects(object_ids); + EXPECT_EQ(planning_scene_interface_.getObjects().size(), std::size_t(0)); +} + +TEST_F(MoveGroupTestFixture, CartPathTest) +{ + SCOPED_TRACE("CartPathTest"); + + // Plan from current pose + const geometry_msgs::PoseStamped start_pose = move_group_->getCurrentPose(); + + std::vector waypoints; + waypoints.push_back(start_pose.pose); + + geometry_msgs::Pose target_waypoint = start_pose.pose; + target_waypoint.position.z -= 0.2; + waypoints.push_back(target_waypoint); // down + + target_waypoint.position.y -= 0.2; + waypoints.push_back(target_waypoint); // right + + target_waypoint.position.z += 0.2; + target_waypoint.position.y += 0.2; + target_waypoint.position.x -= 0.2; + waypoints.push_back(target_waypoint); // up and left + + moveit_msgs::RobotTrajectory trajectory; + const auto jump_threshold = 0.0; + const auto eef_step = 0.01; + move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); + + // Execute trajectory + EXPECT_EQ(move_group_->execute(trajectory), moveit::planning_interface::MoveItErrorCode::SUCCESS); + + // get the pose after the movement + testPose(target_waypoint); +} + +TEST_F(MoveGroupTestFixture, JointSpaceGoalTest) +{ + SCOPED_TRACE("JointSpaceGoalTest"); + + // Next get the current set of joint values for the group. + std::vector plan_joint_positions; + move_group_->getCurrentState()->copyJointGroupPositions( + move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP), plan_joint_positions); + + // Now, let's modify the joint positions. (radians) + ASSERT_EQ(plan_joint_positions.size(), std::size_t(7)); + plan_joint_positions = { 1.2, -1.0, -0.1, -2.4, 0.0, 1.5, 0.6 }; + move_group_->setJointValueTarget(plan_joint_positions); + + // plan and move + planAndMove(); + + // test that we moved to the expected joint positions + testJointPositions(plan_joint_positions); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "move_group_interface_cpp_test"); + testing::InitGoogleTest(&argc, argv); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test new file mode 100644 index 0000000000..9975f257d2 --- /dev/null +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test @@ -0,0 +1,29 @@ + + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + diff --git a/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp new file mode 100644 index 0000000000..4b2d906448 --- /dev/null +++ b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp @@ -0,0 +1,290 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Tyler Weaver + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Robotics nor the + * names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Tyler Weaver */ + +/* These integration tests are based on the tutorials for using move_group to do a pick and place: + * https://ros-planning.github.io/moveit_tutorials/doc/pick_place/pick_place_tutorial.html + */ + +// C++ +#include + +// ROS +#include + +// The Testing Framework and Utils +#include + +// MoveIt +#include +#include + +// TF2 +#include + +static const std::string PLANNING_GROUP = "panda_arm"; +constexpr double PLANNING_TIME_S = 45.0; +constexpr double MAX_VELOCITY_SCALE = 1.0; +constexpr double MAX_ACCELERATION_SCALE = 1.0; + +class PickPlaceTestFixture : public ::testing::Test +{ +public: + void SetUp() override + { + nh_ = ros::NodeHandle("/move_group_pick_place_test"); + move_group_ = std::make_shared(PLANNING_GROUP); + + // set velocity and acceleration scaling factors (full speed) + move_group_->setMaxVelocityScalingFactor(MAX_VELOCITY_SCALE); + move_group_->setMaxAccelerationScalingFactor(MAX_ACCELERATION_SCALE); + + // allow more time for planning + move_group_->setPlanningTime(PLANNING_TIME_S); + } + +protected: + ros::NodeHandle nh_; + moveit::planning_interface::MoveGroupInterfacePtr move_group_; + moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; +}; + +TEST_F(PickPlaceTestFixture, PickPlaceTest) +{ + // Create vector to hold 3 collision objects. + std::vector collision_objects; + collision_objects.resize(3); + + // Add the first table where the cube will originally be kept. + collision_objects[0].id = "table1"; + collision_objects[0].header.frame_id = "panda_link0"; + + /* Create identity rotation quaternion */ + tf2::Quaternion zero_orientation; + zero_orientation.setRPY(0, 0, 0); + geometry_msgs::Quaternion zero_orientation_msg = tf2::toMsg(zero_orientation); + + /* Define the primitive and its dimensions. */ + collision_objects[0].primitives.resize(1); + collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX; + collision_objects[0].primitives[0].dimensions.resize(3); + collision_objects[0].primitives[0].dimensions[0] = 0.2; + collision_objects[0].primitives[0].dimensions[1] = 0.4; + collision_objects[0].primitives[0].dimensions[2] = 0.4; + + /* Define the pose of the table. */ + collision_objects[0].primitive_poses.resize(1); + collision_objects[0].primitive_poses[0].position.x = 0.5; + collision_objects[0].primitive_poses[0].position.y = 0; + collision_objects[0].primitive_poses[0].position.z = 0.2; + collision_objects[0].primitive_poses[0].orientation = zero_orientation_msg; + + collision_objects[0].operation = collision_objects[0].ADD; + + // Add the second table where we will be placing the cube. + collision_objects[1].id = "table2"; + collision_objects[1].header.frame_id = "panda_link0"; + + /* Define the primitive and its dimensions. */ + collision_objects[1].primitives.resize(1); + collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX; + collision_objects[1].primitives[0].dimensions.resize(3); + collision_objects[1].primitives[0].dimensions[0] = 0.4; + collision_objects[1].primitives[0].dimensions[1] = 0.2; + collision_objects[1].primitives[0].dimensions[2] = 0.4; + + /* Define the pose of the table. */ + collision_objects[1].primitive_poses.resize(1); + collision_objects[1].primitive_poses[0].position.x = 0; + collision_objects[1].primitive_poses[0].position.y = 0.5; + collision_objects[1].primitive_poses[0].position.z = 0.2; + collision_objects[1].primitive_poses[0].orientation = zero_orientation_msg; + + collision_objects[1].operation = collision_objects[1].ADD; + + // Define the object that we will be manipulating + collision_objects[2].header.frame_id = "panda_link0"; + collision_objects[2].id = "object"; + + /* Define the primitive and its dimensions. */ + collision_objects[2].primitives.resize(1); + collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX; + collision_objects[2].primitives[0].dimensions.resize(3); + collision_objects[2].primitives[0].dimensions[0] = 0.02; + collision_objects[2].primitives[0].dimensions[1] = 0.02; + collision_objects[2].primitives[0].dimensions[2] = 0.2; + + /* Define the pose of the object. */ + collision_objects[2].primitive_poses.resize(1); + collision_objects[2].primitive_poses[0].position.x = 0.5; + collision_objects[2].primitive_poses[0].position.y = 0; + collision_objects[2].primitive_poses[0].position.z = 0.5; + collision_objects[2].primitive_poses[0].orientation = zero_orientation_msg; + + collision_objects[2].operation = collision_objects[2].ADD; + + planning_scene_interface_.applyCollisionObjects(collision_objects); + + // Create a vector of grasps to be attempted, currently only creating single grasp. + // This is essentially useful when using a grasp generator to generate and test multiple grasps. + std::vector grasps; + grasps.resize(1); + + // Setting grasp pose + // ++++++++++++++++++++++ + // This is the pose of panda_link8. |br| + // Make sure that when you set the grasp_pose, you are setting it to be the pose of the last link in + // your manipulator which in this case would be `"panda_link8"` You will have to compensate for the + // transform from `"panda_link8"` to the palm of the end effector. + grasps[0].grasp_pose.header.frame_id = "panda_link0"; + tf2::Quaternion grasp_orientation; + grasp_orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2); + grasps[0].grasp_pose.pose.orientation = tf2::toMsg(grasp_orientation); + grasps[0].grasp_pose.pose.position.x = 0.415; + grasps[0].grasp_pose.pose.position.y = 0; + grasps[0].grasp_pose.pose.position.z = 0.5; + + // Setting pre-grasp approach + // ++++++++++++++++++++++++++ + /* Defined with respect to frame_id */ + grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0"; + /* Direction is set as positive x axis */ + grasps[0].pre_grasp_approach.direction.vector.x = 1.0; + grasps[0].pre_grasp_approach.min_distance = 0.095; + grasps[0].pre_grasp_approach.desired_distance = 0.115; + + // Setting post-grasp retreat + // ++++++++++++++++++++++++++ + /* Defined with respect to frame_id */ + grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0"; + /* Direction is set as positive z axis */ + grasps[0].post_grasp_retreat.direction.vector.z = 1.0; + grasps[0].post_grasp_retreat.min_distance = 0.1; + grasps[0].post_grasp_retreat.desired_distance = 0.25; + + // Setting posture of eef before grasp + // +++++++++++++++++++++++++++++++++++ + /* Add both finger joints of panda robot. */ + grasps[0].pre_grasp_posture.joint_names.resize(2); + grasps[0].pre_grasp_posture.joint_names[0] = "panda_finger_joint1"; + grasps[0].pre_grasp_posture.joint_names[1] = "panda_finger_joint2"; + + /* Set them as open, wide enough for the object to fit. */ + grasps[0].pre_grasp_posture.points.resize(1); + grasps[0].pre_grasp_posture.points[0].positions.resize(2); + grasps[0].pre_grasp_posture.points[0].positions[0] = 0.04; + grasps[0].pre_grasp_posture.points[0].positions[1] = 0.04; + grasps[0].pre_grasp_posture.points[0].time_from_start = ros::Duration(0.5); + + // Setting posture of eef during grasp + // +++++++++++++++++++++++++++++++++++ + /* Add both finger joints of panda robot and set them as closed. */ + grasps[0].grasp_posture = grasps[0].pre_grasp_posture; + grasps[0].grasp_posture.points[0].positions[0] = 0.00; + grasps[0].grasp_posture.points[0].positions[1] = 0.00; + + // Set support surface as table1. + move_group_->setSupportSurfaceName("table1"); + // Call pick to pick up the object using the grasps given + ASSERT_EQ(move_group_->pick("object", grasps), moveit::planning_interface::MoveItErrorCode::SUCCESS); + + // Ideally, you would create a vector of place locations to be attempted although in this example, we only create + // a single place location. + std::vector place_location; + place_location.resize(1); + + // Setting place location pose + // +++++++++++++++++++++++++++ + place_location[0].place_pose.header.frame_id = "panda_link0"; + tf2::Quaternion place_orientation; + place_orientation.setRPY(0, 0, M_PI / 2); + place_location[0].place_pose.pose.orientation = tf2::toMsg(place_orientation); + + /* For place location, we set the value to the exact location of the center of the object. */ + place_location[0].place_pose.pose.position.x = 0; + place_location[0].place_pose.pose.position.y = 0.5; + place_location[0].place_pose.pose.position.z = 0.5; + + // Setting pre-place approach + // ++++++++++++++++++++++++++ + /* Defined with respect to frame_id */ + place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0"; + /* Direction is set as negative z axis */ + place_location[0].pre_place_approach.direction.vector.z = -1.0; + place_location[0].pre_place_approach.min_distance = 0.095; + place_location[0].pre_place_approach.desired_distance = 0.115; + + // Setting post-grasp retreat + // ++++++++++++++++++++++++++ + /* Defined with respect to frame_id */ + place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0"; + /* Direction is set as negative y axis */ + place_location[0].post_place_retreat.direction.vector.y = -1.0; + place_location[0].post_place_retreat.min_distance = 0.1; + place_location[0].post_place_retreat.desired_distance = 0.25; + + // Setting posture of eef after placing object + // +++++++++++++++++++++++++++++++++++++++++++ + /* Similar to the pick case */ + /* Add both finger joints of panda robot. */ + place_location[0].post_place_posture.joint_names.resize(2); + place_location[0].post_place_posture.joint_names[0] = "panda_finger_joint1"; + place_location[0].post_place_posture.joint_names[1] = "panda_finger_joint2"; + + /* Set them as open, wide enough for the object to fit. */ + place_location[0].post_place_posture.points.resize(1); + place_location[0].post_place_posture.points[0].positions.resize(2); + place_location[0].post_place_posture.points[0].positions[0] = 0.04; + place_location[0].post_place_posture.points[0].positions[1] = 0.04; + place_location[0].post_place_posture.points[0].time_from_start = ros::Duration(0.5); + + // Set support surface as table2. + move_group_->setSupportSurfaceName("table2"); + // Call place to place the object using the place locations given. + ASSERT_EQ(move_group_->place("object", place_location), moveit::planning_interface::MoveItErrorCode::SUCCESS); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "move_group_pick_place_test"); + testing::InitGoogleTest(&argc, argv); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/moveit_ros/planning_interface/test/move_group_pick_place_test.test b/moveit_ros/planning_interface/test/move_group_pick_place_test.test new file mode 100644 index 0000000000..9dd7dbdcc3 --- /dev/null +++ b/moveit_ros/planning_interface/test/move_group_pick_place_test.test @@ -0,0 +1,29 @@ + + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + diff --git a/moveit_ros/planning_interface/test/moveit_cpp.yaml b/moveit_ros/planning_interface/test/moveit_cpp.yaml new file mode 100644 index 0000000000..6d622662c9 --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp.yaml @@ -0,0 +1,18 @@ +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/planning_scene_monitor" + publish_planning_scene_topic: "/publish_planning_scene" + monitored_planning_scene_topic: "/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +planning_pipelines: + pipeline_names: + - ompl + +plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 \ No newline at end of file diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp new file mode 100644 index 0000000000..d1430075e0 --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp @@ -0,0 +1,175 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jafar Abdi + Desc: Test the MoveItCpp and PlanningComponent interfaces +*/ + +// ROS +#include + +// Testing +#include + +// Main class +#include +#include +// Msgs +#include + +namespace moveit +{ +namespace planning_interface +{ +class MoveItCppTest : public ::testing::Test +{ +public: + void SetUp() override + { + nh_ = ros::NodeHandle("/moveit_cpp_test"); + moveit_cpp_ptr = std::make_shared(nh_); + planning_component_ptr = std::make_shared(PLANNING_GROUP, moveit_cpp_ptr); + robot_model_ptr = moveit_cpp_ptr->getRobotModel(); + jmg_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); + + target_pose1.header.frame_id = "panda_link0"; + target_pose1.pose.orientation.w = 1.0; + target_pose1.pose.position.x = 0.28; + target_pose1.pose.position.y = -0.2; + target_pose1.pose.position.z = 0.5; + + start_pose.orientation.w = 1.0; + start_pose.position.x = 0.55; + start_pose.position.y = 0.0; + start_pose.position.z = 0.6; + + target_pose2.orientation.w = 1.0; + target_pose2.position.x = 0.55; + target_pose2.position.y = -0.05; + target_pose2.position.z = 0.8; + } + +protected: + ros::NodeHandle nh_; + MoveItCppPtr moveit_cpp_ptr; + PlanningComponentPtr planning_component_ptr; + moveit::core::RobotModelConstPtr robot_model_ptr; + const moveit::core::JointModelGroup* jmg_ptr; + const std::string PLANNING_GROUP = "panda_arm"; + geometry_msgs::PoseStamped target_pose1; + geometry_msgs::Pose target_pose2; + geometry_msgs::Pose start_pose; +}; + +// Test the current and the initial state of the Panda robot +TEST_F(MoveItCppTest, GetCurrentStateTest) +{ + ros::Duration(1.0).sleep(); // Otherwise joint_states will result in an invalid robot state + auto robot_model = moveit_cpp_ptr->getRobotModel(); + auto robot_state = std::make_shared(robot_model); + EXPECT_TRUE(moveit_cpp_ptr->getCurrentState(robot_state, 0.0)); + // Make sure the Panda robot is in "ready" state which is loaded from fake_controller.yaml + std::vector joints_vals; + robot_state->copyJointGroupPositions(PLANNING_GROUP, joints_vals); + EXPECT_NEAR(joints_vals[0], 0.0, 0.001); // panda_joint1 + EXPECT_NEAR(joints_vals[1], -0.785, 0.001); // panda_joint2 + EXPECT_NEAR(joints_vals[2], 0.0, 0.001); // panda_joint3 + EXPECT_NEAR(joints_vals[3], -2.356, 0.001); // panda_joint4 + EXPECT_NEAR(joints_vals[4], 0.0, 0.001); // panda_joint5 + EXPECT_NEAR(joints_vals[5], 1.571, 0.001); // panda_joint6 + EXPECT_NEAR(joints_vals[6], 0.785, 0.001); // panda_joint7 +} + +// Test the name of the planning group used by PlanningComponent for the Panda robot +TEST_F(MoveItCppTest, NameOfPlanningGroupTest) +{ + EXPECT_STREQ(planning_component_ptr->getPlanningGroupName().c_str(), "panda_arm"); +} + +// Test setting the start state of the plan to the current state of the robot +TEST_F(MoveItCppTest, TestSetStartStateToCurrentState) +{ + planning_component_ptr->setStartStateToCurrentState(); + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(static_cast(planning_component_ptr->plan())); + // TODO(JafarAbdi) adding testing to the soln state +} + +// Test setting the goal using geometry_msgs::PoseStamped and a robot's link name +TEST_F(MoveItCppTest, TestSetGoalFromPoseStamped) +{ + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(static_cast(planning_component_ptr->plan())); +} + +// Test setting the plan start state using moveit::core::RobotState +TEST_F(MoveItCppTest, TestSetStartStateFromRobotState) +{ + auto start_state = *(moveit_cpp_ptr->getCurrentState()); + start_state.setFromIK(jmg_ptr, start_pose); + + planning_component_ptr->setStartState(start_state); + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(static_cast(planning_component_ptr->plan())); +} + +// Test settting the goal of the plan using a moveit::core::RobotState +TEST_F(MoveItCppTest, TestSetGoalFromRobotState) +{ + auto target_state = *(moveit_cpp_ptr->getCurrentState()); + + target_state.setFromIK(jmg_ptr, target_pose2); + + planning_component_ptr->setGoal(target_state); + + ASSERT_TRUE(static_cast(planning_component_ptr->plan())); +} +} // namespace planning_interface +} // namespace moveit + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "moveit_cpp_test"); + + ros::AsyncSpinner spinner(4); + spinner.start(); + + int result = RUN_ALL_TESTS(); + + return result; +} diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.test b/moveit_ros/planning_interface/test/moveit_cpp_test.test new file mode 100644 index 0000000000..306d97553c --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.test @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + ["/moveit_cpp_test/fake_controller_joint_states"] + + + + + + + + diff --git a/moveit_ros/planning_interface/test/python_move_group.py b/moveit_ros/planning_interface/test/python_move_group.py index 4e388a6f16..0cc27cd09d 100755 --- a/moveit_ros/planning_interface/test/python_move_group.py +++ b/moveit_ros/planning_interface/test/python_move_group.py @@ -80,6 +80,13 @@ def test_get_jacobian_matrix(self): [ 1. , 0. , 0. , 0. , 0. , 0. ]]) self.assertTrue(np.allclose(result, expected)) + result = self.group.get_jacobian_matrix(current, [1.0, 1.0, 1.0]) + expected = np.array([[ 1. , 1.8 , -1.2 , 0. , -1. , 0. ], + [ 1.89, 0. , 0. , 1. , 0. , 1. ], + [ 0. , -1.74, 1.74, 1. , 1.1 , 1. ], + [ 0. , 0. , 0. , -1. , 0. , -1. ], + [ 0. , 1. , -1. , 0. , -1. , 0. ], + [ 1. , 0. , 0. , 0. , 0. , 0. ]]) if __name__ == '__main__': PKGNAME = 'moveit_ros_planning_interface' diff --git a/moveit_ros/planning_interface/test/python_move_group.test b/moveit_ros/planning_interface/test/python_move_group.test index 01c6651892..4367da03ed 100644 --- a/moveit_ros/planning_interface/test/python_move_group.test +++ b/moveit_ros/planning_interface/test/python_move_group.test @@ -1,7 +1,7 @@ - + - + diff --git a/moveit_ros/planning_interface/test/robot_state_update.test b/moveit_ros/planning_interface/test/robot_state_update.test index e282f0ab22..f7849d7d7c 100644 --- a/moveit_ros/planning_interface/test/robot_state_update.test +++ b/moveit_ros/planning_interface/test/robot_state_update.test @@ -1,5 +1,5 @@ - + diff --git a/moveit_ros/planning_interface/test/serialize_msg.py b/moveit_ros/planning_interface/test/serialize_msg.py new file mode 100644 index 0000000000..0497d5d9f9 --- /dev/null +++ b/moveit_ros/planning_interface/test/serialize_msg.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, RWTH Aachen University +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of RWTH Aachen University nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Bjarne von Horn +# + +from moveit_ros_planning_interface._moveit_planning_interface_test_serialize_msg_cpp_helper import ByteStringTestHelper +from geometry_msgs.msg import Vector3 +import unittest + +try: + # Try Python 2.7 behaviour first + from StringIO import StringIO + py_version_maj = 2 +except ImportError: + # Use Python 3.x behaviour as fallback and choose the non-unicode version + from io import BytesIO as StringIO + py_version_maj = 3 + + +class PythonMsgSerializeTest(unittest.TestCase): + def setUp(self): + self.helper = ByteStringTestHelper() + + def test_EmbeddedZeros(self): + self.assertTrue(self.helper.compareEmbeddedZeros(b"\xff\xef\x00\x10")) + + def test_ByteStringFromPchar(self): + # ByteString(const char*) constructor + self.assertEqual(self.helper.getBytesPChar(), b"abcdef") + + def test_ByteStringFromStdString(self): + # ByteString(const std::string&) constructor + self.assertEqual(self.helper.getBytesStdString(), b"\xff\xfe\x10\x00\x00") + + def test_ByteStringDefaultCtor(self): + self.assertEqual(self.helper.getDefaultBytes(), b"") + + def test_CxxTupleToPy(self): + # sending a tuple from C++ to Python + ans = self.helper.getTuple() + self.assertIsInstance(ans, tuple) + self.assertEqual(len(ans), 1) + self.assertEqual(ans[0], b"abcdef") + + def test_PyTupleToCxx(self): + # sending a tuple from Python to C++ + self.assertTrue(self.helper.compareTuple((b"mno",))) + + def test_sendMessage(self): + tmp = StringIO() + Vector3(x=1.0, y=-2.0, z=0.25).serialize(tmp) + self.assertTrue(self.helper.compareVector(tmp.getvalue())) + + def test_recieveMessage(self): + tmp = Vector3() + tmp.deserialize(self.helper.getVector()) + self.assertEqual(tmp, Vector3(1.0, -2.0, 0.25)) + + def test_rejectInt(self): + with self.assertRaisesRegexp(Exception, "Python argument types in"): + self.helper.compareEmbeddedZeros(4711) + + def test_rejectIntTuple(self): + with self.assertRaisesRegexp(Exception, "Python argument types in"): + self.helper.compareEmbeddedZeros((4711,)) + + def test_rejectUnicode(self): + with self.assertRaisesRegexp(Exception, "Python argument types in"): + self.helper.compareEmbeddedZeros(u'kdasd') + + @unittest.skipIf(py_version_maj == 2, "does not trigger with python 2.7") + def test_rejectUnicodeTuple(self): + with self.assertRaisesRegexp(RuntimeError, "Underlying python object is not a Bytes/String instance"): + self.helper.compareVectorTuple((u'kdasd',)) + + +if __name__ == '__main__': + unittest.main() diff --git a/moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp b/moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp new file mode 100644 index 0000000000..13c6e446d6 --- /dev/null +++ b/moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp @@ -0,0 +1,134 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, RWTH Aachen University. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of RWTH Aachen University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Bjarne von Horn */ + +#include +#include +#include +#include +#include + +namespace bp = boost::python; +using moveit::py_bindings_tools::ByteString; + +// Helper class to be exposed to Python +class ByteStringTestHelper +{ + // Helper to test whether a vector of unsigned chars has the same content as a bytes Object + bool doCompare(const std::vector& data, PyObject* obj) + { + const char* py_data = PyBytes_AsString(obj); + if (!py_data) + return false; + Py_ssize_t size = PyBytes_GET_SIZE(obj); + if (size < 0 || std::vector::size_type(size) != data.size()) + return false; + return std::memcmp(py_data, &data[0], size) == 0; + } + +public: + bool compareEmbeddedZeros(const ByteString& s) + { + const std::vector testdata{ 0xff, 0xef, 0x00, 0x10 }; + return doCompare(testdata, s.ptr()); + } + bool compareTuple(const bp::tuple& t) + { + const ByteString s(t[0]); + const std::vector testdata{ 'm', 'n', 'o' }; + return doCompare(testdata, s.ptr()); + } + + ByteString getBytesPChar() + { + return ByteString("abcdef"); + } + ByteString getBytesStdString() + { + std::string s; + s.push_back('\xff'); + s.push_back('\xfe'); + s.push_back('\x10'); + s.push_back('\x00'); + s.push_back('\x00'); + return ByteString(s); + } + ByteString getDefaultBytes() + { + return ByteString(); + } + bp::tuple getTuple() + { + return bp::make_tuple(ByteString("abcdef")); + } + ByteString getVector() + { + geometry_msgs::Vector3 v; + v.x = 1.0; + v.y = -2.0; + v.z = 0.25; + return ByteString(v); + } + bool compareVector(const ByteString& s) + { + geometry_msgs::Vector3 v; + s.deserialize(v); + return v.x == 1.0 && v.y == -2.0 && v.z == 0.25; + } + bool compareVectorTuple(const bp::tuple& t) + { + const ByteString s(t[0]); + return compareVector(s); + } + + static void setup() + { + bp::class_ cls("ByteStringTestHelper"); + cls.def("compareEmbeddedZeros", &ByteStringTestHelper::compareEmbeddedZeros); + cls.def("compareTuple", &ByteStringTestHelper::compareTuple); + cls.def("compareVectorTuple", &ByteStringTestHelper::compareVectorTuple); + cls.def("getBytesPChar", &ByteStringTestHelper::getBytesPChar); + cls.def("getBytesStdString", &ByteStringTestHelper::getBytesStdString); + cls.def("getDefaultBytes", &ByteStringTestHelper::getDefaultBytes); + cls.def("getTuple", &ByteStringTestHelper::getTuple); + cls.def("getVector", &ByteStringTestHelper::getVector); + cls.def("compareVector", &ByteStringTestHelper::compareVector); + } +}; + +BOOST_PYTHON_MODULE(_moveit_planning_interface_test_serialize_msg_cpp_helper) +{ + ByteStringTestHelper::setup(); +} diff --git a/moveit_ros/planning_interface/test/subframes_test.cpp b/moveit_ros/planning_interface/test/subframes_test.cpp new file mode 100644 index 0000000000..d18d8b4939 --- /dev/null +++ b/moveit_ros/planning_interface/test/subframes_test.cpp @@ -0,0 +1,201 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Felix von Drigalski, Jacob Aas, Tyler Weaver + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of OMRON SINIC X or PickNik Robotics nor the + * names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Felix von Drigalski, Jacob Aas, Tyler Weaver, Boston Cleek */ + +/* This integration test is based on the tutorial for using subframes + * https://ros-planning.github.io/moveit_tutorials/doc/subframes/subframes_tutorial.html + */ + +// C++ +#include +#include + +// ROS +#include + +// The Testing Framework and Utils +#include + +// MoveIt +#include +#include +#include + +// TF2 +#include +#include + +constexpr double EPSILON = 1e-2; +constexpr double Z_OFFSET = 0.05; +constexpr double PLANNING_TIME_S = 30.0; + +// Function copied from tutorial +// a small helper function to create our planning requests and move the robot. +bool moveToCartPose(const geometry_msgs::PoseStamped& pose, moveit::planning_interface::MoveGroupInterface& group, + const std::string& end_effector_link) +{ + group.clearPoseTargets(); + group.setEndEffectorLink(end_effector_link); + group.setStartStateToCurrentState(); + group.setPoseTarget(pose); + + moveit::planning_interface::MoveGroupInterface::Plan myplan; + if (group.plan(myplan) && group.execute(myplan)) + { + return true; + } + + ROS_WARN("Failed to perform motion."); + return false; +} + +// Function copied from tutorial +// This helper function creates two objects and publishes them to the PlanningScene: a box and a cylinder. +// The box spawns in front of the gripper, the cylinder at the tip of the gripper, as if it had been grasped. +void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface) +{ + const std::string log_name = "spawn_collision_objects"; + double z_offset_box = .25; // The z-axis points away from the gripper + double z_offset_cylinder = .1; + + // First, we start defining the CollisionObject as usual. + moveit_msgs::CollisionObject box; + box.id = "box"; + box.header.frame_id = "panda_hand"; + box.primitives.resize(1); + box.primitive_poses.resize(1); + box.primitives[0].type = box.primitives[0].BOX; + box.primitives[0].dimensions.resize(3); + box.primitives[0].dimensions[0] = 0.05; + box.primitives[0].dimensions[1] = 0.1; + box.primitives[0].dimensions[2] = 0.02; + box.primitive_poses[0].position.z = z_offset_box; + + // Then, we define the subframes of the CollisionObject. + box.subframe_names.resize(1); + box.subframe_poses.resize(1); + box.subframe_names[0] = "bottom"; + box.subframe_poses[0].position.y = -.05; + box.subframe_poses[0].position.z = 0.0 + z_offset_box; + tf2::Quaternion orientation; + orientation.setRPY(90.0 / 180.0 * M_PI, 0, 0); + box.subframe_poses[0].orientation = tf2::toMsg(orientation); + + // Next, define the cylinder + moveit_msgs::CollisionObject cylinder; + cylinder.id = "cylinder"; + cylinder.header.frame_id = "panda_hand"; + cylinder.primitives.resize(1); + cylinder.primitive_poses.resize(1); + cylinder.primitives[0].type = box.primitives[0].CYLINDER; + cylinder.primitives[0].dimensions.resize(2); + cylinder.primitives[0].dimensions[0] = 0.06; // height (along x) + cylinder.primitives[0].dimensions[1] = 0.005; // radius + cylinder.primitive_poses[0].position.x = 0.0; + cylinder.primitive_poses[0].position.y = 0.0; + cylinder.primitive_poses[0].position.z = 0.0 + z_offset_cylinder; + orientation.setRPY(0, 90.0 / 180.0 * M_PI, 0); + cylinder.primitive_poses[0].orientation = tf2::toMsg(orientation); + + cylinder.subframe_poses.resize(1); + cylinder.subframe_names.resize(1); + cylinder.subframe_names[0] = "tip"; + cylinder.subframe_poses[0].position.x = 0.03; + cylinder.subframe_poses[0].position.y = 0.0; + cylinder.subframe_poses[0].position.z = 0.0 + z_offset_cylinder; + orientation.setRPY(0, 90.0 / 180.0 * M_PI, 0); + cylinder.subframe_poses[0].orientation = tf2::toMsg(orientation); + + // Lastly, the objects are published to the PlanningScene. In this tutorial, we publish a box and a cylinder. + box.operation = moveit_msgs::CollisionObject::ADD; + cylinder.operation = moveit_msgs::CollisionObject::ADD; + planning_scene_interface.applyCollisionObjects({ box, cylinder }); +} + +TEST(TestPlanUsingSubframes, SubframesTests) +{ + const std::string log_name = "test_plan_using_subframes"; + ros::NodeHandle nh(log_name); + + auto planning_scene_monitor = std::make_shared("robot_description"); + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + moveit::planning_interface::MoveGroupInterface group("panda_arm"); + group.setPlanningTime(PLANNING_TIME_S); + + spawnCollisionObjects(planning_scene_interface); + moveit_msgs::AttachedCollisionObject att_coll_object; + att_coll_object.object.id = "cylinder"; + att_coll_object.link_name = "panda_hand"; + att_coll_object.object.operation = att_coll_object.object.ADD; + planning_scene_interface.applyAttachedCollisionObject(att_coll_object); + + tf2::Quaternion target_orientation; + target_orientation.setRPY(0, 180.0 / 180.0 * M_PI, 90.0 / 180.0 * M_PI); + geometry_msgs::PoseStamped target_pose_stamped; + target_pose_stamped.pose.orientation = tf2::toMsg(target_orientation); + target_pose_stamped.pose.position.z = Z_OFFSET; + + ROS_INFO_STREAM_NAMED(log_name, "Moving to bottom of box with cylinder tip"); + target_pose_stamped.header.frame_id = "box/bottom"; + ASSERT_TRUE(moveToCartPose(target_pose_stamped, group, "cylinder/tip")); + planning_scene_monitor->requestPlanningSceneState(); + { + planning_scene_monitor::LockedPlanningSceneRO planning_scene(planning_scene_monitor); + + // get the tip and box subframe locations in world + Eigen::Isometry3d eef = planning_scene->getFrameTransform("cylinder/tip"); + Eigen::Isometry3d box_subframe = planning_scene->getFrameTransform(target_pose_stamped.header.frame_id); + Eigen::Isometry3d target_pose; + tf::poseMsgToEigen(target_pose_stamped.pose, target_pose); + + // expect that they are identical + std::stringstream ss; + ss << "target frame: \n" << (box_subframe * target_pose).matrix() << "\ncylinder frame: \n" << eef.matrix(); + EXPECT_TRUE(eef.isApprox(box_subframe * target_pose, EPSILON)) << ss.str(); + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "moveit_test_plan_using_subframes"); + testing::InitGoogleTest(&argc, argv); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/moveit_ros/planning_interface/test/subframes_test.test b/moveit_ros/planning_interface/test/subframes_test.test new file mode 100644 index 0000000000..761b8210ad --- /dev/null +++ b/moveit_ros/planning_interface/test/subframes_test.test @@ -0,0 +1,29 @@ + + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst index 93a596b313..58db765fb2 100644 --- a/moveit_ros/robot_interaction/CHANGELOG.rst +++ b/moveit_ros/robot_interaction/CHANGELOG.rst @@ -2,6 +2,45 @@ Changelog for package moveit_ros_robot_interaction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski + +1.1.0 (2020-09-04) +------------------ +* [feature] Optional cpp version setting (`#2166 `_) +* [fix] Various fixes for upcoming Noetic release `#2180 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#1419 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Dave Coleman, Henning Kayser, Jonathan Binney, Markus Vieth, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Markus Vieth, Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* Contributors: Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -46,7 +85,7 @@ Changelog for package moveit_ros_robot_interaction 0.10.2 (2018-10-24) ------------------- -* [fix] Text refrences to MoveIt! (`#1020 `_) +* [fix] Text refrences to MoveIt (`#1020 `_) * Contributors: Mohmmad Ayman, Robert Haschke, mike lautman 0.10.1 (2018-05-25) @@ -80,7 +119,7 @@ Changelog for package moveit_ros_robot_interaction 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman diff --git a/moveit_ros/robot_interaction/CMakeLists.txt b/moveit_ros/robot_interaction/CMakeLists.txt index 08ef6ef7fd..498b8e1c5d 100644 --- a/moveit_ros/robot_interaction/CMakeLists.txt +++ b/moveit_ros/robot_interaction/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_robot_interaction) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -26,14 +30,16 @@ catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS - moveit_ros_planning interactive_markers + moveit_ros_planning + roscpp tf2_geometry_msgs ) -include_directories(include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS}) +include_directories(include) +include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS}) add_library(${MOVEIT_LIB_NAME} src/interactive_marker_helpers.cpp @@ -53,7 +59,10 @@ if (CATKIN_ENABLE_TESTING) ${Boost_LIBRARIES}) endif() -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(DIRECTORY resources DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h index 1fdafbf667..95121439e0 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley */ -#ifndef MOVEIT_ROBOT_INTERACTION_INTERACTION_ -#define MOVEIT_ROBOT_INTERACTION_INTERACTION_ +#pragma once #include #include @@ -50,7 +49,7 @@ namespace core { class RobotState; } -} +} // namespace moveit namespace robot_interaction { @@ -76,7 +75,7 @@ enum InteractionStyle ORIENTATION_NOSPHERE = ORIENTATION_CIRCLES | ORIENTATION_EEF, SIX_DOF_NOSPHERE = POSITION_NOSPHERE | ORIENTATION_NOSPHERE }; -} +} // namespace InteractionStyle /// Type of function for constructing markers. /// This callback sets up the marker used for an interaction. @@ -85,7 +84,7 @@ enum InteractionStyle /// that will be used to control the interaction. /// @returns true if the function succeeds, false if the function was not able /// to fill in \e marker. -typedef boost::function +typedef boost::function InteractiveMarkerConstructorFn; /// Type of function for processing marker feedback. @@ -99,7 +98,7 @@ typedef boost::function ProcessFeedbackFn; @@ -112,7 +111,7 @@ typedef boost::function InteractiveMarkerUpdateFn; +typedef boost::function InteractiveMarkerUpdateFn; /// Representation of a generic interaction. /// Displays one interactive marker. @@ -174,6 +173,4 @@ struct JointInteraction /// The size of the connecting link (diameter of enclosing sphere) double size; }; -} - -#endif +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h index 5cc6ed89a5..487d1dd79d 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h @@ -34,9 +34,9 @@ /* Author: Ioan Sucan, Adam Leeper */ -#ifndef MOVEIT_ROBOT_INTERACTION_INTERACTION_HANDLER_ -#define MOVEIT_ROBOT_INTERACTION_INTERACTION_HANDLER_ +#pragma once +#include #include #include //#include @@ -46,9 +46,9 @@ namespace robot_interaction { -MOVEIT_CLASS_FORWARD(InteractionHandler); -MOVEIT_CLASS_FORWARD(RobotInteraction); -MOVEIT_CLASS_FORWARD(KinematicOptionsMap); +MOVEIT_CLASS_FORWARD(InteractionHandler); // Defines InteractionHandlerPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RobotInteraction); // Defines RobotInteractionPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(KinematicOptionsMap); // Defines KinematicOptionsMapPtr, ConstPtr, WeakPtr... etc struct EndEffectorInteraction; struct JointInteraction; @@ -78,7 +78,7 @@ class InteractionHandler : public LockedRobotState public: // Use this constructor if you have an initial RobotState already. InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name, - const robot_state::RobotState& initial_robot_state, + const moveit::core::RobotState& initial_robot_state, const std::shared_ptr& tf_buffer = std::shared_ptr()); // Use this constructor to start with a default state. @@ -86,10 +86,10 @@ class InteractionHandler : public LockedRobotState const std::shared_ptr& tf_buffer = std::shared_ptr()); // DEPRECATED. - InteractionHandler(const std::string& name, const robot_state::RobotState& initial_robot_state, + InteractionHandler(const std::string& name, const moveit::core::RobotState& initial_robot_state, const std::shared_ptr& tf_buffer = std::shared_ptr()); // DEPRECATED. - InteractionHandler(const std::string& name, const robot_model::RobotModelConstPtr& model, + InteractionHandler(const std::string& name, const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& tf_buffer = std::shared_ptr()); ~InteractionHandler() override @@ -216,7 +216,7 @@ class InteractionHandler : public LockedRobotState /** \brief Clear any error settings. * This makes the markers appear as if the state is no longer invalid. */ - void clearError(void); + void clearError(); protected: bool transformFeedbackPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback, @@ -231,18 +231,18 @@ class InteractionHandler : public LockedRobotState // Update RobotState using a generic interaction feedback message. // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. - void updateStateGeneric(robot_state::RobotState* state, const GenericInteraction* g, + void updateStateGeneric(moveit::core::RobotState* state, const GenericInteraction* g, const visualization_msgs::InteractiveMarkerFeedbackConstPtr* feedback, StateChangeCallbackFn* callback); // Update RobotState for a new pose of an eef. // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. - void updateStateEndEffector(robot_state::RobotState* state, const EndEffectorInteraction* eef, + void updateStateEndEffector(moveit::core::RobotState* state, const EndEffectorInteraction* eef, const geometry_msgs::Pose* pose, StateChangeCallbackFn* callback); // Update RobotState for a new joint position. // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. - void updateStateJoint(robot_state::RobotState* state, const JointInteraction* vj, const geometry_msgs::Pose* pose, + void updateStateJoint(moveit::core::RobotState* state, const JointInteraction* vj, const geometry_msgs::Pose* pose, StateChangeCallbackFn* callback); // Set the error state for \e name. @@ -310,6 +310,4 @@ class InteractionHandler : public LockedRobotState // remove '_' characters from name static std::string fixName(std::string name); }; -} - -#endif +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h index 5810e7d244..6673198820 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan, Acorn Pooley, Adam Leeper */ -#ifndef MOVEIT_ROBOT_INTERACTION_INTERACTIVE_MARKER_HELPERS_ -#define MOVEIT_ROBOT_INTERACTION_INTERACTIVE_MARKER_HELPERS_ +#pragma once #include #include @@ -43,9 +42,8 @@ namespace robot_interaction { -visualization_msgs::InteractiveMarker makeEmptyInteractiveMarker(const std::string& name, - const geometry_msgs::PoseStamped& stamped, - double scale); +visualization_msgs::InteractiveMarker +makeEmptyInteractiveMarker(const std::string& name, const geometry_msgs::PoseStamped& stamped, double scale); visualization_msgs::InteractiveMarker make6DOFMarker(const std::string& name, const geometry_msgs::PoseStamped& stamped, double scale, bool orientation_fixed = false); @@ -68,6 +66,4 @@ void addPositionControl(visualization_msgs::InteractiveMarker& int_marker, bool void addViewPlaneControl(visualization_msgs::InteractiveMarker& int_marker, double radius, const std_msgs::ColorRGBA& color, bool position = true, bool orientation = true); -} - -#endif +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h index 43b8e7bbc2..7207bd6152 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley */ -#ifndef MOVEIT_ROBOT_INTERACTION_KINEMATIC_OPTIONS_ -#define MOVEIT_ROBOT_INTERACTION_KINEMATIC_OPTIONS_ +#pragma once #include #include @@ -71,7 +70,7 @@ struct KinematicOptions /// @param tip link that will be posed /// @param pose desired pose of tip link /// @param result true if IK succeeded. - bool setStateFromIK(robot_state::RobotState& state, const std::string& group, const std::string& tip, + bool setStateFromIK(moveit::core::RobotState& state, const std::string& group, const std::string& tip, const geometry_msgs::Pose& pose) const; /// Copy a subset of source to this. @@ -83,11 +82,9 @@ struct KinematicOptions double timeout_seconds_; /// This is called to determine if the state is valid - robot_state::GroupStateValidityCallbackFn state_validity_callback_; + moveit::core::GroupStateValidityCallbackFn state_validity_callback_; /// other options kinematics::KinematicsQueryOptions options_; }; -} - -#endif +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h index 033fd0ac4b..40ebd87d1b 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h @@ -34,8 +34,7 @@ /* Author: Acorn Pooley */ -#ifndef MOVEIT_ROBOT_INTERACTION_KINEMATIC_OPTIONS_MAP_ -#define MOVEIT_ROBOT_INTERACTION_KINEMATIC_OPTIONS_MAP_ +#pragma once #include #include @@ -64,7 +63,7 @@ class KinematicOptionsMap /// @param tip link that will be posed /// @param pose desired pose of tip link /// @param result true if IK succeeded. - bool setStateFromIK(robot_state::RobotState& state, const std::string& key, const std::string& group, + bool setStateFromIK(moveit::core::RobotState& state, const std::string& key, const std::string& group, const std::string& tip, const geometry_msgs::Pose& pose) const; /// Get the options to use for a particular key. @@ -103,6 +102,4 @@ class KinematicOptionsMap // PROTECTED BY lock_ M_options options_; }; -} - -#endif +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h index 286a86a95d..ebd2f72de1 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h @@ -35,8 +35,7 @@ /* Author: Ioan Sucan, Acorn Pooley */ -#ifndef MOVEIT_ROBOT_INTERACTION_LOCKED_ROBOT_STATE_ -#define MOVEIT_ROBOT_INTERACTION_LOCKED_ROBOT_STATE_ +#pragma once #include #include @@ -45,7 +44,7 @@ namespace robot_interaction { -MOVEIT_CLASS_FORWARD(LockedRobotState); +MOVEIT_CLASS_FORWARD(LockedRobotState); // Defines LockedRobotStatePtr, ConstPtr, WeakPtr... etc /// Maintain a RobotState in a multithreaded environment. // @@ -61,8 +60,8 @@ MOVEIT_CLASS_FORWARD(LockedRobotState); class LockedRobotState { public: - LockedRobotState(const robot_state::RobotState& state); - LockedRobotState(const robot_model::RobotModelPtr& model); + LockedRobotState(const moveit::core::RobotState& state); + LockedRobotState(const moveit::core::RobotModelPtr& model); virtual ~LockedRobotState(); @@ -73,13 +72,13 @@ class LockedRobotState // it. // // The transforms in the returned state will always be up to date. - robot_state::RobotStateConstPtr getState() const; + moveit::core::RobotStateConstPtr getState() const; /// Set the state to the new value. - void setState(const robot_state::RobotState& state); + void setState(const moveit::core::RobotState& state); // This is a function that can modify the maintained state. - typedef boost::function ModifyStateFunction; + typedef boost::function ModifyStateFunction; // Modify the state. // @@ -106,8 +105,6 @@ class LockedRobotState // The state maintained by this class. // When a modify function is being called this is NULL. // PROTECTED BY state_lock_ - robot_state::RobotStatePtr state_; + moveit::core::RobotStatePtr state_; }; -} - -#endif +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h index a5ebcc0001..6c3eadbfb5 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h @@ -34,12 +34,10 @@ /* Author: Ioan Sucan, Adam Leeper */ -#ifndef MOVEIT_ROBOT_INTERACTION_ROBOT_INTERACTION_ -#define MOVEIT_ROBOT_INTERACTION_ROBOT_INTERACTION_ +#pragma once #include #include -#include #include #include #include @@ -59,9 +57,9 @@ class InteractiveMarkerServer; namespace robot_interaction { -MOVEIT_CLASS_FORWARD(InteractionHandler); -MOVEIT_CLASS_FORWARD(KinematicOptionsMap); -MOVEIT_CLASS_FORWARD(RobotInteraction); +MOVEIT_CLASS_FORWARD(InteractionHandler); // Defines InteractionHandlerPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(KinematicOptionsMap); // Defines KinematicOptionsMapPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RobotInteraction); // Defines RobotInteractionPtr, ConstPtr, WeakPtr... etc // Manage interactive markers for controlling a robot state. // @@ -77,10 +75,10 @@ class RobotInteraction /// The topic name on which the internal Interactive Marker Server operates static const std::string INTERACTIVE_MARKER_TOPIC; - RobotInteraction(const robot_model::RobotModelConstPtr& robot_model, const std::string& ns = ""); + RobotInteraction(const moveit::core::RobotModelConstPtr& robot_model, const std::string& ns = ""); virtual ~RobotInteraction(); - const std::string& getServerTopic(void) const + const std::string& getServerTopic() const { return topic_; } @@ -141,7 +139,7 @@ class RobotInteraction // is needed to actually remove the markers from the display. void clearInteractiveMarkers(); - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -181,7 +179,7 @@ class RobotInteraction // return the diameter of the sphere that certainly can enclose the AABB of the links in this group double computeGroupMarkerSize(const std::string& group); void computeMarkerPose(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef, - const robot_state::RobotState& robot_state, geometry_msgs::Pose& pose, + const moveit::core::RobotState& robot_state, geometry_msgs::Pose& pose, geometry_msgs::Pose& control_to_eef_tf) const; void addEndEffectorMarkers(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef, @@ -200,7 +198,7 @@ class RobotInteraction boost::condition_variable new_feedback_condition_; std::map feedback_map_; - robot_model::RobotModelConstPtr robot_model_; + moveit::core::RobotModelConstPtr robot_model_; std::vector active_eef_; std::vector active_vj_; @@ -233,6 +231,4 @@ class RobotInteraction // options for doing IK KinematicOptionsMapPtr kinematic_options_map_; }; -} - -#endif +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml index 25cc5b7107..e48bdc421d 100644 --- a/moveit_ros/robot_interaction/package.xml +++ b/moveit_ros/robot_interaction/package.xml @@ -1,13 +1,13 @@ moveit_ros_robot_interaction - 1.0.1 - Components of MoveIt! that offer interaction via interactive markers + 1.1.1 + Components of MoveIt that offer interaction via interactive markers Ioan Sucan Michael Ferguson Robert Haschke - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index f437f20c71..e26ccdc182 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -48,7 +48,6 @@ #include #include #include -#include #include #include @@ -56,7 +55,7 @@ namespace robot_interaction { InteractionHandler::InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name, - const robot_state::RobotState& initial_robot_state, + const moveit::core::RobotState& initial_robot_state, const std::shared_ptr& tf_buffer) : LockedRobotState(initial_robot_state) , name_(fixName(name)) @@ -279,7 +278,7 @@ void InteractionHandler::handleJoint(const JointInteraction& vj, } // MUST hold state_lock_ when calling this! -void InteractionHandler::updateStateGeneric(robot_state::RobotState* state, const GenericInteraction* g, +void InteractionHandler::updateStateGeneric(moveit::core::RobotState* state, const GenericInteraction* g, const visualization_msgs::InteractiveMarkerFeedbackConstPtr* feedback, StateChangeCallbackFn* callback) { @@ -290,7 +289,7 @@ void InteractionHandler::updateStateGeneric(robot_state::RobotState* state, cons } // MUST hold state_lock_ when calling this! -void InteractionHandler::updateStateEndEffector(robot_state::RobotState* state, const EndEffectorInteraction* eef, +void InteractionHandler::updateStateEndEffector(moveit::core::RobotState* state, const EndEffectorInteraction* eef, const geometry_msgs::Pose* pose, StateChangeCallbackFn* callback) { // This is called with state_lock_ held, so no additional locking needed to @@ -304,13 +303,13 @@ void InteractionHandler::updateStateEndEffector(robot_state::RobotState* state, } // MUST hold state_lock_ when calling this! -void InteractionHandler::updateStateJoint(robot_state::RobotState* state, const JointInteraction* vj, +void InteractionHandler::updateStateJoint(moveit::core::RobotState* state, const JointInteraction* vj, const geometry_msgs::Pose* feedback_pose, StateChangeCallbackFn* callback) { Eigen::Isometry3d pose; tf2::fromMsg(*feedback_pose, pose); - if (!vj->parent_frame.empty() && !robot_state::Transforms::sameFrame(vj->parent_frame, planning_frame_)) + if (!vj->parent_frame.empty() && !moveit::core::Transforms::sameFrame(vj->parent_frame, planning_frame_)) pose = state->getGlobalLinkTransform(vj->parent_frame).inverse() * pose; state->setJointPositions(vj->joint_name, pose); @@ -330,7 +329,7 @@ bool InteractionHandler::inError(const GenericInteraction& g) const return getErrorState(g.marker_name_suffix); } -bool InteractionHandler::inError(const JointInteraction& vj) const +bool InteractionHandler::inError(const JointInteraction& /*unused*/) const { return false; } diff --git a/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp b/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp index 9fcc48abc0..5dbcd93191 100644 --- a/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp +++ b/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp @@ -42,9 +42,8 @@ namespace robot_interaction { -visualization_msgs::InteractiveMarker makeEmptyInteractiveMarker(const std::string& name, - const geometry_msgs::PoseStamped& stamped, - double scale) +visualization_msgs::InteractiveMarker +makeEmptyInteractiveMarker(const std::string& name, const geometry_msgs::PoseStamped& stamped, double scale) { visualization_msgs::InteractiveMarker int_marker; int_marker.header = stamped.header; diff --git a/moveit_ros/robot_interaction/src/kinematic_options.cpp b/moveit_ros/robot_interaction/src/kinematic_options.cpp index d55440e60f..67a39eeefc 100644 --- a/moveit_ros/robot_interaction/src/kinematic_options.cpp +++ b/moveit_ros/robot_interaction/src/kinematic_options.cpp @@ -44,10 +44,10 @@ robot_interaction::KinematicOptions::KinematicOptions() : timeout_seconds_(0.0) // This is intended to be called as a ModifyStateFunction to modify the state // maintained by a LockedRobotState in place. -bool robot_interaction::KinematicOptions::setStateFromIK(robot_state::RobotState& state, const std::string& group, +bool robot_interaction::KinematicOptions::setStateFromIK(moveit::core::RobotState& state, const std::string& group, const std::string& tip, const geometry_msgs::Pose& pose) const { - const robot_model::JointModelGroup* jmg = state.getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group); if (!jmg) { ROS_ERROR("No getJointModelGroup('%s') found", group.c_str()); @@ -72,7 +72,7 @@ void robot_interaction::KinematicOptions::setOptions(const KinematicOptions& sou // robot_interaction::KinematicOptions except options_ #define O_FIELDS(F) \ F(double, timeout_seconds_, TIMEOUT) \ - F(robot_state::GroupStateValidityCallbackFn, state_validity_callback_, STATE_VALIDITY_CALLBACK) + F(moveit::core::GroupStateValidityCallbackFn, state_validity_callback_, STATE_VALIDITY_CALLBACK) // This needs to represent all the fields in // kinematics::KinematicsQueryOptions diff --git a/moveit_ros/robot_interaction/src/kinematic_options_map.cpp b/moveit_ros/robot_interaction/src/kinematic_options_map.cpp index 4487e92a45..013a9c7a61 100644 --- a/moveit_ros/robot_interaction/src/kinematic_options_map.cpp +++ b/moveit_ros/robot_interaction/src/kinematic_options_map.cpp @@ -35,8 +35,6 @@ /* Author: Acorn Pooley */ #include -#include -#include // These strings have no content. They are compared by address. const std::string robot_interaction::KinematicOptionsMap::DEFAULT = ""; @@ -131,7 +129,7 @@ void robot_interaction::KinematicOptionsMap::merge(const KinematicOptionsMap& ot // This is intended to be called as a ModifyStateFunction to modify the state // maintained by a LockedRobotState in place. -bool robot_interaction::KinematicOptionsMap::setStateFromIK(robot_state::RobotState& state, const std::string& key, +bool robot_interaction::KinematicOptionsMap::setStateFromIK(moveit::core::RobotState& state, const std::string& key, const std::string& group, const std::string& tip, const geometry_msgs::Pose& pose) const { diff --git a/moveit_ros/robot_interaction/src/locked_robot_state.cpp b/moveit_ros/robot_interaction/src/locked_robot_state.cpp index 4ea148e6aa..2576df198f 100644 --- a/moveit_ros/robot_interaction/src/locked_robot_state.cpp +++ b/moveit_ros/robot_interaction/src/locked_robot_state.cpp @@ -38,14 +38,14 @@ #include -robot_interaction::LockedRobotState::LockedRobotState(const robot_state::RobotState& state) - : state_(new robot_state::RobotState(state)) +robot_interaction::LockedRobotState::LockedRobotState(const moveit::core::RobotState& state) + : state_(new moveit::core::RobotState(state)) { state_->update(); } -robot_interaction::LockedRobotState::LockedRobotState(const robot_model::RobotModelPtr& model) - : state_(new robot_state::RobotState(model)) +robot_interaction::LockedRobotState::LockedRobotState(const moveit::core::RobotModelPtr& model) + : state_(new moveit::core::RobotState(model)) { state_->setToDefaultValues(); state_->update(); @@ -53,13 +53,13 @@ robot_interaction::LockedRobotState::LockedRobotState(const robot_model::RobotMo robot_interaction::LockedRobotState::~LockedRobotState() = default; -robot_state::RobotStateConstPtr robot_interaction::LockedRobotState::getState() const +moveit::core::RobotStateConstPtr robot_interaction::LockedRobotState::getState() const { boost::mutex::scoped_lock lock(state_lock_); return state_; } -void robot_interaction::LockedRobotState::setState(const robot_state::RobotState& state) +void robot_interaction::LockedRobotState::setState(const moveit::core::RobotState& state) { { boost::mutex::scoped_lock lock(state_lock_); @@ -69,7 +69,7 @@ void robot_interaction::LockedRobotState::setState(const robot_state::RobotState if (state_.unique()) *state_ = state; else - state_.reset(new robot_state::RobotState(state)); + state_.reset(new moveit::core::RobotState(state)); state_->update(); } @@ -84,7 +84,7 @@ void robot_interaction::LockedRobotState::modifyState(const ModifyStateFunction& // If someone else has a reference to the state, then make a copy. // The old state is orphaned (does not change, but is now out of date). if (!state_.unique()) - state_.reset(new robot_state::RobotState(*state_)); + state_.reset(new moveit::core::RobotState(*state_)); modify(state_.get()); state_->update(); diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index ac11dfc48c..5f7d8f9c6a 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -62,7 +62,7 @@ static const float END_EFFECTOR_REACHABLE_COLOR[4] = { 0.2, 1.0, 0.2, 1.0 }; const std::string RobotInteraction::INTERACTIVE_MARKER_TOPIC = "robot_interaction_interactive_marker_topic"; -RobotInteraction::RobotInteraction(const robot_model::RobotModelConstPtr& robot_model, const std::string& ns) +RobotInteraction::RobotInteraction(const moveit::core::RobotModelConstPtr& robot_model, const std::string& ns) : robot_model_(robot_model), kinematic_options_map_(new KinematicOptionsMap) { topic_ = ns.empty() ? INTERACTIVE_MARKER_TOPIC : ns + "/" + INTERACTIVE_MARKER_TOPIC; @@ -93,8 +93,9 @@ void RobotInteraction::decideActiveComponents(const std::string& group, Interact decideActiveEndEffectors(group, style); decideActiveJoints(group); if (!group.empty() && active_eef_.empty() && active_vj_.empty() && active_generic_.empty()) - ROS_INFO_NAMED("robot_interaction", "No active joints or end effectors found for group '%s'. " - "Make sure that kinematics.yaml is loaded in this node's namespace.", + ROS_INFO_NAMED("robot_interaction", + "No active joints or end effectors found for group '%s'. " + "Make sure that kinematics.yaml is loaded in this node's namespace.", group.c_str()); } @@ -115,7 +116,7 @@ void RobotInteraction::addActiveComponent(const InteractiveMarkerConstructorFn& static const double DEFAULT_SCALE = 0.25; double RobotInteraction::computeLinkMarkerSize(const std::string& link) { - const robot_model::LinkModel* lm = robot_model_->getLinkModel(link); + const moveit::core::LinkModel* lm = robot_model_->getLinkModel(link); double size = 0; while (lm) @@ -131,7 +132,7 @@ double RobotInteraction::computeLinkMarkerSize(const std::string& link) // process kinematic chain upwards (but only following fixed joints) // to find a link with some non-empty shape (to ignore virtual links) - if (lm->getParentJointModel()->getType() == robot_model::JointModel::FIXED) + if (lm->getParentJointModel()->getType() == moveit::core::JointModel::FIXED) lm = lm->getParentLinkModel(); else lm = nullptr; @@ -147,7 +148,7 @@ double RobotInteraction::computeGroupMarkerSize(const std::string& group) { if (group.empty()) return DEFAULT_SCALE; - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg) return 0.0; @@ -159,7 +160,7 @@ double RobotInteraction::computeGroupMarkerSize(const std::string& group) double size = 0; for (const std::string& link : links) { - const robot_model::LinkModel* lm = robot_model_->getLinkModel(link); + const moveit::core::LinkModel* lm = robot_model_->getLinkModel(link); if (!lm) continue; Eigen::Vector3d ext = lm->getShapeExtentsAtOrigin(); @@ -191,7 +192,7 @@ void RobotInteraction::decideActiveJoints(const std::string& group) ROS_DEBUG_NAMED("robot_interaction", "Deciding active joints for group '%s'", group.c_str()); const srdf::ModelConstSharedPtr& srdf = robot_model_->getSRDF(); - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg || !srdf) return; @@ -199,7 +200,7 @@ void RobotInteraction::decideActiveJoints(const std::string& group) std::set used; if (jmg->hasJointModel(robot_model_->getRootJointName())) { - robot_state::RobotState default_state(robot_model_); + moveit::core::RobotState default_state(robot_model_); default_state.setToDefaultValues(); std::vector aabb; default_state.computeAABB(aabb); @@ -228,11 +229,11 @@ void RobotInteraction::decideActiveJoints(const std::string& group) } } - const std::vector& joints = jmg->getJointModels(); - for (const robot_model::JointModel* joint : joints) + const std::vector& joints = jmg->getJointModels(); + for (const moveit::core::JointModel* joint : joints) { - if ((joint->getType() == robot_model::JointModel::PLANAR || - joint->getType() == robot_model::JointModel::FLOATING) && + if ((joint->getType() == moveit::core::JointModel::PLANAR || + joint->getType() == moveit::core::JointModel::FLOATING) && used.find(joint->getName()) == used.end()) { JointInteraction v; @@ -240,7 +241,7 @@ void RobotInteraction::decideActiveJoints(const std::string& group) if (joint->getParentLinkModel()) v.parent_frame = joint->getParentLinkModel()->getName(); v.joint_name = joint->getName(); - if (joint->getType() == robot_model::JointModel::PLANAR) + if (joint->getType() == moveit::core::JointModel::PLANAR) v.dof = 3; else v.dof = 6; @@ -267,7 +268,7 @@ void RobotInteraction::decideActiveEndEffectors(const std::string& group, Intera ROS_DEBUG_NAMED("robot_interaction", "Deciding active end-effectors for group '%s'", group.c_str()); const srdf::ModelConstSharedPtr& srdf = robot_model_->getSRDF(); - const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); if (!jmg || !srdf) { @@ -276,7 +277,7 @@ void RobotInteraction::decideActiveEndEffectors(const std::string& group, Intera } const std::vector& eefs = srdf->getEndEffectors(); - const std::pair& + const std::pair& smap = jmg->getGroupKinematics(); // if we have an IK solver for the selected group, we check if there are any end effectors attached to this group @@ -400,7 +401,7 @@ void RobotInteraction::addEndEffectorMarkers(const InteractionHandlerPtr& handle marker_color.b = color[2]; marker_color.a = color[3]; - robot_state::RobotStateConstPtr rstate = handler->getState(); + moveit::core::RobotStateConstPtr rstate = handler->getState(); const std::vector& link_names = rstate->getJointModelGroup(eef.eef_group)->getLinkModelNames(); visualization_msgs::MarkerArray marker_array; rstate->getRobotMarkers(marker_array, link_names, marker_color, eef.eef_group, ros::Duration()); @@ -412,7 +413,7 @@ void RobotInteraction::addEndEffectorMarkers(const InteractionHandlerPtr& handle for (visualization_msgs::Marker& marker : marker_array.markers) { marker.header = im.header; - marker.mesh_use_embedded_materials = true; + marker.mesh_use_embedded_materials = !marker.mesh_resource.empty(); // - - - - - - Do some math for the offset - - - - - - tf2::Transform tf_root_to_im, tf_root_to_mesh, tf_im_to_eef; tf2::fromMsg(im.pose, tf_root_to_im); @@ -448,10 +449,9 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle { // If scale is left at default size of 0, scale will be based on end effector link size. a good value is between 0-1 std::vector ims; - ros::NodeHandle nh; { boost::unique_lock ulock(marker_access_lock_); - robot_state::RobotStateConstPtr s = handler->getState(); + moveit::core::RobotStateConstPtr s = handler->getState(); for (std::size_t i = 0; i < active_generic_.size(); ++i) { @@ -461,11 +461,9 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle im.name = getMarkerName(handler, active_generic_[i]); shown_markers_[im.name] = i; ims.push_back(im); - ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", im.name.c_str(), - im.scale); + ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", im.name.c_str(), im.scale); } } - ros::NodeHandle nh; for (std::size_t i = 0; i < active_eef_.size(); ++i) { @@ -506,8 +504,7 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle active_eef_[i].interaction & InteractionStyle::ORIENTATION_EEF); ims.push_back(im); registerMoveInteractiveMarkerTopic(marker_name, handler->getName() + "_" + active_eef_[i].parent_link); - ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), - mscale); + ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), mscale); } for (std::size_t i = 0; i < active_vj_.size(); ++i) { @@ -531,8 +528,7 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle } ims.push_back(im); registerMoveInteractiveMarkerTopic(marker_name, handler->getName() + "_" + active_vj_[i].connecting_link); - ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), - mscale); + ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), mscale); } handlers_[handler->getName()] = handler; } @@ -543,8 +539,7 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle for (const visualization_msgs::InteractiveMarker& im : ims) { int_marker_server_->insert(im); - int_marker_server_->setCallback(im.name, - boost::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, _1)); + int_marker_server_->setCallback(im.name, boost::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, _1)); // Add menu handler to all markers that this interaction handler creates. if (std::shared_ptr mh = handler->getMenuHandler()) @@ -554,7 +549,6 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle void RobotInteraction::registerMoveInteractiveMarkerTopic(const std::string& marker_name, const std::string& name) { - ros::NodeHandle nh; std::stringstream ss; ss << "/rviz/moveit/move_marker/"; ss << name; @@ -587,7 +581,7 @@ void RobotInteraction::toggleMoveInteractiveMarkerTopic(bool enable) } void RobotInteraction::computeMarkerPose(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef, - const robot_state::RobotState& robot_state, geometry_msgs::Pose& pose, + const moveit::core::RobotState& robot_state, geometry_msgs::Pose& pose, geometry_msgs::Pose& control_to_eef_tf) const { // Need to allow for control pose offsets @@ -622,7 +616,7 @@ void RobotInteraction::updateInteractiveMarkers(const InteractionHandlerPtr& han { boost::unique_lock ulock(marker_access_lock_); - robot_state::RobotStateConstPtr s = handler->getState(); + moveit::core::RobotStateConstPtr s = handler->getState(); root_link = s->getRobotModel()->getModelFrame(); for (const EndEffectorInteraction& eef : active_eef_) diff --git a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp index fd31cd1f7b..dc820d047f 100644 --- a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp +++ b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp @@ -256,7 +256,7 @@ TEST(LockedRobotState, URDF_sanity) class Super1 : public robot_interaction::LockedRobotState { public: - Super1(const robot_model::RobotModelPtr& model) : LockedRobotState(model), cnt_(0) + Super1(const moveit::core::RobotModelPtr& model) : LockedRobotState(model), cnt_(0) { } @@ -268,7 +268,7 @@ class Super1 : public robot_interaction::LockedRobotState int cnt_; }; -static void modify1(robot_state::RobotState* state) +static void modify1(moveit::core::RobotState* state) { state->setVariablePosition(JOINT_A, 0.00006); } @@ -281,7 +281,7 @@ TEST(LockedRobotState, robotStateChanged) EXPECT_EQ(ls1.cnt_, 0); - robot_state::RobotState cp1(*ls1.getState()); + moveit::core::RobotState cp1(*ls1.getState()); cp1.setVariablePosition(JOINT_A, 0.00001); cp1.setVariablePosition(JOINT_C, 0.00002); cp1.setVariablePosition(JOINT_F, 0.00003); @@ -319,7 +319,7 @@ class MyInfo private: // helper function for modifyThreadFunc - void modifyFunc(robot_state::RobotState* state, double val); + void modifyFunc(moveit::core::RobotState* state, double val); // Checks state for validity and self-consistancy. void checkState(robot_interaction::LockedRobotState& locked_state); @@ -335,9 +335,9 @@ class MyInfo // Check the state. It should always be valid. void MyInfo::checkState(robot_interaction::LockedRobotState& locked_state) { - robot_state::RobotStateConstPtr s = locked_state.getState(); + moveit::core::RobotStateConstPtr s = locked_state.getState(); - robot_state::RobotState cp1(*s); + moveit::core::RobotState cp1(*s); // take some time cnt_lock_.lock(); @@ -350,7 +350,7 @@ void MyInfo::checkState(robot_interaction::LockedRobotState& locked_state) // check mim_f == joint_f EXPECT_EQ(s->getVariablePositions()[MIM_F], s->getVariablePositions()[JOINT_F] * 1.5 + 0.1); - robot_state::RobotState cp2(*s); + moveit::core::RobotState cp2(*s); EXPECT_NE(cp1.getVariablePositions(), cp2.getVariablePositions()); EXPECT_NE(cp1.getVariablePositions(), s->getVariablePositions()); @@ -394,7 +394,7 @@ void MyInfo::setThreadFunc(robot_interaction::LockedRobotState* locked_state, in for (int loops = 0; loops < 100; ++loops) { val += 0.0001; - robot_state::RobotState cp1(*locked_state->getState()); + moveit::core::RobotState cp1(*locked_state->getState()); cp1.setVariablePosition(JOINT_A, val + 0.00001); cp1.setVariablePosition(JOINT_C, val + 0.00002); @@ -415,7 +415,7 @@ void MyInfo::setThreadFunc(robot_interaction::LockedRobotState* locked_state, in } // modify the state in place. Used by MyInfo::modifyThreadFunc() -void MyInfo::modifyFunc(robot_state::RobotState* state, double val) +void MyInfo::modifyFunc(moveit::core::RobotState* state, double val) { state->setVariablePosition(JOINT_A, val + 0.00001); state->setVariablePosition(JOINT_C, val + 0.00002); diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst index 4e24d61666..983d6a24df 100644 --- a/moveit_ros/visualization/CHANGELOG.rst +++ b/moveit_ros/visualization/CHANGELOG.rst @@ -2,6 +2,76 @@ Changelog for package moveit_ros_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature] Clean up Rviz Motion Planning plugin, add tooltips (`#2310 `_) +* [fix] "Clear Octomap" button, disable when no octomap is published (`#2320 `_) +* [fix] clang-tidy warning (`#2334 `_) +* [fix] python3 issues (`#2323 `_) +* [maint] Cleanup MSA includes (`#2351 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, Michael GΓΆrner, Robert Haschke + +1.1.0 (2020-09-04) +------------------ + +1.0.6 (2020-08-19) +------------------ +* [feature] MP display: add units to joints tab (`#2264 `_) +* [feature] Allow adding planning scene shapes from rviz panel (`#2198 `_) +* [feature] Default to Planning tab initially (`#2061 `_) +* [fix] Fix deferred robot model loading (`#2245 `_) +* [maint] Migrate to clang-format-10 +* [maint] Optimize includes (`#2229 `_) +* Contributors: Jorge Nicho, Markus Vieth, Michael GΓΆrner, Robert Haschke, Michael GΓΆrner + +1.0.5 (2020-07-08) +------------------ +* [feature] Improve rviz GUI to add PlanningScene objects. Ask for scaling large meshes. (`#2142 `_) +* [maint] Replace robot_model and robot_state namespaces with moveit::core (`#2135 `_) +* [maint] Fix catkin_lint issues (`#2120 `_) +* [feature] PlanningSceneDisplay speedup (`#2049 `_) +* [feature] Added support for PS4 joystick (`#2060 `_) +* [fix] MP display: planning attempts are natural numbers (`#2076 `_, `#2082 `_) +* Contributors: Felix von Drigalski, Henning Kayser, Jafar Abdi, Michael GΓΆrner, Robert Haschke, Simon Schmeisser, TrippleBender + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [fix] `MotionPlanningDisplay`: change internal shortcut Ctrl+R to Ctrl+I (`#1967 `_) +* [fix] Remove `PlanningSceneInterface` from rviz display, but use its `PlanningSceneMonitor` instead +* [fix] Fix segfault in `RobotStateVisualization` (`#1941 `_) +* [feature] Provide visual feedback on success of requestPlanningSceneState() +* [feature] Wait for `get_planning_scene` in background (`#1934 `_) +* [feature] Reduce step size for pose-adapting widgets +* [fix] Reset `scene_marker` when disabling motion planning panel +* [fix] Enable/disable motion planning panel with display +* [fix] Enable/disable pose+scale group box when collision object is selected/deselected +* [fix] Correctly populate the list of scene objects in the motion planning panel +* [feature] Resize scene marker with collision object +* [feature] Show attached bodies in trajectory trail (`#1766 `_) +* [fix] Fix `REALTIME` trajectory playback (`#1683 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Notice changes in rviz planning panel requiring saving (`#1991 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Improve Python 3 compatibility (`#1870 `_) + * Replaced StringIO with BytesIO for python msg serialization + * Use py_bindings_tools::ByteString as byte-based serialization buffer on C++ side +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] Fix pruning of enclosed nodes when rendering octomap in RViz (`#1685 `_) +* [fix] Fix missing `scene_manager` initialization in OcTreeRender's constructor (`#1817 `_) +* [feature] new `Joints` tab in RViz motion panel (`#1308 `_) +* [feature] Add `` robot state to RViz motion panel (`#1742 `_) +* Contributors: Bjar Ne, Dale Koenig, MarqRazz, Max Krichenbauer, Michael GΓΆrner, Robert Haschke, RyodoTanaka, Sean Yen, Takara Kasai, Yannick Jonetzko, Yu, Yan, v4hn + +1.0.2 (2019-06-28) +------------------ +* [maintenance] Removed unnecessary null pointer checks on deletion (`#1410 `_) +* Contributors: Mahmoud Ahmed Selim + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -113,8 +183,8 @@ Changelog for package moveit_ros_visualization 0.9.5 (2017-03-08) ------------------ -* [fix] correct "simplify widget handling" `#452 `_ This reverts "simplify widget handling (`#442 `_)" -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix] correct "simplify widget handling" `#452 `_ This reverts "simplify widget handling (`#442 `_)" +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) * Contributors: Bence Magyar, Dave Coleman, Isaac I.Y. Saito, Yannick Jonetzko diff --git a/moveit_ros/visualization/CMakeLists.txt b/moveit_ros/visualization/CMakeLists.txt index 5d79b0c50b..395b35c3e6 100644 --- a/moveit_ros/visualization/CMakeLists.txt +++ b/moveit_ros/visualization/CMakeLists.txt @@ -1,7 +1,12 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_visualization) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") # suppress warning in Ogre add_compile_options(-Wno-deprecated-register) @@ -43,14 +48,14 @@ find_package(Eigen3 REQUIRED) # Qt Stuff if(rviz_QT_VERSION VERSION_LESS "5") - find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui) - include(${QT_USE_FILE}) + find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui) + include(${QT_USE_FILE}) macro(qt_wrap_ui) qt4_wrap_ui(${ARGN}) endmacro() else() - find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) - set(QT_LIBRARIES Qt5::Widgets) + find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) + set(QT_LIBRARIES Qt5::Widgets) macro(qt_wrap_ui) qt5_wrap_ui(${ARGN}) endmacro() @@ -58,6 +63,7 @@ endif() set(CMAKE_INCLUDE_CURRENT_DIR ON) set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTORCC ON) add_definitions(-DQT_NO_KEYWORDS) catkin_package( @@ -77,6 +83,8 @@ catkin_package( moveit_ros_planning_interface moveit_ros_robot_interaction object_recognition_msgs + roscpp + rviz DEPENDS EIGEN3 ) @@ -88,11 +96,11 @@ include_directories(rviz_plugin_render_tools/include robot_state_rviz_plugin/include planning_scene_rviz_plugin/include motion_planning_rviz_plugin/include - trajectory_rviz_plugin/include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS}) + trajectory_rviz_plugin/include) include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${QT_INCLUDE_DIR}) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt index 41c9b07b59..3d029d2095 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt @@ -1,9 +1,13 @@ set(HEADERS include/moveit/motion_planning_rviz_plugin/motion_planning_display.h include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h + include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h ) -qt_wrap_ui(UIC_FILES src/ui/motion_planning_rviz_plugin_frame.ui) +qt_wrap_ui(UIC_FILES + src/ui/motion_planning_rviz_plugin_frame.ui + src/ui/motion_planning_rviz_plugin_frame_joints.ui +) include_directories(${CMAKE_CURRENT_BINARY_DIR}) @@ -15,9 +19,11 @@ set(SOURCE_FILES src/motion_planning_frame_objects.cpp src/motion_planning_frame_scenes.cpp src/motion_planning_frame_states.cpp + src/motion_planning_frame_joints_widget.cpp src/motion_planning_display.cpp src/motion_planning_frame_manipulation.cpp src/motion_planning_param_widget.cpp + src/icons/icons.qrc ) set(MOVEIT_LIB_NAME moveit_motion_planning_rviz_plugin) @@ -26,7 +32,7 @@ set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NA target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools moveit_planning_scene_rviz_plugin_core - ${catkin_LIBRARIES} ${rviz_DEFAULT_PLUGIN_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) + ${catkin_LIBRARIES} ${rviz_DEFAULT_PLUGIN_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) add_library(${MOVEIT_LIB_NAME} src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -36,4 +42,5 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index a8c30c9cb0..0fd36a4088 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -34,11 +34,9 @@ /* Author: Ioan Sucan, Dave Coleman, Adam Leeper, Sachin Chitta */ -#ifndef MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_DISPLAY_ -#define MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_DISPLAY_ +#pragma once #include -#include #include #include #include @@ -77,7 +75,7 @@ class RosTopicProperty; class EditableEnumProperty; class ColorProperty; class MovableText; -} +} // namespace rviz namespace moveit_rviz_plugin { @@ -98,16 +96,21 @@ class MotionPlanningDisplay : public PlanningSceneDisplay void setName(const QString& name) override; - robot_state::RobotStateConstPtr getQueryStartState() const + moveit::core::RobotStateConstPtr getQueryStartState() const { return query_start_state_->getState(); } - robot_state::RobotStateConstPtr getQueryGoalState() const + moveit::core::RobotStateConstPtr getQueryGoalState() const { return query_goal_state_->getState(); } + const moveit::core::RobotState& getPreviousState() const + { + return *previous_state_; + } + const robot_interaction::RobotInteractionPtr& getRobotInteraction() const { return robot_interaction_; @@ -128,11 +131,13 @@ class MotionPlanningDisplay : public PlanningSceneDisplay trajectory_visual_->dropTrajectory(); } - void setQueryStartState(const robot_state::RobotState& start); - void setQueryGoalState(const robot_state::RobotState& goal); + void setQueryStartState(const moveit::core::RobotState& start); + void setQueryGoalState(const moveit::core::RobotState& goal); + void updateQueryStates(const moveit::core::RobotState& current_state); void updateQueryStartState(); void updateQueryGoalState(); + void rememberPreviousStartState(); void useApproximateIK(bool flag); @@ -152,6 +157,11 @@ class MotionPlanningDisplay : public PlanningSceneDisplay void toggleSelectPlanningGroupSubscription(bool enable); +Q_SIGNALS: + // signals issued when start/goal states of a query changed + void queryStartStateChanged(); + void queryGoalStateChanged(); + private Q_SLOTS: // ****************************************************************************************** @@ -187,6 +197,7 @@ private Q_SLOTS: }; void onRobotModelLoaded() override; + void onNewPlanningSceneState() override; void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override; void updateInternal(float wall_dt, float ros_dt) override; @@ -207,14 +218,14 @@ private Q_SLOTS: void scheduleDrawQueryStartState(robot_interaction::InteractionHandler* handler, bool error_state_changed); void scheduleDrawQueryGoalState(robot_interaction::InteractionHandler* handler, bool error_state_changed); - bool isIKSolutionCollisionFree(robot_state::RobotState* state, const robot_state::JointModelGroup* group, + bool isIKSolutionCollisionFree(moveit::core::RobotState* state, const moveit::core::JointModelGroup* group, const double* ik_solution) const; void computeMetrics(bool start, const std::string& group, double payload); void computeMetricsInternal(std::map& metrics, const robot_interaction::EndEffectorInteraction& eef, - const robot_state::RobotState& state, double payload); - void updateStateExceptModified(robot_state::RobotState& dest, const robot_state::RobotState& src); + const moveit::core::RobotState& state, double payload); + void updateStateExceptModified(moveit::core::RobotState& dest, const moveit::core::RobotState& src); void updateBackgroundJobProgressBar(); void backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent event, const std::string& jobname); @@ -254,6 +265,8 @@ private Q_SLOTS: std::shared_ptr menu_handler_goal_; std::map status_links_start_; std::map status_links_goal_; + /// remember previous start state (updated before starting execution) + moveit::core::RobotStatePtr previous_state_; /// Hold the names of the groups for which the query states have been updated (and should not be altered when new info /// is received from the planning scene) @@ -301,5 +314,3 @@ private Q_SLOTS: }; } // namespace moveit_rviz_plugin - -#endif diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index d49e100204..5177635d1e 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_FRAME_ -#define MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_FRAME_ +#pragma once #include #include @@ -44,7 +43,6 @@ #ifndef Q_MOC_RUN #include #include -#include #include #include #include @@ -75,14 +73,15 @@ class MotionPlanningUI; namespace moveit_warehouse { -MOVEIT_CLASS_FORWARD(PlanningSceneStorage); -MOVEIT_CLASS_FORWARD(ConstraintsStorage); -MOVEIT_CLASS_FORWARD(RobotStateStorage); -} +MOVEIT_CLASS_FORWARD(PlanningSceneStorage); // Defines PlanningSceneStoragePtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(ConstraintsStorage); // Defines ConstraintsStoragePtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RobotStateStorage); // Defines RobotStateStoragePtr, ConstPtr, WeakPtr... etc +} // namespace moveit_warehouse namespace moveit_rviz_plugin { class MotionPlanningDisplay; +class MotionPlanningFrameJointsWidget; const std::string OBJECT_RECOGNITION_ACTION = "/recognize_objects"; @@ -94,6 +93,8 @@ static const std::string TAB_SCENES = "Stored Scenes"; static const std::string TAB_STATES = "Stored States"; static const std::string TAB_STATUS = "Status"; +static const double LARGE_MESH_THRESHOLD = 10.0; + class MotionPlanningFrame : public QWidget { friend class MotionPlanningDisplay; @@ -101,7 +102,7 @@ class MotionPlanningFrame : public QWidget public: MotionPlanningFrame(const MotionPlanningFrame&) = delete; - MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent = 0); + MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent = nullptr); ~MotionPlanningFrame() override; void changePlanningGroup(); @@ -122,9 +123,9 @@ class MotionPlanningFrame : public QWidget MotionPlanningDisplay* planning_display_; rviz::DisplayContext* context_; Ui::MotionPlanningUI* ui_; + MotionPlanningFrameJointsWidget* joints_tab_; moveit::planning_interface::MoveGroupInterfacePtr move_group_; - moveit::planning_interface::PlanningSceneInterfacePtr planning_scene_interface_; moveit::semantic_world::SemanticWorldPtr semantic_world_; moveit::planning_interface::MoveGroupInterface::PlanPtr current_plan_; @@ -140,12 +141,12 @@ class MotionPlanningFrame : public QWidget Q_SIGNALS: void planningFinished(); + void configChanged(); private Q_SLOTS: // Context tab void databaseConnectButtonClicked(); - void publishSceneButtonClicked(); void planningAlgorithmIndexChanged(int index); void resetDbButtonClicked(); void approximateIKChanged(int state); @@ -161,26 +162,31 @@ private Q_SLOTS: void allowLookingToggled(bool checked); void allowExternalProgramCommunication(bool enable); void pathConstraintsIndexChanged(int index); + void onNewPlanningSceneState(); void startStateTextChanged(const QString& start_state); void goalStateTextChanged(const QString& goal_state); void planningGroupTextChanged(const QString& planning_group); void onClearOctomapClicked(); // Scene Objects tab - void importFileButtonClicked(); - void importUrlButtonClicked(); - void clearSceneButtonClicked(); + void clearScene(); + void publishScene(); + void publishSceneIfNeeded(); + void setLocalSceneEdited(bool dirty = true); + bool isLocalSceneDirty() const; void sceneScaleChanged(int value); void sceneScaleStartChange(); void sceneScaleEndChange(); - void removeObjectButtonClicked(); + void shapesComboBoxChanged(const QString& text); + void addSceneObject(); + void removeSceneObject(); void selectedCollisionObjectChanged(); void objectPoseValueChanged(double value); void collisionObjectChanged(QListWidgetItem* item); void imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback); void copySelectedCollisionObject(); - void exportAsTextButtonClicked(); - void importFromTextButtonClicked(); + void exportGeometryAsTextButtonClicked(); + void importGeometryFromTextButtonClicked(); // Stored scenes tab void saveSceneButtonClicked(); @@ -230,22 +236,22 @@ private Q_SLOTS: void populateConstraintsList(const std::vector& constr); void configureForPlanning(); void configureWorkspace(); - void updateQueryStateHelper(robot_state::RobotState& state, const std::string& v); + void updateQueryStateHelper(moveit::core::RobotState& state, const std::string& v); void fillStateSelectionOptions(); void fillPlanningGroupOptions(); void startStateTextChangedExec(const std::string& start_state); void goalStateTextChangedExec(const std::string& goal_state); // Scene objects tab - void addObject(const collision_detection::WorldPtr& world, const std::string& id, const shapes::ShapeConstPtr& shape, - const Eigen::Isometry3d& pose); void updateCollisionObjectPose(bool update_marker_position); void createSceneInteractiveMarker(); void renameCollisionObject(QListWidgetItem* item); void attachDetachCollisionObject(QListWidgetItem* item); void populateCollisionObjectsList(); - void computeImportFromText(const std::string& path); - void computeExportAsText(const std::string& path); + void computeImportGeometryFromText(const std::string& path); + void computeExportGeometryAsText(const std::string& path); + visualization_msgs::InteractiveMarker + createObjectMarkerMsg(const collision_detection::CollisionEnv::ObjectConstPtr& obj); // Stored scenes tab void computeSaveSceneButtonClicked(); @@ -259,12 +265,12 @@ private Q_SLOTS: void checkPlanningSceneTreeEnabledButtons(); // States tab - void saveRobotStateButtonClicked(const robot_state::RobotState& state); + void saveRobotStateButtonClicked(const moveit::core::RobotState& state); void populateRobotStatesList(); // Pick and place void processDetectedObjects(); - void updateDetectedObjectsList(const std::vector& object_ids, const std::vector& objects); + void updateDetectedObjectsList(const std::vector& object_ids); void publishTables(); void updateSupportSurfacesList(); ros::Publisher object_recognition_trigger_publisher_; @@ -297,7 +303,7 @@ private Q_SLOTS: ros::Subscriber update_custom_goal_state_subscriber_; // General void changePlanningGroupHelper(); - void importResource(const std::string& path); + shapes::ShapePtr loadMeshResource(const std::string& url); void loadStoredStates(const std::string& pattern); void remotePlanCallback(const std_msgs::EmptyConstPtr& msg); @@ -315,7 +321,7 @@ private Q_SLOTS: ros::Publisher planning_scene_publisher_; ros::Publisher planning_scene_world_publisher_; - collision_detection::CollisionWorld::ObjectConstPtr scaled_object_; + collision_detection::CollisionEnv::ObjectConstPtr scaled_object_; std::vector > known_collision_objects_; long unsigned int known_collision_objects_version_; @@ -364,6 +370,4 @@ void MotionPlanningFrame::waitForAction(const T& action, const ros::NodeHandle& else ROS_DEBUG("Connected to '%s'", name.c_str()); }; -} - -#endif +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h new file mode 100644 index 0000000000..02a1fbc769 --- /dev/null +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h @@ -0,0 +1,254 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, CITEC, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of CITEC / Bielefeld University nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Robert Haschke */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +class QSlider; + +namespace Ui +{ +class MotionPlanningFrameJointsUI; +} +namespace robot_interaction +{ +MOVEIT_CLASS_FORWARD(InteractionHandler) +} +namespace moveit_rviz_plugin +{ +/** TableModel to display joint values of a referenced RobotState. + * + * Unfortunately we cannot store the RobotStatePtr (and thus ensure existence of the state during + * the lifetime of this class instance), because RobotInteraction (which is the initial use case) + * allocates internally a new RobotState if any other copy is held somewhere else. + * Hence, we also store an (unsafe) raw pointer. Lifetime of this raw pointer needs to be ensured. + */ +class JMGItemModel : public QAbstractTableModel +{ + Q_OBJECT + moveit::core::RobotState robot_state_; + const moveit::core::JointModelGroup* jmg_; + +public: + JMGItemModel(const moveit::core::RobotState& robot_state, const std::string& group_name, QObject* parent = nullptr); + + // QAbstractItemModel interface + int rowCount(const QModelIndex& parent = QModelIndex()) const override; + int columnCount(const QModelIndex& parent = QModelIndex()) const override; + Qt::ItemFlags flags(const QModelIndex& index) const override; + QVariant data(const QModelIndex& index, int role) const override; + QVariant headerData(int section, Qt::Orientation orientation, int role) const override; + bool setData(const QModelIndex& index, const QVariant& value, int role) override; + + /// call this on any external change of the RobotState + void updateRobotState(const moveit::core::RobotState& state); + + moveit::core::RobotState& getRobotState() + { + return robot_state_; + } + const moveit::core::RobotState& getRobotState() const + { + return robot_state_; + } + const moveit::core::JointModelGroup* getJointModelGroup() const + { + return jmg_; + } + +private: + /// retrieve the JointModel corresponding to the variable referenced by index + const moveit::core::JointModel* getJointModel(const QModelIndex& index) const; + /// retrieve the variable bounds referenced by variable index + const moveit::core::VariableBounds* getVariableBounds(const moveit::core::JointModel* jm, + const QModelIndex& index) const; +}; + +class JointsWidgetEventFilter : public QObject +{ + Q_OBJECT + +public: + JointsWidgetEventFilter(QAbstractItemView* view); + +protected: + bool eventFilter(QObject* target, QEvent* event) override; +}; + +class MotionPlanningDisplay; +class MotionPlanningFrameJointsWidget : public QWidget +{ + Q_OBJECT + +public: + MotionPlanningFrameJointsWidget(const MotionPlanningFrameJointsWidget&) = delete; + MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent = nullptr); + ~MotionPlanningFrameJointsWidget() override; + + void changePlanningGroup(const std::string& group_name, + const robot_interaction::InteractionHandlerPtr& start_state_handler, + const robot_interaction::InteractionHandlerPtr& goal_state_handler); + +public Q_SLOTS: + void queryStartStateChanged(); + void queryGoalStateChanged(); + void jogNullspace(double value); + +protected: + void setActiveModel(JMGItemModel* model); + void triggerUpdate(JMGItemModel* model); + void updateNullspaceSliders(); + QSlider* createNSSlider(int i); + +private: + Ui::MotionPlanningFrameJointsUI* ui_; + MotionPlanningDisplay* planning_display_; + robot_interaction::InteractionHandlerPtr start_state_handler_; + robot_interaction::InteractionHandlerPtr goal_state_handler_; + std::unique_ptr start_state_model_; + std::unique_ptr goal_state_model_; + // break circular loop of stateChanged() -> dataChanged() |-> PlanningDisplay::setQuery*State() + bool ignore_state_changes_ = false; + + Eigen::JacobiSVD svd_; + Eigen::MatrixXd nullspace_; + std::vector ns_sliders_; +}; + +/// Delegate to show the joint value as with a progress bar indicator between min and max. +class ProgressBarDelegate : public QStyledItemDelegate +{ + Q_OBJECT + +public: + enum CustomRole + { + JointTypeRole = Qt::UserRole, // NOLINT(readability-identifier-naming) + VariableBoundsRole // NOLINT(readability-identifier-naming) + }; + + ProgressBarDelegate(QWidget* parent = nullptr) : QStyledItemDelegate(parent) + { + } + + void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override; + QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option, const QModelIndex& index) const override; + +private Q_SLOTS: + void commitAndCloseEditor(); +}; + +/// Number editor via progress bar dragging +class ProgressBarEditor : public QWidget +{ + Q_OBJECT + Q_PROPERTY(float value READ value WRITE setValue NOTIFY valueChanged USER true) + +public: + /// Create a progressbar-like slider for editing values in range mix..max + ProgressBarEditor(QWidget* parent = nullptr, float min = -1.0, float max = 0.0, int digits = 0); + + void setValue(float value) + { + value_ = value; + } + float value() const + { + return value_; + } + +Q_SIGNALS: + void valueChanged(float value); + void editingFinished(); + +protected: + void paintEvent(QPaintEvent* event) override; + void mousePressEvent(QMouseEvent* event) override; + void mouseMoveEvent(QMouseEvent* event) override; + void mouseReleaseEvent(QMouseEvent* event) override; + +private: + float value_; + float min_; + float max_; + int digits_; ///< number of decimal digits for formatting of the value +}; + +/// Slider that jumps back to zero +class JogSlider : public QSlider +{ + Q_OBJECT + int timer_id_; + int timer_interval_; // ms + double maximum_; + +public: + JogSlider(QWidget* parent = nullptr); + + int timerInterval() const + { + return timer_interval_; + } + void setTimerInterval(int ms); + void setResolution(unsigned int resolution); + void setMaximum(double value); + double value() const + { + return QSlider::value() * maximum_ / QSlider::maximum(); + } + +protected: + void timerEvent(QTimerEvent* event) override; + void mousePressEvent(QMouseEvent* event) override; + void mouseReleaseEvent(QMouseEvent* event) override; + +private: + using QSlider::setMaximum; + using QSlider::setMinimum; + using QSlider::setRange; + +Q_SIGNALS: + void triggered(double value); +}; +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h index 91242225c4..4761e293d0 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h @@ -34,18 +34,18 @@ /* Author: Robert Haschke */ -#ifndef MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_PARAM_WIDGET_ -#define MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_PARAM_WIDGET_ +#pragma once -#include #include +#include + namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(MoveGroupInterface); -} +MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc } +} // namespace moveit namespace moveit_rviz_plugin { @@ -54,7 +54,7 @@ class MotionPlanningParamWidget : public rviz::PropertyTreeWidget Q_OBJECT public: MotionPlanningParamWidget(const MotionPlanningParamWidget&) = delete; - MotionPlanningParamWidget(QWidget* parent = 0); + MotionPlanningParamWidget(QWidget* parent = nullptr); ~MotionPlanningParamWidget() override; void setMoveGroup(const moveit::planning_interface::MoveGroupInterfacePtr& mg); @@ -76,6 +76,4 @@ private Q_SLOTS: std::string group_name_; std::string planner_id_; }; -} - -#endif +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/edit-clear.png b/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/edit-clear.png new file mode 100644 index 0000000000..4c309f6f06 Binary files /dev/null and b/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/edit-clear.png differ diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/icons.qrc b/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/icons.qrc new file mode 100644 index 0000000000..90fae4c9f1 --- /dev/null +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/icons.qrc @@ -0,0 +1,7 @@ + + + list-add.png + list-remove.png + edit-clear.png + + diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/list-add.png b/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/list-add.png new file mode 100644 index 0000000000..818abbe146 Binary files /dev/null and b/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/list-add.png differ diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/list-remove.png b/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/list-remove.png new file mode 100644 index 0000000000..213b471c51 Binary files /dev/null and b/moveit_ros/visualization/motion_planning_rviz_plugin/src/icons/list-remove.png differ diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 49de9cd852..47d13295f9 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -38,6 +38,7 @@ #include #include #include + #include #include #include @@ -93,9 +94,10 @@ MotionPlanningDisplay::MotionPlanningDisplay() path_category_ = new rviz::Property("Planned Path", QVariant(), "", this); // Metrics category ----------------------------------------------------------------------------------------- - compute_weight_limit_property_ = new rviz::BoolProperty( - "Show Weight Limit", false, "Shows the weight limit at a particular pose for an end-effector", metrics_category_, - SLOT(changedShowWeightLimit()), this); + compute_weight_limit_property_ = + new rviz::BoolProperty("Show Weight Limit", false, + "Shows the weight limit at a particular pose for an end-effector", metrics_category_, + SLOT(changedShowWeightLimit()), this); show_manipulability_index_property_ = new rviz::BoolProperty("Show Manipulability Index", false, "Shows the manipulability index for an end-effector", @@ -120,11 +122,13 @@ MotionPlanningDisplay::MotionPlanningDisplay() // Planning request category ----------------------------------------------------------------------------------------- - planning_group_property_ = new rviz::EditableEnumProperty( - "Planning Group", "", "The name of the group of links to plan for (from the ones defined in the SRDF)", - plan_category_, SLOT(changedPlanningGroup()), this); - show_workspace_property_ = new rviz::BoolProperty("Show Workspace", false, "Shows the axis-aligned bounding box for " - "the workspace allowed for planning", + planning_group_property_ = + new rviz::EditableEnumProperty("Planning Group", "", + "The name of the group of links to plan for (from the ones defined in the SRDF)", + plan_category_, SLOT(changedPlanningGroup()), this); + show_workspace_property_ = new rviz::BoolProperty("Show Workspace", false, + "Shows the axis-aligned bounding box for " + "the workspace allowed for planning", plan_category_, SLOT(changedWorkspace()), this); query_start_state_property_ = new rviz::BoolProperty("Query Start State", false, "Set a custom start state for the motion planning query", @@ -212,8 +216,7 @@ void MotionPlanningDisplay::onInitialize() color.a = 1.0f; query_robot_start_->setDefaultAttachedObjectColor(color); - query_robot_goal_.reset( - new RobotStateVisualization(planning_scene_node_, context_, "Planning Request Goal", nullptr)); + query_robot_goal_.reset(new RobotStateVisualization(planning_scene_node_, context_, "Planning Request Goal", nullptr)); query_robot_goal_->setCollisionVisible(false); query_robot_goal_->setVisualVisible(true); query_robot_goal_->setVisible(query_goal_state_property_->getBool()); @@ -225,6 +228,7 @@ void MotionPlanningDisplay::onInitialize() rviz::WindowManagerInterface* window_context = context_->getWindowManager(); frame_ = new MotionPlanningFrame(this, context_, window_context ? window_context->getParentWindow() : nullptr); + connect(frame_, SIGNAL(configChanged()), this->getModel(), SIGNAL(configChanged())); resetStatusTextColor(); addStatusText("Initialized."); @@ -253,7 +257,7 @@ void MotionPlanningDisplay::onInitialize() if (context_ && context_->getWindowManager() && context_->getWindowManager()->getParentWindow()) { QShortcut* im_reset_shortcut = - new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_R), context_->getWindowManager()->getParentWindow()); + new QShortcut(QKeySequence("Ctrl+I"), context_->getWindowManager()->getParentWindow()); connect(im_reset_shortcut, SIGNAL(activated()), this, SLOT(resetInteractiveMarkers())); } } @@ -483,7 +487,7 @@ void MotionPlanningDisplay::computeMetrics(bool start, const std::string& group, return; boost::mutex::scoped_lock slock(update_metrics_lock_); - robot_state::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState(); + moveit::core::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState(); for (const robot_interaction::EndEffectorInteraction& ee : eef) if (ee.parent_group == group) computeMetricsInternal(computed_metrics_[std::make_pair(start, group)], ee, *state, payload); @@ -491,7 +495,7 @@ void MotionPlanningDisplay::computeMetrics(bool start, const std::string& group, void MotionPlanningDisplay::computeMetricsInternal(std::map& metrics, const robot_interaction::EndEffectorInteraction& ee, - const robot_state::RobotState& state, double payload) + const moveit::core::RobotState& state, double payload) { metrics.clear(); dynamics_solver::DynamicsSolverPtr ds; @@ -547,7 +551,7 @@ inline void copyItemIfExists(const std::map& source, std::m if (it != source.end()) dest[key] = it->second; } -} +} // namespace void MotionPlanningDisplay::displayMetrics(bool start) { @@ -559,7 +563,7 @@ void MotionPlanningDisplay::displayMetrics(bool start) if (eef.empty()) return; - robot_state::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState(); + moveit::core::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState(); for (const robot_interaction::EndEffectorInteraction& ee : eef) { @@ -586,8 +590,8 @@ void MotionPlanningDisplay::displayMetrics(bool start) } } - const robot_state::LinkModel* lm = nullptr; - const robot_model::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(ee.parent_group); + const moveit::core::LinkModel* lm = nullptr; + const moveit::core::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(ee.parent_group); if (jmg) if (!jmg->getLinkModelNames().empty()) lm = state->getLinkModel(jmg->getLinkModelNames().back()); @@ -615,7 +619,7 @@ void MotionPlanningDisplay::drawQueryStartState() { if (isEnabled()) { - robot_state::RobotStateConstPtr state = getQueryStartState(); + moveit::core::RobotStateConstPtr state = getQueryStartState(); // update link poses query_robot_start_->update(state); @@ -640,11 +644,11 @@ void MotionPlanningDisplay::drawQueryStartState() } if (!getCurrentPlanningGroup().empty()) { - const robot_model::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup()); + const moveit::core::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup()); if (jmg) { std::vector outside_bounds; - const std::vector& jmodels = jmg->getActiveJointModels(); + const std::vector& jmodels = jmg->getActiveJointModels(); for (const moveit::core::JointModel* jmodel : jmodels) if (!state->satisfiesBounds(jmodel, jmodel->getMaximumExtent() * 1e-2)) { @@ -736,7 +740,7 @@ void MotionPlanningDisplay::drawQueryGoalState() { if (isEnabled()) { - robot_state::RobotStateConstPtr state = getQueryGoalState(); + moveit::core::RobotStateConstPtr state = getQueryGoalState(); // update link poses query_robot_goal_->update(state); @@ -762,10 +766,10 @@ void MotionPlanningDisplay::drawQueryGoalState() if (!getCurrentPlanningGroup().empty()) { - const robot_model::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup()); + const moveit::core::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup()); if (jmg) { - const std::vector& jmodels = jmg->getActiveJointModels(); + const std::vector& jmodels = jmg->getActiveJointModels(); std::vector outside_bounds; for (const moveit::core::JointModel* jmodel : jmodels) if (!state->satisfiesBounds(jmodel, jmodel->getMaximumExtent() * 1e-2)) @@ -905,9 +909,7 @@ void MotionPlanningDisplay::scheduleDrawQueryStartState(robot_interaction::Inter return; addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed), "publishInteractiveMarkers"); - recomputeQueryStartStateMetrics(); - addMainLoopJob(boost::bind(&MotionPlanningDisplay::drawQueryStartState, this)); - context_->queueRender(); + updateQueryStartState(); } void MotionPlanningDisplay::scheduleDrawQueryGoalState(robot_interaction::InteractionHandler* /*unused*/, @@ -917,13 +919,12 @@ void MotionPlanningDisplay::scheduleDrawQueryGoalState(robot_interaction::Intera return; addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed), "publishInteractiveMarkers"); - recomputeQueryGoalStateMetrics(); - addMainLoopJob(boost::bind(&MotionPlanningDisplay::drawQueryGoalState, this)); - context_->queueRender(); + updateQueryGoalState(); } void MotionPlanningDisplay::updateQueryStartState() { + queryStartStateChanged(); recomputeQueryStartStateMetrics(); addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedQueryStartState, this)); context_->queueRender(); @@ -931,18 +932,24 @@ void MotionPlanningDisplay::updateQueryStartState() void MotionPlanningDisplay::updateQueryGoalState() { + queryGoalStateChanged(); recomputeQueryGoalStateMetrics(); addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedQueryGoalState, this)); context_->queueRender(); } -void MotionPlanningDisplay::setQueryStartState(const robot_state::RobotState& start) +void MotionPlanningDisplay::rememberPreviousStartState() +{ + *previous_state_ = *query_start_state_->getState(); +} + +void MotionPlanningDisplay::setQueryStartState(const moveit::core::RobotState& start) { query_start_state_->setState(start); updateQueryStartState(); } -void MotionPlanningDisplay::setQueryGoalState(const robot_state::RobotState& goal) +void MotionPlanningDisplay::setQueryGoalState(const moveit::core::RobotState& goal) { query_goal_state_->setState(goal); updateQueryGoalState(); @@ -960,8 +967,8 @@ void MotionPlanningDisplay::useApproximateIK(bool flag) } } -bool MotionPlanningDisplay::isIKSolutionCollisionFree(robot_state::RobotState* state, - const robot_model::JointModelGroup* group, +bool MotionPlanningDisplay::isIKSolutionCollisionFree(moveit::core::RobotState* state, + const moveit::core::JointModelGroup* group, const double* ik_solution) const { if (frame_->ui_->collision_aware_ik->isChecked() && planning_scene_monitor_) @@ -1053,13 +1060,13 @@ std::string MotionPlanningDisplay::getCurrentPlanningGroup() const void MotionPlanningDisplay::setQueryStateHelper(bool use_start_state, const std::string& state_name) { - robot_state::RobotState state = use_start_state ? *getQueryStartState() : *getQueryGoalState(); + moveit::core::RobotState state = use_start_state ? *getQueryStartState() : *getQueryGoalState(); std::string v = "<" + state_name + ">"; if (v == "") { - if (const robot_state::JointModelGroup* jmg = state.getJointModelGroup(getCurrentPlanningGroup())) + if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(getCurrentPlanningGroup())) state.setToRandomPositions(jmg); } else if (v == "") @@ -1079,7 +1086,7 @@ void MotionPlanningDisplay::setQueryStateHelper(bool use_start_state, const std: else { // maybe it is a named state - if (const robot_model::JointModelGroup* jmg = state.getJointModelGroup(getCurrentPlanningGroup())) + if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(getCurrentPlanningGroup())) state.setToDefaultValues(jmg, state_name); } @@ -1135,10 +1142,11 @@ void MotionPlanningDisplay::onRobotModelLoaded() query_robot_start_->load(*getRobotModel()->getURDF()); query_robot_goal_->load(*getRobotModel()->getURDF()); - robot_state::RobotStatePtr ks(new robot_state::RobotState(getPlanningSceneRO()->getCurrentState())); - query_start_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "start", *ks, + // initialize previous state, start state, and goal state to current state + previous_state_ = std::make_shared(getPlanningSceneRO()->getCurrentState()); + query_start_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "start", *previous_state_, planning_scene_monitor_->getTFClient())); - query_goal_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "goal", *ks, + query_goal_state_.reset(new robot_interaction::InteractionHandler(robot_interaction_, "goal", *previous_state_, planning_scene_monitor_->getTFClient())); query_start_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this, _1, _2)); query_goal_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this, _1, _2)); @@ -1176,15 +1184,20 @@ void MotionPlanningDisplay::onRobotModelLoaded() if (frame_) frame_->fillPlanningGroupOptions(); - addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedPlanningGroup, this)); + changedPlanningGroup(); +} +void MotionPlanningDisplay::onNewPlanningSceneState() +{ + frame_->onNewPlanningSceneState(); } -void MotionPlanningDisplay::updateStateExceptModified(robot_state::RobotState& dest, const robot_state::RobotState& src) +void MotionPlanningDisplay::updateStateExceptModified(moveit::core::RobotState& dest, + const moveit::core::RobotState& src) { - robot_state::RobotState src_copy = src; + moveit::core::RobotState src_copy = src; for (const std::string& modified_group : modified_groups_) { - const robot_model::JointModelGroup* jmg = dest.getJointModelGroup(modified_group); + const moveit::core::JointModelGroup* jmg = dest.getJointModelGroup(modified_group); if (jmg) { std::vector values_to_keep; @@ -1197,27 +1210,30 @@ void MotionPlanningDisplay::updateStateExceptModified(robot_state::RobotState& d dest = src_copy; } -void MotionPlanningDisplay::onSceneMonitorReceivedUpdate( - planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) +void MotionPlanningDisplay::updateQueryStates(const moveit::core::RobotState& current_state) { - PlanningSceneDisplay::onSceneMonitorReceivedUpdate(update_type); - robot_state::RobotState current_state = getPlanningSceneRO()->getCurrentState(); std::string group = planning_group_property_->getStdString(); - if (query_start_state_property_->getBool() && !group.empty()) + if (query_start_state_ && query_start_state_property_->getBool() && !group.empty()) { - robot_state::RobotState start = *getQueryStartState(); + moveit::core::RobotState start = *getQueryStartState(); updateStateExceptModified(start, current_state); setQueryStartState(start); } - if (query_goal_state_property_->getBool() && !group.empty()) + if (query_goal_state_ && query_goal_state_property_->getBool() && !group.empty()) { - robot_state::RobotState goal = *getQueryGoalState(); + moveit::core::RobotState goal = *getQueryGoalState(); updateStateExceptModified(goal, current_state); setQueryGoalState(goal); } +} +void MotionPlanningDisplay::onSceneMonitorReceivedUpdate( + planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) +{ + PlanningSceneDisplay::onSceneMonitorReceivedUpdate(update_type); + updateQueryStates(getPlanningSceneRO()->getCurrentState()); if (frame_) frame_->sceneUpdate(update_type); } @@ -1240,8 +1256,7 @@ void MotionPlanningDisplay::onEnable() int_marker_display_->setEnabled(true); int_marker_display_->setFixedFrame(fixed_frame_); - if (frame_ && frame_->parentWidget()) - frame_->parentWidget()->show(); + frame_->enable(); } // ****************************************************************************************** @@ -1262,8 +1277,7 @@ void MotionPlanningDisplay::onDisable() // Planned Path Display trajectory_visual_->onDisable(); - if (frame_ && frame_->parentWidget()) - frame_->parentWidget()->hide(); + frame_->disable(); } // ****************************************************************************************** @@ -1313,27 +1327,24 @@ void MotionPlanningDisplay::load(const rviz::Config& config) int attempts; if (config.mapGetInt("MoveIt_Planning_Attempts", &attempts)) frame_->ui_->planning_attempts->setValue(attempts); - if (config.mapGetFloat("MoveIt_Goal_Tolerance", &d)) - frame_->ui_->goal_tolerance->setValue(d); + if (config.mapGetFloat("Velocity_Scaling_Factor", &d)) + frame_->ui_->velocity_scaling_factor->setValue(d); + if (config.mapGetFloat("Acceleration_Scaling_Factor", &d)) + frame_->ui_->acceleration_scaling_factor->setValue(d); + bool b; + if (config.mapGetBool("MoveIt_Allow_Replanning", &b)) + frame_->ui_->allow_replanning->setChecked(b); + if (config.mapGetBool("MoveIt_Allow_Sensor_Positioning", &b)) + frame_->ui_->allow_looking->setChecked(b); + if (config.mapGetBool("MoveIt_Allow_External_Program", &b)) + frame_->ui_->allow_external_program->setChecked(b); if (config.mapGetBool("MoveIt_Use_Cartesian_Path", &b)) frame_->ui_->use_cartesian_path->setChecked(b); if (config.mapGetBool("MoveIt_Use_Constraint_Aware_IK", &b)) frame_->ui_->collision_aware_ik->setChecked(b); if (config.mapGetBool("MoveIt_Allow_Approximate_IK", &b)) frame_->ui_->approximate_ik->setChecked(b); - if (config.mapGetBool("MoveIt_Allow_External_Program", &b)) - frame_->ui_->allow_external_program->setChecked(b); - if (config.mapGetBool("MoveIt_Allow_Replanning", &b)) - frame_->ui_->allow_replanning->setChecked(b); - if (config.mapGetBool("MoveIt_Allow_Sensor_Positioning", &b)) - frame_->ui_->allow_looking->setChecked(b); - - float v; - if (config.mapGetFloat("Velocity_Scaling_Factor", &v)) - frame_->ui_->velocity_scaling_factor->setValue(v); - if (config.mapGetFloat("Acceleration_Scaling_Factor", &v)) - frame_->ui_->acceleration_scaling_factor->setValue(v); rviz::Config workspace = config.mapGetChild("MoveIt_Workspace"); rviz::Config ws_center = workspace.mapGetChild("Center"); @@ -1378,8 +1389,6 @@ void MotionPlanningDisplay::save(rviz::Config config) const config.mapSetValue("MoveIt_Warehouse_Host", frame_->ui_->database_host->text()); config.mapSetValue("MoveIt_Warehouse_Port", frame_->ui_->database_port->value()); - config.mapSetValue("MoveIt_Goal_Tolerance", frame_->ui_->goal_tolerance->value()); - // "Options" Section config.mapSetValue("MoveIt_Planning_Time", frame_->ui_->planning_time->value()); config.mapSetValue("MoveIt_Planning_Attempts", frame_->ui_->planning_attempts->value()); @@ -1429,8 +1438,7 @@ void MotionPlanningDisplay::visualizePlaceLocations(const std::vectorgetSceneManager())); place_locations_display_[i]->setColor(1.0f, 0.0f, 0.0f, 0.3f); - Ogre::Vector3 center(place_poses[i].pose.position.x, place_poses[i].pose.position.y, - place_poses[i].pose.position.z); + Ogre::Vector3 center(place_poses[i].pose.position.x, place_poses[i].pose.position.y, place_poses[i].pose.position.z); Ogre::Vector3 extents(0.02, 0.02, 0.02); place_locations_display_[i]->setScale(extents); place_locations_display_[i]->setPosition(center); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 9a5e5ab110..9568527651 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -49,14 +50,15 @@ #include #include +#include +#include #include #include "ui_motion_planning_rviz_plugin_frame.h" namespace moveit_rviz_plugin { -MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, - QWidget* parent) +MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent) : QWidget(parent) , planning_display_(pdisplay) , context_(context) @@ -66,6 +68,26 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: { // set up the GUI ui_->setupUi(this); + ui_->shapes_combo_box->addItem("Box", shapes::BOX); + ui_->shapes_combo_box->addItem("Sphere", shapes::SPHERE); + ui_->shapes_combo_box->addItem("Cylinder", shapes::CYLINDER); + ui_->shapes_combo_box->addItem("Cone", shapes::CONE); + ui_->shapes_combo_box->addItem("Mesh from file", shapes::MESH); + ui_->shapes_combo_box->addItem("Mesh from URL", shapes::MESH); + setLocalSceneEdited(false); + + // add more tabs + joints_tab_ = new MotionPlanningFrameJointsWidget(planning_display_, ui_->tabWidget); + ui_->tabWidget->addTab(joints_tab_, "Joints"); + connect(planning_display_, SIGNAL(queryStartStateChanged()), joints_tab_, SLOT(queryStartStateChanged())); + connect(planning_display_, SIGNAL(queryGoalStateChanged()), joints_tab_, SLOT(queryGoalStateChanged())); + + // Set initial velocity and acceleration scaling factors from ROS parameters + double factor; + nh_.param("robot_description_planning/default_velocity_scaling_factor", factor, 0.1); + ui_->velocity_scaling_factor->setValue(factor); + nh_.param("robot_description_planning/default_acceleration_scaling_factor", factor, 0.1); + ui_->acceleration_scaling_factor->setValue(factor); // connect bottons to actions; each action usually registers the function pointer for the actual computation, // to keep the GUI more responsive (using the background job processing) @@ -92,20 +114,18 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: SLOT(planningAlgorithmIndexChanged(int))); connect(ui_->planning_algorithm_combo_box, SIGNAL(currentIndexChanged(int)), this, SLOT(planningAlgorithmIndexChanged(int))); - connect(ui_->import_file_button, SIGNAL(clicked()), this, SLOT(importFileButtonClicked())); - connect(ui_->import_url_button, SIGNAL(clicked()), this, SLOT(importUrlButtonClicked())); - connect(ui_->clear_scene_button, SIGNAL(clicked()), this, SLOT(clearSceneButtonClicked())); + connect(ui_->clear_scene_button, SIGNAL(clicked()), this, SLOT(clearScene())); connect(ui_->scene_scale, SIGNAL(valueChanged(int)), this, SLOT(sceneScaleChanged(int))); connect(ui_->scene_scale, SIGNAL(sliderPressed()), this, SLOT(sceneScaleStartChange())); connect(ui_->scene_scale, SIGNAL(sliderReleased()), this, SLOT(sceneScaleEndChange())); - connect(ui_->remove_object_button, SIGNAL(clicked()), this, SLOT(removeObjectButtonClicked())); + connect(ui_->remove_object_button, SIGNAL(clicked()), this, SLOT(removeSceneObject())); connect(ui_->object_x, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double))); connect(ui_->object_y, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double))); connect(ui_->object_z, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double))); connect(ui_->object_rx, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double))); connect(ui_->object_ry, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double))); connect(ui_->object_rz, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double))); - connect(ui_->publish_current_scene_button, SIGNAL(clicked()), this, SLOT(publishSceneButtonClicked())); + connect(ui_->publish_current_scene_button, SIGNAL(clicked()), this, SLOT(publishScene())); connect(ui_->collision_objects_list, SIGNAL(itemSelectionChanged()), this, SLOT(selectedCollisionObjectChanged())); connect(ui_->collision_objects_list, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(collisionObjectChanged(QListWidgetItem*))); @@ -115,8 +135,11 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: connect(ui_->planning_scene_tree, SIGNAL(itemChanged(QTreeWidgetItem*, int)), this, SLOT(warehouseItemNameChanged(QTreeWidgetItem*, int))); connect(ui_->reset_db_button, SIGNAL(clicked()), this, SLOT(resetDbButtonClicked())); - connect(ui_->export_scene_text_button, SIGNAL(clicked()), this, SLOT(exportAsTextButtonClicked())); - connect(ui_->import_scene_text_button, SIGNAL(clicked()), this, SLOT(importFromTextButtonClicked())); + + connect(ui_->add_object_button, &QPushButton::clicked, this, &MotionPlanningFrame::addSceneObject); + connect(ui_->shapes_combo_box, &QComboBox::currentTextChanged, this, &MotionPlanningFrame::shapesComboBoxChanged); + connect(ui_->export_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(exportGeometryAsTextButtonClicked())); + connect(ui_->import_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(importGeometryFromTextButtonClicked())); connect(ui_->load_state_button, SIGNAL(clicked()), this, SLOT(loadStateButtonClicked())); connect(ui_->save_start_state_button, SIGNAL(clicked()), this, SLOT(saveStartStateButtonClicked())); connect(ui_->save_goal_state_button, SIGNAL(clicked()), this, SLOT(saveGoalStateButtonClicked())); @@ -136,6 +159,29 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: connect(ui_->tabWidget, SIGNAL(currentChanged(int)), this, SLOT(tabChanged(int))); + /* Notice changes to be safed in config file */ + connect(ui_->database_host, SIGNAL(textChanged(QString)), this, SIGNAL(configChanged())); + connect(ui_->database_port, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + + connect(ui_->planning_time, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->planning_attempts, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + connect(ui_->velocity_scaling_factor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->acceleration_scaling_factor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + + connect(ui_->allow_replanning, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(ui_->allow_looking, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(ui_->allow_external_program, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(ui_->use_cartesian_path, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(ui_->collision_aware_ik, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(ui_->approximate_ik, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + + connect(ui_->wcenter_x, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->wcenter_y, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->wcenter_z, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->wsize_x, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->wsize_y, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(ui_->wsize_z, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + QShortcut* copy_object_shortcut = new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_C), ui_->collision_objects_list); connect(copy_object_shortcut, SIGNAL(activated()), this, SLOT(copySelectedCollisionObject())); @@ -143,7 +189,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: ui_->background_job_progress->hide(); ui_->background_job_progress->setMaximum(0); - ui_->tabWidget->setCurrentIndex(0); + ui_->tabWidget->setCurrentIndex(1); known_collision_objects_version_ = 0; @@ -168,14 +214,6 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz:: object_recognition_client_.reset(); } } - try - { - planning_scene_interface_.reset(new moveit::planning_interface::PlanningSceneInterface()); - } - catch (std::exception& ex) - { - ROS_ERROR("%s", ex.what()); - } try { @@ -255,7 +293,7 @@ void MotionPlanningFrame::fillPlanningGroupOptions() const QSignalBlocker planning_group_blocker(ui_->planning_group_combo_box); ui_->planning_group_combo_box->clear(); - const robot_model::RobotModelConstPtr& kmodel = planning_display_->getRobotModel(); + const moveit::core::RobotModelConstPtr& kmodel = planning_display_->getRobotModel(); for (const std::string& group_name : kmodel->getJointModelGroupNames()) ui_->planning_group_combo_box->addItem(QString::fromStdString(group_name)); } @@ -270,22 +308,24 @@ void MotionPlanningFrame::fillStateSelectionOptions() if (!planning_display_->getPlanningSceneMonitor()) return; - const robot_model::RobotModelConstPtr& robot_model = planning_display_->getRobotModel(); + const moveit::core::RobotModelConstPtr& robot_model = planning_display_->getRobotModel(); std::string group = planning_display_->getCurrentPlanningGroup(); if (group.empty()) return; - const robot_model::JointModelGroup* jmg = robot_model->getJointModelGroup(group); + const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); if (jmg) { ui_->start_state_combo_box->addItem(QString("")); ui_->start_state_combo_box->addItem(QString("")); ui_->start_state_combo_box->addItem(QString("")); ui_->start_state_combo_box->addItem(QString("")); + ui_->start_state_combo_box->addItem(QString("")); ui_->goal_state_combo_box->addItem(QString("")); ui_->goal_state_combo_box->addItem(QString("")); ui_->goal_state_combo_box->addItem(QString("")); ui_->goal_state_combo_box->addItem(QString("")); + ui_->goal_state_combo_box->addItem(QString("")); const std::vector& known_states = jmg->getDefaultStateNames(); if (!known_states.empty()) @@ -313,7 +353,7 @@ void MotionPlanningFrame::changePlanningGroupHelper() planning_display_->addMainLoopJob( boost::bind(&MotionPlanningFrame::populateConstraintsList, this, std::vector())); - const robot_model::RobotModelConstPtr& robot_model = planning_display_->getRobotModel(); + const moveit::core::RobotModelConstPtr& robot_model = planning_display_->getRobotModel(); std::string group = planning_display_->getCurrentPlanningGroup(); planning_display_->addMainLoopJob( boost::bind(&MotionPlanningParamWidget::setGroupName, ui_->planner_param_treeview, group)); @@ -383,6 +423,9 @@ void MotionPlanningFrame::changePlanningGroup() { planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::changePlanningGroupHelper, this), "Frame::changePlanningGroup"); + joints_tab_->changePlanningGroup(planning_display_->getCurrentPlanningGroup(), + planning_display_->getQueryStartStateHandler(), + planning_display_->getQueryGoalStateHandler()); } void MotionPlanningFrame::sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) @@ -391,98 +434,132 @@ void MotionPlanningFrame::sceneUpdate(planning_scene_monitor::PlanningSceneMonit planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); } -void MotionPlanningFrame::importResource(const std::string& path) +void MotionPlanningFrame::addSceneObject() { - if (planning_display_->getPlanningSceneMonitor()) + static const double MIN_VAL = 1e-6; + + if (!planning_display_->getPlanningSceneMonitor()) + { + return; + } + + // get size values + double x_length = ui_->shape_size_x_spin_box->isEnabled() ? ui_->shape_size_x_spin_box->value() : MIN_VAL; + double y_length = ui_->shape_size_y_spin_box->isEnabled() ? ui_->shape_size_y_spin_box->value() : MIN_VAL; + double z_length = ui_->shape_size_z_spin_box->isEnabled() ? ui_->shape_size_z_spin_box->value() : MIN_VAL; + if (x_length < MIN_VAL || y_length < MIN_VAL || z_length < MIN_VAL) { - shapes::Mesh* mesh = shapes::createMeshFromResource(path); - if (mesh) + QMessageBox::warning(this, QString("Dimension is too small"), QString("Size values need to be >= %1").arg(MIN_VAL)); + return; + } + + // by default, name object by shape type + std::string selected_shape = ui_->shapes_combo_box->currentText().toStdString(); + shapes::ShapeConstPtr shape; + switch (ui_->shapes_combo_box->currentData().toInt()) // fetch shape ID from current combobox item + { + case shapes::BOX: + shape = std::make_shared(x_length, y_length, z_length); + break; + case shapes::SPHERE: + shape = std::make_shared(0.5 * x_length); + break; + case shapes::CONE: + shape = std::make_shared(0.5 * x_length, z_length); + break; + case shapes::CYLINDER: + shape = std::make_shared(0.5 * x_length, z_length); + break; + case shapes::MESH: { - std::size_t slash = path.find_last_of("/\\"); - std::string name = path.substr(slash + 1); - shapes::ShapeConstPtr shape(mesh); - Eigen::Isometry3d pose; - pose.setIdentity(); + QUrl url; + if (ui_->shapes_combo_box->currentText().contains("file")) // open from file + url = QFileDialog::getOpenFileUrl(this, tr("Import Object Mesh"), QString(), + "CAD files (*.stl *.obj *.dae);;All files (*.*)"); + else // open from URL + url = QInputDialog::getText(this, tr("Import Object Mesh"), tr("URL for file to import from:"), + QLineEdit::Normal, QString("http://")); + if (!url.isEmpty()) + shape = loadMeshResource(url.toString().toStdString()); + if (!shape) + return; + // name mesh objects by their file name + selected_shape = url.fileName().toStdString(); + break; + } + default: + QMessageBox::warning(this, QString("Unsupported shape"), + QString("The '%1' is not supported.").arg(ui_->shapes_combo_box->currentText())); + } + + // find available (initial) name of object + int idx = 0; + std::string shape_name = selected_shape + "_" + std::to_string(idx); + while (planning_display_->getPlanningSceneRO()->getWorld()->hasObject(shape_name)) + { + idx++; + shape_name = selected_shape + "_" + std::to_string(idx); + } - if (planning_display_->getPlanningSceneRO()->getCurrentState().hasAttachedBody(name)) + // Actually add object to the plugin's PlanningScene + { + planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); + ps->getWorldNonConst()->addToObject(shape_name, shape, Eigen::Isometry3d::Identity()); + } + setLocalSceneEdited(); + + planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); + + // Automatically select the inserted object so that its IM is displayed + planning_display_->addMainLoopJob( + boost::bind(&MotionPlanningFrame::setItemSelectionInList, this, shape_name, true, ui_->collision_objects_list)); + + planning_display_->queueRenderSceneGeometry(); +} + +shapes::ShapePtr MotionPlanningFrame::loadMeshResource(const std::string& url) +{ + shapes::Mesh* mesh = shapes::createMeshFromResource(url); + if (mesh) + { + // If the object is very large, ask the user if the scale should be reduced. + bool object_is_very_large = false; + for (unsigned int i = 0; i < mesh->vertex_count; i++) + { + if ((abs(mesh->vertices[i * 3 + 0]) > LARGE_MESH_THRESHOLD) || + (abs(mesh->vertices[i * 3 + 1]) > LARGE_MESH_THRESHOLD) || + (abs(mesh->vertices[i * 3 + 2]) > LARGE_MESH_THRESHOLD)) { - QMessageBox::warning(this, QString("Duplicate names"), QString("An attached object named '") - .append(name.c_str()) - .append("' already exists. Please rename the " - "attached object before importing.")); - return; + object_is_very_large = true; + break; } - - // If the object already exists, ask the user whether to overwrite or rename - if (planning_display_->getPlanningSceneRO()->getWorld()->hasObject(name)) + } + if (object_is_very_large) + { + QMessageBox msg_box; + msg_box.setText( + "The object is very large (greater than 10 m). The file may be in millimeters instead of meters."); + msg_box.setInformativeText("Attempt to fix the size by shrinking the object?"); + msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No); + msg_box.setDefaultButton(QMessageBox::Yes); + if (msg_box.exec() == QMessageBox::Yes) { - QMessageBox msg_box; - msg_box.setText("There exists another object with the same name."); - msg_box.setInformativeText("Would you like to overwrite it?"); - msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); - msg_box.setDefaultButton(QMessageBox::No); - int ret = msg_box.exec(); - - switch (ret) + for (unsigned int i = 0; i < mesh->vertex_count; ++i) { - case QMessageBox::Yes: - // Overwrite was clicked - { - planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); - if (ps) - { - ps->getWorldNonConst()->removeObject(name); - addObject(ps->getWorldNonConst(), name, shape, pose); - } - } - break; - case QMessageBox::No: - { - // Don't overwrite was clicked. Ask for another name - bool ok = false; - QString text = QInputDialog::getText( - this, tr("Choose a new name"), tr("Import the new object under the name:"), QLineEdit::Normal, - QString::fromStdString(name + "-" + boost::lexical_cast( - planning_display_->getPlanningSceneRO()->getWorld()->size())), - &ok); - if (ok) - { - if (!text.isEmpty()) - { - name = text.toStdString(); - planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); - if (ps) - { - if (ps->getWorld()->hasObject(name)) - QMessageBox::warning( - this, "Name already exists", - QString("The name '").append(name.c_str()).append("' already exists. Not importing object.")); - else - addObject(ps->getWorldNonConst(), name, shape, pose); - } - } - else - QMessageBox::warning(this, "Object not imported", "Cannot use an empty name for an imported object"); - } - break; - } - default: - // Pressed cancel, do nothing - break; + unsigned int i3 = i * 3; + mesh->vertices[i3] *= 0.001; + mesh->vertices[i3 + 1] *= 0.001; + mesh->vertices[i3 + 2] *= 0.001; } } - else - { - planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); - if (ps) - addObject(ps->getWorldNonConst(), name, shape, pose); - } - } - else - { - QMessageBox::warning(this, QString("Import error"), QString("Unable to import scene")); - return; } + + return shapes::ShapePtr(mesh); + } + else + { + QMessageBox::warning(this, QString("Import error"), QString("Unable to import object")); + return shapes::ShapePtr(); } } @@ -500,6 +577,7 @@ void MotionPlanningFrame::enable() void MotionPlanningFrame::disable() { move_group_.reset(); + scene_marker_.reset(); parentWidget()->hide(); } @@ -511,7 +589,7 @@ void MotionPlanningFrame::tabChanged(int index) selectedCollisionObjectChanged(); } -void MotionPlanningFrame::updateSceneMarkers(float wall_dt, float ros_dt) +void MotionPlanningFrame::updateSceneMarkers(float wall_dt, float /*ros_dt*/) { if (scene_marker_) scene_marker_->update(wall_dt); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp index f2773ed915..038a0ef72d 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp @@ -57,17 +57,6 @@ void MotionPlanningFrame::databaseConnectButtonClicked() "connect to database"); } -void MotionPlanningFrame::publishSceneButtonClicked() -{ - const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); - if (ps) - { - moveit_msgs::PlanningScene msg; - ps->getPlanningSceneMsg(msg); - planning_scene_publisher_.publish(msg); - } -} - void MotionPlanningFrame::planningAlgorithmIndexChanged(int index) { std::string planner_id = ui_->planning_algorithm_combo_box->itemText(index).toStdString(); @@ -81,8 +70,9 @@ void MotionPlanningFrame::planningAlgorithmIndexChanged(int index) void MotionPlanningFrame::resetDbButtonClicked() { - if (QMessageBox::warning(this, "Data about to be deleted", "The following dialog will allow you to drop a MoveIt! " - "Warehouse database. Are you sure you want to continue?", + if (QMessageBox::warning(this, "Data about to be deleted", + "The following dialog will allow you to drop a MoveIt " + "Warehouse database. Are you sure you want to continue?", QMessageBox::Yes | QMessageBox::No) == QMessageBox::No) return; diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp new file mode 100644 index 0000000000..d33563a76d --- /dev/null +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -0,0 +1,573 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, CITEC, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of CITEC / Bielefeld University nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Robert Haschke */ + +#include +#include + +#include "ui_motion_planning_rviz_plugin_frame_joints.h" +#include +#include + +namespace moveit_rviz_plugin +{ +JMGItemModel::JMGItemModel(const moveit::core::RobotState& robot_state, const std::string& group_name, QObject* parent) + : QAbstractTableModel(parent), robot_state_(robot_state), jmg_(nullptr) +{ + if (robot_state_.getRobotModel()->hasJointModelGroup(group_name)) + jmg_ = robot_state_.getRobotModel()->getJointModelGroup(group_name); +} + +int JMGItemModel::rowCount(const QModelIndex& /*parent*/) const +{ + if (!jmg_) + return robot_state_.getVariableCount(); + else + return jmg_->getVariableCount(); +} + +int JMGItemModel::columnCount(const QModelIndex& /*parent*/) const +{ + return 2; +} + +Qt::ItemFlags JMGItemModel::flags(const QModelIndex& index) const +{ + if (!index.isValid()) + return Qt::ItemFlags(); + + Qt::ItemFlags f = QAbstractTableModel::flags(index); + if (index.column() == 1) + { + const moveit::core::JointModel* jm = getJointModel(index); + if (!jm->isPassive() && !jm->getMimic()) // these are not editable + f |= Qt::ItemIsEditable; + } + return f; +} + +QVariant JMGItemModel::data(const QModelIndex& index, int role) const +{ + if (!index.isValid()) + return QVariant(); + + int idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row(); + switch (index.column()) + { + case 0: // joint name column + switch (role) + { + case Qt::DisplayRole: + return QString::fromStdString(robot_state_.getVariableNames()[idx]); + case Qt::TextAlignmentRole: + return Qt::AlignLeft; + } + break; + case 1: // joint value column + { + double value = robot_state_.getVariablePosition(idx); + const moveit::core::JointModel* jm = robot_state_.getRobotModel()->getJointOfVariable(idx); + switch (role) + { + case Qt::DisplayRole: + return value; + case Qt::EditRole: + if (jm) + return jm->getType() == moveit::core::JointModel::REVOLUTE ? value * 180 / M_PI : value; + break; + case ProgressBarDelegate::JointTypeRole: + if (jm) + return jm->getType(); + break; + case ProgressBarDelegate::VariableBoundsRole: + if (const moveit::core::VariableBounds* bounds = getVariableBounds(jm, index)) + return QPointF(bounds->min_position_, bounds->max_position_); + break; + case Qt::TextAlignmentRole: + return Qt::AlignLeft; + } + } + } + return QVariant(); +} + +QVariant JMGItemModel::headerData(int section, Qt::Orientation orientation, int role) const +{ + if (orientation == Qt::Horizontal && role == Qt::DisplayRole) + return section == 0 ? "Joint Name" : "Value"; + return QAbstractTableModel::headerData(section, orientation, role); +} + +bool JMGItemModel::setData(const QModelIndex& index, const QVariant& value, int role) +{ + if (index.column() != 1 || role != Qt::EditRole) + return false; + + int var_idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row(); + const moveit::core::JointModel* jm = robot_state_.getRobotModel()->getJointOfVariable(var_idx); + if (!value.canConvert()) + return false; + + bool ok; + double v = value.toDouble(&ok); + if (!ok) + return false; + + // for revolute joints, we convert degrees to radians + if (jm && jm->getType() == moveit::core::JointModel::REVOLUTE) + v *= M_PI / 180; + + robot_state_.setVariablePosition(var_idx, v); + jm->enforcePositionBounds(robot_state_.getVariablePositions() + jm->getFirstVariableIndex()); + dataChanged(index, index); + return true; +} + +const moveit::core::JointModel* JMGItemModel::getJointModel(const QModelIndex& index) const +{ + if (!index.isValid()) + return nullptr; + int var_idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row(); + return robot_state_.getRobotModel()->getJointOfVariable(var_idx); +} + +const moveit::core::VariableBounds* JMGItemModel::getVariableBounds(const moveit::core::JointModel* jm, + const QModelIndex& index) const +{ + if (!jm) + return nullptr; + int var_idx = jmg_ ? jmg_->getVariableIndexList()[index.row()] : index.row(); + const moveit::core::VariableBounds* bounds = &jm->getVariableBounds()[var_idx - jm->getFirstVariableIndex()]; + return bounds->position_bounded_ ? bounds : nullptr; +} + +// copy positions from new_state and notify about these changes +void JMGItemModel::updateRobotState(const moveit::core::RobotState& state) +{ + if (robot_state_.getRobotModel() != state.getRobotModel()) + return; + robot_state_.setVariablePositions(state.getVariablePositions()); + + dataChanged(index(0, 1), index(rowCount() - 1, 1)); +} + +MotionPlanningFrameJointsWidget::MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent) + : QWidget(parent), ui_(new Ui::MotionPlanningFrameJointsUI()), planning_display_(display) +{ + ui_->setupUi(this); + // intercept mouse events delivered to joints_view_ to open editor on first mouse press + ui_->joints_view_->viewport()->installEventFilter(new JointsWidgetEventFilter(ui_->joints_view_)); + ui_->joints_view_->setItemDelegateForColumn(1, new ProgressBarDelegate(this)); + svd_.setThreshold(0.001); +} + +MotionPlanningFrameJointsWidget::~MotionPlanningFrameJointsWidget() +{ + delete ui_; +} + +void MotionPlanningFrameJointsWidget::changePlanningGroup( + const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler, + const robot_interaction::InteractionHandlerPtr& goal_state_handler) +{ + // release previous models (if any) + ui_->joints_view_->setModel(nullptr); + start_state_model_.reset(); + goal_state_model_.reset(); + + // create new models + start_state_handler_ = start_state_handler; + goal_state_handler_ = goal_state_handler; + start_state_model_.reset(new JMGItemModel(*start_state_handler_->getState(), group_name, this)); + goal_state_model_.reset(new JMGItemModel(*goal_state_handler_->getState(), group_name, this)); + + // forward model updates to the PlanningDisplay + connect(start_state_model_.get(), &JMGItemModel::dataChanged, this, [this]() { + if (!ignore_state_changes_) + planning_display_->setQueryStartState(start_state_model_->getRobotState()); + }); + connect(goal_state_model_.get(), &JMGItemModel::dataChanged, this, [this]() { + if (!ignore_state_changes_) + planning_display_->setQueryGoalState(goal_state_model_->getRobotState()); + }); + + // show the goal state by default + setActiveModel(goal_state_model_.get()); + updateNullspaceSliders(); +} + +void MotionPlanningFrameJointsWidget::queryStartStateChanged() +{ + if (!start_state_model_ || !start_state_handler_) + return; + ignore_state_changes_ = true; + start_state_model_->updateRobotState(*start_state_handler_->getState()); + ignore_state_changes_ = false; + setActiveModel(start_state_model_.get()); + updateNullspaceSliders(); +} + +void MotionPlanningFrameJointsWidget::queryGoalStateChanged() +{ + if (!goal_state_model_ || !goal_state_handler_) + return; + ignore_state_changes_ = true; + goal_state_model_->updateRobotState(*goal_state_handler_->getState()); + ignore_state_changes_ = false; + setActiveModel(goal_state_model_.get()); + updateNullspaceSliders(); +} + +void MotionPlanningFrameJointsWidget::setActiveModel(JMGItemModel* model) +{ + ui_->joints_view_->setModel(model); + ui_->joints_view_label_->setText( + QString("Group joints of %1 state").arg(model == start_state_model_.get() ? "start" : "goal")); +} + +void MotionPlanningFrameJointsWidget::triggerUpdate(JMGItemModel* model) +{ + if (model == start_state_model_.get()) + planning_display_->setQueryStartState(model->getRobotState()); + else + planning_display_->setQueryGoalState(model->getRobotState()); +} + +// Find matching key vector in columns of haystack and return the best-aligned column index. +// Only consider available indexes (> 0). If no match is found, return largest available index. +// sign becomes -1 if key has changed sign compared to the matching haystack column. +static Eigen::Index findMatching(const Eigen::VectorXd& key, const Eigen::MatrixXd& haystack, + const Eigen::VectorXi& available, double& sign) +{ + Eigen::Index result = available.array().maxCoeff(); + double best_match = 0.0; + for (unsigned int i = 0; i < available.rows(); ++i) + { + int index = available[i]; + if (index < 0) // index already taken + continue; + if (index >= haystack.cols()) + return result; + double match = haystack.col(available[i]).transpose() * key; + double abs_match = std::abs(match); + if (abs_match > 0.5 && abs_match > best_match) + { + best_match = abs_match; + result = index; + sign = match > 0 ? 1.0 : -1.0; + } + } + return result; +} + +void MotionPlanningFrameJointsWidget::updateNullspaceSliders() +{ + JMGItemModel* model = dynamic_cast(ui_->joints_view_->model()); + std::size_t i = 0; + if (model && model->getJointModelGroup() && model->getJointModelGroup()->isChain()) + { + model->getRobotState().updateLinkTransforms(); + Eigen::MatrixXd jacobian; + if (!model->getRobotState().getJacobian(model->getJointModelGroup(), + model->getJointModelGroup()->getLinkModels().back(), + Eigen::Vector3d::Zero(), jacobian, false)) + goto cleanup; + + svd_.compute(jacobian, Eigen::ComputeFullV); + Eigen::Index rank = svd_.rank(); + std::size_t ns_dim = svd_.cols() - rank; + Eigen::MatrixXd ns(svd_.cols(), ns_dim); + Eigen::VectorXi available(ns_dim); + for (std::size_t j = 0; j < ns_dim; ++j) + available[j] = j; + + ns_sliders_.reserve(ns_dim); + // create/unhide sliders + for (; i < ns_dim; ++i) + { + if (i >= ns_sliders_.size()) + ns_sliders_.push_back(createNSSlider(i + 1)); + ns_sliders_[i]->show(); + + // Find matching null-space basis vector in previous nullspace_ + double sign = 1.0; + const Eigen::VectorXd& current = svd_.matrixV().col(rank + i); + Eigen::Index index = findMatching(current, nullspace_, available, sign); + ns.col(index).noalias() = sign * current; + available[index] = -1; // mark index as taken + } + nullspace_ = ns; + } + +cleanup: + if (i == 0) + nullspace_.resize(0, 0); + + // hide remaining sliders + for (; i < ns_sliders_.size(); ++i) + ns_sliders_[i]->hide(); +} + +QSlider* MotionPlanningFrameJointsWidget::createNSSlider(int i) +{ + JogSlider* slider = new JogSlider(this); + slider->setOrientation(Qt::Horizontal); + slider->setMaximum(0.1); + slider->setToolTip(QString("Nullspace dim #%1").arg(i)); + ui_->nullspace_layout_->addWidget(slider); + connect(slider, SIGNAL(triggered(double)), this, SLOT(jogNullspace(double))); + return slider; +} + +void MotionPlanningFrameJointsWidget::jogNullspace(double value) +{ + if (value == 0) + return; + + std::size_t index = std::find(ns_sliders_.begin(), ns_sliders_.end(), sender()) - ns_sliders_.begin(); + if (static_cast(index) >= nullspace_.cols()) + return; + + JMGItemModel* model = dynamic_cast(ui_->joints_view_->model()); + if (!model) + return; + + Eigen::VectorXd values; + model->getRobotState().copyJointGroupPositions(model->getJointModelGroup(), values); + values += value * nullspace_.col(index); + model->getRobotState().setJointGroupPositions(model->getJointModelGroup(), values); + model->getRobotState().harmonizePositions(model->getJointModelGroup()); + triggerUpdate(model); +} + +void ProgressBarDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const +{ + // copied from QStyledItemDelegate::paint + QStyle* style = option.widget ? option.widget->style() : QApplication::style(); + QStyleOptionViewItem style_option = option; + initStyleOption(&style_option, index); + + if (index.column() == 1) + { + QVariant joint_type = index.data(JointTypeRole); + double value = index.data().toDouble(); + if (joint_type.isValid()) + { + switch (joint_type.toInt()) + { + case moveit::core::JointModel::REVOLUTE: + style_option.text = option.locale.toString(value * 180 / M_PI, 'f', 0).append("Β°"); + break; + case moveit::core::JointModel::PRISMATIC: + style_option.text = option.locale.toString(value, 'f', 3).append("m"); + break; + default: + break; + } + } + + QVariant vbounds = index.data(VariableBoundsRole); + if (vbounds.isValid()) + { + QPointF bounds = vbounds.toPointF(); + const float min = bounds.x(); + const float max = bounds.y(); + + QStyleOptionProgressBar opt; + opt.rect = option.rect; + opt.minimum = 0; + opt.maximum = 1000; + opt.progress = 1000. * (value - min) / (max - min); + opt.text = style_option.text; + opt.textAlignment = style_option.displayAlignment; + opt.textVisible = true; + style->drawControl(QStyle::CE_ProgressBar, &opt, painter); + return; + } + } + + style->drawControl(QStyle::CE_ItemViewItem, &style_option, painter, option.widget); +} + +QWidget* ProgressBarDelegate::createEditor(QWidget* parent, const QStyleOptionViewItem& option, + const QModelIndex& index) const + +{ + if (index.column() == 1) + { + QVariant vbounds = index.data(VariableBoundsRole); + if (vbounds.isValid()) + { + QPointF bounds = vbounds.toPointF(); + float min = bounds.x(); + float max = bounds.y(); + bool is_revolute = (index.data(JointTypeRole).toInt() == moveit::core::JointModel::REVOLUTE); + if (is_revolute) + { + min *= 180. / M_PI; + max *= 180. / M_PI; + } + auto* editor = new ProgressBarEditor(parent, min, max, is_revolute ? 0 : 3); + connect(editor, &ProgressBarEditor::editingFinished, this, &ProgressBarDelegate::commitAndCloseEditor); + connect(editor, &ProgressBarEditor::valueChanged, this, [=](float value) { + const_cast(index.model())->setData(index, value, Qt::EditRole); + }); + return editor; + } + } + return QStyledItemDelegate::createEditor(parent, option, index); +} + +void ProgressBarDelegate::commitAndCloseEditor() +{ + ProgressBarEditor* editor = qobject_cast(sender()); + commitData(editor); + closeEditor(editor); +} + +JointsWidgetEventFilter::JointsWidgetEventFilter(QAbstractItemView* view) : QObject(view) +{ +} + +bool JointsWidgetEventFilter::eventFilter(QObject* /*target*/, QEvent* event) +{ + if (event->type() == QEvent::MouseButtonPress) + { + QAbstractItemView* view = qobject_cast(parent()); + QModelIndex index = view->indexAt(static_cast(event)->pos()); + if (index.isValid() && index.column() == 1) // mouse event on any of joint indexes? + { + view->setCurrentIndex(index); + view->edit(index); + return true; // event handled + } + } + return false; +} + +ProgressBarEditor::ProgressBarEditor(QWidget* parent, float min, float max, int digits) + : QWidget(parent), min_(min), max_(max), digits_(digits) +{ + // if left mouse button is pressed, grab all future mouse events until button(s) released + if (QApplication::mouseButtons() & Qt::LeftButton) + this->grabMouse(); +} + +void ProgressBarEditor::paintEvent(QPaintEvent* /*event*/) +{ + QPainter painter(this); + + QStyleOptionProgressBar opt; + opt.rect = rect(); + opt.palette = this->palette(); + opt.minimum = 0; + opt.maximum = 1000; + opt.progress = 1000. * (value_ - min_) / (max_ - min_); + opt.text = QLocale().toString(value_, 'f', digits_); + opt.textAlignment = Qt::AlignRight; + opt.textVisible = true; + style()->drawControl(QStyle::CE_ProgressBar, &opt, &painter); +} + +void ProgressBarEditor::mousePressEvent(QMouseEvent* event) +{ + if (event->button() == Qt::LeftButton) + mouseMoveEvent(event); +} + +void ProgressBarEditor::mouseMoveEvent(QMouseEvent* event) +{ + float v = std::min(max_, std::max(min_, min_ + event->x() * (max_ - min_) / width())); + if (value_ != v) + { + value_ = v; + valueChanged(v); + update(); + } + event->accept(); +} + +void ProgressBarEditor::mouseReleaseEvent(QMouseEvent* event) +{ + if (event->button() == Qt::LeftButton) + { + event->accept(); + editingFinished(); + } +} + +JogSlider::JogSlider(QWidget* parent) : QSlider(parent) +{ + setTimerInterval(50); + setResolution(1000); + setMaximum(1.0); +} + +void JogSlider::setTimerInterval(int ms) +{ + timer_interval_ = ms; +} + +void JogSlider::setResolution(unsigned int resolution) +{ + QSlider::setRange(-resolution, +resolution); +} + +void JogSlider::setMaximum(double value) +{ + maximum_ = value; +} + +void JogSlider::timerEvent(QTimerEvent* event) +{ + QSlider::timerEvent(event); + if (event->timerId() == timer_id_) + triggered(value()); +} + +void JogSlider::mousePressEvent(QMouseEvent* event) +{ + QSlider::mousePressEvent(event); + timer_id_ = startTimer(timer_interval_); +} + +void JogSlider::mouseReleaseEvent(QMouseEvent* event) +{ + killTimer(timer_id_); + QSlider::mouseReleaseEvent(event); + setValue(0); +} + +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp index aea72abcf2..c130bde929 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp @@ -41,6 +41,8 @@ #include #include +#include + #include "ui_motion_planning_rviz_plugin_frame.h" namespace moveit_rviz_plugin @@ -68,7 +70,7 @@ void MotionPlanningFrame::processDetectedObjects() { pick_object_name_.clear(); - std::vector objects, object_ids; + std::vector object_ids; double min_x = ui_->roi_center_x->value() - ui_->roi_size_x->value() / 2.0; double min_y = ui_->roi_center_y->value() - ui_->roi_size_y->value() / 2.0; double min_z = ui_->roi_center_z->value() - ui_->roi_size_z->value() / 2.0; @@ -80,13 +82,31 @@ void MotionPlanningFrame::processDetectedObjects() ros::Time start_time = ros::Time::now(); while (object_ids.empty() && ros::Time::now() - start_time <= ros::Duration(3.0)) { - object_ids = - planning_scene_interface_->getKnownObjectNamesInROI(min_x, min_y, min_z, max_x, max_y, max_z, true, objects); + // collect all objects in region of interest + { + const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); + const collision_detection::WorldConstPtr& world = ps->getWorld(); + object_ids.clear(); + for (const auto& object : *world) + { + if (!object.second->shape_poses_.empty()) + { + const auto& position = object.second->shape_poses_[0].translation(); + if (position.x() >= min_x && position.x() <= max_x && position.y() >= min_y && position.y() <= max_y && + position.z() >= min_z && position.z() <= max_z) + { + object_ids.push_back(object.first); + } + } + } + if (!object_ids.empty()) + break; + } ros::Duration(0.5).sleep(); } ROS_DEBUG("Found %d objects", (int)object_ids.size()); - updateDetectedObjectsList(object_ids, objects); + updateDetectedObjectsList(object_ids); } void MotionPlanningFrame::selectedDetectedObjectChanged() @@ -113,7 +133,7 @@ void MotionPlanningFrame::selectedDetectedObjectChanged() } } -void MotionPlanningFrame::detectedObjectChanged(QListWidgetItem* item) +void MotionPlanningFrame::detectedObjectChanged(QListWidgetItem* /*item*/) { } @@ -147,14 +167,13 @@ void MotionPlanningFrame::triggerObjectDetection() } } -void MotionPlanningFrame::listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& msg) +void MotionPlanningFrame::listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& /*msg*/) { ros::Duration(1.0).sleep(); planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::processDetectedObjects, this)); } -void MotionPlanningFrame::updateDetectedObjectsList(const std::vector& object_ids, - const std::vector& objects) +void MotionPlanningFrame::updateDetectedObjectsList(const std::vector& object_ids) { ui_->detected_objects_list->setUpdatesEnabled(false); bool old_state = ui_->detected_objects_list->blockSignals(true); @@ -265,13 +284,12 @@ void MotionPlanningFrame::pickObjectButtonClicked() { if (semantic_world_) { - std::vector object_names; - object_names.push_back(pick_object_name_[group_name]); - std::map object_poses = planning_scene_interface_->getObjectPoses(object_names); - if (object_poses.find(pick_object_name_[group_name]) != object_poses.end()) + const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); + if (ps->getWorld()->hasObject(pick_object_name_[group_name])) { - ROS_DEBUG("Finding current table for object: %s", pick_object_name_[group_name].c_str()); - support_surface_name_ = semantic_world_->findObjectTable(object_poses[pick_object_name_[group_name]]); + geometry_msgs::Pose object_pose(tf2::toMsg(ps->getWorld()->getTransform(pick_object_name_[group_name]))); + ROS_DEBUG_STREAM("Finding current table for object: " << pick_object_name_[group_name]); + support_surface_name_ = semantic_world_->findObjectTable(object_pose); } else support_surface_name_.clear(); @@ -301,14 +319,14 @@ void MotionPlanningFrame::placeObjectButtonClicked() ui_->pick_button->setEnabled(false); ui_->place_button->setEnabled(false); - std::vector attached_bodies; + std::vector attached_bodies; const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (!ps) { ROS_ERROR("No planning scene"); return; } - const robot_model::JointModelGroup* jmg = ps->getCurrentState().getJointModelGroup(group_name); + const moveit::core::JointModelGroup* jmg = ps->getCurrentState().getJointModelGroup(group_name); if (jmg) ps->getCurrentState().getAttachedBodies(attached_bodies, jmg); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 9914f945de..c16bc46399 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -56,25 +56,86 @@ #include "ui_motion_planning_rviz_plugin_frame.h" +namespace +{ +QString subframe_poses_to_qstring(const moveit::core::FixedTransformsMap& subframes) +{ + QString status_text = "\nIt has the subframes '"; + for (const auto& subframe : subframes) + { + status_text += QString::fromStdString(subframe.first) + "', '"; + } + status_text.chop(3); + status_text += "."; + return status_text; +} +} // namespace + namespace moveit_rviz_plugin { -void MotionPlanningFrame::importFileButtonClicked() +void MotionPlanningFrame::shapesComboBoxChanged(const QString& /*text*/) { - QString path = QFileDialog::getOpenFileName(this, tr("Import Object")); - if (!path.isEmpty()) - importResource("file://" + path.toStdString()); + switch (ui_->shapes_combo_box->currentData().toInt()) // fetch shape ID from current combobox item + { + case shapes::BOX: + ui_->shape_size_x_spin_box->setEnabled(true); + ui_->shape_size_y_spin_box->setEnabled(true); + ui_->shape_size_z_spin_box->setEnabled(true); + break; + case shapes::SPHERE: + ui_->shape_size_x_spin_box->setEnabled(true); + ui_->shape_size_y_spin_box->setEnabled(false); + ui_->shape_size_z_spin_box->setEnabled(false); + break; + case shapes::CYLINDER: + case shapes::CONE: + ui_->shape_size_x_spin_box->setEnabled(true); + ui_->shape_size_y_spin_box->setEnabled(false); + ui_->shape_size_z_spin_box->setEnabled(true); + break; + case shapes::MESH: + ui_->shape_size_x_spin_box->setEnabled(false); + ui_->shape_size_y_spin_box->setEnabled(false); + ui_->shape_size_z_spin_box->setEnabled(false); + break; + default: + break; + } +} + +void MotionPlanningFrame::setLocalSceneEdited(bool dirty) +{ + ui_->publish_current_scene_button->setEnabled(dirty); +} + +bool MotionPlanningFrame::isLocalSceneDirty() const +{ + return ui_->publish_current_scene_button->isEnabled(); +} + +void MotionPlanningFrame::publishScene() +{ + const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); + if (ps) + { + moveit_msgs::PlanningScene msg; + ps->getPlanningSceneMsg(msg); + planning_scene_publisher_.publish(msg); + setLocalSceneEdited(false); + } } -void MotionPlanningFrame::importUrlButtonClicked() +void MotionPlanningFrame::publishSceneIfNeeded() { - bool ok = false; - QString url = QInputDialog::getText(this, tr("Import Object"), tr("URL for file to import:"), QLineEdit::Normal, - QString("http://"), &ok); - if (ok && !url.isEmpty()) - importResource(url.toStdString()); + if (isLocalSceneDirty() && + QMessageBox::question(this, "Update PlanningScene", + "You have local changes to your planning scene.\n" + "Publish them to the move_group node?", + QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes) == QMessageBox::Yes) + publishScene(); } -void MotionPlanningFrame::clearSceneButtonClicked() +void MotionPlanningFrame::clearScene() { planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); if (ps) @@ -84,6 +145,7 @@ void MotionPlanningFrame::clearSceneButtonClicked() moveit_msgs::PlanningScene msg; ps->getPlanningSceneMsg(msg); planning_scene_publisher_.publish(msg); + setLocalSceneEdited(false); planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); planning_display_->queueRenderSceneGeometry(); } @@ -98,6 +160,7 @@ void MotionPlanningFrame::sceneScaleChanged(int value) { if (ps->getWorld()->hasObject(scaled_object_->id_)) { + const moveit::core::FixedTransformsMap subframes = scaled_object_->subframe_poses_; // Keep subframes ps->getWorldNonConst()->removeObject(scaled_object_->id_); for (std::size_t i = 0; i < scaled_object_->shapes_.size(); ++i) { @@ -106,6 +169,10 @@ void MotionPlanningFrame::sceneScaleChanged(int value) ps->getWorldNonConst()->addToObject(scaled_object_->id_, shapes::ShapeConstPtr(s), scaled_object_->shape_poses_[i]); } + // TODO(felixvd): Scale subframes too + ps->getWorldNonConst()->setSubframesOfObject(scaled_object_->id_, subframes); + setLocalSceneEdited(); + scene_marker_->processMessage(createObjectMarkerMsg(ps->getWorld()->getObject(scaled_object_->id_))); planning_display_->queueRenderSceneGeometry(); } else @@ -137,7 +204,7 @@ void MotionPlanningFrame::sceneScaleEndChange() ui_->scene_scale->setSliderPosition(100); } -void MotionPlanningFrame::removeObjectButtonClicked() +void MotionPlanningFrame::removeSceneObject() { QList sel = ui_->collision_objects_list->selectedItems(); if (sel.empty()) @@ -151,12 +218,13 @@ void MotionPlanningFrame::removeObjectButtonClicked() else ps->getCurrentStateNonConst().clearAttachedBody(sel[i]->text().toStdString()); scene_marker_.reset(); + setLocalSceneEdited(); planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); planning_display_->queueRenderSceneGeometry(); } } -static QString decideStatusText(const collision_detection::CollisionWorld::ObjectConstPtr& obj) +static QString decideStatusText(const collision_detection::CollisionEnv::ObjectConstPtr& obj) { QString status_text = "'" + QString::fromStdString(obj->id_) + "' is a collision object with "; if (obj->shapes_.empty()) @@ -174,14 +242,23 @@ static QString decideStatusText(const collision_detection::CollisionWorld::Objec for (const QString& shape_name : shape_names) status_text += " " + shape_name; } + status_text += "."; + } + if (!obj->subframe_poses_.empty()) + { + status_text += subframe_poses_to_qstring(obj->subframe_poses_); } return status_text; } -static QString decideStatusText(const robot_state::AttachedBody* attached_body) +static QString decideStatusText(const moveit::core::AttachedBody* attached_body) { QString status_text = "'" + QString::fromStdString(attached_body->getName()) + "' is attached to '" + - QString::fromStdString(attached_body->getAttachedLinkName()) + "'"; + QString::fromStdString(attached_body->getAttachedLinkName()) + "'."; + if (!attached_body->getSubframeTransforms().empty()) + { + status_text += subframe_poses_to_qstring(attached_body->getSubframeTransforms()); + } return status_text; } @@ -216,19 +293,19 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() ui_->object_status->setText(""); scene_marker_.reset(); - ui_->scene_scale->setEnabled(false); + ui_->pose_scale_group_box->setEnabled(false); } else if (planning_display_->getPlanningSceneMonitor()) { // if this is a CollisionWorld element if (sel[0]->checkState() == Qt::Unchecked) { - ui_->scene_scale->setEnabled(true); + ui_->pose_scale_group_box->setEnabled(true); bool update_scene_marker = false; Eigen::Isometry3d obj_pose; { const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); - const collision_detection::CollisionWorld::ObjectConstPtr& obj = + const collision_detection::CollisionEnv::ObjectConstPtr& obj = ps->getWorld()->getObject(sel[0]->text().toStdString()); if (obj) { @@ -236,8 +313,8 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() if (obj->shapes_.size() == 1) { - obj_pose = obj->shape_poses_[0]; - Eigen::Vector3d xyz = obj_pose.rotation().eulerAngles(0, 1, 2); + obj_pose = obj->shape_poses_[0]; // valid isometry by contract + Eigen::Vector3d xyz = obj_pose.linear().eulerAngles(0, 1, 2); update_scene_marker = true; // do the marker update outside locked scope to avoid deadlock bool old_state = ui_->object_x->blockSignals(true); @@ -275,11 +352,11 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() } else { - ui_->scene_scale->setEnabled(false); + ui_->pose_scale_group_box->setEnabled(false); // if it is an attached object scene_marker_.reset(); const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); - const robot_state::AttachedBody* attached_body = + const moveit::core::AttachedBody* attached_body = ps->getCurrentState().getAttachedBody(sel[0]->text().toStdString()); if (attached_body) ui_->object_status->setText(decideStatusText(attached_body)); @@ -302,7 +379,7 @@ void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position) planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); if (ps) { - collision_detection::CollisionWorld::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString()); + collision_detection::CollisionEnv::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString()); if (obj && obj->shapes_.size() == 1) { Eigen::Isometry3d p; @@ -317,11 +394,13 @@ void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position) ps->getWorldNonConst()->moveShapeInObject(obj->id_, obj->shapes_[0], p); planning_display_->queueRenderSceneGeometry(); + setLocalSceneEdited(); // Update the interactive marker pose to match the manually introduced one if (update_marker_position && scene_marker_) { - Eigen::Quaterniond eq(p.rotation()); + // p is isometry by construction + Eigen::Quaterniond eq(p.linear()); scene_marker_->setPose(Ogre::Vector3(ui_->object_x->value(), ui_->object_y->value(), ui_->object_z->value()), Ogre::Quaternion(eq.w(), eq.x(), eq.y(), eq.z()), ""); } @@ -392,7 +471,7 @@ void MotionPlanningFrame::copySelectedCollisionObject() for (const QListWidgetItem* item : sel) { std::string name = item->text().toStdString(); - collision_detection::CollisionWorld::ObjectConstPtr obj = ps->getWorld()->getObject(name); + collision_detection::CollisionEnv::ObjectConstPtr obj = ps->getWorld()->getObject(name); if (!obj) continue; @@ -409,6 +488,7 @@ void MotionPlanningFrame::copySelectedCollisionObject() ps->getWorldNonConst()->addToObject(name, obj->shapes_, obj->shape_poses_); ROS_DEBUG("Copied collision object to '%s'", name.c_str()); } + setLocalSceneEdited(); planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); } @@ -640,12 +720,13 @@ void MotionPlanningFrame::computeLoadQueryButtonClicked() if (got_q) { - robot_state::RobotStatePtr start_state(new robot_state::RobotState(*planning_display_->getQueryStartState())); - robot_state::robotStateMsgToRobotState(planning_display_->getPlanningSceneRO()->getTransforms(), - mp->start_state, *start_state); + moveit::core::RobotStatePtr start_state( + new moveit::core::RobotState(*planning_display_->getQueryStartState())); + moveit::core::robotStateMsgToRobotState(planning_display_->getPlanningSceneRO()->getTransforms(), + mp->start_state, *start_state); planning_display_->setQueryStartState(*start_state); - robot_state::RobotStatePtr goal_state(new robot_state::RobotState(*planning_display_->getQueryGoalState())); + moveit::core::RobotStatePtr goal_state(new moveit::core::RobotState(*planning_display_->getQueryGoalState())); for (const moveit_msgs::Constraints& goal_constraint : mp->goal_constraints) if (!goal_constraint.joint_constraints.empty()) { @@ -665,18 +746,22 @@ void MotionPlanningFrame::computeLoadQueryButtonClicked() } } -void MotionPlanningFrame::addObject(const collision_detection::WorldPtr& world, const std::string& id, - const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& pose) +visualization_msgs::InteractiveMarker +MotionPlanningFrame::createObjectMarkerMsg(const collision_detection::CollisionEnv::ObjectConstPtr& obj) { - world->addToObject(id, shape, pose); - - planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); - - // Automatically select the inserted object so that its IM is displayed - planning_display_->addMainLoopJob( - boost::bind(&MotionPlanningFrame::setItemSelectionInList, this, id, true, ui_->collision_objects_list)); - - planning_display_->queueRenderSceneGeometry(); + Eigen::Vector3d center; + double scale; + shapes::computeShapeBoundingSphere(obj->shapes_[0].get(), center, scale); + geometry_msgs::PoseStamped shape_pose = tf2::toMsg(tf2::Stamped( + obj->shape_poses_[0], ros::Time(), planning_display_->getRobotModel()->getModelFrame())); + scale = (scale + center.cwiseAbs().maxCoeff()) * 2.0 * 1.2; // add padding of 20% size + + // create an interactive marker msg for the given shape + visualization_msgs::InteractiveMarker imarker = + robot_interaction::make6DOFMarker("marker_scene_object", shape_pose, scale); + imarker.description = obj->id_; + interactive_markers::autoComplete(imarker); + return imarker; } void MotionPlanningFrame::createSceneInteractiveMarker() @@ -689,34 +774,16 @@ void MotionPlanningFrame::createSceneInteractiveMarker() if (!ps) return; - const collision_detection::CollisionWorld::ObjectConstPtr& obj = + const collision_detection::CollisionEnv::ObjectConstPtr& obj = ps->getWorld()->getObject(sel[0]->text().toStdString()); if (obj && obj->shapes_.size() == 1) { - Eigen::Quaterniond eq(obj->shape_poses_[0].rotation()); - geometry_msgs::PoseStamped shape_pose; - shape_pose.pose.position.x = obj->shape_poses_[0].translation()[0]; - shape_pose.pose.position.y = obj->shape_poses_[0].translation()[1]; - shape_pose.pose.position.z = obj->shape_poses_[0].translation()[2]; - shape_pose.pose.orientation.x = eq.x(); - shape_pose.pose.orientation.y = eq.y(); - shape_pose.pose.orientation.z = eq.z(); - shape_pose.pose.orientation.w = eq.w(); - - // create an interactive marker for moving the shape in the world - visualization_msgs::InteractiveMarker int_marker = - robot_interaction::make6DOFMarker(std::string("marker_") + sel[0]->text().toStdString(), shape_pose, 1.0); - int_marker.header.frame_id = planning_display_->getRobotModel()->getModelFrame(); - int_marker.description = sel[0]->text().toStdString(); - - rviz::InteractiveMarker* imarker = new rviz::InteractiveMarker(planning_display_->getSceneNode(), context_); - interactive_markers::autoComplete(int_marker); - imarker->processMessage(int_marker); - imarker->setShowAxes(false); - scene_marker_.reset(imarker); + scene_marker_ = std::make_shared(planning_display_->getSceneNode(), context_); + scene_marker_->processMessage(createObjectMarkerMsg(obj)); + scene_marker_->setShowAxes(false); // Connect signals - connect(imarker, SIGNAL(userFeedback(visualization_msgs::InteractiveMarkerFeedback&)), this, + connect(scene_marker_.get(), SIGNAL(userFeedback(visualization_msgs::InteractiveMarkerFeedback&)), this, SLOT(imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback&))); } else @@ -755,14 +822,16 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item) if (item->checkState() == Qt::Unchecked) { planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); - collision_detection::CollisionWorld::ObjectConstPtr obj = + collision_detection::CollisionEnv::ObjectConstPtr obj = ps->getWorld()->getObject(known_collision_objects_[item->type()].first); if (obj) { known_collision_objects_[item->type()].first = item_text; + const moveit::core::FixedTransformsMap subframes = obj->subframe_poses_; // Keep subframes + // TODO(felixvd): Scale the subframes with the object ps->getWorldNonConst()->removeObject(obj->id_); - ps->getWorldNonConst()->addToObject(known_collision_objects_[item->type()].first, obj->shapes_, - obj->shape_poses_); + ps->getWorldNonConst()->addToObject(known_collision_objects_[item->type()].first, obj->shapes_, obj->shape_poses_); + ps->getWorldNonConst()->setSubframesOfObject(obj->id_, subframes); if (scene_marker_) { scene_marker_.reset(); @@ -774,18 +843,20 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item) { // rename attached body planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); - robot_state::RobotState& cs = ps->getCurrentStateNonConst(); - const robot_state::AttachedBody* ab = cs.getAttachedBody(known_collision_objects_[item->type()].first); + moveit::core::RobotState& cs = ps->getCurrentStateNonConst(); + const moveit::core::AttachedBody* ab = cs.getAttachedBody(known_collision_objects_[item->type()].first); if (ab) { known_collision_objects_[item->type()].first = item_text; - robot_state::AttachedBody* new_ab = new robot_state::AttachedBody( - ab->getAttachedLink(), known_collision_objects_[item->type()].first, ab->getShapes(), - ab->getFixedTransforms(), ab->getTouchLinks(), ab->getDetachPosture(), ab->getSubframeTransforms()); + moveit::core::AttachedBody* new_ab = + new moveit::core::AttachedBody(ab->getAttachedLink(), known_collision_objects_[item->type()].first, + ab->getShapes(), ab->getFixedTransforms(), ab->getTouchLinks(), + ab->getDetachPosture(), ab->getSubframeTransforms()); cs.clearAttachedBody(ab->getName()); cs.attachBody(new_ab); } } + setLocalSceneEdited(); } void MotionPlanningFrame::attachDetachCollisionObject(QListWidgetItem* item) @@ -817,7 +888,7 @@ void MotionPlanningFrame::attachDetachCollisionObject(QListWidgetItem* item) else // we need to detach an attached object { const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); - const robot_state::AttachedBody* attached_body = ps->getCurrentState().getAttachedBody(data.first); + const moveit::core::AttachedBody* attached_body = ps->getCurrentState().getAttachedBody(data.first); if (attached_body) { aco.link_name = attached_body->getAttachedLinkName(); @@ -825,6 +896,8 @@ void MotionPlanningFrame::attachDetachCollisionObject(QListWidgetItem* item) aco.object.operation = moveit_msgs::CollisionObject::REMOVE; } } + + moveit::core::RobotState rs(planning_display_->getRobotModel()); { planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); // we loop through the list in case updates were received since the start of the function @@ -835,9 +908,12 @@ void MotionPlanningFrame::attachDetachCollisionObject(QListWidgetItem* item) break; } ps->processAttachedCollisionObjectMsg(aco); + rs = ps->getCurrentState(); } selectedCollisionObjectChanged(); + setLocalSceneEdited(); + planning_display_->updateQueryStates(rs); planning_display_->queueRenderSceneGeometry(); } @@ -845,6 +921,7 @@ void MotionPlanningFrame::populateCollisionObjectsList() { ui_->collision_objects_list->setUpdatesEnabled(false); bool old_state = ui_->collision_objects_list->blockSignals(true); + bool octomap_in_scene = false; { QList sel = ui_->collision_objects_list->selectedItems(); @@ -862,7 +939,10 @@ void MotionPlanningFrame::populateCollisionObjectsList() for (std::size_t i = 0; i < collision_object_names.size(); ++i) { if (collision_object_names[i] == planning_scene::PlanningScene::OCTOMAP_NS) + { + octomap_in_scene = true; continue; + } QListWidgetItem* item = new QListWidgetItem(QString::fromStdString(collision_object_names[i]), ui_->collision_objects_list, (int)i); @@ -875,8 +955,8 @@ void MotionPlanningFrame::populateCollisionObjectsList() known_collision_objects_.push_back(std::make_pair(collision_object_names[i], false)); } - const robot_state::RobotState& cs = ps->getCurrentState(); - std::vector attached_bodies; + const moveit::core::RobotState& cs = ps->getCurrentState(); + std::vector attached_bodies; cs.getAttachedBodies(attached_bodies); for (std::size_t i = 0; i < attached_bodies.size(); ++i) { @@ -894,21 +974,22 @@ void MotionPlanningFrame::populateCollisionObjectsList() } } + ui_->clear_octomap_button->setEnabled(octomap_in_scene); ui_->collision_objects_list->blockSignals(old_state); ui_->collision_objects_list->setUpdatesEnabled(true); selectedCollisionObjectChanged(); } -void MotionPlanningFrame::exportAsTextButtonClicked() +void MotionPlanningFrame::exportGeometryAsTextButtonClicked() { QString path = QFileDialog::getSaveFileName(this, tr("Export Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)")); if (!path.isEmpty()) planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeExportAsText, this, path.toStdString()), "export as text"); + boost::bind(&MotionPlanningFrame::computeExportGeometryAsText, this, path.toStdString()), "export as text"); } -void MotionPlanningFrame::computeExportAsText(const std::string& path) +void MotionPlanningFrame::computeExportGeometryAsText(const std::string& path) { planning_scene_monitor::LockedPlanningSceneRO ps = planning_display_->getPlanningSceneRO(); if (ps) @@ -926,7 +1007,7 @@ void MotionPlanningFrame::computeExportAsText(const std::string& path) } } -void MotionPlanningFrame::computeImportFromText(const std::string& path) +void MotionPlanningFrame::computeImportGeometryFromText(const std::string& path) { planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW(); if (ps) @@ -937,21 +1018,23 @@ void MotionPlanningFrame::computeImportFromText(const std::string& path) ROS_INFO("Loaded scene geometry from '%s'", path.c_str()); planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this)); planning_display_->queueRenderSceneGeometry(); + setLocalSceneEdited(); } else { - QMessageBox::warning(nullptr, "Loading scene geometry", "Failed to load scene geometry.\n" - "See console output for more details."); + QMessageBox::warning(nullptr, "Loading scene geometry", + "Failed to load scene geometry.\n" + "See console output for more details."); } } } -void MotionPlanningFrame::importFromTextButtonClicked() +void MotionPlanningFrame::importGeometryFromTextButtonClicked() { QString path = QFileDialog::getOpenFileName(this, tr("Import Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)")); if (!path.isEmpty()) planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::computeImportFromText, this, path.toStdString()), "import from text"); + boost::bind(&MotionPlanningFrame::computeImportGeometryFromText, this, path.toStdString()), "import from text"); } } // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 7febadd195..f0b32d8935 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -52,6 +52,7 @@ namespace moveit_rviz_plugin { void MotionPlanningFrame::planButtonClicked() { + publishSceneIfNeeded(); planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanButtonClicked, this), "compute plan"); } @@ -65,6 +66,7 @@ void MotionPlanningFrame::executeButtonClicked() void MotionPlanningFrame::planAndExecuteButtonClicked() { + publishSceneIfNeeded(); ui_->plan_and_execute_button->setEnabled(false); ui_->execute_button->setEnabled(false); // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution @@ -108,16 +110,17 @@ void MotionPlanningFrame::onClearOctomapClicked() { std_srvs::Empty srv; clear_octomap_service_client_.call(srv); + ui_->clear_octomap_button->setEnabled(false); } bool MotionPlanningFrame::computeCartesianPlan() { ros::WallTime start = ros::WallTime::now(); // get goal pose - robot_state::RobotState goal = *planning_display_->getQueryGoalState(); + moveit::core::RobotState goal = *planning_display_->getQueryGoalState(); std::vector waypoints; const std::string& link_name = move_group_->getEndEffectorLink(); - const robot_model::LinkModel* link = move_group_->getRobotModel()->getLinkModel(link_name); + const moveit::core::LinkModel* link = move_group_->getRobotModel()->getLinkModel(link_name); if (!link) { ROS_ERROR_STREAM("Failed to determine unique end-effector link: " << link_name); @@ -172,6 +175,7 @@ void MotionPlanningFrame::computePlanButtonClicked() ui_->result_label->setText("Planning..."); configureForPlanning(); + planning_display_->rememberPreviousStartState(); bool success = (ui_->use_cartesian_path->isEnabled() && ui_->use_cartesian_path->checkState()) ? computeCartesianPlan() : computeJointSpacePlan(); @@ -206,6 +210,7 @@ void MotionPlanningFrame::computePlanAndExecuteButtonClicked() if (!move_group_) return; configureForPlanning(); + planning_display_->rememberPreviousStartState(); // move_group::move() on the server side, will always start from the current state // to suppress a warning, we pass an empty state (which encodes "start from current state") move_group_->setStartStateToCurrentState(); @@ -242,19 +247,36 @@ void MotionPlanningFrame::onFinishedExecution(bool success) // update query start state to current if neccessary if (ui_->start_state_combo_box->currentText() == "") startStateTextChanged(ui_->start_state_combo_box->currentText()); + + // auto-update goal to stored previous state (but only on success) + // on failure, the user must update the goal to the previous state himself + if (ui_->goal_state_combo_box->currentText() == "") + goalStateTextChanged(ui_->goal_state_combo_box->currentText()); +} + +void MotionPlanningFrame::onNewPlanningSceneState() +{ + moveit::core::RobotState current(planning_display_->getPlanningSceneRO()->getCurrentState()); + if (ui_->start_state_combo_box->currentText() == "") + { + planning_display_->setQueryStartState(current); + planning_display_->rememberPreviousStartState(); + } + if (ui_->goal_state_combo_box->currentText() == "") + planning_display_->setQueryGoalState(current); } void MotionPlanningFrame::startStateTextChanged(const QString& start_state) { // use background job: fetching the current state might take up to a second - planning_display_->addBackgroundJob( - boost::bind(&MotionPlanningFrame::startStateTextChangedExec, this, start_state.toStdString()), - "update start state"); + planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::startStateTextChangedExec, this, + start_state.toStdString()), + "update start state"); } void MotionPlanningFrame::startStateTextChangedExec(const std::string& start_state) { - robot_state::RobotState start = *planning_display_->getQueryStartState(); + moveit::core::RobotState start = *planning_display_->getQueryStartState(); updateQueryStateHelper(start, start_state); planning_display_->setQueryStartState(start); } @@ -268,7 +290,7 @@ void MotionPlanningFrame::goalStateTextChanged(const QString& goal_state) void MotionPlanningFrame::goalStateTextChangedExec(const std::string& goal_state) { - robot_state::RobotState goal = *planning_display_->getQueryGoalState(); + moveit::core::RobotState goal = *planning_display_->getQueryGoalState(); updateQueryStateHelper(goal, goal_state); planning_display_->setQueryGoalState(goal); } @@ -278,12 +300,12 @@ void MotionPlanningFrame::planningGroupTextChanged(const QString& planning_group planning_display_->changePlanningGroup(planning_group.toStdString()); } -void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState& state, const std::string& v) +void MotionPlanningFrame::updateQueryStateHelper(moveit::core::RobotState& state, const std::string& v) { if (v == "") { configureWorkspace(); - if (const robot_model::JointModelGroup* jmg = + if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) state.setToRandomPositions(jmg); return; @@ -293,7 +315,7 @@ void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState& state, { configureWorkspace(); - if (const robot_model::JointModelGroup* jmg = + if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) { // Loop until a collision free state is found @@ -344,8 +366,14 @@ void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState& state, return; } + if (v == "") + { + state = planning_display_->getPreviousState(); + return; + } + // maybe it is a named state - if (const robot_model::JointModelGroup* jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) + if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) state.setToDefaultValues(jmg, v); } @@ -413,15 +441,15 @@ void MotionPlanningFrame::constructPlanningRequest(moveit_msgs::MotionPlanReques mreq.allowed_planning_time = ui_->planning_time->value(); mreq.max_velocity_scaling_factor = ui_->velocity_scaling_factor->value(); mreq.max_acceleration_scaling_factor = ui_->acceleration_scaling_factor->value(); - robot_state::robotStateToRobotStateMsg(*planning_display_->getQueryStartState(), mreq.start_state); + moveit::core::robotStateToRobotStateMsg(*planning_display_->getQueryStartState(), mreq.start_state); mreq.workspace_parameters.min_corner.x = ui_->wcenter_x->value() - ui_->wsize_x->value() / 2.0; mreq.workspace_parameters.min_corner.y = ui_->wcenter_y->value() - ui_->wsize_y->value() / 2.0; mreq.workspace_parameters.min_corner.z = ui_->wcenter_z->value() - ui_->wsize_z->value() / 2.0; mreq.workspace_parameters.max_corner.x = ui_->wcenter_x->value() + ui_->wsize_x->value() / 2.0; mreq.workspace_parameters.max_corner.y = ui_->wcenter_y->value() + ui_->wsize_y->value() / 2.0; mreq.workspace_parameters.max_corner.z = ui_->wcenter_z->value() + ui_->wsize_z->value() / 2.0; - robot_state::RobotStateConstPtr s = planning_display_->getQueryGoalState(); - const robot_state::JointModelGroup* jmg = s->getJointModelGroup(mreq.group_name); + moveit::core::RobotStateConstPtr s = planning_display_->getQueryGoalState(); + const moveit::core::JointModelGroup* jmg = s->getJointModelGroup(mreq.group_name); if (jmg) { mreq.goal_constraints.resize(1); @@ -431,10 +459,10 @@ void MotionPlanningFrame::constructPlanningRequest(moveit_msgs::MotionPlanReques void MotionPlanningFrame::configureWorkspace() { - robot_model::VariableBounds bx, by, bz; + moveit::core::VariableBounds bx, by, bz; bx.position_bounded_ = by.position_bounded_ = bz.position_bounded_ = true; - robot_model::JointModel::Bounds b(3); + moveit::core::JointModel::Bounds b(3); bx.min_position_ = ui_->wcenter_x->value() - ui_->wsize_x->value() / 2.0; bx.max_position_ = ui_->wcenter_x->value() + ui_->wsize_x->value() / 2.0; by.min_position_ = ui_->wcenter_y->value() - ui_->wsize_y->value() / 2.0; @@ -449,15 +477,15 @@ void MotionPlanningFrame::configureWorkspace() // get non-const access to the robot_model and update planar & floating joints as indicated by the workspace settings if (psm && psm->getRobotModelLoader() && psm->getRobotModelLoader()->getModel()) { - const robot_model::RobotModelPtr& robot_model = psm->getRobotModelLoader()->getModel(); - const std::vector& jm = robot_model->getJointModels(); + const moveit::core::RobotModelPtr& robot_model = psm->getRobotModelLoader()->getModel(); + const std::vector& jm = robot_model->getJointModels(); for (moveit::core::JointModel* joint : jm) - if (joint->getType() == robot_model::JointModel::PLANAR) + if (joint->getType() == moveit::core::JointModel::PLANAR) { joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[0], bx); joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[1], by); } - else if (joint->getType() == robot_model::JointModel::FLOATING) + else if (joint->getType() == moveit::core::JointModel::FLOATING) { joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[0], bx); joint->setVariableBounds(joint->getName() + "/" + joint->getLocalVariableNames()[1], by); @@ -479,22 +507,22 @@ void MotionPlanningFrame::configureForPlanning() planning_display_->dropVisualizedTrajectory(); } -void MotionPlanningFrame::remotePlanCallback(const std_msgs::EmptyConstPtr& msg) +void MotionPlanningFrame::remotePlanCallback(const std_msgs::EmptyConstPtr& /*msg*/) { planButtonClicked(); } -void MotionPlanningFrame::remoteExecuteCallback(const std_msgs::EmptyConstPtr& msg) +void MotionPlanningFrame::remoteExecuteCallback(const std_msgs::EmptyConstPtr& /*msg*/) { executeButtonClicked(); } -void MotionPlanningFrame::remoteStopCallback(const std_msgs::EmptyConstPtr& msg) +void MotionPlanningFrame::remoteStopCallback(const std_msgs::EmptyConstPtr& /*msg*/) { stopButtonClicked(); } -void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr& msg) +void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr& /*msg*/) { if (move_group_ && planning_display_) { @@ -502,13 +530,13 @@ void MotionPlanningFrame::remoteUpdateStartStateCallback(const std_msgs::EmptyCo const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (ps) { - robot_state::RobotState state = ps->getCurrentState(); + moveit::core::RobotState state = ps->getCurrentState(); planning_display_->setQueryStartState(state); } } } -void MotionPlanningFrame::remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr& msg) +void MotionPlanningFrame::remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr& /*msg*/) { if (move_group_ && planning_display_) { @@ -516,7 +544,7 @@ void MotionPlanningFrame::remoteUpdateGoalStateCallback(const std_msgs::EmptyCon const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (ps) { - robot_state::RobotState state = ps->getCurrentState(); + moveit::core::RobotState state = ps->getCurrentState(); planning_display_->setQueryGoalState(state); } } @@ -533,8 +561,8 @@ void MotionPlanningFrame::remoteUpdateCustomStartStateCallback(const moveit_msgs const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (ps) { - robot_state::RobotStatePtr state(new robot_state::RobotState(ps->getCurrentState())); - robot_state::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state); + moveit::core::RobotStatePtr state(new moveit::core::RobotState(ps->getCurrentState())); + moveit::core::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state); planning_display_->setQueryStartState(*state); } } @@ -551,8 +579,8 @@ void MotionPlanningFrame::remoteUpdateCustomGoalStateCallback(const moveit_msgs: const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO(); if (ps) { - robot_state::RobotStatePtr state(new robot_state::RobotState(ps->getCurrentState())); - robot_state::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state); + moveit::core::RobotStatePtr state(new moveit::core::RobotState(ps->getCurrentState())); + moveit::core::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state); planning_display_->setQueryGoalState(*state); } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp index d2ab420ee3..e4a39f442f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp @@ -86,8 +86,9 @@ void MotionPlanningFrame::saveSceneButtonClicked() if (q->clickedButton() == rename.get()) { bool ok = false; - QString new_name = QInputDialog::getText(this, "Rename Planning Scene", "New name for the planning scene:", - QLineEdit::Normal, QString::fromStdString(name), &ok); + QString new_name = QInputDialog::getText(this, "Rename Planning Scene", + "New name for the planning scene:", QLineEdit::Normal, + QString::fromStdString(name), &ok); if (ok) { planning_display_->getPlanningSceneRW()->setName(new_name.toStdString()); @@ -161,9 +162,9 @@ void MotionPlanningFrame::saveQueryButtonClicked() if (q->clickedButton() == rename.get()) { bool ok = false; - QString new_name = - QInputDialog::getText(this, "Rename Planning Query", "New name for the planning query:", - QLineEdit::Normal, QString::fromStdString(query_name), &ok); + QString new_name = QInputDialog::getText(this, "Rename Planning Query", + "New name for the planning query:", QLineEdit::Normal, + QString::fromStdString(query_name), &ok); if (ok) query_name = new_name.toStdString(); else @@ -237,10 +238,11 @@ void MotionPlanningFrame::warehouseItemNameChanged(QTreeWidgetItem* item, int co if (planning_scene_storage->hasPlanningQuery(scene, new_name)) { planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this)); - QMessageBox::warning(this, "Query not renamed", QString("The query name '") - .append(item->text(column)) - .append("' already exists for scene ") - .append(item->parent()->text(0))); + QMessageBox::warning(this, "Query not renamed", + QString("The query name '") + .append(item->text(column)) + .append("' already exists for scene ") + .append(item->parent()->text(0))); return; } else diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp index 3e3982bb47..b8b47752a9 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp @@ -119,7 +119,7 @@ void MotionPlanningFrame::loadStoredStates(const std::string& pattern) populateRobotStatesList(); } -void MotionPlanningFrame::saveRobotStateButtonClicked(const robot_state::RobotState& state) +void MotionPlanningFrame::saveRobotStateButtonClicked(const moveit::core::RobotState& state) { bool ok = false; @@ -137,14 +137,13 @@ void MotionPlanningFrame::saveRobotStateButtonClicked(const robot_state::RobotSt { name = text.toStdString(); if (robot_states_.find(name) != robot_states_.end()) - QMessageBox::warning( - this, "Name already exists", - QString("The name '").append(name.c_str()).append("' already exists. Not creating state.")); + QMessageBox::warning(this, "Name already exists", + QString("The name '").append(name.c_str()).append("' already exists. Not creating state.")); else { // Store the current start state moveit_msgs::RobotState msg; - robot_state::robotStateToRobotStateMsg(state, msg); + moveit::core::robotStateToRobotStateMsg(state, msg); robot_states_.insert(RobotStatePair(name, msg)); // Save to the database if connected @@ -161,8 +160,7 @@ void MotionPlanningFrame::saveRobotStateButtonClicked(const robot_state::RobotSt } else { - QMessageBox::warning(this, "Warning", - "Not connected to a database. The state will be created but not stored"); + QMessageBox::warning(this, "Warning", "Not connected to a database. The state will be created but not stored"); } } } @@ -188,8 +186,8 @@ void MotionPlanningFrame::setAsStartStateButtonClicked() if (item) { - robot_state::RobotState robot_state(*planning_display_->getQueryStartState()); - robot_state::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state); + moveit::core::RobotState robot_state(*planning_display_->getQueryStartState()); + moveit::core::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state); planning_display_->setQueryStartState(robot_state); } } @@ -200,8 +198,8 @@ void MotionPlanningFrame::setAsGoalStateButtonClicked() if (item) { - robot_state::RobotState robot_state(*planning_display_->getQueryGoalState()); - robot_state::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state); + moveit::core::RobotState robot_state(*planning_display_->getQueryGoalState()); + moveit::core::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state); planning_display_->setQueryGoalState(robot_state); } } @@ -245,8 +243,21 @@ void MotionPlanningFrame::removeStateButtonClicked() void MotionPlanningFrame::clearStatesButtonClicked() { - robot_states_.clear(); - populateRobotStatesList(); + QMessageBox msg_box; + msg_box.setText("Clear all stored robot states (from memory, not from the database)?"); + msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); + msg_box.setDefaultButton(QMessageBox::Yes); + int ret = msg_box.exec(); + switch (ret) + { + case QMessageBox::Yes: + { + robot_states_.clear(); + populateRobotStatesList(); + } + break; + } + return; } } // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index b0debfae37..aa322f4a70 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -6,12 +6,12 @@ 0 0 - 692 - 414 + 507 + 380 - MoveIt! Planning Frame + MoveIt Planning Frame @@ -26,7 +26,7 @@ false - 0 + 1 @@ -96,6 +96,9 @@ + + false + 0 @@ -157,6 +160,12 @@ + + + 0 + 0 + + color:red @@ -355,6 +364,9 @@ + + 0 + @@ -369,6 +381,15 @@ 4 + + 6 + + + 6 + + + 6 + @@ -389,7 +410,7 @@ - Plan and E&xecute + Plan && E&xecute @@ -398,6 +419,9 @@ false + + Stop execution by sending a stop command to the move_group. + &Stop @@ -419,6 +443,16 @@ + + + + false + + + Clear octomap + + + @@ -427,17 +461,17 @@ Query - + - 4 + 6 - 4 + 6 - 4 + 6 - + @@ -451,7 +485,7 @@ - + @@ -465,7 +499,7 @@ - + @@ -479,273 +513,410 @@ - - - - Clear octomap - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - + + + + Qt::Vertical + + + QSizePolicy::Expanding + + + + 20 + 32 + + + + Path Constraints + + 6 + 4 + + 6 + + 6 + + + + + Select path constraints from the database. + + + + + + + + + + + + 0 + + + + + Options + + + 1 + + 4 + - + + + 1 + + + + + Total time allowed for planning. Planning stops if time or number of attempts is exceeded, or goal is reached. + + + Planning Time (s): + + + + + + + + 0 + 0 + + + + + 40 + 0 + + + + Total time allowed for planning. Planning stops if time or number of attempts is exceeded, or goal is reached. + + + 1 + + + 300.000000000000000 + + + 5.000000000000000 + + + + - + + + 1 + - + + + Allowed number of planning attempts. Planning stops if time or number of attempts is exceeded, or goal is reached. + - Goal Tolerance: + Planning Attempts: - + + + + 0 + 0 + + + + + 40 + 0 + + + + Allowed number of planning attempts. Planning stops if time or number of attempts is exceeded, or goal is reached. + + + 1000 + + + 10 + + - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - - - Options - - - - 2 - - - - - 1 - - - - Planning Time (s): + + + 1 - + + + + Factor (between 0 and 1) to scale down maximum joint velocities + + + Velocity Scaling: + + + + + + + + 40 + 0 + + + + Factor (between 0 and 1) to scale down maximum joint velocities + + + 1.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + + + 1 + + + + + Factor (between 0 and 1) to scale down maximum joint accelerations + + + Accel. Scaling: + + + + + + + + 40 + 0 + + + + Factor (between 0 and 1) to scale down maximum joint accelerations + + + 1.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + - + - + 0 0 - - 300.000000000000000 + + + 30 + 0 + - - 5.000000000000000 + + Check this to generate a linear path in Cartesian (3D) space. + + This does not plan around obstacles. - - - - - - - - 1 - - - - Planning Attempts: + Use Cartesian Path + + + false - + - + 0 0 - - 1000.000000000000000 + + + 30 + 0 + - - 10.000000000000000 + + If checked, the IK solver tries to avoid collisions when searching for a start/end state set via the interactive marker. + +This is usually achieved by random seeding, which can flip the robot configuration (e.g. elbow up/down). Note that motion planning is always collision-aware, regardless of this checkbox. - - - - - - - - 1 - - - - Velocity Scaling: + Collision-aware IK + + + false - - - 1.000000000000000 + + + + 0 + 0 + - - 0.010000000000000 + + + 30 + 0 + - - 1.000000000000000 + + Allow approximate IK solutions, which deviate from the target pose. Useful when an exact solution cannot be found, e.g. for underactuated arms or when trying to reach an unreachable target. - - - - - - - - 1 - - - - Acceleration Scaling: + Approx IK Solutions - - - 1.000000000000000 + + + + 0 + 0 + - - 0.010000000000000 + + + 30 + 0 + - - 1.000000000000000 + + Allow other nodes to control start and end poses as well as planning and execution, using ROS topics (/rviz/moveit/ ). - - - - - - - - Allow Replanning - - - false - - - - - - - Allow Sensor Positioning - - - false - - - - - - - Allow External Comm. - - - false - - - - - - - Use Cartesian Path - - - false - - - - - - - Use Collision-Aware IK - - - true - - - - - - - Allow Approx IK Solutions - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - + + External Comm. + + + false + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + EXPERIMENTAL: Trigger replanning if new collisions are detected during plan execution. Only works for the "Plan & Execute" button. + + + false + + + Replanning + + + false + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + EXPERIMENTAL: Allows the robot to reposition its sensor to look around to resolve apparent collisions. A sensor needs to be set up and the new measurements have to affect the collision scene, otherwise it has no effect. See tutorial. + + + Sensor Positioning + + + false + + + + + + + @@ -824,25 +995,20 @@ ROI - - - - 0.010000000000000 - - - 1.500000000000000 - - - - - - - Size (m): - - - + + + 0 + 0 + + + + + 40 + 0 + + -99.000000000000000 @@ -851,48 +1017,93 @@ - - + + + + + 0 + 0 + + + + + 40 + 0 + + 0.010000000000000 - 1.200000000000000 + 4.000000000000000 - - - - Center (m): + + + + + 0 + 0 + + + + + 40 + 0 + - - - - 0.010000000000000 - 4.000000000000000 + 1.200000000000000 - - - - -99.000000000000000 + + + + + 0 + 0 + + + + + 40 + 0 + 0.010000000000000 - 0.600000000000000 + 1.500000000000000 + + + + + + + Size (m): + + + 0 + 0 + + + + + 40 + 0 + + -99.000000000000000 @@ -901,31 +1112,37 @@ - - - - Qt::Horizontal - - - - 40 - 20 - + + + + Center (m): - + - - - - Qt::Horizontal + + + + + 0 + 0 + - + 40 - 20 + 0 - + + -99.000000000000000 + + + 0.010000000000000 + + + 0.600000000000000 + + @@ -936,299 +1153,601 @@ Scene Objects - - - - - Object Status - - - - - - false + + + + + + + Current Scene Objects + + + + 6 - - QFrame::StyledPanel + + 6 - - QFrame::Raised + + 6 - - Status + + 6 - - Qt::AutoText + + + + + 0 + 0 + + + + Press Ctrl+C to duplicate an object + + + QAbstractItemView::DoubleClicked|QAbstractItemView::EditKeyPressed + + + + + + + + + + + 0 + 0 + + + + Add/Remove scene object(s) + + + + 6 - - - - - - - - - Manage Pose and Scale - - - - + + 6 + + + 6 + + + 6 + + + + + + + + 0 + 0 + + + + + 20 + 0 + + + + x dimension + + + 0.050000000000000 + + + 0.200000000000000 + + + + + + + + 0 + 0 + + + + + 20 + 0 + + + + y dimension + + + 0.050000000000000 + + + 0.200000000000000 + + + + + + + + 0 + 0 + + + + + 20 + 0 + + + + z dimension + + + 0.050000000000000 + + + 0.200000000000000 + + + + + + + + + + + 30 + 0 + + + + + + + + + 0 + 0 + + + + Add object at scene's origin + + + Add + + + + :/icons/list-add.png:/icons/list-add.png + + + + + + + + 0 + 0 + + + + Remove selected object + + + Del + + + + :/icons/list-remove.png:/icons/list-remove.png + + + + + + + + 0 + 0 + + + + Clear planning scene + + + Clr + + + + :/icons/edit-clear.png:/icons/edit-clear.png + + + + + + + + + + + + + + + + Change the pose and scale of the selected object + + + Change object pose/scale + + + + 6 + + + 6 + + + 6 + + + 6 + + + 6 + + - Position (XYZ): + Position: - + + + + Rotation: + + + + + + + 20 + 0 + + - -1000.000000000000000 + -999.990000000000009 - 1000.000000000000000 + 999.990000000000009 + + + 0.100000000000000 - - + + + + + 20 + 0 + + - -1000.000000000000000 + -3.140000000000000 - 1000.000000000000000 + 3.140000000000000 + + + 0.100000000000000 - - + + + + + 20 + 0 + + - -1000.000000000000000 + -3.140000000000000 - 1000.000000000000000 + 3.140000000000000 + + + 0.100000000000000 - - - - - - - - Rotation (RPY): + + + + + 20 + 0 + + + + -3.140000000000000 + + + 3.140000000000000 + + + 0.100000000000000 - - + + + + + 20 + 0 + + - -3.140000000000000 + -999.990000000000009 - 3.140000000000000 + 999.990000000000009 - 0.200000000000000 + 0.100000000000000 - - + + + + + + Scale: + + + + + + + 0% + + + + + + + + 40 + 0 + + + + 200 + + + 100 + + + 100 + + + Qt::Horizontal + + + false + + + QSlider::NoTicks + + + 0 + + + + + + + 200% + + + + + + + + + + 20 + 0 + + - -3.140000000000000 + -999.990000000000009 - 3.140000000000000 + 999.990000000000009 - 0.200000000000000 + 0.100000000000000 + + + + + + + Describes the selected object's pose, geometry and subframes. + + + Object status + + + + 6 + + + 6 + + + 6 + + + 6 + - - - -3.140000000000000 + + + true - - 3.140000000000000 + + QFrame::NoFrame - - 0.200000000000000 + + QFrame::Raised + + + Status + + + Qt::AutoText + + + true + + + true - - - + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Scene Geometry + + + + 6 + + + 6 + + + 6 + + + 6 + - + + + + 0 + 0 + + + + + 40 + 0 + + - Scale: + &Publish - - - 0% + + + Qt::Horizontal - + + + 40 + 20 + + + - + + + + 0 + 0 + + - 160 + 40 0 - - 200 - - - 100 - - - 100 - - - Qt::Horizontal - - - false - - - QSlider::NoTicks + + Export scene geometry to a .scene file (only geometry information is preserved) - - 0 + + &Export - + + + + 0 + 0 + + + + + 40 + 0 + + + + Import scene geometry from a .scene file (only geometry information is preserved) + - 200% + &Import - - - - - - - - Scene Geometry - - - - - - - 0 - 0 - - - - &Publish Scene - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - &Export As Text - - - - - - - &Import From Text - - - - - - - - - - Press Ctrl+C to duplicate an object - - - Current Scene Objects - - - - - - Import &URL - - - - - - - Clear - - - - - - - Remove - - - - - - - Import &File - - - - - - - QAbstractItemView::DoubleClicked|QAbstractItemView::EditKeyPressed - - - - - - - - - - Qt::Vertical - - - - 20 - 40 - - - + + + @@ -1387,18 +1906,24 @@ - Stored States + Stored Robot States + + Load a robot state + - Filter + Load + + Clear robot states from list + Clear @@ -1410,7 +1935,7 @@ - Selected State + Selected Robot State @@ -1440,11 +1965,14 @@ - Current State + Current Robot State + + Save the current start state + Save Start @@ -1452,6 +1980,9 @@ + + Save the current goal state + Save Goal @@ -1619,25 +2150,35 @@ database_port reset_db_button database_connect_button + wcenter_x + wcenter_y + wcenter_z + wsize_x + wsize_y + wsize_z plan_button execute_button plan_and_execute_button stop_button + clear_octomap_button planning_group_combo_box start_state_combo_box goal_state_combo_box - clear_octomap_button + path_constraints_combo_box planning_time planning_attempts velocity_scaling_factor acceleration_scaling_factor - allow_replanning - allow_looking - allow_external_program + use_cartesian_path collision_aware_ik approximate_ik - path_constraints_combo_box - goal_tolerance + allow_external_program + allow_replanning + allow_looking + detected_objects_list + detect_objects_button + support_surfaces_list + pick_button place_button roi_center_x roi_center_y @@ -1646,8 +2187,11 @@ roi_size_y roi_size_z collision_objects_list - import_file_button - import_url_button + shape_size_x_spin_box + shape_size_z_spin_box + shape_size_y_spin_box + shapes_combo_box + add_object_button remove_object_button clear_scene_button object_x @@ -1658,8 +2202,8 @@ object_rz scene_scale publish_current_scene_button - export_scene_text_button - import_scene_text_button + export_scene_geometry_text_button + import_scene_geometry_text_button planning_scene_tree load_scene_button load_query_button @@ -1677,16 +2221,6 @@ save_goal_state_button status_text tabWidget - wcenter_x - wcenter_y - wcenter_z - wsize_x - wsize_y - wsize_z - pick_button - support_surfaces_list - detected_objects_list - detect_objects_button diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui new file mode 100644 index 0000000000..389365e9bd --- /dev/null +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui @@ -0,0 +1,61 @@ + + + MotionPlanningFrameJointsUI + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + + Group joints: + + + + + + + QAbstractItemView::EditKeyPressed + + + QAbstractItemView::NoSelection + + + false + + + false + + + false + + + + + + + Nullspace exploration: + + + + + + + + + + + + + diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index 7b93f92cb1..cb462642ff 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -1,7 +1,7 @@ moveit_ros_visualization - 1.0.1 - Components of MoveIt! that offer visualization + 1.1.1 + Components of MoveIt that offer visualization Ioan Sucan Dave Coleman @@ -9,7 +9,7 @@ Jon Binney Michael Ferguson - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt index 77a06066ac..a5d5a1d205 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt @@ -4,7 +4,7 @@ add_library(${MOVEIT_LIB_NAME}_core src/planning_scene_display.cpp include/moveit/planning_scene_rviz_plugin/planning_scene_display.h) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools ${catkin_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools ${catkin_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) add_library(${MOVEIT_LIB_NAME} src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -14,4 +14,5 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index b18c4a0140..ff32a3e5d4 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_PLUGIN_SCENE_DISPLAY_ -#define MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_PLUGIN_SCENE_DISPLAY_ +#pragma once #include @@ -61,7 +60,7 @@ class FloatProperty; class RosTopicProperty; class ColorProperty; class EnumProperty; -} +} // namespace rviz namespace moveit_rviz_plugin { @@ -103,7 +102,7 @@ class PlanningSceneDisplay : public rviz::Display void clearJobs(); const std::string getMoveGroupNS() const; - const robot_model::RobotModelConstPtr& getRobotModel() const; + const moveit::core::RobotModelConstPtr& getRobotModel() const; /// wait for robot state more recent than t bool waitForCurrentRobotState(const ros::Time& t = ros::Time::now()); @@ -131,6 +130,7 @@ private Q_SLOTS: void changedSceneDisplayTime(); void changedOctreeRenderMode(); void changedOctreeColorMode(); + void setSceneName(const QString& name); protected Q_SLOTS: virtual void changedAttachedBodyColor(); @@ -150,6 +150,8 @@ protected Q_SLOTS: /// This is an event called by loadRobotModel() in the MainLoop; do not call directly virtual void onRobotModelLoaded(); + /// This is called upon successful retrieval of the (initial) planning scene state + virtual void onNewPlanningSceneState(); /** * \brief Set the scene node's position, given the target frame and the planning frame @@ -190,7 +192,10 @@ protected Q_SLOTS: RobotStateVisualizationPtr planning_scene_robot_; PlanningSceneRenderPtr planning_scene_render_; + // full update required bool planning_scene_needs_render_; + // or only the robot position (excluding attached object changes) + bool robot_state_needs_render_; float current_scene_time_; rviz::Property* scene_category_; @@ -213,5 +218,3 @@ protected Q_SLOTS: }; } // namespace moveit_rviz_plugin - -#endif diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 5a08240a98..067ead8875 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -66,12 +66,14 @@ namespace moveit_rviz_plugin PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool show_scene_robot) : Display(), model_is_loading_(false), planning_scene_needs_render_(true), current_scene_time_(0.0f) { - move_group_ns_property_ = new rviz::StringProperty("Move Group Namespace", "", "The name of the ROS namespace in " - "which the move_group node is running", + move_group_ns_property_ = new rviz::StringProperty("Move Group Namespace", "", + "The name of the ROS namespace in " + "which the move_group node is running", this, SLOT(changedMoveGroupNS()), this); - robot_description_property_ = new rviz::StringProperty( - "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", - this, SLOT(changedRobotDescription()), this); + robot_description_property_ = + new rviz::StringProperty("Robot Description", "robot_description", + "The name of the ROS parameter where the URDF for the robot is loaded", this, + SLOT(changedRobotDescription()), this); if (listen_to_planning_scene) planning_scene_topic_property_ = @@ -97,9 +99,10 @@ PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool s scene_alpha_property_->setMin(0.0); scene_alpha_property_->setMax(1.0); - scene_color_property_ = new rviz::ColorProperty( - "Scene Color", QColor(50, 230, 50), "The color for the planning scene obstacles (if a color is not defined)", - scene_category_, SLOT(changedSceneColor()), this); + scene_color_property_ = + new rviz::ColorProperty("Scene Color", QColor(50, 230, 50), + "The color for the planning scene obstacles (if a color is not defined)", scene_category_, + SLOT(changedSceneColor()), this); octree_render_property_ = new rviz::EnumProperty("Voxel Rendering", "Occupied Voxels", "Select voxel type.", scene_category_, SLOT(changedOctreeRenderMode()), this); @@ -114,25 +117,27 @@ PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool s octree_coloring_property_->addOption("Z-Axis", OCTOMAP_Z_AXIS_COLOR); octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR); - scene_display_time_property_ = - new rviz::FloatProperty("Scene Display Time", 0.2f, "The amount of wall-time to wait in between rendering " - "updates to the planning scene (if any)", - scene_category_, SLOT(changedSceneDisplayTime()), this); + scene_display_time_property_ = new rviz::FloatProperty("Scene Display Time", 0.01f, + "The amount of wall-time to wait in between rendering " + "updates to the planning scene (if any)", + scene_category_, SLOT(changedSceneDisplayTime()), this); scene_display_time_property_->setMin(0.0001); if (show_scene_robot) { robot_category_ = new rviz::Property("Scene Robot", QVariant(), "", this); - scene_robot_visual_enabled_property_ = new rviz::BoolProperty( - "Show Robot Visual", true, "Indicates whether the robot state specified by the planning scene should be " - "displayed as defined for visualisation purposes.", - robot_category_, SLOT(changedSceneRobotVisualEnabled()), this); + scene_robot_visual_enabled_property_ = + new rviz::BoolProperty("Show Robot Visual", true, + "Indicates whether the robot state specified by the planning scene should be " + "displayed as defined for visualisation purposes.", + robot_category_, SLOT(changedSceneRobotVisualEnabled()), this); - scene_robot_collision_enabled_property_ = new rviz::BoolProperty( - "Show Robot Collision", false, "Indicates whether the robot state specified by the planning scene should be " - "displayed as defined for collision detection purposes.", - robot_category_, SLOT(changedSceneRobotCollisionEnabled()), this); + scene_robot_collision_enabled_property_ = + new rviz::BoolProperty("Show Robot Collision", false, + "Indicates whether the robot state specified by the planning scene should be " + "displayed as defined for collision detection purposes.", + robot_category_, SLOT(changedSceneRobotCollisionEnabled()), this); robot_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 1.0f, "Specifies the alpha for the robot links", robot_category_, SLOT(changedRobotSceneAlpha()), this); @@ -162,7 +167,7 @@ PlanningSceneDisplay::~PlanningSceneDisplay() planning_scene_render_.reset(); if (context_ && context_->getSceneManager() && planning_scene_node_) - context_->getSceneManager()->destroySceneNode(planning_scene_node_->getName()); + context_->getSceneManager()->destroySceneNode(planning_scene_node_); if (planning_scene_robot_) planning_scene_robot_.reset(); planning_scene_monitor_.reset(); @@ -268,13 +273,13 @@ const std::string PlanningSceneDisplay::getMoveGroupNS() const return move_group_ns_property_->getStdString(); } -const robot_model::RobotModelConstPtr& PlanningSceneDisplay::getRobotModel() const +const moveit::core::RobotModelConstPtr& PlanningSceneDisplay::getRobotModel() const { if (planning_scene_monitor_) return planning_scene_monitor_->getRobotModel(); else { - static robot_model::RobotModelConstPtr empty; + static moveit::core::RobotModelConstPtr empty; return empty; } } @@ -336,10 +341,17 @@ void PlanningSceneDisplay::renderPlanningScene() try { const planning_scene_monitor::LockedPlanningSceneRO& ps = getPlanningSceneRO(); - planning_scene_render_->renderPlanningScene( - ps, env_color, attached_color, static_cast(octree_render_property_->getOptionInt()), - static_cast(octree_coloring_property_->getOptionInt()), - scene_alpha_property_->getFloat()); + if (planning_scene_needs_render_) + { + planning_scene_render_->renderPlanningScene( + ps, env_color, attached_color, static_cast(octree_render_property_->getOptionInt()), + static_cast(octree_coloring_property_->getOptionInt()), + scene_alpha_property_->getFloat()); + } + else + { + planning_scene_render_->updateRobotPosition(ps); + } } catch (std::exception& ex) { @@ -370,7 +382,13 @@ void PlanningSceneDisplay::changedPlanningSceneTopic() std::string service_name = planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE; if (!getMoveGroupNS().empty()) service_name = ros::names::append(getMoveGroupNS(), service_name); - planning_scene_monitor_->requestPlanningSceneState(service_name); + auto bg_func = [=]() { + if (planning_scene_monitor_->requestPlanningSceneState(service_name)) + addMainLoopJob(boost::bind(&PlanningSceneDisplay::onNewPlanningSceneState, this)); + else + setStatus(rviz::StatusProperty::Warn, "PlanningScene", "Requesting initial scene failed"); + }; + addBackgroundJob(bg_func, "requestPlanningSceneState"); } } @@ -416,7 +434,7 @@ void PlanningSceneDisplay::setGroupColor(rviz::Robot* robot, const std::string& { if (getRobotModel()) { - const robot_model::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name); if (jmg) { const std::vector& links = jmg->getLinkModelNamesWithCollisionGeometry(); @@ -440,7 +458,7 @@ void PlanningSceneDisplay::unsetGroupColor(rviz::Robot* robot, const std::string { if (getRobotModel()) { - const robot_model::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name); if (jmg) { const std::vector& links = jmg->getLinkModelNamesWithCollisionGeometry(); @@ -518,6 +536,7 @@ void PlanningSceneDisplay::loadRobotModel() if (psm->getPlanningScene()) { planning_scene_monitor_.swap(psm); + planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, _1)); addMainLoopJob(boost::bind(&PlanningSceneDisplay::onRobotModelLoaded, this)); setStatus(rviz::StatusProperty::Ok, "PlanningScene", "Planning Scene Loaded Successfully"); waitForAllMainLoopJobs(); @@ -527,13 +546,10 @@ void PlanningSceneDisplay::loadRobotModel() setStatus(rviz::StatusProperty::Error, "PlanningScene", "No Planning Scene Loaded"); } - if (planning_scene_monitor_) - planning_scene_monitor_->addUpdateCallback( - boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, _1)); - model_is_loading_ = false; } +// This should always run in the main GUI thread! void PlanningSceneDisplay::onRobotModelLoaded() { changedPlanningSceneTopic(); @@ -544,9 +560,9 @@ void PlanningSceneDisplay::onRobotModelLoaded() if (planning_scene_robot_) { planning_scene_robot_->load(*getRobotModel()->getURDF()); - robot_state::RobotState* rs = new robot_state::RobotState(ps->getCurrentState()); + moveit::core::RobotState* rs = new moveit::core::RobotState(ps->getCurrentState()); rs->update(); - planning_scene_robot_->update(robot_state::RobotStateConstPtr(rs)); + planning_scene_robot_->update(moveit::core::RobotStateConstPtr(rs)); } bool old_state = scene_name_property_->blockSignals(true); @@ -554,6 +570,10 @@ void PlanningSceneDisplay::onRobotModelLoaded() scene_name_property_->blockSignals(old_state); } +void PlanningSceneDisplay::onNewPlanningSceneState() +{ +} + void PlanningSceneDisplay::sceneMonitorReceivedUpdate( planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) { @@ -561,16 +581,19 @@ void PlanningSceneDisplay::sceneMonitorReceivedUpdate( } void PlanningSceneDisplay::onSceneMonitorReceivedUpdate( - planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) + planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType /*update_type*/) { - bool old_state = scene_name_property_->blockSignals(true); getPlanningSceneRW()->getCurrentStateNonConst().update(); - scene_name_property_->setStdString(getPlanningSceneRO()->getName()); - scene_name_property_->blockSignals(old_state); - + QMetaObject::invokeMethod(this, "setSceneName", Qt::QueuedConnection, + Q_ARG(QString, QString::fromStdString(getPlanningSceneRO()->getName()))); planning_scene_needs_render_ = true; } +void PlanningSceneDisplay::setSceneName(const QString& name) +{ + scene_name_property_->setString(name); +} + void PlanningSceneDisplay::onEnable() { Display::onEnable(); @@ -621,15 +644,17 @@ void PlanningSceneDisplay::update(float wall_dt, float ros_dt) updateInternal(wall_dt, ros_dt); } -void PlanningSceneDisplay::updateInternal(float wall_dt, float ros_dt) +void PlanningSceneDisplay::updateInternal(float wall_dt, float /*ros_dt*/) { current_scene_time_ += wall_dt; - if (current_scene_time_ > scene_display_time_property_->getFloat() && planning_scene_render_ && + if ((current_scene_time_ > scene_display_time_property_->getFloat() && planning_scene_render_ && + robot_state_needs_render_) || planning_scene_needs_render_) { renderPlanningScene(); calculateOffsetPosition(); current_scene_time_ = 0.0f; + robot_state_needs_render_ = false; planning_scene_needs_render_ = false; } } diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt index 6a4d92f2f7..e70644888d 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt @@ -5,7 +5,7 @@ add_library(${MOVEIT_LIB_NAME}_core include/moveit/robot_state_rviz_plugin/robot_state_display.h) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools - ${catkin_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) + ${catkin_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) add_library(${MOVEIT_LIB_NAME} src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -15,5 +15,5 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h index c1692d94d7..fb6db32af9 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h +++ b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_VISUALIZATION_ROBOT_STATE_DISPLAY_RVIZ_ROBOT_STATE_DISPLAY_ -#define MOVEIT_VISUALIZATION_ROBOT_STATE_DISPLAY_RVIZ_ROBOT_STATE_DISPLAY_ +#pragma once #include @@ -46,11 +45,6 @@ #include #endif -namespace Ogre -{ -class SceneNode; -} - namespace rviz { class Robot; @@ -59,7 +53,7 @@ class BoolProperty; class FloatProperty; class RosTopicProperty; class ColorProperty; -} +} // namespace rviz namespace moveit_rviz_plugin { @@ -73,10 +67,11 @@ class RobotStateDisplay : public rviz::Display RobotStateDisplay(); ~RobotStateDisplay() override; + void load(const rviz::Config& config) override; void update(float wall_dt, float ros_dt) override; void reset() override; - const robot_model::RobotModelConstPtr& getRobotModel() const + const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; } @@ -84,6 +79,9 @@ class RobotStateDisplay : public rviz::Display void setLinkColor(const std::string& link_name, const QColor& color); void unsetLinkColor(const std::string& link_name); +public Q_SLOTS: + void setVisible(bool visible); + private Q_SLOTS: // ****************************************************************************************** @@ -128,11 +126,10 @@ private Q_SLOTS: RobotStateVisualizationPtr robot_; rdf_loader::RDFLoaderPtr rdf_loader_; - robot_model::RobotModelConstPtr robot_model_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; std::map highlights_; bool update_state_; - bool load_robot_model_; // for delayed robot initialization rviz::StringProperty* robot_description_property_; rviz::StringProperty* root_link_name_property_; @@ -146,5 +143,3 @@ private Q_SLOTS: }; } // namespace moveit_rviz_plugin - -#endif diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 0865765a83..9568dd8978 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -58,16 +59,18 @@ namespace moveit_rviz_plugin // ****************************************************************************************** // Base class contructor // ****************************************************************************************** -RobotStateDisplay::RobotStateDisplay() : Display(), update_state_(false), load_robot_model_(false) +RobotStateDisplay::RobotStateDisplay() : Display(), update_state_(false) { - robot_description_property_ = new rviz::StringProperty( - "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", - this, SLOT(changedRobotDescription()), this); + robot_description_property_ = + new rviz::StringProperty("Robot Description", "robot_description", + "The name of the ROS parameter where the URDF for the robot is loaded", this, + SLOT(changedRobotDescription()), this); - robot_state_topic_property_ = new rviz::RosTopicProperty( - "Robot State Topic", "display_robot_state", ros::message_traits::datatype(), - "The topic on which the moveit_msgs::DisplayRobotState messages are received", this, - SLOT(changedRobotStateTopic()), this); + robot_state_topic_property_ = + new rviz::RosTopicProperty("Robot State Topic", "display_robot_state", + ros::message_traits::datatype(), + "The topic on which the moveit_msgs::DisplayRobotState messages are received", this, + SLOT(changedRobotStateTopic()), this); // Planning scene category ------------------------------------------------------------------------------------------- root_link_name_property_ = @@ -117,8 +120,8 @@ void RobotStateDisplay::reset() robot_->clear(); rdf_loader_.reset(); Display::reset(); - - loadRobotModel(); + if (isEnabled()) + onEnable(); } void RobotStateDisplay::changedAllLinks() @@ -289,6 +292,8 @@ void RobotStateDisplay::changedRobotStateTopic() if (static_cast(robot_state_)) robot_state_->setToDefaultValues(); update_state_ = true; + robot_->setVisible(false); + setStatus(rviz::StatusProperty::Warn, "RobotState", "No msg received"); robot_state_subscriber_ = root_nh_.subscribe(robot_state_topic_property_->getStdString(), 10, &RobotStateDisplay::newRobotStateCallback, this); @@ -299,21 +304,32 @@ void RobotStateDisplay::newRobotStateCallback(const moveit_msgs::DisplayRobotSta if (!robot_model_) return; if (!robot_state_) - robot_state_.reset(new robot_state::RobotState(robot_model_)); - // possibly use TF to construct a robot_state::Transforms object to pass in to the conversion function? + robot_state_.reset(new moveit::core::RobotState(robot_model_)); + // possibly use TF to construct a moveit::core::Transforms object to pass in to the conversion function? try { - robot_state::robotStateMsgToRobotState(state_msg->state, *robot_state_); + if (!moveit::core::isEmpty(state_msg->state)) + moveit::core::robotStateMsgToRobotState(state_msg->state, *robot_state_); setRobotHighlights(state_msg->highlight_links); - setStatus(rviz::StatusProperty::Ok, "RobotState", ""); } catch (const moveit::Exception& e) { robot_state_->setToDefaultValues(); setRobotHighlights(moveit_msgs::DisplayRobotState::_highlight_links_type()); setStatus(rviz::StatusProperty::Error, "RobotState", e.what()); + robot_->setVisible(false); return; } + + if (robot_->isVisible() != !state_msg->hide) + { + robot_->setVisible(!state_msg->hide); + if (robot_->isVisible()) + setStatus(rviz::StatusProperty::Ok, "RobotState", ""); + else + setStatus(rviz::StatusProperty::Warn, "RobotState", "Hidden"); + } + update_state_ = true; } @@ -327,6 +343,11 @@ void RobotStateDisplay::unsetLinkColor(const std::string& link_name) unsetLinkColor(&robot_->getRobot(), link_name); } +void RobotStateDisplay::setVisible(bool visible) +{ + robot_->setVisible(visible); +} + void RobotStateDisplay::setLinkColor(rviz::Robot* robot, const std::string& link_name, const QColor& color) { rviz::RobotLink* link = robot->getLink(link_name); @@ -350,38 +371,52 @@ void RobotStateDisplay::unsetLinkColor(rviz::Robot* robot, const std::string& li // ****************************************************************************************** void RobotStateDisplay::loadRobotModel() { - load_robot_model_ = false; - if (!rdf_loader_) - rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); + rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); if (rdf_loader_->getURDF()) { - const srdf::ModelSharedPtr& srdf = - rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); - robot_model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf)); - robot_->load(*robot_model_->getURDF()); - robot_state_.reset(new robot_state::RobotState(robot_model_)); - robot_state_->setToDefaultValues(); - bool old_state = root_link_name_property_->blockSignals(true); - root_link_name_property_->setStdString(getRobotModel()->getRootLinkName()); - root_link_name_property_->blockSignals(old_state); - update_state_ = true; - setStatus(rviz::StatusProperty::Ok, "RobotState", "Planning Model Loaded Successfully"); - - changedEnableVisualVisible(); - changedEnableCollisionVisible(); - robot_->setVisible(true); + try + { + const srdf::ModelSharedPtr& srdf = + rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); + robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); + robot_->load(*robot_model_->getURDF()); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); + robot_state_->setToDefaultValues(); + bool old_state = root_link_name_property_->blockSignals(true); + root_link_name_property_->setStdString(getRobotModel()->getRootLinkName()); + root_link_name_property_->blockSignals(old_state); + update_state_ = true; + setStatus(rviz::StatusProperty::Ok, "RobotModel", "Loaded successfully"); + + changedEnableVisualVisible(); + changedEnableCollisionVisible(); + } + catch (std::exception& e) + { + setStatus(rviz::StatusProperty::Error, "RobotModel", QString("Loading failed: %1").arg(e.what())); + } } else - setStatus(rviz::StatusProperty::Error, "RobotState", "No Planning Model Loaded"); + setStatus(rviz::StatusProperty::Error, "RobotModel", "Loading failed"); highlights_.clear(); } +void RobotStateDisplay::load(const rviz::Config& config) +{ + // This property needs to be loaded in onEnable() below, which is triggered + // in the beginning of Display::load() before the other property would be available + robot_description_property_->load(config.mapGetChild("Robot Description")); + Display::load(config); +} + void RobotStateDisplay::onEnable() { Display::onEnable(); - load_robot_model_ = true; // allow loading of robot model in update() + if (!rdf_loader_) + loadRobotModel(); + changedRobotStateTopic(); calculateOffsetPosition(); } @@ -399,13 +434,6 @@ void RobotStateDisplay::onDisable() void RobotStateDisplay::update(float wall_dt, float ros_dt) { Display::update(wall_dt, ros_dt); - - if (load_robot_model_) - { - loadRobotModel(); - changedRobotStateTopic(); - } - calculateOffsetPosition(); if (robot_ && update_state_ && robot_state_) { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt index 9baca76cfb..000aac5ecc 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt +++ b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt @@ -26,7 +26,6 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} - ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} ) @@ -35,4 +34,5 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h index 92bd6dd2bb..d573733472 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h @@ -34,29 +34,19 @@ /* Author: Julius Kammerl */ -#ifndef MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_OCTOMAP_RENDER_ -#define MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_OCTOMAP_RENDER_ +#pragma once #include #include #include - #include +#include namespace octomap { class OcTree; } -namespace Ogre -{ -class SceneManager; -class SceneNode; -class AxisAlignedBox; -class Vector3; -class Quaternion; -} - namespace moveit_rviz_plugin { enum OctreeVoxelRenderMode @@ -75,8 +65,7 @@ class OcTreeRender { public: OcTreeRender(const std::shared_ptr& octree, OctreeVoxelRenderMode octree_voxel_rendering, - OctreeVoxelColorMode octree_color_mode, std::size_t max_octree_depth, Ogre::SceneManager* scene_manager, - Ogre::SceneNode* parent_node); + OctreeVoxelColorMode octree_color_mode, std::size_t max_octree_depth, Ogre::SceneNode* parent_node); virtual ~OcTreeRender(); void setPosition(const Ogre::Vector3& position); @@ -94,10 +83,8 @@ class OcTreeRender std::shared_ptr octree_; Ogre::SceneNode* scene_node_; - Ogre::SceneManager* scene_manager_; double colorFactor_; std::size_t octree_depth_; }; -} -#endif +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h index 0f8f417f37..0ce29cacb9 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h @@ -34,19 +34,18 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_PLANNING_LINK_UPDATER_ -#define MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_PLANNING_LINK_UPDATER_ +#pragma once #include #include namespace moveit_rviz_plugin { -/** \brief Update the links of an rviz::Robot using a robot_state::RobotState */ +/** \brief Update the links of an rviz::Robot using a moveit::core::RobotState */ class PlanningLinkUpdater : public rviz::LinkUpdater { public: - PlanningLinkUpdater(const robot_state::RobotStateConstPtr& state) : kinematic_state_(state) + PlanningLinkUpdater(const moveit::core::RobotStateConstPtr& state) : kinematic_state_(state) { } @@ -55,8 +54,6 @@ class PlanningLinkUpdater : public rviz::LinkUpdater Ogre::Quaternion& collision_orientation) const override; private: - robot_state::RobotStateConstPtr kinematic_state_; + moveit::core::RobotStateConstPtr kinematic_state_; }; -} - -#endif +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h index 84106db313..3f673c24ed 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_PLUGIN_PLANNING_SCENE_RENDER_ -#define MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_PLUGIN_PLANNING_SCENE_RENDER_ +#pragma once #include #include @@ -55,9 +54,9 @@ class DisplayContext; namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(RobotStateVisualization); -MOVEIT_CLASS_FORWARD(RenderShapes); -MOVEIT_CLASS_FORWARD(PlanningSceneRender); +MOVEIT_CLASS_FORWARD(RobotStateVisualization); // Defines RobotStateVisualizationPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RenderShapes); // Defines RenderShapesPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(PlanningSceneRender); // Defines PlanningSceneRenderPtr, ConstPtr, WeakPtr... etc class PlanningSceneRender { @@ -76,6 +75,8 @@ class PlanningSceneRender return scene_robot_; } + void updateRobotPosition(const planning_scene::PlanningSceneConstPtr& scene); + void renderPlanningScene(const planning_scene::PlanningSceneConstPtr& scene, const rviz::Color& default_scene_color, const rviz::Color& default_attached_color, OctreeVoxelRenderMode voxel_render_mode, OctreeVoxelColorMode voxel_color_mode, float default_scene_alpha); @@ -87,6 +88,4 @@ class PlanningSceneRender RenderShapesPtr render_shapes_; RobotStateVisualizationPtr scene_robot_; }; -} - -#endif +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h index fa891bcccd..194d4d2faa 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h @@ -34,15 +34,13 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_RENDER_SHAPES_ -#define MOVEIT_VISUALIZATION_SCENE_DISPLAY_RVIZ_RENDER_SHAPES_ +#pragma once #include #include #include #include #include -#include #include namespace Ogre @@ -54,12 +52,12 @@ namespace rviz { class DisplayContext; class Shape; -} +} // namespace rviz namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(OcTreeRender); -MOVEIT_CLASS_FORWARD(RenderShapes); +MOVEIT_CLASS_FORWARD(OcTreeRender); // Defines OcTreeRenderPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RenderShapes); // Defines RenderShapesPtr, ConstPtr, WeakPtr... etc class RenderShapes { @@ -70,6 +68,7 @@ class RenderShapes void renderShape(Ogre::SceneNode* node, const shapes::Shape* s, const Eigen::Isometry3d& p, OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode, const rviz::Color& color, float alpha); + void updateShapeColors(float r, float g, float b, float a); void clear(); private: @@ -78,6 +77,4 @@ class RenderShapes std::vector > scene_shapes_; std::vector octree_voxel_grids_; }; -} - -#endif +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index 24f246dcc3..f8fdf6d039 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_ROBOT_STATE_VISUALIZATION_ -#define MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_ROBOT_STATE_VISUALIZATION_ +#pragma once #include #include @@ -44,10 +43,10 @@ namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(RenderShapes); -MOVEIT_CLASS_FORWARD(RobotStateVisualization); +MOVEIT_CLASS_FORWARD(RenderShapes); // Defines RenderShapesPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RobotStateVisualization); // Defines RobotStateVisualizationPtr, ConstPtr, WeakPtr... etc -/** \brief Update the links of an rviz::Robot using a robot_state::RobotState */ +/** \brief Update the links of an rviz::Robot using a moveit::core::RobotState */ class RobotStateVisualization { public: @@ -62,13 +61,21 @@ class RobotStateVisualization void load(const urdf::ModelInterface& descr, bool visual = true, bool collision = true); void clear(); - void update(const robot_state::RobotStateConstPtr& kinematic_state); - void update(const robot_state::RobotStateConstPtr& kinematic_state, + void update(const moveit::core::RobotStateConstPtr& kinematic_state); + void update(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color); - void update(const robot_state::RobotStateConstPtr& kinematic_state, + void update(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color, const std::map& color_map); + void updateKinematicState(const moveit::core::RobotStateConstPtr& kinematic_state); void setDefaultAttachedObjectColor(const std_msgs::ColorRGBA& default_attached_object_color); + /// update color of all attached object shapes + void updateAttachedObjectColors(const std_msgs::ColorRGBA& attached_object_color); + + bool isVisible() const + { + return visible_; + } /** * \brief Set the robot as a whole to be visible or not @@ -91,7 +98,7 @@ class RobotStateVisualization void setAlpha(float alpha); private: - void updateHelper(const robot_state::RobotStateConstPtr& kinematic_state, + void updateHelper(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color, const std::map* color_map); rviz::Robot robot_; @@ -104,6 +111,4 @@ class RobotStateVisualization bool visual_visible_; bool collision_visible_; }; -} - -#endif +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h index e4c7d731d1..73d2e6c01f 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h @@ -34,8 +34,7 @@ /* Author: Yannick Jonetzko */ -#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TRAJECTORY_PANEL_ -#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TRAJECTORY_PANEL_ +#pragma once #ifndef Q_MOC_RUN #include @@ -54,7 +53,7 @@ class TrajectoryPanel : public rviz::Panel Q_OBJECT public: - TrajectoryPanel(QWidget* parent = 0); + TrajectoryPanel(QWidget* parent = nullptr); ~TrajectoryPanel() override; @@ -93,5 +92,3 @@ private Q_SLOTS: }; } // namespace moveit_rviz_plugin - -#endif diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h index 1c309a46d7..2af227f429 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_VISUALIZATION -#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_VISUALIZATION +#pragma once #include #include @@ -65,11 +64,11 @@ class RosTopicProperty; class EditableEnumProperty; class ColorProperty; class MovableText; -} +} // namespace rviz namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(TrajectoryVisualization); +MOVEIT_CLASS_FORWARD(TrajectoryVisualization); // Defines TrajectoryVisualizationPtr, ConstPtr, WeakPtr... etc class TrajectoryVisualization : public QObject { @@ -90,7 +89,7 @@ class TrajectoryVisualization : public QObject virtual void reset(); void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context, const ros::NodeHandle& update_nh); - void onRobotModelLoaded(const robot_model::RobotModelConstPtr& robot_model); + void onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model); void onEnable(); void onDisable(); void setName(const QString& name); @@ -128,6 +127,7 @@ private Q_SLOTS: // Handles actually drawing the robot along motion plans RobotStateVisualizationPtr display_path_robot_; + std_msgs::ColorRGBA default_attached_object_color_; // Handle colouring of robot void setRobotColor(rviz::Robot* robot, const QColor& color); @@ -135,7 +135,7 @@ private Q_SLOTS: robot_trajectory::RobotTrajectoryPtr displaying_trajectory_message_; robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_; - std::vector trajectory_trail_; + std::vector trajectory_trail_; ros::Subscriber trajectory_topic_sub_; bool animating_path_; bool drop_displaying_trajectory_; @@ -143,8 +143,8 @@ private Q_SLOTS: float current_state_time_; boost::mutex update_trajectory_message_; - robot_model::RobotModelConstPtr robot_model_; - robot_state::RobotStatePtr robot_state_; + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; // Pointers from parent display taht we save rviz::Display* display_; // the parent display that this class populates @@ -170,5 +170,3 @@ private Q_SLOTS: }; } // namespace moveit_rviz_plugin - -#endif diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp old mode 100644 new mode 100755 index f427a7315b..dc25cf06d6 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp @@ -51,15 +51,9 @@ typedef std::vector VVPoint; OcTreeRender::OcTreeRender(const std::shared_ptr& octree, OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode, - std::size_t max_octree_depth, Ogre::SceneManager* scene_manager, - Ogre::SceneNode* parent_node = nullptr) + std::size_t max_octree_depth, Ogre::SceneNode* parent_node) : octree_(octree), colorFactor_(0.8) { - if (!parent_node) - { - parent_node = scene_manager_->getRootSceneNode(); - } - if (!max_octree_depth) { octree_depth_ = octree->getTreeDepth(); @@ -169,6 +163,8 @@ void OcTreeRender::octreeDecoding(const std::shared_ptr& size_t point_count = 0; { + int step_size = 1 << (octree->getTreeDepth() - octree_depth_); // for pruning of occluded voxels + // traverse all leafs in the tree: for (octomap::OcTree::iterator it = octree->begin(octree_depth_), end = octree->end(); it != end; ++it) { @@ -181,20 +177,35 @@ void OcTreeRender::octreeDecoding(const std::shared_ptr& bool all_neighbors_found = true; octomap::OcTreeKey key; - octomap::OcTreeKey n_key = it.getKey(); + octomap::OcTreeKey n_key = it.getIndexKey(); // key of the maximum-depth voxel at the current voxel corner + + // determine indices of potentially neighboring voxels for depths < maximum tree depth + // +/-1 at maximum depth, -1 and +depth_difference on other depths + int diff_base = 1 << (octree->getTreeDepth() - it.getDepth()); + int diff[2] = { -1, diff_base }; - for (key[2] = n_key[2] - 1; all_neighbors_found && key[2] <= n_key[2] + 1; ++key[2]) + // cells with adjacent faces can occlude a voxel, iterate over the cases x,y,z (idxCase) and +/- (diff) + for (unsigned int idx_case = 0; idx_case < 3; ++idx_case) { - for (key[1] = n_key[1] - 1; all_neighbors_found && key[1] <= n_key[1] + 1; ++key[1]) + int idx_0 = idx_case % 3; + int idx_1 = (idx_case + 1) % 3; + int idx_2 = (idx_case + 2) % 3; + + for (int i = 0; all_neighbors_found && i < 2; ++i) { - for (key[0] = n_key[0] - 1; all_neighbors_found && key[0] <= n_key[0] + 1; ++key[0]) + key[idx_0] = n_key[idx_0] + diff[i]; + // if rendering is restricted to treeDepth < maximum tree depth inner nodes with distance step_size can + // already occlude a voxel + for (key[idx_1] = n_key[idx_1] + diff[0] + 1; all_neighbors_found && key[idx_1] < n_key[idx_1] + diff[1]; + key[idx_1] += step_size) { - if (key != n_key) + for (key[idx_2] = n_key[idx_2] + diff[0] + 1; all_neighbors_found && key[idx_2] < n_key[idx_2] + diff[1]; + key[idx_2] += step_size) { - octomap::OcTreeNode* node = octree->search(key); + octomap::OcTreeNode* node = octree->search(key, octree_depth_); // the left part evaluates to 1 for free voxels and 2 for occupied voxels - if (!(node && (((int)octree->isNodeOccupied(node)) + 1) & render_mode_mask)) + if (!(node && ((((int)octree->isNodeOccupied(node)) + 1) & render_mode_mask))) { // we do not have a neighbor => break! all_neighbors_found = false; diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp index 89baece404..26259978cb 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp @@ -44,15 +44,16 @@ bool moveit_rviz_plugin::PlanningLinkUpdater::getLinkTransforms(const std::strin Ogre::Vector3& collision_position, Ogre::Quaternion& collision_orientation) const { - const robot_model::LinkModel* link_model = kinematic_state_->getLinkModel(link_name); + const moveit::core::LinkModel* link_model = kinematic_state_->getLinkModel(link_name); if (!link_model) { return false; } + // getGlobalLinkTransform() returns a valid isometry by contract const Eigen::Vector3d& robot_visual_position = kinematic_state_->getGlobalLinkTransform(link_model).translation(); - Eigen::Quaterniond robot_visual_orientation(kinematic_state_->getGlobalLinkTransform(link_model).rotation()); + Eigen::Quaterniond robot_visual_orientation(kinematic_state_->getGlobalLinkTransform(link_model).linear()); visual_position = Ogre::Vector3(robot_visual_position.x(), robot_visual_position.y(), robot_visual_position.z()); visual_orientation = Ogre::Quaternion(robot_visual_orientation.w(), robot_visual_orientation.x(), robot_visual_orientation.y(), robot_visual_orientation.z()); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp index 85ba49b3e3..c24ca4adc2 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp @@ -53,7 +53,17 @@ PlanningSceneRender::PlanningSceneRender(Ogre::SceneNode* node, rviz::DisplayCon PlanningSceneRender::~PlanningSceneRender() { - context_->getSceneManager()->destroySceneNode(planning_scene_geometry_node_->getName()); + context_->getSceneManager()->destroySceneNode(planning_scene_geometry_node_); +} + +void PlanningSceneRender::updateRobotPosition(const planning_scene::PlanningSceneConstPtr& scene) +{ + if (scene_robot_) + { + auto rs = std::make_shared(scene->getCurrentState()); + rs->update(); + scene_robot_->updateKinematicState(rs); + } } void PlanningSceneRender::clear() @@ -74,7 +84,7 @@ void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningScen if (scene_robot_) { - robot_state::RobotState* rs = new robot_state::RobotState(scene->getCurrentState()); + moveit::core::RobotState* rs = new moveit::core::RobotState(scene->getCurrentState()); rs->update(); std_msgs::ColorRGBA color; @@ -84,13 +94,13 @@ void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningScen color.a = 1.0f; planning_scene::ObjectColorMap color_map; scene->getKnownObjectColors(color_map); - scene_robot_->update(robot_state::RobotStateConstPtr(rs), color, color_map); + scene_robot_->update(moveit::core::RobotStateConstPtr(rs), color, color_map); } const std::vector& ids = scene->getWorld()->getObjectIds(); for (const std::string& id : ids) { - collision_detection::CollisionWorld::ObjectConstPtr object = scene->getWorld()->getObject(id); + collision_detection::CollisionEnv::ObjectConstPtr object = scene->getWorld()->getObject(id); rviz::Color color = default_env_color; float alpha = default_scene_alpha; if (scene->hasObjectColor(id)) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index 586efb91f2..4ab9391922 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -78,7 +79,8 @@ void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, co rviz::Shape* ogre_shape = nullptr; Eigen::Vector3d translation = p.translation(); Ogre::Vector3 position(translation.x(), translation.y(), translation.z()); - Eigen::Quaterniond q(p.rotation()); + ASSERT_ISOMETRY(p) // unsanitized input, could contain a non-isometry + Eigen::Quaterniond q(p.linear()); Ogre::Quaternion orientation(q.w(), q.x(), q.y(), q.z()); // we don't know how to render cones directly, but we can convert them to a mesh @@ -157,7 +159,7 @@ void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, co case shapes::OCTREE: { OcTreeRenderPtr octree(new OcTreeRender(static_cast(s)->octree, octree_voxel_rendering, - octree_color_mode, 0u, context_->getSceneManager(), node)); + octree_color_mode, 0u, node)); octree->setPosition(position); octree->setOrientation(orientation); octree_voxel_grids_.push_back(octree); @@ -186,4 +188,11 @@ void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, co scene_shapes_.emplace_back(ogre_shape); } } + +void RenderShapes::updateShapeColors(float r, float g, float b, float a) +{ + for (const std::unique_ptr& shape : scene_shapes_) + shape->setColor(r, g, b, a); +} + } // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp index db6192c7ad..2f4183daa2 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include namespace moveit_rviz_plugin @@ -66,7 +67,6 @@ void RobotStateVisualization::load(const urdf::ModelInterface& descr, bool visua robot_.setVisualVisible(visual_visible_); robot_.setCollisionVisible(collision_visible_); robot_.setVisible(visible_); - QApplication::processEvents(); } void RobotStateVisualization::clear() @@ -80,34 +80,40 @@ void RobotStateVisualization::setDefaultAttachedObjectColor(const std_msgs::Colo default_attached_object_color_ = default_attached_object_color; } -void RobotStateVisualization::update(const robot_state::RobotStateConstPtr& kinematic_state) +void RobotStateVisualization::updateAttachedObjectColors(const std_msgs::ColorRGBA& attached_object_color) +{ + render_shapes_->updateShapeColors(attached_object_color.r, attached_object_color.g, attached_object_color.b, + robot_.getAlpha()); +} + +void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kinematic_state) { updateHelper(kinematic_state, default_attached_object_color_, nullptr); } -void RobotStateVisualization::update(const robot_state::RobotStateConstPtr& kinematic_state, +void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color) { updateHelper(kinematic_state, default_attached_object_color, nullptr); } -void RobotStateVisualization::update(const robot_state::RobotStateConstPtr& kinematic_state, +void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color, const std::map& color_map) { updateHelper(kinematic_state, default_attached_object_color, &color_map); } -void RobotStateVisualization::updateHelper(const robot_state::RobotStateConstPtr& kinematic_state, +void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPtr& kinematic_state, const std_msgs::ColorRGBA& default_attached_object_color, const std::map* color_map) { robot_.update(PlanningLinkUpdater(kinematic_state)); render_shapes_->clear(); - std::vector attached_bodies; + std::vector attached_bodies; kinematic_state->getAttachedBodies(attached_bodies); - for (const robot_state::AttachedBody* attached_body : attached_bodies) + for (const moveit::core::AttachedBody* attached_body : attached_bodies) { std_msgs::ColorRGBA color = default_attached_object_color; float alpha = robot_.getAlpha(); @@ -122,14 +128,20 @@ void RobotStateVisualization::updateHelper(const robot_state::RobotStateConstPtr alpha = color.a = it->second.a; } } + rviz::RobotLink* link = robot_.getLink(attached_body->getAttachedLinkName()); + if (!link) + { + ROS_ERROR_STREAM("Link " << attached_body->getAttachedLinkName() << " not found in rviz::Robot"); + continue; + } rviz::Color rcolor(color.r, color.g, color.b); - const EigenSTL::vector_Isometry3d& ab_t = attached_body->getGlobalCollisionBodyTransforms(); + const EigenSTL::vector_Isometry3d& ab_t = attached_body->getFixedTransforms(); const std::vector& ab_shapes = attached_body->getShapes(); for (std::size_t j = 0; j < ab_shapes.size(); ++j) { - render_shapes_->renderShape(robot_.getVisualNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_, + render_shapes_->renderShape(link->getVisualNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_, octree_voxel_color_mode_, rcolor, alpha); - render_shapes_->renderShape(robot_.getCollisionNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_, + render_shapes_->renderShape(link->getCollisionNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_, octree_voxel_color_mode_, rcolor, alpha); } } @@ -138,6 +150,11 @@ void RobotStateVisualization::updateHelper(const robot_state::RobotStateConstPtr robot_.setVisible(visible_); } +void RobotStateVisualization::updateKinematicState(const moveit::core::RobotStateConstPtr& kinematic_state) +{ + robot_.update(PlanningLinkUpdater(kinematic_state)); +} + void RobotStateVisualization::setVisible(bool visible) { visible_ = visible; diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 64afac8b56..b8a62aabba 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -43,17 +43,17 @@ #include #include -#include -#include -#include +#include +#include #include +#include +#include #include +#include +#include #include -#include -#include -#include +#include #include -#include #include namespace moveit_rviz_plugin @@ -74,13 +74,15 @@ TrajectoryVisualization::TrajectoryVisualization(rviz::Property* widget, rviz::D SLOT(changedTrajectoryTopic()), this); display_path_visual_enabled_property_ = - new rviz::BoolProperty("Show Robot Visual", true, "Indicates whether the geometry of the robot as defined for " - "visualisation purposes should be displayed", + new rviz::BoolProperty("Show Robot Visual", true, + "Indicates whether the geometry of the robot as defined for " + "visualisation purposes should be displayed", widget, SLOT(changedDisplayPathVisualEnabled()), this); display_path_collision_enabled_property_ = - new rviz::BoolProperty("Show Robot Collision", false, "Indicates whether the geometry of the robot as defined " - "for collision detection purposes should be displayed", + new rviz::BoolProperty("Show Robot Collision", false, + "Indicates whether the geometry of the robot as defined " + "for collision detection purposes should be displayed", widget, SLOT(changedDisplayPathCollisionEnabled()), this); robot_path_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links", @@ -97,21 +99,24 @@ TrajectoryVisualization::TrajectoryVisualization(rviz::Property* widget, rviz::D state_display_time_property_->addOptionStd("0.1 s"); state_display_time_property_->addOptionStd("0.5 s"); - loop_display_property_ = new rviz::BoolProperty("Loop Animation", false, "Indicates whether the last received path " - "is to be animated in a loop", + loop_display_property_ = new rviz::BoolProperty("Loop Animation", false, + "Indicates whether the last received path " + "is to be animated in a loop", widget, SLOT(changedLoopDisplay()), this); trail_display_property_ = new rviz::BoolProperty("Show Trail", false, "Show a path trail", widget, SLOT(changedShowTrail()), this); - trail_step_size_property_ = new rviz::IntProperty("Trail Step Size", 1, "Specifies the step size of the samples " - "shown in the trajectory trail.", + trail_step_size_property_ = new rviz::IntProperty("Trail Step Size", 1, + "Specifies the step size of the samples " + "shown in the trajectory trail.", widget, SLOT(changedTrailStepSize()), this); trail_step_size_property_->setMin(1); - interrupt_display_property_ = new rviz::BoolProperty( - "Interrupt Display", false, - "Immediately show newly planned trajectory, interrupting the currently displayed one.", widget); + interrupt_display_property_ = + new rviz::BoolProperty("Interrupt Display", false, + "Immediately show newly planned trajectory, interrupting the currently displayed one.", + widget); robot_color_property_ = new rviz::ColorProperty( "Robot Color", QColor(150, 50, 150), "The color of the animated robot", widget, SLOT(changedRobotColor()), this); @@ -164,7 +169,7 @@ void TrajectoryVisualization::setName(const QString& name) trajectory_slider_dock_panel_->setWindowTitle(name + " - Slider"); } -void TrajectoryVisualization::onRobotModelLoaded(const robot_model::RobotModelConstPtr& robot_model) +void TrajectoryVisualization::onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model) { robot_model_ = robot_model; @@ -176,7 +181,7 @@ void TrajectoryVisualization::onRobotModelLoaded(const robot_model::RobotModelCo } // Load robot state - robot_state_.reset(new robot_state::RobotState(robot_model_)); + robot_state_.reset(new moveit::core::RobotState(robot_model_)); robot_state_->setToDefaultValues(); // Load rviz robot @@ -201,8 +206,6 @@ void TrajectoryVisualization::reset() void TrajectoryVisualization::clearTrajectoryTrail() { - for (rviz::Robot* waypoint_state : trajectory_trail_) - delete waypoint_state; trajectory_trail_.clear(); } @@ -231,17 +234,17 @@ void TrajectoryVisualization::changedShowTrail() for (std::size_t i = 0; i < trajectory_trail_.size(); i++) { int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point - rviz::Robot* r = - new rviz::Robot(scene_node_, context_, "Trail Robot " + boost::lexical_cast(i), nullptr); + auto r = std::make_unique(scene_node_, context_, + "Trail Robot " + boost::lexical_cast(i), nullptr); r->load(*robot_model_->getURDF()); r->setVisualVisible(display_path_visual_enabled_property_->getBool()); r->setCollisionVisible(display_path_collision_enabled_property_->getBool()); r->setAlpha(robot_path_alpha_property_->getFloat()); - r->update(PlanningLinkUpdater(t->getWayPointPtr(waypoint_i))); + r->update(t->getWayPointPtr(waypoint_i), default_attached_object_color_); if (enable_robot_color_property_->getBool()) - setRobotColor(r, robot_color_property_->getColor()); + setRobotColor(&(r->getRobot()), robot_color_property_->getColor()); r->setVisible(display_->isEnabled() && (!animating_path_ || waypoint_i <= current_state_)); - trajectory_trail_[i] = r; + trajectory_trail_[i] = std::move(r); } } @@ -254,8 +257,8 @@ void TrajectoryVisualization::changedTrailStepSize() void TrajectoryVisualization::changedRobotPathAlpha() { display_path_robot_->setAlpha(robot_path_alpha_property_->getFloat()); - for (rviz::Robot* waypoint_state : trajectory_trail_) - waypoint_state->setAlpha(robot_path_alpha_property_->getFloat()); + for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_) + r->setAlpha(robot_path_alpha_property_->getFloat()); } void TrajectoryVisualization::changedTrajectoryTopic() @@ -275,8 +278,8 @@ void TrajectoryVisualization::changedDisplayPathVisualEnabled() { display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool()); display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_); - for (rviz::Robot* waypoint_state : trajectory_trail_) - waypoint_state->setVisualVisible(display_path_visual_enabled_property_->getBool()); + for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_) + r->setVisualVisible(display_path_visual_enabled_property_->getBool()); } } @@ -290,8 +293,8 @@ void TrajectoryVisualization::changedDisplayPathCollisionEnabled() { display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool()); display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_); - for (rviz::Robot* waypoint_state : trajectory_trail_) - waypoint_state->setCollisionVisible(display_path_collision_enabled_property_->getBool()); + for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_) + r->setCollisionVisible(display_path_collision_enabled_property_->getBool()); } } @@ -302,11 +305,11 @@ void TrajectoryVisualization::onEnable() display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool()); display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool()); display_path_robot_->setVisible(displaying_trajectory_message_ && animating_path_); - for (rviz::Robot* waypoint_state : trajectory_trail_) + for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_) { - waypoint_state->setVisualVisible(display_path_visual_enabled_property_->getBool()); - waypoint_state->setCollisionVisible(display_path_collision_enabled_property_->getBool()); - waypoint_state->setVisible(true); + r->setVisualVisible(display_path_visual_enabled_property_->getBool()); + r->setCollisionVisible(display_path_collision_enabled_property_->getBool()); + r->setVisible(true); } changedTrajectoryTopic(); // load topic at startup if default used @@ -315,8 +318,8 @@ void TrajectoryVisualization::onEnable() void TrajectoryVisualization::onDisable() { display_path_robot_->setVisible(false); - for (rviz::Robot* waypoint_state : trajectory_trail_) - waypoint_state->setVisible(false); + for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_) + r->setVisible(false); displaying_trajectory_message_.reset(); animating_path_ = false; if (trajectory_slider_panel_) @@ -359,7 +362,7 @@ void TrajectoryVisualization::dropTrajectory() drop_displaying_trajectory_ = true; } -void TrajectoryVisualization::update(float wall_dt, float ros_dt) +void TrajectoryVisualization::update(float wall_dt, float /*ros_dt*/) { if (drop_displaying_trajectory_) { @@ -413,7 +416,10 @@ void TrajectoryVisualization::update(float wall_dt, float ros_dt) float tm = getStateDisplayTime(); if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible() && trajectory_slider_panel_->isPaused()) + { current_state_ = trajectory_slider_panel_->getSliderPosition(); + current_state_time_ = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_); + } else if (current_state_ < 0) { // special case indicating restart of animation current_state_ = 0; @@ -421,11 +427,13 @@ void TrajectoryVisualization::update(float wall_dt, float ros_dt) } else if (tm < 0.0) { // using realtime: skip to next waypoint based on elapsed display time - while (current_state_ < waypoint_count && - (tm = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1)) < - current_state_time_) + while (current_state_ < waypoint_count && (tm = displaying_trajectory_message_->getWayPointDurationFromPrevious( + current_state_ + 1)) < current_state_time_) { current_state_time_ -= tm; + if (tm < current_state_time_) // if we are stuck in the while loop we should + // move the robot along the path to keep up + display_path_robot_->update(displaying_trajectory_message_->getWayPointPtr(current_state_)); ++current_state_; } } @@ -449,7 +457,11 @@ void TrajectoryVisualization::update(float wall_dt, float ros_dt) } else { - animating_path_ = false; // animation finished + animating_path_ = false; // animation finished + if (trajectory_slider_panel_) // make sure we move the slider to the end + // so the user can re-play + trajectory_slider_panel_->setSliderPosition(waypoint_count); + display_path_robot_->update(displaying_trajectory_message_->getWayPointPtr(waypoint_count - 1)); display_path_robot_->setVisible(loop_display_property_->getBool()); if (!loop_display_property_->getBool() && trajectory_slider_panel_) trajectory_slider_panel_->pauseButton(true); @@ -531,15 +543,18 @@ void TrajectoryVisualization::unsetRobotColor(rviz::Robot* robot) void TrajectoryVisualization::setDefaultAttachedObjectColor(const QColor& color) { - if (!display_path_robot_) - return; + default_attached_object_color_.r = color.redF(); + default_attached_object_color_.g = color.greenF(); + default_attached_object_color_.b = color.blueF(); + default_attached_object_color_.a = color.alphaF(); - std_msgs::ColorRGBA color_msg; - color_msg.r = color.redF(); - color_msg.g = color.greenF(); - color_msg.b = color.blueF(); - color_msg.a = color.alphaF(); - display_path_robot_->setDefaultAttachedObjectColor(color_msg); + if (display_path_robot_) + { + display_path_robot_->setDefaultAttachedObjectColor(default_attached_object_color_); + display_path_robot_->updateAttachedObjectColors(default_attached_object_color_); + } + for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_) + r->updateAttachedObjectColors(default_attached_object_color_); } void TrajectoryVisualization::setRobotColor(rviz::Robot* robot, const QColor& color) diff --git a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py index 0ad0a176d6..1bb5e7da7a 100644 --- a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py +++ b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python # ******************************************************************** # Software License Agreement (BSD License) # @@ -35,7 +34,7 @@ # ********************************************************************/ # Author: Ryohei Ueda, Dave Coleman -# Desc: Interface between PS3/XBox controller and MoveIt! Motion Planning Rviz Plugin +# Desc: Interface between PS3/XBox controller and MoveIt Motion Planning Rviz Plugin from __future__ import print_function @@ -322,6 +321,163 @@ def __init__(self, msg): self.right_analog_y = msg.axes[3] self.orig_msg = msg +class PS4Status(JoyStatus): + def __init__(self, msg): + JoyStatus.__init__(self) + #creating from sensor_msg/Joy + if msg.buttons[12] == 1: + self.center = True + else: + self.center = False + if msg.buttons[8] == 1: + self.select = True + else: + self.select = False + if msg.buttons[9] == 1: + self.start = True + else: + self.start = False + if msg.buttons[10] == 1: + self.L3 = True + else: + self.L3 = False + if msg.buttons[11] == 1: + self.R3 = True + else: + self.R3 = False + if msg.buttons[0] == 1: + self.square = True + else: + self.square = False + if msg.axes[10] < 0: + self.up = True + else: + self.up = False + if msg.axes[10] > 0: + self.down = True + else: + self.down = False + if msg.axes[9] < 0: + self.left = True + else: + self.left = False + if msg.axes[9] > 0: + self.right = True + else: + self.right = False + if msg.buttons[3] == 1: + self.triangle = True + else: + self.triangle = False + if msg.buttons[1] == 1: + self.cross = True + else: + self.cross = False + if msg.buttons[2] == 1: + self.circle = True + else: + self.circle = False + if msg.buttons[4] == 1: + self.L1 = True + else: + self.L1 = False + if msg.buttons[5] == 1: + self.R1 = True + else: + self.R1 = False + if msg.buttons[6] == 1: + self.L2 = True + else: + self.L2 = False + if msg.buttons[7] == 1: + self.R2 = True + else: + self.R2 = False + self.left_analog_x = msg.axes[0] + self.left_analog_y = msg.axes[1] + self.right_analog_x = msg.axes[5] + self.right_analog_y = msg.axes[2] + self.orig_msg = msg + +class PS4WiredStatus(JoyStatus): + def __init__(self, msg): + JoyStatus.__init__(self) + #creating from sensor_msg/Joy + if msg.buttons[10] == 1: + self.center = True + else: + self.center = False + if msg.buttons[8] == 1: + self.select = True + else: + self.select = False + if msg.buttons[9] == 1: + self.start = True + else: + self.start = False + if msg.buttons[11] == 1: + self.L3 = True + else: + self.L3 = False + if msg.buttons[12] == 1: + self.R3 = True + else: + self.R3 = False + if msg.buttons[3] == 1: + self.square = True + else: + self.square = False + if msg.axes[7] < 0: + self.up = True + else: + self.up = False + if msg.axes[7] > 0: + self.down = True + else: + self.down = False + if msg.axes[6] < 0: + self.left = True + else: + self.left = False + if msg.axes[6] > 0: + self.right = True + else: + self.right = False + if msg.buttons[2] == 1: + self.triangle = True + else: + self.triangle = False + if msg.buttons[0] == 1: + self.cross = True + else: + self.cross = False + if msg.buttons[1] == 1: + self.circle = True + else: + self.circle = False + if msg.buttons[4] == 1: + self.L1 = True + else: + self.L1 = False + if msg.buttons[5] == 1: + self.R1 = True + else: + self.R1 = False + if msg.buttons[6] == 1: + self.L2 = True + else: + self.L2 = False + if msg.buttons[7] == 1: + self.R2 = True + else: + self.R2 = False + self.left_analog_x = msg.axes[0] + self.left_analog_y = msg.axes[1] + self.right_analog_x = msg.axes[3] + self.right_analog_y = msg.axes[4] + self.orig_msg = msg + + class StatusHistory(): def __init__(self, max_length=10): self.max_length = max_length @@ -453,7 +609,7 @@ def waitForInitialPose(self, next_topic, timeout=None): self.marker_lock.acquire() self.initialize_poses = True topic_suffix = next_topic.split("/")[-1] - if self.initial_poses.has_key(topic_suffix): + if topic_suffix in self.initial_poses: self.pre_pose = PoseStamped(pose=self.initial_poses[topic_suffix]) self.initialize_poses = False return True @@ -471,6 +627,10 @@ def joyCB(self, msg): status = XBoxStatus(msg) elif len(msg.axes) == 20 and len(msg.buttons) == 17: status = PS3Status(msg) + elif len(msg.axes) == 14 and len(msg.buttons) == 14: + status = PS4Status(msg) + elif len(msg.axes) == 8 and len(msg.buttons) == 13: + status = PS4WiredStatus(msg) else: raise Exception("Unknown joystick") self.run(status) diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt index 081058868c..1708a975e9 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt @@ -16,7 +16,7 @@ set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NA target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools moveit_planning_scene_rviz_plugin_core - ${catkin_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} + ${catkin_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} ) # Plugin Initializer @@ -27,5 +27,6 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ${catkin_LIBRAR install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(TARGETS ${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h index 209778d44f..a2de42443d 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h +++ b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h @@ -36,8 +36,7 @@ Desc: Wraps a trajectory_visualization playback class for Rviz into a stand alone display */ -#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_DISPLAY -#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_DISPLAY +#pragma once #include @@ -66,6 +65,7 @@ class TrajectoryDisplay : public rviz::Display void loadRobotModel(); + void load(const rviz::Config& config) override; void update(float wall_dt, float ros_dt) override; void reset() override; @@ -87,14 +87,11 @@ private Q_SLOTS: // Load robot model rdf_loader::RDFLoaderPtr rdf_loader_; - robot_model::RobotModelConstPtr robot_model_; - robot_state::RobotStatePtr robot_state_; - bool load_robot_model_; // for delayed robot initialization + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; // Properties rviz::StringProperty* robot_description_property_; }; } // namespace moveit_rviz_plugin - -#endif diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp index 971c31f87e..a3b5edb7d1 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -41,13 +41,14 @@ namespace moveit_rviz_plugin { -TrajectoryDisplay::TrajectoryDisplay() : Display(), load_robot_model_(false) +TrajectoryDisplay::TrajectoryDisplay() : Display() { // The robot description property is only needed when using the trajectory playback standalone (not within motion // planning plugin) - robot_description_property_ = new rviz::StringProperty( - "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", - this, SLOT(changedRobotDescription()), this); + robot_description_property_ = + new rviz::StringProperty("Robot Description", "robot_description", + "The name of the ROS parameter where the URDF for the robot is loaded", this, + SLOT(changedRobotDescription()), this); trajectory_visual_.reset(new TrajectoryVisualization(this, this)); } @@ -63,24 +64,29 @@ void TrajectoryDisplay::onInitialize() void TrajectoryDisplay::loadRobotModel() { - load_robot_model_ = false; - rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); - - if (!rdf_loader_->getURDF()) + try { - this->setStatus(rviz::StatusProperty::Error, "Robot Model", - "Failed to load from parameter " + robot_description_property_->getString()); - return; + rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); + + if (!rdf_loader_->getURDF()) + { + this->setStatus(rviz::StatusProperty::Error, "Robot Model", + "Failed to load from parameter " + robot_description_property_->getString()); + return; + } + this->setStatus(rviz::StatusProperty::Ok, "Robot Model", "Successfully loaded"); + + const srdf::ModelSharedPtr& srdf = + rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); + robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); + + // Send to child class + trajectory_visual_->onRobotModelLoaded(robot_model_); + } + catch (std::exception& e) + { + setStatus(rviz::StatusProperty::Error, "RobotModel", QString("Loading failed: %1").arg(e.what())); } - this->setStatus(rviz::StatusProperty::Ok, "Robot Model", "Successfully loaded"); - - const srdf::ModelSharedPtr& srdf = - rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); - robot_model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf)); - - // Send to child class - trajectory_visual_->onRobotModelLoaded(robot_model_); - trajectory_visual_->onEnable(); } void TrajectoryDisplay::reset() @@ -90,10 +96,20 @@ void TrajectoryDisplay::reset() trajectory_visual_->reset(); } +void TrajectoryDisplay::load(const rviz::Config& config) +{ + // This property needs to be loaded in onEnable() below, which is triggered + // in the beginning of Display::load() before the other property would be available + robot_description_property_->load(config.mapGetChild("Robot Description")); + Display::load(config); +} + void TrajectoryDisplay::onEnable() { Display::onEnable(); - load_robot_model_ = true; // allow loading of robot model in update() + if (!rdf_loader_) + loadRobotModel(); + trajectory_visual_->onEnable(); } void TrajectoryDisplay::onDisable() @@ -105,10 +121,6 @@ void TrajectoryDisplay::onDisable() void TrajectoryDisplay::update(float wall_dt, float ros_dt) { Display::update(wall_dt, ros_dt); - - if (load_robot_model_) - loadRobotModel(); - trajectory_visual_->update(wall_dt, ros_dt); } diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst index e3d3c6dd37..aceba5378e 100644 --- a/moveit_ros/warehouse/CHANGELOG.rst +++ b/moveit_ros/warehouse/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package moveit_ros_warehouse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [maint] Cleanup MSA includes (`#2351 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, Robert Haschke + +1.1.0 (2020-09-04) +------------------ +* [feature] Optional cpp version setting (`#2166 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#1419 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Ayush Garg, Dave Coleman, Henning Kayser, Jonathan Binney, Max Krichenbauer, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan + +1.0.6 (2020-08-19) +------------------ +* [maint] Migrate to clang-format-10 +* Contributors: Robert Haschke + +1.0.5 (2020-07-08) +------------------ + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [fix] Fix potential use of uninitialized variable (`#1818 `_) +* Contributors: Max Krichenbauer, Robert Haschke, Sean Yen, Yu, Yan + +1.0.2 (2019-06-28) +------------------ + 1.0.1 (2019-03-08) ------------------ * [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) @@ -32,7 +73,7 @@ Changelog for package moveit_ros_warehouse 0.10.2 (2018-10-24) ------------------- -* [fix] Text refrences to MoveIt! (`#1020 `_) +* [fix] Text refrences to MoveIt (`#1020 `_) * [enhancement] warehouse: added params for timeout + #retries (`#1008 `_) * [maintenance] various compiler warnings (`#1038 `_) * Contributors: Mohmmad Ayman, Robert Haschke, dg-shadow, mike lautman @@ -65,7 +106,7 @@ Changelog for package moveit_ros_warehouse 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * Contributors: Dave Coleman 0.9.4 (2017-02-06) diff --git a/moveit_ros/warehouse/CMakeLists.txt b/moveit_ros/warehouse/CMakeLists.txt index 2cb1205312..d974831537 100644 --- a/moveit_ros/warehouse/CMakeLists.txt +++ b/moveit_ros/warehouse/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_ros_warehouse) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -25,9 +29,11 @@ catkin_package( warehouse/include CATKIN_DEPENDS moveit_ros_planning - warehouse_ros) + warehouse_ros + roscpp +) include_directories(warehouse/include) -include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) add_subdirectory(warehouse) diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml index 976cdd1c79..1d117afe12 100644 --- a/moveit_ros/warehouse/package.xml +++ b/moveit_ros/warehouse/package.xml @@ -1,12 +1,12 @@ moveit_ros_warehouse - 1.0.1 - Components of MoveIt! connecting to MongoDB + 1.1.1 + Components of MoveIt connecting to MongoDB Ioan Sucan Michael Ferguson - MoveIt! Release Team + MoveIt Release Team BSD diff --git a/moveit_ros/warehouse/warehouse/CMakeLists.txt b/moveit_ros/warehouse/warehouse/CMakeLists.txt index 2a459f97c0..5f1b0e000c 100644 --- a/moveit_ros/warehouse/warehouse/CMakeLists.txt +++ b/moveit_ros/warehouse/warehouse/CMakeLists.txt @@ -32,12 +32,17 @@ target_link_libraries(moveit_warehouse_services ${catkin_LIBRARIES} ${MOVEIT_LIB install( TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +install( + TARGETS moveit_save_to_warehouse moveit_warehouse_broadcast moveit_warehouse_import_from_text moveit_warehouse_save_as_text moveit_init_demo_warehouse moveit_warehouse_services - LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h index 4bee5469a5..4b3be06f82 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_CONSTRAINTS_STORAGE_ -#define MOVEIT_MOVEIT_WAREHOUSE_CONSTRAINTS_STORAGE_ +#pragma once #include "moveit/warehouse/moveit_message_storage.h" #include @@ -46,7 +45,7 @@ namespace moveit_warehouse typedef warehouse_ros::MessageWithMetadata::ConstPtr ConstraintsWithMetadata; typedef warehouse_ros::MessageCollection::Ptr ConstraintsCollection; -MOVEIT_CLASS_FORWARD(ConstraintsStorage); +MOVEIT_CLASS_FORWARD(ConstraintsStorage); // Defines ConstraintsStoragePtr, ConstPtr, WeakPtr... etc class ConstraintsStorage : public MoveItMessageStorage { @@ -59,8 +58,7 @@ class ConstraintsStorage : public MoveItMessageStorage ConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn); - void addConstraints(const moveit_msgs::Constraints& msg, const std::string& robot = "", - const std::string& group = ""); + void addConstraints(const moveit_msgs::Constraints& msg, const std::string& robot = "", const std::string& group = ""); bool hasConstraints(const std::string& name, const std::string& robot = "", const std::string& group = "") const; void getKnownConstraints(std::vector& names, const std::string& robot = "", const std::string& group = "") const; @@ -83,6 +81,4 @@ class ConstraintsStorage : public MoveItMessageStorage ConstraintsCollection constraints_collection_; }; -} - -#endif +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/moveit_message_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/moveit_message_storage.h index 6b8b179258..fbc6077e47 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/moveit_message_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/moveit_message_storage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_MOVEIT_MESSAGE_STORAGE_ -#define MOVEIT_MOVEIT_WAREHOUSE_MOVEIT_MESSAGE_STORAGE_ +#pragma once #include #include @@ -65,6 +64,4 @@ class MoveItMessageStorage /// \brief Load a database connection typename warehouse_ros::DatabaseConnection::Ptr loadDatabase(); -} - -#endif +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h index ec7d65c4b3..bb1e24a07a 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_STORAGE_ -#define MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_STORAGE_ +#pragma once #include "moveit/warehouse/moveit_message_storage.h" #include @@ -53,7 +52,7 @@ typedef warehouse_ros::MessageCollection::Ptr Planni typedef warehouse_ros::MessageCollection::Ptr MotionPlanRequestCollection; typedef warehouse_ros::MessageCollection::Ptr RobotTrajectoryCollection; -MOVEIT_CLASS_FORWARD(PlanningSceneStorage); +MOVEIT_CLASS_FORWARD(PlanningSceneStorage); // Defines PlanningSceneStoragePtr, ConstPtr, WeakPtr... etc class PlanningSceneStorage : public MoveItMessageStorage { @@ -119,6 +118,4 @@ class PlanningSceneStorage : public MoveItMessageStorage MotionPlanRequestCollection motion_plan_request_collection_; RobotTrajectoryCollection robot_trajectory_collection_; }; -} - -#endif +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_world_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_world_storage.h index bafb6ab688..8e1b34af82 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_world_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_world_storage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_WORLD_STORAGE_ -#define MOVEIT_MOVEIT_WAREHOUSE_PLANNING_SCENE_WORLD_STORAGE_ +#pragma once #include #include @@ -72,6 +71,4 @@ class PlanningSceneWorldStorage : public MoveItMessageStorage PlanningSceneWorldCollection planning_scene_world_collection_; }; -} - -#endif +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h index fed8808143..3651cb1144 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h @@ -34,8 +34,7 @@ /* Author: Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_STATE_STORAGE_ -#define MOVEIT_MOVEIT_WAREHOUSE_STATE_STORAGE_ +#pragma once #include #include @@ -46,7 +45,7 @@ namespace moveit_warehouse typedef warehouse_ros::MessageWithMetadata::ConstPtr RobotStateWithMetadata; typedef warehouse_ros::MessageCollection::Ptr RobotStateCollection; -MOVEIT_CLASS_FORWARD(RobotStateStorage); +MOVEIT_CLASS_FORWARD(RobotStateStorage); // Defines RobotStateStoragePtr, ConstPtr, WeakPtr... etc class RobotStateStorage : public MoveItMessageStorage { @@ -78,6 +77,4 @@ class RobotStateStorage : public MoveItMessageStorage RobotStateCollection state_collection_; }; -} - -#endif +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h index 1b0cbe501d..934f510a36 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h @@ -34,8 +34,7 @@ /* Author: Mario Prats, Ioan Sucan */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_TRAJECTORY_CONSTRAINTS_STORAGE_ -#define MOVEIT_MOVEIT_WAREHOUSE_TRAJECTORY_CONSTRAINTS_STORAGE_ +#pragma once #include "moveit/warehouse/moveit_message_storage.h" #include @@ -47,7 +46,7 @@ typedef warehouse_ros::MessageWithMetadata:: TrajectoryConstraintsWithMetadata; typedef warehouse_ros::MessageCollection::Ptr TrajectoryConstraintsCollection; -MOVEIT_CLASS_FORWARD(TrajectoryConstraintsStorage); +MOVEIT_CLASS_FORWARD(TrajectoryConstraintsStorage); // Defines TrajectoryConstraintsStoragePtr, ConstPtr, WeakPtr... etc class TrajectoryConstraintsStorage : public MoveItMessageStorage { @@ -79,13 +78,11 @@ class TrajectoryConstraintsStorage : public MoveItMessageStorage void removeTrajectoryConstraints(const std::string& name, const std::string& robot = "", const std::string& group = ""); - void reset(void); + void reset(); private: - void createCollections(void); + void createCollections(); TrajectoryConstraintsCollection constraints_collection_; }; -} - -#endif +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/warehouse_connector.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/warehouse_connector.h index d61df45bae..04e279a3ca 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/warehouse_connector.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/warehouse_connector.h @@ -34,8 +34,7 @@ /* Author: E. Gil Jones */ -#ifndef MOVEIT_MOVEIT_WAREHOUSE_WAREHOUSE_CONNECTOR_ -#define MOVEIT_MOVEIT_WAREHOUSE_WAREHOUSE_CONNECTOR_ +#pragma once #include @@ -54,6 +53,4 @@ class WarehouseConnector std::string dbexec_; int child_pid_; }; -} - -#endif +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/warehouse/src/broadcast.cpp index 53f8241a91..51954964b6 100644 --- a/moveit_ros/warehouse/warehouse/src/broadcast.cpp +++ b/moveit_ros/warehouse/warehouse/src/broadcast.cpp @@ -61,10 +61,11 @@ int main(int argc, char** argv) double delay = 0.001; boost::program_options::options_description desc; - desc.add_options()("help", "Show help message")("host", boost::program_options::value(), "Host for the " - "DB.")( - "port", boost::program_options::value(), - "Port for the DB.")("scene", boost::program_options::value(), "Name of scene to publish.")( + desc.add_options()("help", "Show help message")("host", boost::program_options::value(), + "Host for the " + "DB.")("port", boost::program_options::value(), + "Port for the DB.")( + "scene", boost::program_options::value(), "Name of scene to publish.")( "planning_requests", "Also publish the planning requests that correspond to the scene")( "planning_results", "Also publish the planning results that correspond to the scene")( "constraint", boost::program_options::value(), "Name of constraint to publish.")( diff --git a/moveit_ros/warehouse/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/warehouse/src/import_from_text.cpp old mode 100644 new mode 100755 index 78fcdfd91b..944d5e90f4 --- a/moveit_ros/warehouse/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/warehouse/src/import_from_text.cpp @@ -73,18 +73,20 @@ void parseStart(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* if (marker != "=") joint = "."; else + { in >> value; - v[joint] = value; + v[joint] = value; + } if (joint != ".") in >> joint; } } if (!v.empty()) { - robot_state::RobotState st = psm->getPlanningScene()->getCurrentState(); + moveit::core::RobotState st = psm->getPlanningScene()->getCurrentState(); st.setVariablePositions(v); moveit_msgs::RobotState msg; - robot_state::robotStateToRobotStateMsg(st, msg); + moveit::core::robotStateToRobotStateMsg(st, msg); ROS_INFO("Parsed start state '%s'", name.c_str()); rs->addRobotState(msg, name); } diff --git a/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp b/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp index 28cc32568f..8dab206f3e 100644 --- a/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp +++ b/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp @@ -55,9 +55,10 @@ int main(int argc, char** argv) ros::init(argc, argv, "initialize_demo_db", ros::init_options::AnonymousName); boost::program_options::options_description desc; - desc.add_options()("help", "Show help message")("host", boost::program_options::value(), "Host for the " - "DB.")( - "port", boost::program_options::value(), "Port for the DB."); + desc.add_options()("help", "Show help message")("host", boost::program_options::value(), + "Host for the " + "DB.")("port", boost::program_options::value(), + "Port for the DB."); boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); @@ -101,14 +102,14 @@ int main(int argc, char** argv) ROS_INFO("Added default scene: '%s'", psmsg.name.c_str()); moveit_msgs::RobotState rsmsg; - robot_state::robotStateToRobotStateMsg(psm.getPlanningScene()->getCurrentState(), rsmsg); + moveit::core::robotStateToRobotStateMsg(psm.getPlanningScene()->getCurrentState(), rsmsg); rs.addRobotState(rsmsg, "default"); ROS_INFO("Added default state"); const std::vector& gnames = psm.getRobotModel()->getJointModelGroupNames(); for (const std::string& gname : gnames) { - const robot_model::JointModelGroup* jmg = psm.getRobotModel()->getJointModelGroup(gname); + const moveit::core::JointModelGroup* jmg = psm.getRobotModel()->getJointModelGroup(gname); if (!jmg->isChain()) continue; const std::vector& lnames = jmg->getLinkModelNames(); @@ -132,7 +133,7 @@ int main(int argc, char** argv) cs.addConstraints(cmsg, psm.getRobotModel()->getName(), jmg->getName()); ROS_INFO("Added default constraint: '%s'", cmsg.name.c_str()); } - ROS_INFO("Default MoveIt! Warehouse was reset."); + ROS_INFO("Default MoveIt Warehouse was reset."); ros::shutdown(); diff --git a/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp b/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp index b0f5bef94e..d45cffc0bd 100644 --- a/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp +++ b/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp @@ -36,7 +36,6 @@ #include #include -//#include #include #include #include @@ -46,8 +45,7 @@ moveit_warehouse::MoveItMessageStorage::MoveItMessageStorage(warehouse_ros::Data { } -void moveit_warehouse::MoveItMessageStorage::filterNames(const std::string& regex, - std::vector& names) const +void moveit_warehouse::MoveItMessageStorage::filterNames(const std::string& regex, std::vector& names) const { if (!regex.empty()) { diff --git a/moveit_ros/warehouse/warehouse/src/planning_scene_storage.cpp b/moveit_ros/warehouse/warehouse/src/planning_scene_storage.cpp index 18bffebbf1..4010ee5b00 100644 --- a/moveit_ros/warehouse/warehouse/src/planning_scene_storage.cpp +++ b/moveit_ros/warehouse/warehouse/src/planning_scene_storage.cpp @@ -92,8 +92,9 @@ bool moveit_warehouse::PlanningSceneStorage::hasPlanningScene(const std::string& return !planning_scenes.empty(); } -std::string moveit_warehouse::PlanningSceneStorage::getMotionPlanRequestName( - const moveit_msgs::MotionPlanRequest& planning_query, const std::string& scene_name) const +std::string +moveit_warehouse::PlanningSceneStorage::getMotionPlanRequestName(const moveit_msgs::MotionPlanRequest& planning_query, + const std::string& scene_name) const { // get all existing motion planning requests for this planning scene Query::Ptr q = motion_plan_request_collection_->createQuery(); diff --git a/moveit_ros/warehouse/warehouse/src/save_as_text.cpp b/moveit_ros/warehouse/warehouse/src/save_as_text.cpp index 7926c60edc..eb5df51ca1 100644 --- a/moveit_ros/warehouse/warehouse/src/save_as_text.cpp +++ b/moveit_ros/warehouse/warehouse/src/save_as_text.cpp @@ -79,9 +79,10 @@ int main(int argc, char** argv) ros::init(argc, argv, "save_warehouse_as_text", ros::init_options::AnonymousName); boost::program_options::options_description desc; - desc.add_options()("help", "Show help message")("host", boost::program_options::value(), "Host for the " - "DB.")( - "port", boost::program_options::value(), "Port for the DB."); + desc.add_options()("help", "Show help message")("host", boost::program_options::value(), + "Host for the " + "DB.")("port", boost::program_options::value(), + "Port for the DB."); boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); @@ -123,7 +124,7 @@ int main(int argc, char** argv) fout.close(); std::vector robot_state_names; - robot_model::RobotModelConstPtr km = psm.getRobotModel(); + moveit::core::RobotModelConstPtr km = psm.getRobotModel(); // Get start states for scene std::stringstream rsregex; rsregex << ".*" << scene_name << ".*"; @@ -150,8 +151,8 @@ int main(int argc, char** argv) qfout << robot_state_name << std::endl; moveit_warehouse::RobotStateWithMetadata robot_state; rss.getRobotState(robot_state, robot_state_name); - robot_state::RobotState ks(km); - robot_state::robotStateMsgToRobotState(*robot_state, ks, false); + moveit::core::RobotState ks(km); + moveit::core::robotStateMsgToRobotState(*robot_state, ks, false); ks.printStateInfo(qfout); qfout << "." << std::endl; } diff --git a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp index fe5b2e8713..4b1afa32a3 100644 --- a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp @@ -117,9 +117,10 @@ int main(int argc, char** argv) ros::init(argc, argv, "save_to_warehouse", ros::init_options::AnonymousName); boost::program_options::options_description desc; - desc.add_options()("help", "Show help message")("host", boost::program_options::value(), "Host for the " - "DB.")( - "port", boost::program_options::value(), "Port for the DB."); + desc.add_options()("help", "Show help message")("host", boost::program_options::value(), + "Host for the " + "DB.")("port", boost::program_options::value(), + "Port for the DB."); boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); diff --git a/moveit_ros/warehouse/warehouse/src/trajectory_constraints_storage.cpp b/moveit_ros/warehouse/warehouse/src/trajectory_constraints_storage.cpp index e2983b22ab..49bc25504b 100644 --- a/moveit_ros/warehouse/warehouse/src/trajectory_constraints_storage.cpp +++ b/moveit_ros/warehouse/warehouse/src/trajectory_constraints_storage.cpp @@ -47,8 +47,7 @@ const std::string moveit_warehouse::TrajectoryConstraintsStorage::ROBOT_NAME = " using warehouse_ros::Metadata; using warehouse_ros::Query; -moveit_warehouse::TrajectoryConstraintsStorage::TrajectoryConstraintsStorage( - warehouse_ros::DatabaseConnection::Ptr conn) +moveit_warehouse::TrajectoryConstraintsStorage::TrajectoryConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn) : MoveItMessageStorage(std::move(conn)) { createCollections(); diff --git a/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp b/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp index 97e89d29d7..cc1614f668 100644 --- a/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp +++ b/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp @@ -96,7 +96,8 @@ bool getState(moveit_msgs::GetRobotStateFromWarehouse::Request& request, } bool renameState(moveit_msgs::RenameRobotStateInWarehouse::Request& request, - moveit_msgs::RenameRobotStateInWarehouse::Response& response, moveit_warehouse::RobotStateStorage* rs) + moveit_msgs::RenameRobotStateInWarehouse::Response& /*response*/, + moveit_warehouse::RobotStateStorage* rs) { if (!rs->hasRobotState(request.old_name, request.robot)) { @@ -108,7 +109,7 @@ bool renameState(moveit_msgs::RenameRobotStateInWarehouse::Request& request, } bool deleteState(moveit_msgs::DeleteRobotStateFromWarehouse::Request& request, - moveit_msgs::DeleteRobotStateFromWarehouse::Response& response, + moveit_msgs::DeleteRobotStateFromWarehouse::Response& /*response*/, moveit_warehouse::RobotStateStorage* rs) { if (!rs->hasRobotState(request.name, request.robot)) diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst index bb959b1866..93bff64b2d 100644 --- a/moveit_runtime/CHANGELOG.rst +++ b/moveit_runtime/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package moveit_runtime ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ + +1.1.0 (2020-09-04) +------------------ +* [feature] Use standard cmake text for metapackages (`#1620 `_) +* [feature] Use CMAKE_CXX_STANDARD to enforce c++14 for portability (`#1607 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* Contributors: Dave Coleman, Jonathan Binney, Robert Haschke, Sean Yen, Tyler Weaver + 1.0.1 (2019-03-08) ------------------ diff --git a/moveit_runtime/CMakeLists.txt b/moveit_runtime/CMakeLists.txt index 18700d6157..0224cde26c 100644 --- a/moveit_runtime/CMakeLists.txt +++ b/moveit_runtime/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(moveit_runtime) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml index 8efd4a159f..e13e5882b8 100644 --- a/moveit_runtime/package.xml +++ b/moveit_runtime/package.xml @@ -1,8 +1,8 @@ moveit_runtime - 1.0.1 - moveit_runtime meta package contains MoveIt! packages that are essential for its runtime (e.g. running MoveIt! on robots). + 1.1.1 + moveit_runtime meta package contains MoveIt packages that are essential for its runtime (e.g. running MoveIt on robots). Isaac I. Y. Saito Isaac I. Y. Saito diff --git a/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/CHANGELOG.rst index a117cec6fa..3ec3076863 100644 --- a/moveit_setup_assistant/CHANGELOG.rst +++ b/moveit_setup_assistant/CHANGELOG.rst @@ -2,6 +2,89 @@ Changelog for package moveit_setup_assistant ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.1 (2020-10-13) +------------------ +* [feature] Allow showing both, visual and collision geometry (`#2352 `_) +* [fix] layout (`#2349 `_) +* [fix] group editing (`#2350 `_) +* [fix] only write default_planner_config field if any is selected (`#2293 `_) +* [fix] Segfault when editing pose in moveit_setup_assistant (`#2340 `_) +* [fix] disappearing robot on change of reference frame (`#2335 `_) +* [fix] robot_description is already loaded in move_group.launch (`#2313 `_) +* [maint] Cleanup MSA includes (`#2351 `_) +* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) +* Contributors: Felix von Drigalski, Michael GΓΆrner, Robert Haschke, Tyler Weaver, Yoan Mollard + +1.1.0 (2020-09-04) +------------------ +* [feature] Start new joint_state_publisher_gui on param use_gui (`#2257 `_) +* [feature] Optional cpp version setting (`#2166 `_) +* [feature] Add default velocity/acceleration scaling factors (`#1890 `_) +* [feature] MSA: use matching group/state name for default controller state (`#1936 `_) +* [feature] MSA: Restore display of current directory (`#1932 `_) +* [feature] Cleanup: use range-based for-loop (`#1830 `_) +* [feature] Add delete process to the doneEditing() function in end_effectors_widgets (`#1829 `_) +* [feature] Fix Rviz argument in demo_gazebo.launch (`#1797 `_) +* [feature] Allow user to specify planner termination condition. (`#1695 `_) +* [feature] Add OMPL planner 'AnytimePathShortening' (`#1686 `_) +* [feature] MVP TrajOpt Planner Plugin (`#1593 `_) +* [feature] Use QDir::currentPath() rather than getenv("PWD") (`#1618 `_) +* [feature] Add named frames to CollisionObjects (`#1439 `_) +* [fix] Various fixes for upcoming Noetic release (`#2180 `_) +* [fix] Fix ordering of request adapters (`#2053 `_) +* [fix] Fix some clang tidy issues (`#2004 `_) +* [fix] Fix usage of panda_moveit_config (`#1904 `_) +* [fix] Fix compiler warnings (`#1773 `_) +* [fix] Use portable string() on filesystem::path. (`#1571 `_) +* [fix] Fix test utilities in moveit core (`#1409 `_) +* [maint] clang-tidy fixes (`#2050 `_, `#1419 `_) +* [maint] Replace namespaces robot_state and robot_model with moveit::core (`#1924 `_) +* [maint] Switch from include guards to pragma once (`#1615 `_) +* [maint] Remove ! from MoveIt name (`#1590 `_) +* [maint] remove obsolete moveit_resources/config.h (`#1412 `_) +* Contributors: AndyZe, Ayush Garg, Daniel Wang, Dave Coleman, Felix von Drigalski, Henning Kayser, Jafar Abdi, Jonathan Binney, Mark Moll, Max Krichenbauer, Michael GΓΆrner, Mike Lautman, Mohmmad Ayman, Omid Heidari, Robert Haschke, Sandro MagalhΓ£es, Sean Yen, Simon Schmeisser, Tejas Kumar Shastha, Tyler Weaver, Yoan Mollard, Yu, Yan, jschleicher, tnaka, v4hn + +1.0.6 (2020-08-19) +------------------ +* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) +* [maint] Migrate to clang-format-10, fix warnings +* [fix] Define planning adapters for chomp planning pipeline (`#2242 `_) +* [maint] Remove urdf package as build_depend from package.xml (`#2207 `_) +* Contributors: Jafar Abdi, Robert Haschke, tnaka, Michael GΓΆrner + +1.0.5 (2020-07-08) +------------------ +* [fix] Fix catkin_lint issues (`#2120 `_) +* [feature] Add use_rviz to demo.launch in setup_assistant (`#2019 `_) +* Contributors: Henning Kayser, Jafar Abdi, Michael GΓΆrner, Robert Haschke, Tyler Weaver + +1.0.4 (2020-05-30) +------------------ + +1.0.3 (2020-04-26) +------------------ +* [feature] Allow loading of additional kinematics parameters file (`#1997 `_) +* [feature] Allow adding initial poses to fake_controllers.yaml (`#1892 `_) +* [feature] Display robot poses on selection, not only on click (`#1930 `_) +* [fix] Fix invalid iterator (`#1623 `_) +* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) +* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) +* [maint] Windows build fixes + * Fix header inclusion and other MSVC build errors (`#1636 `_) + * Fix binary artifact install locations. (`#1575 `_) + * Favor ros::Duration.sleep over sleep. (`#1634 `_) + * Fix binary artifact install locations. (`#1575 `_) +* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) +* [feature] Add support for pos_vel_controllers and pos_vel_acc_controllers (`#1806 `_) +* [feature] Add joint state controller config by default (`#1024 `_) +* Contributors: AndyZe, Daniel Wang, Felix von Drigalski, Jafar Abdi, Max Krichenbauer, Michael GΓΆrner, Mohmmad Ayman, Robert Haschke, Sandro MagalhΓ£es, Sean Yen, Simon Schmeisser, Tejas Kumar Shastha, Yu, Yan, v4hn + +1.0.2 (2019-06-28) +------------------ +* [fix] static transform publisher does not take a rate (`#1494 `_) +* [feature] Add arguments `load_robot_description`, `pipeline`, `rviz config_file` to launch file templates (`#1397 `_) +* Contributors: Mike Lautman, Robert Haschke, jschleicher + 1.0.1 (2019-03-08) ------------------ * [fix] re-add required build dependencies (`#1373 `_) @@ -51,7 +134,7 @@ Changelog for package moveit_setup_assistant * [capability][chomp] Failure recovery options for CHOMP by tweaking parameters (`#987 `_) * [capability] New screen for automatically generating interfaces to low level controllers(`#951 `_, `#994 `_, `#908 `_) * [capability] Perception screen for using laser scanner point clouds. (`#969 `_) -* [enhancement][GUI] Logo for MoveIt! 2.0, cleanup appearance (`#1059 `_) +* [enhancement][GUI] Logo for MoveIt 2.0, cleanup appearance (`#1059 `_) * [enhancement][GUI] added a setup assistant window icon (`#1028 `_) * [enhancement][GUI] Planning Groups screen (`#1017 `_) * [enhancement] use panda for test, and write test file in tmp dir (`#1042 `_) @@ -105,7 +188,7 @@ Changelog for package moveit_setup_assistant 0.9.5 (2017-03-08) ------------------ -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ +* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ * Contributors: Dave Coleman 0.9.4 (2017-02-06) @@ -131,7 +214,7 @@ Changelog for package moveit_setup_assistant 0.7.1 (2016-06-24) ------------------ -* [sys] Qt adjustment. +* [sys] Qt adjustment. * relax Qt-version requirement. Minor Qt version updates are ABI-compatible with each other: https://wiki.qt.io/Qt-Version-Compatibility * auto-select Qt version matching the one from rviz `#114 `_ * Allow to conditionally compile against Qt5 by setting -DUseQt5=On diff --git a/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/CMakeLists.txt index dc6339422f..c39d7cfe23 100644 --- a/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/CMakeLists.txt @@ -1,7 +1,12 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.1.3) project(moveit_setup_assistant) -add_compile_options(-std=c++14) +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") # suppress warning in Ogre add_compile_options(-Wno-deprecated-register) @@ -25,7 +30,9 @@ find_package(catkin REQUIRED COMPONENTS urdf srdfdom ) -include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +find_package(ompl REQUIRED) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${OMPL_INCLUDE_DIRS}) +include_directories(include) # Qt Stuff if(rviz_QT_VERSION VERSION_LESS "5") @@ -42,7 +49,7 @@ add_definitions(-DQT_NO_KEYWORDS) # Support new yaml-cpp API. find_package(PkgConfig) pkg_check_modules(YAMLCPP REQUIRED yaml-cpp>=0.5) -include_directories(${YAMLCPP_INCLUDE_DIRS}) +include_directories(SYSTEM ${YAMLCPP_INCLUDE_DIRS}) catkin_package( INCLUDE_DIRS @@ -52,6 +59,7 @@ catkin_package( CATKIN_DEPENDS moveit_ros_planning moveit_ros_visualization + roscpp ) # Header files that need Qt Moc pre-processing for use with Qt signals, etc: @@ -139,9 +147,14 @@ set_target_properties(${PROJECT_NAME}_updater PROPERTIES OUTPUT_NAME collisions_updater PREFIX "") -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_widgets ${PROJECT_NAME}_tools ${PROJECT_NAME}_updater - LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION} +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_updater RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(TARGETS ${PROJECT_NAME}_widgets ${PROJECT_NAME}_tools + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) @@ -150,9 +163,6 @@ install(DIRECTORY templates DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) # Unit tests if(CATKIN_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) catkin_add_gtest(test_reading_writing_config test/moveit_config_data_test.cpp) - target_link_libraries(test_reading_writing_config - ${PROJECT_NAME}_tools ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_reading_writing_config ${PROJECT_NAME}_tools) endif() diff --git a/moveit_setup_assistant/README.md b/moveit_setup_assistant/README.md index 07feef853a..f346ead9ae 100644 --- a/moveit_setup_assistant/README.md +++ b/moveit_setup_assistant/README.md @@ -1 +1 @@ -# MoveIt! Setup Assistant +# MoveIt Setup Assistant diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h index 207e24bd13..f7f41b1b66 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.h @@ -34,14 +34,13 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_TOOLS_COMPUTE_DEFAULT_COLLISIONS_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_TOOLS_COMPUTE_DEFAULT_COLLISIONS_ +#pragma once #include #include namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene); +MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc } namespace moveit_setup_assistant @@ -109,6 +108,4 @@ const std::string disabledReasonToString(DisabledReason reason); * \return reason as struct */ DisabledReason disabledReasonFromString(const std::string& reason); -} - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h index 788cfa0111..4601e24d65 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h +++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_TOOLS_MOVEIT_CONFIG_DATA_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_TOOLS_MOVEIT_CONFIG_DATA_ +#pragma once #include #include // for getting kinematic model @@ -44,6 +43,8 @@ #include // to share throughout app #include // for writing srdf data +#include + namespace moveit_setup_assistant { // ****************************************************************************************** @@ -55,8 +56,8 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; static const std::string MOVEIT_ROBOT_STATE = "moveit_robot_state"; // Default kin solver values -static const double DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION_ = 0.005; -static const double DEFAULT_KIN_SOLVER_TIMEOUT_ = 0.005; +static const double DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION = 0.005; +static const double DEFAULT_KIN_SOLVER_TIMEOUT = 0.005; // ****************************************************************************************** // Structs @@ -70,6 +71,7 @@ struct GroupMetaData std::string kinematics_solver_; // Name of kinematics plugin to use double kinematics_solver_search_resolution_; // resolution to use with solver double kinematics_solver_timeout_; // solver timeout + std::string kinematics_parameters_file_; // file for additional kinematics parameters std::string default_planner_; // Name of the default planner to use }; @@ -143,15 +145,15 @@ class GenericParameter void setName(std::string name) { - name_ = name; + name_ = std::move(name); }; void setValue(std::string value) { - value_ = value; + value_ = std::move(value); }; void setComment(std::string comment) { - comment_ = comment; + comment_ = std::move(comment); }; std::string getName() { @@ -172,10 +174,10 @@ class GenericParameter std::string comment_; // comment briefly describing what this parameter does }; -MOVEIT_CLASS_FORWARD(MoveItConfigData); +MOVEIT_CLASS_FORWARD(MoveItConfigData); // Defines MoveItConfigDataPtr, ConstPtr, WeakPtr... etc /** \brief This class is shared with all widgets and contains the common configuration data - needed for generating each robot's MoveIt! configuration package. + needed for generating each robot's MoveIt configuration package. All SRDF data is contained in a subclass of this class - srdf_writer.cpp. This class also contains the functions for writing @@ -203,7 +205,7 @@ class MoveItConfigData }; unsigned long changes; // bitfield of changes (composed of InformationFields) - // All of the data needed for creating a MoveIt! Configuration Files + // All of the data needed for creating a MoveIt Configuration Files // ****************************************************************************************** // URDF Data @@ -281,7 +283,7 @@ class MoveItConfigData void setRobotModel(const moveit::core::RobotModelPtr& robot_model); /// Provide a shared kinematic model loader - robot_model::RobotModelConstPtr getRobotModel(); + moveit::core::RobotModelConstPtr getRobotModel(); /// Update the Kinematic Model with latest SRDF modifications void updateRobotModel(); @@ -363,6 +365,13 @@ class MoveItConfigData */ bool inputKinematicsYAML(const std::string& file_path); + /** + * Input planning_context.launch for editing its values + * @param file_path path to planning_context.launch in the input package + * @return true if the file was read correctly + */ + bool inputPlanningContextLaunch(const std::string& file_path); + /** * Helper function for parsing ros_controllers.yaml file * @param YAML::Node - individual controller to be parsed @@ -397,6 +406,16 @@ class MoveItConfigData */ bool setPackagePath(const std::string& pkg_path); + /** + * determine the package name containing a given file path + * @param path to a file + * @param package_name holds the ros package name if found + * @param relative_filepath holds the relative path of the file to the package root + * @return whether the file belongs to a known package + */ + bool extractPackageNameFromPath(const std::string& path, std::string& package_name, + std::string& relative_filepath) const; + /** * Resolve path to .setup_assistant file * @param path resolved path @@ -411,7 +430,7 @@ class MoveItConfigData bool createFullSRDFPath(const std::string& package_path); /** - * Input .setup_assistant file - contains data used for the MoveIt! Setup Assistant + * Input .setup_assistant file - contains data used for the MoveIt Setup Assistant * * @param file_path path to .setup_assistant file * @return true if the file was read correctly @@ -482,15 +501,20 @@ class MoveItConfigData */ std::vector > getSensorPluginConfig(); + /** + * \brief Helper function to get the default start pose for moveit_sim_hw_interface + */ + srdf::Model::GroupState getDefaultStartPose(); + /** * \brief Custom std::set comparator, used for sorting the joint_limits.yaml file into alphabetical order * \param jm1 - a pointer to the first joint model to compare * \param jm2 - a pointer to the second joint model to compare * \return bool of alphabetical sorting comparison */ - struct joint_model_compare + struct JointModelCompare { - bool operator()(const robot_model::JointModel* jm1, const robot_model::JointModel* jm2) const + bool operator()(const moveit::core::JointModel* jm1, const moveit::core::JointModel* jm2) const { return jm1->getName() < jm2->getName(); } @@ -505,7 +529,7 @@ class MoveItConfigData std::vector > sensors_plugin_config_parameter_list_; /// Shared kinematic model - robot_model::RobotModelPtr robot_model_; + moveit::core::RobotModelPtr robot_model_; /// ROS Controllers config data std::vector ros_controllers_config_; @@ -514,6 +538,4 @@ class MoveItConfigData planning_scene::PlanningScenePtr planning_scene_; }; -} // namespace - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/launch/setup_assistant.launch b/moveit_setup_assistant/launch/setup_assistant.launch index 4d908ba325..104dc655da 100644 --- a/moveit_setup_assistant/launch/setup_assistant.launch +++ b/moveit_setup_assistant/launch/setup_assistant.launch @@ -1,4 +1,4 @@ - + @@ -8,7 +8,7 @@ diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml index 6db06b5a51..4f12137953 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/package.xml @@ -1,14 +1,14 @@ moveit_setup_assistant - 1.0.1 + 1.1.1 - Generates a configuration package that makes it easy to use MoveIt! + Generates a configuration package that makes it easy to use MoveIt Dave Coleman Dave Coleman Robert Haschke - MoveIt! Release Team + MoveIt Release Team BSD @@ -21,19 +21,21 @@ libogre-dev qtbase5-dev libqt5-opengl-dev + ompl + rviz moveit_core moveit_ros_planning moveit_ros_visualization rosconsole roscpp - rviz urdf yaml-cpp srdfdom xacro + rviz - moveit_resources + moveit_resources_panda_moveit_config rosunit diff --git a/moveit_setup_assistant/resources/source/MoveIt_Setup_Asst_Sm.xcf b/moveit_setup_assistant/resources/source/MoveIt_Setup_Asst_Sm.xcf index a3935559fd..a468ef5730 100644 Binary files a/moveit_setup_assistant/resources/source/MoveIt_Setup_Asst_Sm.xcf and b/moveit_setup_assistant/resources/source/MoveIt_Setup_Asst_Sm.xcf differ diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp index b5f16d98c4..89b418e96e 100644 --- a/moveit_setup_assistant/src/collisions_updater.cpp +++ b/moveit_setup_assistant/src/collisions_updater.cpp @@ -34,7 +34,6 @@ /* Author: Mathias LΓΌdtke */ -#include #include #include @@ -127,7 +126,7 @@ int main(int argc, char* argv[]) uint32_t never_trials = 0; po::options_description desc("Allowed options"); - desc.add_options()("help", "show help")("config-pkg", po::value(&config_pkg_path), "path to MoveIt! config package")( + desc.add_options()("help", "show help")("config-pkg", po::value(&config_pkg_path), "path to MoveIt config package")( "urdf", po::value(&urdf_path), "path to URDF ( or xacro)")("srdf", po::value(&srdf_path), "path to SRDF ( or xacro)")("output", po::value(&output_path), "output path for SRDF") diff --git a/moveit_setup_assistant/src/setup_assistant_main.cpp b/moveit_setup_assistant/src/setup_assistant_main.cpp index a514696622..8614ed71cb 100644 --- a/moveit_setup_assistant/src/setup_assistant_main.cpp +++ b/moveit_setup_assistant/src/setup_assistant_main.cpp @@ -42,7 +42,7 @@ #include #include -static void siginthandler(int param) +static void siginthandler(int /*param*/) { QApplication::quit(); } @@ -95,8 +95,8 @@ int main(int argc, char** argv) // Load Qt Widget moveit_setup_assistant::SetupAssistantWidget saw(nullptr, vm); - saw.setMinimumWidth(980); - saw.setMinimumHeight(550); + saw.setMinimumWidth(1090); + saw.setMinimumHeight(600); // saw.setWindowState( Qt::WindowMaximized ); saw.show(); diff --git a/moveit_setup_assistant/src/tools/collision_linear_model.cpp b/moveit_setup_assistant/src/tools/collision_linear_model.cpp index ea737dc63a..f28bd9390a 100644 --- a/moveit_setup_assistant/src/tools/collision_linear_model.cpp +++ b/moveit_setup_assistant/src/tools/collision_linear_model.cpp @@ -78,23 +78,23 @@ QModelIndex CollisionLinearModel::mapToSource(const QModelIndex& proxyIndex) con return sourceModel()->index(r, c); } -int CollisionLinearModel::rowCount(const QModelIndex& parent) const +int CollisionLinearModel::rowCount(const QModelIndex& /*parent*/) const { int n = this->sourceModel()->rowCount(); return (n * (n - 1) / 2); } -int CollisionLinearModel::columnCount(const QModelIndex& parent) const +int CollisionLinearModel::columnCount(const QModelIndex& /*parent*/) const { return 4; } -QModelIndex CollisionLinearModel::index(int row, int column, const QModelIndex& parent) const +QModelIndex CollisionLinearModel::index(int row, int column, const QModelIndex& /*parent*/) const { return createIndex(row, column); } -QModelIndex CollisionLinearModel::parent(const QModelIndex& child) const +QModelIndex CollisionLinearModel::parent(const QModelIndex& /*child*/) const { return QModelIndex(); } @@ -262,7 +262,7 @@ bool operator<(const QVariant& left, const QVariant& right) else return left.toString() < right.toString(); } -} +} // namespace #endif bool SortFilterProxyModel::lessThan(const QModelIndex& src_left, const QModelIndex& src_right) const diff --git a/moveit_setup_assistant/src/tools/collision_linear_model.h b/moveit_setup_assistant/src/tools/collision_linear_model.h index 9744170243..dd3dd4006a 100644 --- a/moveit_setup_assistant/src/tools/collision_linear_model.h +++ b/moveit_setup_assistant/src/tools/collision_linear_model.h @@ -34,8 +34,7 @@ /* Author: Robert Haschke */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_COLLISION_LINEAR_MODEL_H -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_COLLISION_LINEAR_MODEL_H +#pragma once #include #include @@ -52,7 +51,7 @@ class CollisionLinearModel : public QAbstractProxyModel Q_OBJECT public: - CollisionLinearModel(CollisionMatrixModel* src, QObject* parent = NULL); + CollisionLinearModel(CollisionMatrixModel* src, QObject* parent = nullptr); ~CollisionLinearModel() override; // reimplement to return the model index in the proxy model that to the sourceIndex from the source model @@ -82,7 +81,7 @@ class SortFilterProxyModel : public QSortFilterProxyModel Q_OBJECT public: - SortFilterProxyModel(QObject* parent = 0); + SortFilterProxyModel(QObject* parent = nullptr); QVariant headerData(int section, Qt::Orientation orientation, int role) const override; void sort(int column, Qt::SortOrder order) override; void setShowAll(bool show_all); @@ -100,5 +99,3 @@ private Q_SLOTS: QVector sort_columns_; // sorting history QVector sort_orders_; // corresponding sort orders }; - -#endif // MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_COLLISION_LINEAR_MODEL_H diff --git a/moveit_setup_assistant/src/tools/collision_matrix_model.h b/moveit_setup_assistant/src/tools/collision_matrix_model.h index dccbc2eed1..8d88b68bf5 100644 --- a/moveit_setup_assistant/src/tools/collision_matrix_model.h +++ b/moveit_setup_assistant/src/tools/collision_matrix_model.h @@ -34,8 +34,7 @@ /* Author: Robert Haschke */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_COLLISION_MATRIX_MODEL_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_COLLISION_MATRIX_MODEL_ +#pragma once #include @@ -50,7 +49,7 @@ class CollisionMatrixModel : public QAbstractTableModel Q_OBJECT public: CollisionMatrixModel(moveit_setup_assistant::LinkPairMap& pairs, const std::vector& names, - QObject* parent = NULL); + QObject* parent = nullptr); int rowCount(const QModelIndex& parent = QModelIndex()) const override; int columnCount(const QModelIndex& parent = QModelIndex()) const override; QVariant data(const QModelIndex& index, int role = Qt::DisplayRole) const override; @@ -59,7 +58,7 @@ class CollisionMatrixModel : public QAbstractTableModel // for editing Qt::ItemFlags flags(const QModelIndex& index) const override; - bool setData(const QModelIndex&, const QVariant& value, int role) override; + bool setData(const QModelIndex& /*index*/, const QVariant& value, int role) override; void setEnabled(const QItemSelection& selection, bool value); void setEnabled(const QModelIndexList& indexes, bool value); @@ -79,5 +78,3 @@ public Q_SLOTS: QList q_names; // names of links QList visual_to_index; // map from visual index to actual index }; - -#endif // MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_COLLISION_MATRIX_MODEL_ diff --git a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp index 05dc0d8808..58421c7b40 100644 --- a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp +++ b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp @@ -84,7 +84,7 @@ struct ThreadComputation }; // LinkGraph defines a Link's model and a set of unique links it connects -typedef std::map > LinkGraph; +typedef std::map > LinkGraph; // ****************************************************************************************** // Static Prototypes @@ -106,14 +106,14 @@ static bool setLinkPair(const std::string& linkA, const std::string& linkB, cons * \param link The root link to begin a breadth first search on * \param link_graph A representation of all bi-direcitonal joint connections between links in robot_description */ -static void computeConnectionGraph(const robot_model::LinkModel* link, LinkGraph& link_graph); +static void computeConnectionGraph(const moveit::core::LinkModel* link, LinkGraph& link_graph); /** * \brief Recursively build the adj list of link connections * \param link The root link to begin a breadth first search on * \param link_graph A representation of all bi-direcitonal joint connections between links in robot_description */ -static void computeConnectionGraphRec(const robot_model::LinkModel* link, LinkGraph& link_graph); +static void computeConnectionGraphRec(const moveit::core::LinkModel* link, LinkGraph& link_graph); /** * \brief Disable collision checking for adjacent links, or adjacent with no geometry links between them @@ -339,7 +339,7 @@ void computeLinkPairs(const planning_scene::PlanningScene& scene, LinkPairMap& l // ****************************************************************************************** // Build the robot links connection graph and then check for links with no geomotry // ****************************************************************************************** -void computeConnectionGraph(const robot_model::LinkModel* start_link, LinkGraph& link_graph) +void computeConnectionGraph(const moveit::core::LinkModel* start_link, LinkGraph& link_graph) { link_graph.clear(); // make sure the edges structure is clear @@ -358,7 +358,7 @@ void computeConnectionGraph(const robot_model::LinkModel* start_link, LinkGraph& if (edge_it->first->getShapes().empty()) // link in adjList "link_graph" does not have shape, remove! { // Temporary list for connected links - std::vector temp_list; + std::vector temp_list; // Copy link's parent and child links to temp_list for (const moveit::core::LinkModel* adj_it : edge_it->second) @@ -390,14 +390,14 @@ void computeConnectionGraph(const robot_model::LinkModel* start_link, LinkGraph& // ****************************************************************************************** // Recursively build the adj list of link connections // ****************************************************************************************** -void computeConnectionGraphRec(const robot_model::LinkModel* start_link, LinkGraph& link_graph) +void computeConnectionGraphRec(const moveit::core::LinkModel* start_link, LinkGraph& link_graph) { if (start_link) // check that the link is a valid pointer { // Loop through every link attached to start_link for (std::size_t i = 0; i < start_link->getChildJointModels().size(); ++i) { - const robot_model::LinkModel* next = start_link->getChildJointModels()[i]->getChildLinkModel(); + const moveit::core::LinkModel* next = start_link->getChildJointModels()[i]->getChildLinkModel(); // Bi-directional connection link_graph[next].insert(start_link); @@ -422,7 +422,7 @@ unsigned int disableAdjacentLinks(planning_scene::PlanningScene& scene, LinkGrap for (LinkGraph::const_iterator link_graph_it = link_graph.begin(); link_graph_it != link_graph.end(); ++link_graph_it) { // disable all connected links to current link by looping through them - for (std::set::const_iterator adj_it = link_graph_it->second.begin(); + for (std::set::const_iterator adj_it = link_graph_it->second.begin(); adj_it != link_graph_it->second.end(); ++adj_it) { // ROS_INFO("Disabled %s to %s", link_graph_it->first->getName().c_str(), (*adj_it)->getName().c_str() ); @@ -613,7 +613,7 @@ void disableNeverInCollisionThread(ThreadComputation tc) const unsigned int progress_interval = tc.num_trials_ / 20; // show progress update every 5% // Create a new kinematic state for this thread to work on - robot_state::RobotState robot_state(tc.scene_.getRobotModel()); + moveit::core::RobotState robot_state(tc.scene_.getRobotModel()); // Do a large number of tests for (unsigned int i = 0; i < tc.num_trials_; ++i) @@ -667,7 +667,7 @@ DisabledReason disabledReasonFromString(const std::string& reason) { r = REASONS_FROM_STRING.at(reason); } - catch (std::out_of_range) + catch (const std::out_of_range&) { r = USER; } diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index b563b7e882..2ce3543a18 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -46,6 +46,9 @@ #include #include // for getting file path for loading images +// OMPL version +#include + namespace moveit_setup_assistant { // File system @@ -63,7 +66,7 @@ MoveItConfigData::MoveItConfigData() : config_pkg_generated_timestamp_(0) // Not in debug mode debug_ = false; - // Get MoveIt! Setup Assistant package path + // Get MoveIt Setup Assistant package path setup_assistant_path_ = ros::package::getPath("moveit_setup_assistant"); if (setup_assistant_path_.empty()) { @@ -79,7 +82,7 @@ MoveItConfigData::~MoveItConfigData() = default; // ****************************************************************************************** // Load a robot model // ****************************************************************************************** -void MoveItConfigData::setRobotModel(const robot_model::RobotModelPtr& robot_model) +void MoveItConfigData::setRobotModel(const moveit::core::RobotModelPtr& robot_model) { robot_model_ = robot_model; } @@ -87,12 +90,12 @@ void MoveItConfigData::setRobotModel(const robot_model::RobotModelPtr& robot_mod // ****************************************************************************************** // Provide a kinematic model. Load a new one if necessary // ****************************************************************************************** -robot_model::RobotModelConstPtr MoveItConfigData::getRobotModel() +moveit::core::RobotModelConstPtr MoveItConfigData::getRobotModel() { if (!robot_model_) { // Initialize with a URDF Model Interface and a SRDF Model - robot_model_.reset(new robot_model::RobotModel(urdf_model_, srdf_->srdf_model_)); + robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_->srdf_model_)); } return robot_model_; @@ -109,7 +112,7 @@ void MoveItConfigData::updateRobotModel() srdf_->updateSRDFModel(*urdf_model_); // Create new kin model - robot_model_.reset(new robot_model::RobotModel(urdf_model_, srdf_->srdf_model_)); + robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_->srdf_model_)); // Reset the planning scene planning_scene_.reset(); @@ -140,15 +143,14 @@ void MoveItConfigData::loadAllowedCollisionMatrix() allowed_collision_matrix_.clear(); // Update the allowed collision matrix, in case there has been a change - for (std::vector::const_iterator pair_it = srdf_->disabled_collisions_.begin(); - pair_it != srdf_->disabled_collisions_.end(); ++pair_it) + for (const auto& disabled_collision : srdf_->disabled_collisions_) { - allowed_collision_matrix_.setEntry(pair_it->link1_, pair_it->link2_, true); + allowed_collision_matrix_.setEntry(disabled_collision.link1_, disabled_collision.link2_, true); } } // ****************************************************************************************** -// Output MoveIt! Setup Assistant hidden settings file +// Output MoveIt Setup Assistant hidden settings file // ****************************************************************************************** bool MoveItConfigData::outputSetupAssistantFile(const std::string& file_path) { @@ -240,7 +242,8 @@ bool MoveItConfigData::outputOMPLPlanningYAML(const std::string& file_path) emitter << YAML::Key << group.name_; emitter << YAML::Value << YAML::BeginMap; // Output associated planners - emitter << YAML::Key << "default_planner_config" << YAML::Value << group_meta_data_[group.name_].default_planner_; + if (!group_meta_data_[group.name_].default_planner_.empty()) + emitter << YAML::Key << "default_planner_config" << YAML::Value << group_meta_data_[group.name_].default_planner_; emitter << YAML::Key << "planner_configs"; emitter << YAML::Value << YAML::BeginSeq; for (const std::string& pconfig : pconfigs) @@ -270,7 +273,7 @@ bool MoveItConfigData::outputOMPLPlanningYAML(const std::string& file_path) return false; } - output_stream << emitter.c_str(); + output_stream << emitter.c_str() << std::endl; output_stream.close(); return true; // file created successfully @@ -508,25 +511,73 @@ bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path) for (srdf::Model::Group& group : srdf_->groups_) { // Get list of associated joints - const robot_model::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); + const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); emitter << YAML::BeginMap; - const std::vector& joint_models = joint_model_group->getActiveJointModels(); + const std::vector& joint_models = joint_model_group->getActiveJointModels(); emitter << YAML::Key << "name"; emitter << YAML::Value << "fake_" + group.name_ + "_controller"; + emitter << YAML::Key << "type"; + emitter << YAML::Value << "$(arg execution_type)"; emitter << YAML::Key << "joints"; emitter << YAML::Value << YAML::BeginSeq; // Iterate through the joints - for (const robot_model::JointModel* joint : joint_models) + for (const moveit::core::JointModel* joint : joint_models) { - if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == robot_model::JointModel::FIXED) + if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED) continue; emitter << joint->getName(); } emitter << YAML::EndSeq; emitter << YAML::EndMap; } + emitter << YAML::EndSeq; + + // Add an initial pose for each group + emitter << YAML::Key << "initial" << YAML::Comment("Define initial robot poses."); + + bool poses_found = false; + std::string default_group_name; + for (const srdf::Model::Group& group : srdf_->groups_) + { + if (default_group_name.empty()) + default_group_name = group.name_; + for (const srdf::Model::GroupState& group_state : srdf_->group_states_) + { + if (group.name_ == group_state.group_) + { + if (!poses_found) + { + poses_found = true; + emitter << YAML::Value << YAML::BeginSeq; + } + emitter << YAML::BeginMap; + emitter << YAML::Key << "group"; + emitter << YAML::Value << group.name_; + emitter << YAML::Key << "pose"; + emitter << YAML::Value << group_state.name_; + emitter << YAML::EndMap; + break; + } + } + } + if (poses_found) + emitter << YAML::EndSeq; + else + { + // Add commented lines to show how the feature can be used + if (default_group_name.empty()) + default_group_name = "group"; + emitter << YAML::Newline; + emitter << YAML::Comment(" - group: " + default_group_name) << YAML::Newline; + emitter << YAML::Comment(" pose: home") << YAML::Newline; + + // Add empty list for valid yaml + emitter << YAML::BeginSeq; + emitter << YAML::EndSeq; + } + emitter << YAML::EndMap; std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc); @@ -546,6 +597,21 @@ std::vector MoveItConfigData::getOMPLPlanners() { std::vector planner_des; + OMPLPlannerDescription aps("AnytimePathShortening", "geometric"); + aps.addParameter("shortcut", "true", "Attempt to shortcut all new solution paths"); + aps.addParameter("hybridize", "true", "Compute hybrid solution trajectories"); + aps.addParameter("max_hybrid_paths", "24", "Number of hybrid paths generated per iteration"); + aps.addParameter("num_planners", "4", "The number of default planners to use for planning"); +// TODO: remove when ROS Melodic and older are no longer supported +#if OMPL_VERSION_VALUE >= 1005000 + // This parameter was added in OMPL 1.5.0 + aps.addParameter("planners", "", + "A comma-separated list of planner types (e.g., \"PRM,EST,RRTConnect\"" + "Optionally, planner parameters can be passed to change the default:" + "\"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]\""); +#endif + planner_des.push_back(aps); + OMPLPlannerDescription sbl("SBL", "geometric"); sbl.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()"); planner_des.push_back(sbl); @@ -556,28 +622,33 @@ std::vector MoveItConfigData::getOMPLPlanners() planner_des.push_back(est); OMPLPlannerDescription lbkpiece("LBKPIECE", "geometric"); - lbkpiece.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + lbkpiece.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); lbkpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9"); lbkpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); planner_des.push_back(lbkpiece); OMPLPlannerDescription bkpiece("BKPIECE", "geometric"); - bkpiece.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + bkpiece.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); bkpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9"); - bkpiece.addParameter("failed_expansion_score_factor", "0.5", "When extending motion fails, scale score by factor. " - "default: 0.5"); + bkpiece.addParameter("failed_expansion_score_factor", "0.5", + "When extending motion fails, scale score by factor. " + "default: 0.5"); bkpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); planner_des.push_back(bkpiece); OMPLPlannerDescription kpiece("KPIECE", "geometric"); - kpiece.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + kpiece.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); kpiece.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); kpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9 (0.0,1.]"); - kpiece.addParameter("failed_expansion_score_factor", "0.5", "When extending motion fails, scale score by factor. " - "default: 0.5"); + kpiece.addParameter("failed_expansion_score_factor", "0.5", + "When extending motion fails, scale score by factor. " + "default: 0.5"); kpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); planner_des.push_back(kpiece); @@ -587,16 +658,19 @@ std::vector MoveItConfigData::getOMPLPlanners() planner_des.push_back(rrt); OMPLPlannerDescription rrt_connect("RRTConnect", "geometric"); - rrt_connect.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + rrt_connect.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); planner_des.push_back(rrt_connect); OMPLPlannerDescription rr_tstar("RRTstar", "geometric"); - rr_tstar.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + rr_tstar.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); rr_tstar.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); - rr_tstar.addParameter("delay_collision_checking", "1", "Stop collision checking as soon as C-free parent found. " - "default 1"); + rr_tstar.addParameter("delay_collision_checking", "1", + "Stop collision checking as soon as C-free parent found. " + "default 1"); planner_des.push_back(rr_tstar); OMPLPlannerDescription trrt("TRRT", "geometric"); @@ -606,8 +680,9 @@ std::vector MoveItConfigData::getOMPLPlanners() trrt.addParameter("temp_change_factor", "2.0", "how much to increase or decrease temp. default: 2.0"); trrt.addParameter("min_temperature", "10e-10", "lower limit of temp change. default: 10e-10"); trrt.addParameter("init_temperature", "10e-6", "initial temperature. default: 10e-6"); - trrt.addParameter("frountier_threshold", "0.0", "dist new state to nearest neighbor to disqualify as frontier. " - "default: 0.0 set in setup()"); + trrt.addParameter("frountier_threshold", "0.0", + "dist new state to nearest neighbor to disqualify as frontier. " + "default: 0.0 set in setup()"); trrt.addParameter("frountierNodeRatio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1"); trrt.addParameter("k_constant", "0.0", "value used to normalize expresssion. default: 0.0 set in setup()"); planner_des.push_back(trrt); @@ -625,24 +700,29 @@ std::vector MoveItConfigData::getOMPLPlanners() fmt.addParameter("nearest_k", "1", "use Knearest strategy. default: 1"); fmt.addParameter("cache_cc", "1", "use collision checking cache. default: 1"); fmt.addParameter("heuristics", "0", "activate cost to go heuristics. default: 0"); - fmt.addParameter("extended_fmt", "1", "activate the extended FMT*: adding new samples if planner does not finish " - "successfully. default: 1"); + fmt.addParameter("extended_fmt", "1", + "activate the extended FMT*: adding new samples if planner does not finish " + "successfully. default: 1"); planner_des.push_back(fmt); OMPLPlannerDescription bfmt("BFMT", "geometric"); bfmt.addParameter("num_samples", "1000", "number of states that the planner should sample. default: 1000"); - bfmt.addParameter("radius_multiplier", "1.0", "multiplier used for the nearest neighbors search radius. default: " - "1.0"); + bfmt.addParameter("radius_multiplier", "1.0", + "multiplier used for the nearest neighbors search radius. default: " + "1.0"); bfmt.addParameter("nearest_k", "1", "use the Knearest strategy. default: 1"); - bfmt.addParameter("balanced", "0", "exploration strategy: balanced true expands one tree every iteration. False will " - "select the tree with lowest maximum cost to go. default: 1"); - bfmt.addParameter("optimality", "1", "termination strategy: optimality true finishes when the best possible path is " - "found. Otherwise, the algorithm will finish when the first feasible path is " - "found. default: 1"); + bfmt.addParameter("balanced", "0", + "exploration strategy: balanced true expands one tree every iteration. False will " + "select the tree with lowest maximum cost to go. default: 1"); + bfmt.addParameter("optimality", "1", + "termination strategy: optimality true finishes when the best possible path is " + "found. Otherwise, the algorithm will finish when the first feasible path is " + "found. default: 1"); bfmt.addParameter("heuristics", "1", "activates cost to go heuristics. default: 1"); bfmt.addParameter("cache_cc", "1", "use the collision checking cache. default: 1"); - bfmt.addParameter("extended_fmt", "1", "Activates the extended FMT*: adding new samples if planner does not finish " - "successfully. default: 1"); + bfmt.addParameter("extended_fmt", "1", + "Activates the extended FMT*: adding new samples if planner does not finish " + "successfully. default: 1"); planner_des.push_back(bfmt); OMPLPlannerDescription pdst("PDST", "geometric"); @@ -650,14 +730,17 @@ std::vector MoveItConfigData::getOMPLPlanners() planner_des.push_back(pdst); OMPLPlannerDescription stride("STRIDE", "geometric"); - stride.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + stride.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); stride.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); - stride.addParameter("use_projected_distance", "0", "whether nearest neighbors are computed based on distances in a " - "projection of the state rather distances in the state space " - "itself. default: 0"); - stride.addParameter("degree", "16", "desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). " - "default: 16"); + stride.addParameter("use_projected_distance", "0", + "whether nearest neighbors are computed based on distances in a " + "projection of the state rather distances in the state space " + "itself. default: 0"); + stride.addParameter("degree", "16", + "desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). " + "default: 16"); stride.addParameter("max_degree", "18", "max degree of a node in the GNAT. default: 12"); stride.addParameter("min_degree", "12", "min degree of a node in the GNAT. default: 12"); stride.addParameter("max_pts_per_leaf", "6", "max points per leaf in the GNAT. default: 6"); @@ -666,57 +749,67 @@ std::vector MoveItConfigData::getOMPLPlanners() planner_des.push_back(stride); OMPLPlannerDescription bi_trrt("BiTRRT", "geometric"); - bi_trrt.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + bi_trrt.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); bi_trrt.addParameter("temp_change_factor", "0.1", "how much to increase or decrease temp. default: 0.1"); bi_trrt.addParameter("init_temperature", "100", "initial temperature. default: 100"); - bi_trrt.addParameter("frountier_threshold", "0.0", "dist new state to nearest neighbor to disqualify as frontier. " - "default: 0.0 set in setup()"); + bi_trrt.addParameter("frountier_threshold", "0.0", + "dist new state to nearest neighbor to disqualify as frontier. " + "default: 0.0 set in setup()"); bi_trrt.addParameter("frountier_node_ratio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1"); - bi_trrt.addParameter("cost_threshold", "1e300", "the cost threshold. Any motion cost that is not better will not be " - "expanded. default: inf"); + bi_trrt.addParameter("cost_threshold", "1e300", + "the cost threshold. Any motion cost that is not better will not be " + "expanded. default: inf"); planner_des.push_back(bi_trrt); OMPLPlannerDescription lbtrrt("LBTRRT", "geometric"); - lbtrrt.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + lbtrrt.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); lbtrrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); lbtrrt.addParameter("epsilon", "0.4", "optimality approximation factor. default: 0.4"); planner_des.push_back(lbtrrt); OMPLPlannerDescription bi_est("BiEST", "geometric"); - bi_est.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + bi_est.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); planner_des.push_back(bi_est); OMPLPlannerDescription proj_est("ProjEST", "geometric"); - proj_est.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + proj_est.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); proj_est.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); planner_des.push_back(proj_est); OMPLPlannerDescription lazy_prm("LazyPRM", "geometric"); - lazy_prm.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); + lazy_prm.addParameter("range", "0.0", + "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " + "setup()"); planner_des.push_back(lazy_prm); OMPLPlannerDescription lazy_pr_mstar("LazyPRMstar", "geometric"); // no declares in code planner_des.push_back(lazy_pr_mstar); OMPLPlannerDescription spars("SPARS", "geometric"); - spars.addParameter("stretch_factor", "3.0", "roadmap spanner stretch factor. multiplicative upper bound on path " - "quality. It does not make sense to make this parameter more than 3. " - "default: 3.0"); - spars.addParameter("sparse_delta_fraction", "0.25", "delta fraction for connection distance. This value represents " - "the visibility range of sparse samples. default: 0.25"); + spars.addParameter("stretch_factor", "3.0", + "roadmap spanner stretch factor. multiplicative upper bound on path " + "quality. It does not make sense to make this parameter more than 3. " + "default: 3.0"); + spars.addParameter("sparse_delta_fraction", "0.25", + "delta fraction for connection distance. This value represents " + "the visibility range of sparse samples. default: 0.25"); spars.addParameter("dense_delta_fraction", "0.001", "delta fraction for interface detection. default: 0.001"); spars.addParameter("max_failures", "1000", "maximum consecutive failure limit. default: 1000"); planner_des.push_back(spars); OMPLPlannerDescription spar_stwo("SPARStwo", "geometric"); - spar_stwo.addParameter("stretch_factor", "3.0", "roadmap spanner stretch factor. multiplicative upper bound on path " - "quality. It does not make sense to make this parameter more than 3. " - "default: 3.0"); + spar_stwo.addParameter("stretch_factor", "3.0", + "roadmap spanner stretch factor. multiplicative upper bound on path " + "quality. It does not make sense to make this parameter more than 3. " + "default: 3.0"); spar_stwo.addParameter("sparse_delta_fraction", "0.25", "delta fraction for connection distance. This value represents " "the visibility range of sparse samples. default: 0.25"); @@ -775,7 +868,7 @@ void MoveItConfigData::outputFollowJointTrajectoryYAML(YAML::Emitter& emitter, emitter << YAML::EndMap; } } - ros_controllers_config_output.erase(controller_it); + controller_it = ros_controllers_config_output.erase(controller_it); emitter << YAML::EndMap; } else @@ -787,6 +880,17 @@ void MoveItConfigData::outputFollowJointTrajectoryYAML(YAML::Emitter& emitter, } } +// ****************************************************************************************** +// Helper function to get the default start pose for moveit_sim_hw_interface +// ****************************************************************************************** +srdf::Model::GroupState MoveItConfigData::getDefaultStartPose() +{ + if (!srdf_->group_states_.empty()) + return srdf_->group_states_[0]; + else + return srdf::Model::GroupState{ .name_ = "todo_state_name", .group_ = "todo_group_name", .joint_values_ = {} }; +} + // ****************************************************************************************** // Output controllers config files // ****************************************************************************************** @@ -804,12 +908,12 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) for (srdf::Model::Group& group : srdf_->groups_) { // Get list of associated joints - const robot_model::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); - const std::vector& joint_models = joint_model_group->getActiveJointModels(); + const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); + const std::vector& joint_models = joint_model_group->getActiveJointModels(); // Iterate through the joints and push into group_joints vector. - for (const robot_model::JointModel* joint : joint_models) + for (const moveit::core::JointModel* joint : joint_models) { - if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == robot_model::JointModel::FIXED) + if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED) continue; else group_joints.push_back(joint->getName()); @@ -823,19 +927,23 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) emitter << YAML::BeginMap; { - emitter << YAML::Comment("MoveIt-specific simulation settings"); + emitter << YAML::Comment("Simulation settings for using moveit_sim_controllers"); emitter << YAML::Key << "moveit_sim_hw_interface" << YAML::Value << YAML::BeginMap; - // MoveIt! Simulation Controller settings for setting initial pose + // MoveIt Simulation Controller settings for setting initial pose { + // Use the first planning group if initial joint_model_group was not set, else write a default value emitter << YAML::Key << "joint_model_group"; - emitter << YAML::Value << "controllers_initial_group_"; + emitter << YAML::Value << getDefaultStartPose().group_; + + // Use the first robot pose if initial joint_model_group_pose was not set, else write a default value emitter << YAML::Key << "joint_model_group_pose"; - emitter << YAML::Value << "controllers_initial_pose_"; + emitter << YAML::Value << getDefaultStartPose().name_; + emitter << YAML::EndMap; } // Settings for ros_control control loop emitter << YAML::Newline; - emitter << YAML::Comment("Settings for ros_control control loop"); + emitter << YAML::Comment("Settings for ros_control_boilerplate control loop"); emitter << YAML::Key << "generic_hw_control_loop" << YAML::Value << YAML::BeginMap; { emitter << YAML::Key << "loop_hz"; @@ -850,7 +958,7 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) emitter << YAML::Key << "hardware_interface" << YAML::Value << YAML::BeginMap; { // Get list of all joints for the robot - const std::vector& joint_models = getRobotModel()->getJointModels(); + const std::vector& joint_models = getRobotModel()->getJointModels(); emitter << YAML::Key << "joints"; { @@ -858,11 +966,11 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) { emitter << YAML::Value << YAML::BeginSeq; // Iterate through the joints - for (std::vector::const_iterator joint_it = joint_models.begin(); + for (std::vector::const_iterator joint_it = joint_models.begin(); joint_it < joint_models.end(); ++joint_it) { if ((*joint_it)->isPassive() || (*joint_it)->getMimic() != nullptr || - (*joint_it)->getType() == robot_model::JointModel::FIXED) + (*joint_it)->getType() == moveit::core::JointModel::FIXED) continue; else emitter << (*joint_it)->getName(); @@ -897,23 +1005,22 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) // Writes Follow Joint Trajectory ROS controllers to ros_controller.yaml outputFollowJointTrajectoryYAML(emitter, ros_controllers_config_output); - for (std::vector::const_iterator controller_it = ros_controllers_config_output.begin(); - controller_it != ros_controllers_config_output.end(); ++controller_it) + for (const auto& controller : ros_controllers_config_output) { - emitter << YAML::Key << controller_it->name_; + emitter << YAML::Key << controller.name_; emitter << YAML::Value << YAML::BeginMap; emitter << YAML::Key << "type"; - emitter << YAML::Value << controller_it->type_; + emitter << YAML::Value << controller.type_; // Write joints emitter << YAML::Key << "joints"; { - if (controller_it->joints_.size() != 1) + if (controller.joints_.size() != 1) { emitter << YAML::Value << YAML::BeginSeq; // Iterate through the joints - for (const std::string& joint : controller_it->joints_) + for (const std::string& joint : controller.joints_) { emitter << joint; } @@ -922,7 +1029,7 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) else { emitter << YAML::Value << YAML::BeginMap; - emitter << controller_it->joints_[0]; + emitter << controller.joints_[0]; emitter << YAML::EndMap; } } @@ -931,7 +1038,7 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) emitter << YAML::Value << YAML::BeginMap; { // Iterate through the joints - for (const std::string& joint : controller_it->joints_) + for (const std::string& joint : controller.joints_) { emitter << YAML::Key << joint << YAML::Value << YAML::BeginMap; emitter << YAML::Key << "p"; @@ -1011,21 +1118,40 @@ bool MoveItConfigData::output3DSensorPluginYAML(const std::string& file_path) bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) { YAML::Emitter emitter; + emitter << YAML::Comment("joint_limits.yaml allows the dynamics properties specified in the URDF " + "to be overwritten or augmented as needed"); + emitter << YAML::Newline; + emitter << YAML::BeginMap; + emitter << YAML::Comment("For beginners, we downscale velocity and acceleration limits.") << YAML::Newline; + emitter << YAML::Comment("You can always specify higher scaling factors (<= 1.0) in your motion requests."); + emitter << YAML::Comment("Increase the values below to 1.0 to always move at maximum speed."); + emitter << YAML::Key << "default_velocity_scaling_factor"; + emitter << YAML::Value << "0.1"; + + emitter << YAML::Key << "default_acceleration_scaling_factor"; + emitter << YAML::Value << "0.1"; + + emitter << YAML::Newline << YAML::Newline; + emitter << YAML::Comment("Specific joint properties can be changed with the keys " + "[max_position, min_position, max_velocity, max_acceleration]") + << YAML::Newline; + emitter << YAML::Comment("Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]"); + emitter << YAML::Key << "joint_limits"; emitter << YAML::Value << YAML::BeginMap; // Union all the joints in groups. Uses a custom comparator to allow the joints to be sorted by name - std::set joints; + std::set joints; // Loop through groups for (srdf::Model::Group& group : srdf_->groups_) { // Get list of associated joints - const robot_model::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); + const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); - const std::vector& joint_models = joint_model_group->getJointModels(); + const std::vector& joint_models = joint_model_group->getJointModels(); // Iterate through the joints for (const moveit::core::JointModel* joint_model : joint_models) @@ -1042,7 +1168,7 @@ bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) emitter << YAML::Key << joint->getName(); emitter << YAML::Value << YAML::BeginMap; - const robot_model::VariableBounds& b = joint->getVariableBounds()[0]; + const moveit::core::VariableBounds& b = joint->getVariableBounds()[0]; // Output property emitter << YAML::Key << "has_velocity_limits"; @@ -1077,14 +1203,6 @@ bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) ROS_ERROR_STREAM("Unable to open file for writing " << file_path); return false; } - // Add documentation into joint_limits.yaml - output_stream << "# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or " - "augmented as needed" - << std::endl; - output_stream << "# Specific joint properties can be changed with the keys [max_position, min_position, " - "max_velocity, max_acceleration]" - << std::endl; - output_stream << "# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]" << std::endl; output_stream << emitter.c_str(); output_stream.close(); @@ -1102,7 +1220,7 @@ class SortableDisabledCollision : dc_(dc), key_(dc.link1_ < dc.link2_ ? (dc.link1_ + "|" + dc.link2_) : (dc.link2_ + "|" + dc.link1_)) { } - operator const srdf::Model::DisabledCollision&() const + operator const srdf::Model::DisabledCollision &() const { return dc_; } @@ -1152,14 +1270,14 @@ std::string MoveItConfigData::decideProjectionJoints(const std::string& planning std::string joint_pair = ""; // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = getRobotModel(); + const moveit::core::RobotModelConstPtr& model = getRobotModel(); // Error check if (!model->hasJointModelGroup(planning_group)) return joint_pair; // Get the joint model group - const robot_model::JointModelGroup* group = model->getJointModelGroup(planning_group); + const moveit::core::JointModelGroup* group = model->getJointModelGroup(planning_group); // get vector of joint names const std::vector& joints = group->getJointModelNames(); @@ -1261,8 +1379,8 @@ bool MoveItConfigData::inputKinematicsYAML(const std::string& file_path) parse(group, "kinematics_solver", meta_data.kinematics_solver_); parse(group, "kinematics_solver_search_resolution", meta_data.kinematics_solver_search_resolution_, - DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION_); - parse(group, "kinematics_solver_timeout", meta_data.kinematics_solver_timeout_, DEFAULT_KIN_SOLVER_TIMEOUT_); + DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION); + parse(group, "kinematics_solver_timeout", meta_data.kinematics_solver_timeout_, DEFAULT_KIN_SOLVER_TIMEOUT); // Assign meta data to vector group_meta_data_[group_name] = meta_data; @@ -1277,6 +1395,46 @@ bool MoveItConfigData::inputKinematicsYAML(const std::string& file_path) return true; // file created successfully } +// ****************************************************************************************** +// Input planning_context.launch file +// ****************************************************************************************** +bool MoveItConfigData::inputPlanningContextLaunch(const std::string& file_path) +{ + TiXmlDocument launch_document(file_path); + if (!launch_document.LoadFile()) + { + ROS_ERROR_STREAM("Failed parsing " << file_path); + return false; + } + + // find the kinematics section + TiXmlHandle doc(&launch_document); + TiXmlElement* kinematics_group = doc.FirstChild("launch").FirstChild("group").ToElement(); + while (kinematics_group && kinematics_group->Attribute("ns") != std::string("$(arg robot_description)_kinematics")) + { + kinematics_group = kinematics_group->NextSiblingElement("group"); + } + if (!kinematics_group) + { + ROS_ERROR(" not found"); + return false; + } + + // iterate over all elements + // and if 'group' matches an existing group, copy the filename + for (TiXmlElement* kinematics_parameter_file = kinematics_group->FirstChildElement("rosparam"); + kinematics_parameter_file; kinematics_parameter_file = kinematics_parameter_file->NextSiblingElement("rosparam")) + { + const char* ns = kinematics_parameter_file->Attribute("ns"); + if (ns && (group_meta_data_.find(ns) != group_meta_data_.end())) + { + group_meta_data_[ns].kinematics_parameters_file_ = kinematics_parameter_file->Attribute("file"); + } + } + + return true; +} + // ****************************************************************************************** // Helper function for parsing an individual ROSController from ros_controllers yaml file // ****************************************************************************************** @@ -1418,13 +1576,13 @@ bool MoveItConfigData::addDefaultControllers() { ROSControlConfig group_controller; // Get list of associated joints - const robot_model::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group_it.name_); - const std::vector& joint_models = joint_model_group->getActiveJointModels(); + const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group_it.name_); + const std::vector& joint_models = joint_model_group->getActiveJointModels(); // Iterate through the joints - for (const robot_model::JointModel* joint : joint_models) + for (const moveit::core::JointModel* joint : joint_models) { - if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == robot_model::JointModel::FIXED) + if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED) continue; group_controller.joints_.push_back(joint->getName()); } @@ -1467,6 +1625,46 @@ bool MoveItConfigData::setPackagePath(const std::string& pkg_path) return true; } +// ****************************************************************************************** +// Extract the package/stack name from an absolute file path +// Input: path +// Output: package name and relative path +// ****************************************************************************************** +bool MoveItConfigData::extractPackageNameFromPath(const std::string& path, std::string& package_name, + std::string& relative_filepath) const +{ + fs::path sub_path = path; // holds the directory less one folder + fs::path relative_path; // holds the path after the sub_path + + bool package_found = false; + + // truncate path step by step and check if it contains a package.xml + while (!sub_path.empty()) + { + ROS_DEBUG_STREAM("checking in " << sub_path.make_preferred().string()); + if (fs::is_regular_file(sub_path / "package.xml")) + { + ROS_DEBUG_STREAM("Found package.xml in " << sub_path.make_preferred().string()); + package_found = true; + relative_filepath = relative_path.string(); + package_name = sub_path.leaf().string(); + break; + } + relative_path = sub_path.leaf() / relative_path; + sub_path.remove_leaf(); + } + + // Assign data to moveit_config_data + if (!package_found) + { + // No package name found, we must be outside ROS + return false; + } + + ROS_DEBUG_STREAM("Package name for file \"" << path << "\" is \"" << package_name << "\""); + return true; +} + // ****************************************************************************************** // Resolve path to .setup_assistant file // ****************************************************************************************** @@ -1522,7 +1720,7 @@ bool MoveItConfigData::createFullSRDFPath(const std::string& package_path) } // ****************************************************************************************** -// Input .setup_assistant file - contains data used for the MoveIt! Setup Assistant +// Input .setup_assistant file - contains data used for the MoveIt Setup Assistant // ****************************************************************************************** bool MoveItConfigData::inputSetupAssistantYAML(const std::string& file_path) { @@ -1693,7 +1891,7 @@ std::string MoveItConfigData::appendPaths(const std::string& path1, const std::s { fs::path result = path1; result /= path2; - return result.make_preferred().native(); + return result.make_preferred().string(); } srdf::Model::Group* MoveItConfigData::findGroupByName(const std::string& name) @@ -1712,8 +1910,9 @@ srdf::Model::Group* MoveItConfigData::findGroupByName(const std::string& name) // Check if subgroup was found if (searched_group == nullptr) // not found - ROS_FATAL_STREAM("An internal error has occured while searching for groups. Group '" << name << "' was not found " - "in the SRDF."); + ROS_FATAL_STREAM("An internal error has occured while searching for groups. Group '" << name + << "' was not found " + "in the SRDF."); return searched_group; } @@ -1786,7 +1985,7 @@ std::vector& MoveItConfigData::getROSControllers() // Used to add a sensor plugin configuation parameter to the sensor plugin configuration parameter list // ****************************************************************************************** void MoveItConfigData::addGenericParameterToSensorPluginConfig(const std::string& name, const std::string& value, - const std::string& comment) + const std::string& /*comment*/) { // Use index 0 since we only write one plugin GenericParameter new_parameter; diff --git a/moveit_setup_assistant/src/tools/rotated_header_view.h b/moveit_setup_assistant/src/tools/rotated_header_view.h index 27854b7140..41e371b441 100644 --- a/moveit_setup_assistant/src/tools/rotated_header_view.h +++ b/moveit_setup_assistant/src/tools/rotated_header_view.h @@ -34,8 +34,7 @@ /* Author: Robert Haschke */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROTATED_HEADERVIEW_H -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROTATED_HEADERVIEW_H +#pragma once #include @@ -44,11 +43,9 @@ namespace moveit_setup_assistant class RotatedHeaderView : public QHeaderView { public: - RotatedHeaderView(Qt::Orientation orientation, QWidget* parent = NULL); + RotatedHeaderView(Qt::Orientation orientation, QWidget* parent = nullptr); void paintSection(QPainter* painter, const QRect& rect, int logicalIndex) const override; QSize sectionSizeFromContents(int logicalIndex) const override; int sectionSizeHint(int logicalIndex) const; }; -} - -#endif // MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROTATED_HEADERVIEW_H +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/author_information_widget.cpp b/moveit_setup_assistant/src/widgets/author_information_widget.cpp index fc9dc50727..6d7c1e5ded 100644 --- a/moveit_setup_assistant/src/widgets/author_information_widget.cpp +++ b/moveit_setup_assistant/src/widgets/author_information_widget.cpp @@ -39,15 +39,15 @@ #include #include #include -#include -// ROS +#include +#include #include "author_information_widget.h" -#include +#include "header_widget.h" namespace moveit_setup_assistant { // ****************************************************************************************** -// Outer User Interface for MoveIt! Configuration Assistant +// Outer User Interface for MoveIt Configuration Assistant // ****************************************************************************************** AuthorInformationWidget::AuthorInformationWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) : SetupScreenWidget(parent), config_data_(config_data) @@ -66,7 +66,7 @@ AuthorInformationWidget::AuthorInformationWidget(QWidget* parent, const MoveItCo layout->addWidget(header); QLabel* name_title = new QLabel(this); - name_title->setText("Name of the maintainer this MoveIt! configuration:"); + name_title->setText("Name of the maintainer this MoveIt configuration:"); layout->addWidget(name_title); name_edit_ = new QLineEdit(this); @@ -74,7 +74,7 @@ AuthorInformationWidget::AuthorInformationWidget(QWidget* parent, const MoveItCo layout->addWidget(name_edit_); QLabel* email_title = new QLabel(this); - email_title->setText("Email of the maintainer of this MoveIt! configuration:"); + email_title->setText("Email of the maintainer of this MoveIt configuration:"); layout->addWidget(email_title); email_edit_ = new QLineEdit(this); diff --git a/moveit_setup_assistant/src/widgets/author_information_widget.h b/moveit_setup_assistant/src/widgets/author_information_widget.h index 15ed0036ec..4f9709c8f2 100644 --- a/moveit_setup_assistant/src/widgets/author_information_widget.h +++ b/moveit_setup_assistant/src/widgets/author_information_widget.h @@ -34,18 +34,14 @@ /* Author: Dave Coleman, Michael 'v4hn' Goerner */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_AUTHOR_INFORMATION_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_AUTHOR_INFORMATION_WIDGET_ +#pragma once -#include -#include -#include +class QLineEdit; #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant @@ -86,5 +82,3 @@ private Q_SLOTS: }; } // namespace moveit_setup_assistant - -#endif diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp index 1f87a5a506..3840253681 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp @@ -35,15 +35,21 @@ /* Author: Dave Coleman */ // Qt -#include -#include -#include #include -#include +#include +#include +#include +#include +#include +#include +#include #include -// ROS +#include +#include + #include "configuration_files_widget.h" -#include +#include "header_widget.h" + // Boost #include // for string find and replace in templates #include // for creating folders/files @@ -60,7 +66,7 @@ namespace fs = boost::filesystem; const std::string SETUP_ASSISTANT_FILE = ".setup_assistant"; // ****************************************************************************************** -// Outer User Interface for MoveIt! Configuration Assistant +// Outer User Interface for MoveIt Configuration Assistant // ****************************************************************************************** ConfigurationFilesWidget::ConfigurationFilesWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) : SetupScreenWidget(parent), config_data_(config_data), has_generated_pkg_(false), first_focusGiven_(true) @@ -82,7 +88,7 @@ ConfigurationFilesWidget::ConfigurationFilesWidget(QWidget* parent, const MoveIt // Stack Path Dialog stack_path_ = new LoadPathWidget("Configuration Package Save Path", - "Specify the desired directory for the MoveIt! configuration package to be " + "Specify the desired directory for the MoveIt configuration package to be " "generated. Overwriting an existing configuration package directory is acceptable. " "Example: /u/robot/ros/panda_moveit_config", this, true); // is directory @@ -183,7 +189,7 @@ bool ConfigurationFilesWidget::loadGenFiles() fs::path template_package_path = config_data_->setup_assistant_path_; template_package_path /= "templates"; template_package_path /= "moveit_config_pkg_template"; - config_data_->template_package_path_ = template_package_path.make_preferred().native(); + config_data_->template_package_path_ = template_package_path.make_preferred().string(); if (!fs::is_directory(config_data_->template_package_path_)) { @@ -225,7 +231,7 @@ bool ConfigurationFilesWidget::loadGenFiles() // config/ -------------------------------------------------------------------------------------- file.file_name_ = "config/"; file.rel_path_ = file.file_name_; - file.description_ = "Folder containing all MoveIt! configuration files for your robot. This folder is required and " + file.description_ = "Folder containing all MoveIt configuration files for your robot. This folder is required and " "cannot be disabled."; file.gen_func_ = boost::bind(&ConfigurationFilesWidget::createFolder, this, _1); file.write_on_changes = 0; @@ -291,6 +297,17 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = 0; // Can they be changed? gen_files_.push_back(file); + // cartesian_limits.yaml -------------------------------------------------------------------------------------- + file.file_name_ = "cartesian_limits.yaml"; + file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); + template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_); + file.description_ = "Cartesian velocity for planning in the workspace." + "The velocity is used by pilz industrial motion planner as maximum velocity for cartesian " + "planning requests scaled by the velocity scaling factor of an individual planning request."; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.write_on_changes = 0; // Can they be changed? + gen_files_.push_back(file); + // fake_controllers.yaml -------------------------------------------------------------------------------------- file.file_name_ = "fake_controllers.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); @@ -325,9 +342,8 @@ bool ConfigurationFilesWidget::loadGenFiles() // launch/ -------------------------------------------------------------------------------------- file.file_name_ = "launch/"; file.rel_path_ = file.file_name_; - file.description_ = - "Folder containing all MoveIt! launch files for your robot. This folder is required and cannot be " - "disabled."; + file.description_ = "Folder containing all MoveIt launch files for your robot. " + "This folder is required and cannot be disabled."; file.gen_func_ = boost::bind(&ConfigurationFilesWidget::createFolder, this, _1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -375,6 +391,30 @@ bool ConfigurationFilesWidget::loadGenFiles() file.write_on_changes = 0; gen_files_.push_back(file); + // pilz_industrial_motion_planner_planning_pipeline.launch + // -------------------------------------------------------------------------------------- + file.file_name_ = "pilz_industrial_motion_planner_planning_pipeline.launch.xml"; + file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); + template_path = config_data_->appendPaths(template_launch_path, file.file_name_); + file.description_ = "Intended to be included in other launch files that require the Pilz industrial motion planner " + "planning plugin. Defines the proper plugin name on the parameter server and a default selection " + "of planning request adapters."; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.write_on_changes = 0; + gen_files_.push_back(file); + + // trajopt_planning_pipeline.launch + // -------------------------------------------------------------------------------------- + file.file_name_ = "trajopt_planning_pipeline.launch.xml"; + file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); + template_path = config_data_->appendPaths(template_launch_path, file.file_name_); + file.description_ = "Intended to be included in other launch files that require the TrajOpt planning plugin. Defines " + "the proper plugin name on the parameter server and a default selection of planning request " + "adapters."; + file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); + file.write_on_changes = 0; + gen_files_.push_back(file); + // chomp_planning_pipeline.launch // -------------------------------------------------------------------------------------- file.file_name_ = "chomp_planning_pipeline.launch.xml"; @@ -445,7 +485,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.file_name_ = robot_name + "_moveit_controller_manager.launch.xml"; file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, "moveit_controller_manager.launch.xml"); - file.description_ = "Placeholder for settings specific to the MoveIt! controller manager implemented for you robot."; + file.description_ = "Placeholder for settings specific to the MoveIt controller manager implemented for you robot."; file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -454,7 +494,7 @@ bool ConfigurationFilesWidget::loadGenFiles() file.file_name_ = robot_name + "_moveit_sensor_manager.launch.xml"; file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); template_path = config_data_->appendPaths(template_launch_path, "moveit_sensor_manager.launch.xml"); - file.description_ = "Placeholder for settings specific to the MoveIt! sensor manager implemented for you robot."; + file.description_ = "Placeholder for settings specific to the MoveIt sensor manager implemented for you robot."; file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); file.write_on_changes = 0; gen_files_.push_back(file); @@ -520,7 +560,7 @@ bool ConfigurationFilesWidget::loadGenFiles() template_path = config_data_->appendPaths( template_launch_path, "edit_configuration_package.launch"); // named this so that this launch file is not mixed // up with the SA's real launch file - file.description_ = "Launch file for easily re-starting the MoveIt! Setup Assistant to edit this robot's generated " + file.description_ = "Launch file for easily re-starting the MoveIt Setup Assistant to edit this robot's generated " "configuration package."; file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1); file.write_on_changes = 0; @@ -552,7 +592,7 @@ bool ConfigurationFilesWidget::loadGenFiles() // .setup_assistant ------------------------------------------------------------------ file.file_name_ = SETUP_ASSISTANT_FILE; file.rel_path_ = file.file_name_; - file.description_ = "MoveIt! Setup Assistant's hidden settings file. You should not need to edit this file."; + file.description_ = "MoveIt Setup Assistant's hidden settings file. You should not need to edit this file."; file.gen_func_ = boost::bind(&MoveItConfigData::outputSetupAssistantFile, config_data_, _1); file.write_on_changes = -1; // write on any changes gen_files_.push_back(file); @@ -635,7 +675,7 @@ bool ConfigurationFilesWidget::checkDependencies() if (!required_actions) { dep_message.append("
Press Ok to continue generating files."); - if (QMessageBox::question(this, "Incomplete MoveIt! Setup Assistant Steps", dep_message, + if (QMessageBox::question(this, "Incomplete MoveIt Setup Assistant Steps", dep_message, QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) { return false; // abort @@ -643,7 +683,7 @@ bool ConfigurationFilesWidget::checkDependencies() } else { - QMessageBox::warning(this, "Incomplete MoveIt! Setup Assistant Steps", dep_message); + QMessageBox::warning(this, "Incomplete MoveIt Setup Assistant Steps", dep_message); return false; } } @@ -688,8 +728,9 @@ void ConfigurationFilesWidget::changeCheckedState(QListWidgetItem* item) if (!generate && (gen_files_[index].write_on_changes & config_data_->changes)) { - QMessageBox::warning(this, "Package Generation", "You should generate this file to ensure your changes will take " - "effect."); + QMessageBox::warning(this, "Package Generation", + "You should generate this file to ensure your changes will take " + "effect."); } // Enable/disable file @@ -884,8 +925,9 @@ bool ConfigurationFilesWidget::generatePackage() // Check that a valid stack package name has been given if (new_package_path.empty()) { - QMessageBox::warning(this, "Error Generating", "No package path provided. Please choose a directory location to " - "generate the MoveIt! configuration files."); + QMessageBox::warning(this, "Error Generating", + "No package path provided. Please choose a directory location to " + "generate the MoveIt configuration files."); return false; } @@ -913,7 +955,7 @@ bool ConfigurationFilesWidget::generatePackage() { QMessageBox::warning( this, "Incorrect Folder/Package", - QString("The chosen package location already exists but was not previously created using this MoveIt! Setup " + QString("The chosen package location already exists but was not previously created using this MoveIt Setup " "Assistant. " "If this is a mistake, add the missing file: ") .append(setup_assistant_file.c_str())); @@ -921,12 +963,12 @@ bool ConfigurationFilesWidget::generatePackage() } // Confirm overwrite - if (QMessageBox::question( - this, "Confirm Package Update", - QString("Are you sure you want to overwrite this existing package with updated configurations?
") - .append(new_package_path.c_str()) - .append(""), - QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) + if (QMessageBox::question(this, "Confirm Package Update", + QString("Are you sure you want to overwrite this existing package with updated " + "configurations?
") + .append(new_package_path.c_str()) + .append(""), + QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) { return false; // abort } @@ -963,14 +1005,18 @@ bool ConfigurationFilesWidget::generatePackage() absolute_path = config_data_->appendPaths(new_package_path, file->rel_path_); ROS_DEBUG_STREAM("Creating file " << absolute_path); + // Clear template strings in case export is run multiple times with changes in between + template_strings_.clear(); + // Run the generate function if (!file->gen_func_(absolute_path)) { // Error occured - QMessageBox::critical(this, "Error Generating File", QString("Failed to generate folder or file: '") - .append(file->rel_path_.c_str()) - .append("' at location:\n") - .append(absolute_path.c_str())); + QMessageBox::critical(this, "Error Generating File", + QString("Failed to generate folder or file: '") + .append(file->rel_path_.c_str()) + .append("' at location:\n") + .append(absolute_path.c_str())); return false; } updateProgress(); // Increment and update GUI @@ -984,10 +1030,9 @@ bool ConfigurationFilesWidget::generatePackage() // ****************************************************************************************** void ConfigurationFilesWidget::exitSetupAssistant() { - if (has_generated_pkg_ || - QMessageBox::question(this, "Exit Setup Assistant", - QString("Are you sure you want to exit the MoveIt! Setup Assistant?"), - QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Ok) + if (has_generated_pkg_ || QMessageBox::question(this, "Exit Setup Assistant", + QString("Are you sure you want to exit the MoveIt Setup Assistant?"), + QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Ok) { QApplication::quit(); } @@ -1008,7 +1053,7 @@ const std::string ConfigurationFilesWidget::getPackageName(std::string package_p std::string package_name; fs::path fs_package_path = package_path; - package_name = fs_package_path.filename().c_str(); + package_name = fs_package_path.filename().string(); // check for empty if (package_name.empty()) @@ -1023,24 +1068,23 @@ const std::string ConfigurationFilesWidget::getPackageName(std::string package_p bool ConfigurationFilesWidget::noGroupsEmpty() { // Loop through all groups - for (std::vector::const_iterator group_it = config_data_->srdf_->groups_.begin(); - group_it != config_data_->srdf_->groups_.end(); ++group_it) + for (const auto& group : config_data_->srdf_->groups_) { // Whenever 1 of the 4 component types are found, stop checking this group - if (!group_it->joints_.empty()) + if (!group.joints_.empty()) continue; - if (!group_it->links_.empty()) + if (!group.links_.empty()) continue; - if (!group_it->chains_.empty()) + if (!group.chains_.empty()) continue; - if (!group_it->subgroups_.empty()) + if (!group.subgroups_.empty()) continue; // This group has no contents, bad QMessageBox::warning( this, "Empty Group", QString("The planning group '") - .append(group_it->name_.c_str()) + .append(group.name_.c_str()) .append("' is empty and has no subcomponents associated with it (joints/links/chains/subgroups). You must " "edit or remove this planning group before this configuration package can be saved.")); return false; @@ -1098,7 +1142,6 @@ void ConfigurationFilesWidget::loadTemplateStrings() else { std::stringstream deps; - deps << "" << config_data_->urdf_pkg_name_ << "\n"; deps << " " << config_data_->urdf_pkg_name_ << "\n"; addTemplateString("[OTHER_DEPENDENCIES]", deps.str()); // not relative to a ROS package } @@ -1120,6 +1163,24 @@ void ConfigurationFilesWidget::loadTemplateStrings() addTemplateString("[ROS_CONTROLLERS]", controllers.str()); } + // Pair 10 - Add parameter files for the kinematics solvers that should be loaded + // in addition to kinematics.yaml by planning_context.launch + std::string kinematics_parameters_files_block; + for (const auto& groups : config_data_->group_meta_data_) + { + if (groups.second.kinematics_parameters_file_.empty()) + continue; + + // add a linebreak if we have more than one entry + if (!kinematics_parameters_files_block.empty()) + kinematics_parameters_files_block += "\n"; + + std::string line = " "; + kinematics_parameters_files_block += line; + } + addTemplateString("[KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK]", kinematics_parameters_files_block); + addTemplateString("[AUTHOR_NAME]", config_data_->author_name_); addTemplateString("[AUTHOR_EMAIL]", config_data_->author_email_); } diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.h b/moveit_setup_assistant/src/widgets/configuration_files_widget.h index f0787af004..edc7cf8271 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.h +++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.h @@ -34,26 +34,25 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_CONFIGURATION_FILES_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_CONFIGURATION_FILES_WIDGET_ - -#include -#include -#include -#include -#include -#include +#pragma once + #include +class QLabel; +class QListWidget; +class QListWidgetItem; +class QProgressBar; +class QPushButton; #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant { +class LoadPathWidget; + // Struct for storing all the file operations struct GenerateFile { @@ -205,5 +204,3 @@ private Q_SLOTS: }; } // namespace moveit_setup_assistant - -#endif diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp index 2d4140a4e8..38be579c2f 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp @@ -33,12 +33,16 @@ /* Author: Mohamad Ayman */ -#include +#include +#include +#include #include +#include +#include #include -#include +#include #include -#include +#include #include "controller_edit_widget.h" namespace moveit_setup_assistant @@ -114,10 +118,8 @@ ControllerEditWidget::ControllerEditWidget(QWidget* parent, const MoveItConfigDa new_buttons_widget_->setLayout(new_buttons_layout); layout->addWidget(new_buttons_widget_); - // Verticle Spacer ----------------------------------------------------- - QWidget* vspacer = new QWidget(this); - vspacer->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); - layout->addWidget(vspacer); + // Vertical Spacer + layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Minimum, QSizePolicy::Expanding)); // Bottom Controls --------------------------------------------------------- QHBoxLayout* controls_layout = new QHBoxLayout(); @@ -130,9 +132,7 @@ ControllerEditWidget::ControllerEditWidget(QWidget* parent, const MoveItConfigDa controls_layout->setAlignment(btn_delete_, Qt::AlignRight); // Horizontal Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save btn_save_ = new QPushButton("&Save", this); @@ -195,12 +195,12 @@ void ControllerEditWidget::loadControllersTypesComboBox() return; has_loaded_ = true; - const std::array default_types = { + const std::array default_types = { "effort_controllers/JointTrajectoryController", "effort_controllers/JointPositionController", "effort_controllers/JointVelocityController", "effort_controllers/JointEffortController", - "joint_state_controller/JointStateController", "position_controllers/JointPositionController", - "position_controllers/JointTrajectoryController", "velocity_controllers/JointTrajectoryController", - "velocity_controllers/JointvelocityController" + "position_controllers/JointPositionController", "position_controllers/JointTrajectoryController", + "velocity_controllers/JointTrajectoryController", "velocity_controllers/JointVelocityController", + "pos_vel_controllers/JointTrajectoryController", "pos_vel_acc_controllers/JointTrajectoryController" }; // Remove all old items diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.h b/moveit_setup_assistant/src/widgets/controller_edit_widget.h index 2113005c8a..0b5b8c4de0 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.h +++ b/moveit_setup_assistant/src/widgets/controller_edit_widget.h @@ -33,14 +33,13 @@ /* Author: Mohamad Ayman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_CONTROLLER_EDIT_WIDGET_H -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_CONTROLLER_EDIT_WIDGET_H +#pragma once #include -#include -#include -#include -#include +class QComboBox; +class QLabel; +class QLineEdit; +class QPushButton; #ifndef Q_MOC_RUN #include @@ -141,6 +140,4 @@ private Q_SLOTS: /// Contains all the configuration data for the setup assistant moveit_setup_assistant::MoveItConfigDataPtr config_data_; }; -} - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp index 33240ed4d6..76be30b8db 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp @@ -34,16 +34,26 @@ /* Author: Dave Coleman */ -#include -#include -#include -#include -#include +#include #include #include -#include +#include +#include +#include +#include #include +#include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "default_collisions_widget.h" #include "header_widget.h" @@ -187,12 +197,12 @@ DefaultCollisionsWidget::DefaultCollisionsWidget(QWidget* parent, const MoveItCo QRadioButton* radio_btn; radio_btn = new QRadioButton("linear view"); bottom_layout->addWidget(radio_btn); - view_mode_buttons_->addButton(radio_btn, LinearMode); + view_mode_buttons_->addButton(radio_btn, LINEAR_MODE); radio_btn->setChecked(true); radio_btn = new QRadioButton("matrix view"); bottom_layout->addWidget(radio_btn); - view_mode_buttons_->addButton(radio_btn, MatrixMode); + view_mode_buttons_->addButton(radio_btn, MATRIX_MODE); connect(view_mode_buttons_, SIGNAL(buttonClicked(int)), this, SLOT(loadCollisionTable())); // Revert Button @@ -282,7 +292,7 @@ void DefaultCollisionsWidget::loadCollisionTable() link_pairs_, config_data_->getPlanningScene()->getRobotModel()->getLinkModelNamesWithCollisionGeometry()); QAbstractItemModel* model; - if (view_mode_buttons_->checkedId() == MatrixMode) + if (view_mode_buttons_->checkedId() == MATRIX_MODE) { model = matrix_model; } @@ -311,7 +321,7 @@ void DefaultCollisionsWidget::loadCollisionTable() QHeaderView *horizontal_header, *vertical_header; // activate some model-specific settings - if (view_mode_buttons_->checkedId() == MatrixMode) + if (view_mode_buttons_->checkedId() == MATRIX_MODE) { connect(selection_model_, SIGNAL(currentChanged(QModelIndex, QModelIndex)), this, SLOT(previewSelectedMatrix(QModelIndex))); @@ -377,12 +387,11 @@ void DefaultCollisionsWidget::collisionsChanged(const QModelIndex& index) if (!index.isValid()) return; // Hm. For some reason, QTableView doesn't change selection if we click a checkbox - bool linear_mode = (view_mode_buttons_->checkedId() == LinearMode); + bool linear_mode = (view_mode_buttons_->checkedId() == LINEAR_MODE); const QItemSelection& selection = selection_model_->selection(); - if ((linear_mode && !selection.contains(index)) || // in linear mode: index not in selection - (!linear_mode && - !(selection.contains(index) || // in matrix mode: index or symmetric index not in selection - selection.contains(model_->index(index.column(), index.row()))))) + if ((linear_mode && !selection.contains(index)) || // in linear mode: index not in selection + (!linear_mode && !(selection.contains(index) || // in matrix mode: index or symmetric index not in selection + selection.contains(model_->index(index.column(), index.row()))))) { QItemSelectionModel::SelectionFlags flags = QItemSelectionModel::Select | QItemSelectionModel::Current; if (linear_mode) @@ -592,7 +601,7 @@ void DefaultCollisionsWidget::toggleSelection(QItemSelection selection) // set all selected items to inverse value of current item const QModelIndex& cur_idx = selection_model_->currentIndex(); - if (view_mode_buttons_->checkedId() == MatrixMode) + if (view_mode_buttons_->checkedId() == MATRIX_MODE) { QModelIndex input_index; if (cur_idx.flags() & Qt::ItemIsUserCheckable) @@ -709,18 +718,16 @@ void DefaultCollisionsWidget::linkPairsFromSRDF() std::pair link_pair; // Loop through all disabled collisions in SRDF and update the comprehensive list that has already been created - for (std::vector::const_iterator collision_it = - config_data_->srdf_->disabled_collisions_.begin(); - collision_it != config_data_->srdf_->disabled_collisions_.end(); ++collision_it) + for (const auto& disabled_collision : config_data_->srdf_->disabled_collisions_) { // Set the link names - link_pair.first = collision_it->link1_; - link_pair.second = collision_it->link2_; + link_pair.first = disabled_collision.link1_; + link_pair.second = disabled_collision.link2_; if (link_pair.first >= link_pair.second) std::swap(link_pair.first, link_pair.second); // Set the link meta data - link_pair_data.reason = moveit_setup_assistant::disabledReasonFromString(collision_it->reason_); + link_pair_data.reason = moveit_setup_assistant::disabledReasonFromString(disabled_collision.reason_); link_pair_data.disable_check = true; // disable checking the collision btw the 2 links // Insert into map diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.h b/moveit_setup_assistant/src/widgets/default_collisions_widget.h index cef78d3c0b..983f7cb73d 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.h +++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.h @@ -34,22 +34,25 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_DEFAULT_COLLISIONS_WIDGET__ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_DEFAULT_COLLISIONS_WIDGET__ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#pragma once #include -#include -#include +class QAbstractItemModel; +class QAction; +class QButtonGroup; +class QCheckBox; +class QGroupBox; +class QHeaderView; +class QItemSelection; +class QItemSelectionModel; +class QLabel; +class QLineEdit; +class QProgressBar; +class QPushButton; +class QRadioButton; +class QSlider; +class QSpinBox; +class QTableView; +class QVBoxLayout; #ifndef Q_MOC_RUN #include @@ -73,8 +76,8 @@ class DefaultCollisionsWidget : public SetupScreenWidget public: enum ViewMode { - MatrixMode = 0, - LinearMode = 1 + MATRIX_MODE = 0, + LINEAR_MODE = 1 }; // ****************************************************************************************** @@ -140,8 +143,8 @@ private Q_SLOTS: void revertChanges(); /** - * \brief Called when current row has changed - */ + * \brief Called when current row has changed + */ void previewSelectedMatrix(const QModelIndex& index); void previewSelectedLinear(const QModelIndex& index); @@ -237,7 +240,7 @@ class MonitorThread : public QThread Q_OBJECT public: - MonitorThread(const boost::function& f, QProgressBar* progress_bar = NULL); + MonitorThread(const boost::function& f, QProgressBar* progress_bar = nullptr); void run() override; void cancel() { @@ -256,6 +259,4 @@ class MonitorThread : public QThread unsigned int progress_; bool canceled_; }; -} - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.cpp b/moveit_setup_assistant/src/widgets/double_list_widget.cpp index f3769bfa70..a0747c3f57 100644 --- a/moveit_setup_assistant/src/widgets/double_list_widget.cpp +++ b/moveit_setup_assistant/src/widgets/double_list_widget.cpp @@ -34,15 +34,17 @@ /* Author: Dave Coleman */ -#include -#include -#include -#include #include +#include +#include +#include +#include #include #include +#include #include -#include +#include +#include #include "double_list_widget.h" namespace moveit_setup_assistant @@ -140,9 +142,7 @@ DoubleListWidget::DoubleListWidget(QWidget* parent, const MoveItConfigDataPtr& c controls_layout->setContentsMargins(0, 25, 0, 15); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save QPushButton* btn_save = new QPushButton("&Save", this); @@ -299,7 +299,7 @@ void DoubleListWidget::deselectDataButtonClicked() // ****************************************************************************************** // Highlight links of robot for left list // ****************************************************************************************** -void DoubleListWidget::previewSelectedLeft(const QItemSelection& selected, const QItemSelection& deselected) +void DoubleListWidget::previewSelectedLeft(const QItemSelection& /*selected*/, const QItemSelection& /*deselected*/) { const QList selected_items = data_table_->selectedItems(); previewSelected(selected_items); @@ -308,7 +308,7 @@ void DoubleListWidget::previewSelectedLeft(const QItemSelection& selected, const // ****************************************************************************************** // Highlight links of robot for right list // ****************************************************************************************** -void DoubleListWidget::previewSelectedRight(const QItemSelection& selected, const QItemSelection& deselected) +void DoubleListWidget::previewSelectedRight(const QItemSelection& /*selected*/, const QItemSelection& /*deselected*/) { const QList selected_items = selected_data_table_->selectedItems(); previewSelected(selected_items); diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.h b/moveit_setup_assistant/src/widgets/double_list_widget.h index c4e511bd14..509f96402f 100644 --- a/moveit_setup_assistant/src/widgets/double_list_widget.h +++ b/moveit_setup_assistant/src/widgets/double_list_widget.h @@ -34,12 +34,13 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_DOUBLE_LIST_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_DOUBLE_LIST_WIDGET_ +#pragma once #include -#include -#include +class QLabel; +class QTableWidget; +class QTableWidgetItem; +class QItemSelection; #ifndef Q_MOC_RUN #include @@ -69,7 +70,7 @@ class DoubleListWidget : public QWidget /// Set the right box void setSelected(const std::vector& items); - void clearContents(void); + void clearContents(); /// Convenience function for reusing set table code void setTable(const std::vector& items, QTableWidget* table); @@ -141,5 +142,3 @@ private Q_SLOTS: }; } // namespace moveit_setup_assistant - -#endif diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp index 4c3889108b..d05cec207e 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp @@ -36,10 +36,24 @@ // SA #include "end_effectors_widget.h" +#include "header_widget.h" + // Qt +#include +#include #include +#include +#include +#include +#include #include -#include +#include +#include +#include +#include +#include +#include +#include namespace moveit_setup_assistant { @@ -54,12 +68,12 @@ EndEffectorsWidget::EndEffectorsWidget(QWidget* parent, const MoveItConfigDataPt // Top Header Area ------------------------------------------------ - HeaderWidget* header = - new HeaderWidget("Define End Effectors", "Setup your robot's end effectors. These are planning groups " - "corresponding to grippers or tools, attached to a parent " - "planning group (an arm). The specified parent link is used as the " - "reference frame for IK attempts.", - this); + HeaderWidget* header = new HeaderWidget("Define End Effectors", + "Setup your robot's end effectors. These are planning groups " + "corresponding to grippers or tools, attached to a parent " + "planning group (an arm). The specified parent link is used as the " + "reference frame for IK attempts.", + this); layout->addWidget(header); // Create contents screens --------------------------------------- @@ -68,15 +82,10 @@ EndEffectorsWidget::EndEffectorsWidget(QWidget* parent, const MoveItConfigDataPt effector_edit_widget_ = createEditWidget(); // Create stacked layout ----------------------------------------- - stacked_layout_ = new QStackedLayout(this); - stacked_layout_->addWidget(effector_list_widget_); // screen index 0 - stacked_layout_->addWidget(effector_edit_widget_); // screen index 1 - - // Create Widget wrapper for layout - QWidget* stacked_layout_widget = new QWidget(this); - stacked_layout_widget->setLayout(stacked_layout_); - - layout->addWidget(stacked_layout_widget); + stacked_widget_ = new QStackedWidget(this); + stacked_widget_->addWidget(effector_list_widget_); // screen index 0 + stacked_widget_->addWidget(effector_edit_widget_); // screen index 1 + layout->addWidget(stacked_widget_); // Finish Layout -------------------------------------------------- this->setLayout(layout); @@ -116,9 +125,7 @@ QWidget* EndEffectorsWidget::createContentsWidget() QHBoxLayout* controls_layout = new QHBoxLayout(); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Edit Selected Button btn_edit_ = new QPushButton("&Edit Selected", this); @@ -196,9 +203,7 @@ QWidget* EndEffectorsWidget::createEditWidget() controls_layout->setContentsMargins(0, 25, 0, 15); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save btn_save_ = new QPushButton("&Save", this); @@ -238,7 +243,7 @@ void EndEffectorsWidget::showNewScreen() parent_group_name_field_->clearEditText(); // actually this just chooses first option // Switch to screen - stacked_layout_->setCurrentIndex(1); + stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode Q_EMIT isModal(true); @@ -247,7 +252,7 @@ void EndEffectorsWidget::showNewScreen() // ****************************************************************************************** // Edit whatever element is selected // ****************************************************************************************** -void EndEffectorsWidget::editDoubleClicked(int row, int column) +void EndEffectorsWidget::editDoubleClicked(int /*row*/, int /*column*/) { editSelected(); } @@ -255,7 +260,7 @@ void EndEffectorsWidget::editDoubleClicked(int row, int column) // ****************************************************************************************** // Preview whatever element is selected // ****************************************************************************************** -void EndEffectorsWidget::previewClicked(int row, int column) +void EndEffectorsWidget::previewClicked(int /*row*/, int /*column*/) { // Get list of all selected items QList selected = data_table_->selectedItems(); @@ -280,7 +285,7 @@ void EndEffectorsWidget::previewClicked(int row, int column) void EndEffectorsWidget::previewClickedString(const QString& name) { // Don't highlight if we are on the overview end effectors screen. we are just populating drop down box - if (stacked_layout_->currentIndex() == 0) + if (stacked_widget_->currentIndex() == 0) return; // Unhighlight all links @@ -348,7 +353,7 @@ void EndEffectorsWidget::edit(const std::string& name) parent_group_name_field_->setCurrentIndex(index); // Switch to screen - stacked_layout_->setCurrentIndex(1); + stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode Q_EMIT isModal(true); @@ -381,10 +386,10 @@ void EndEffectorsWidget::loadParentComboBox() parent_name_field_->clear(); // Get all links in robot model - std::vector link_models = config_data_->getRobotModel()->getLinkModels(); + std::vector link_models = config_data_->getRobotModel()->getLinkModels(); // Add all links to combo box - for (std::vector::const_iterator link_it = link_models.begin(); + for (std::vector::const_iterator link_it = link_models.begin(); link_it < link_models.end(); ++link_it) { parent_name_field_->addItem((*link_it)->getName().c_str()); @@ -486,13 +491,12 @@ void EndEffectorsWidget::doneEditing() } // Check that the effector name is unique - for (std::vector::const_iterator data_it = config_data_->srdf_->end_effectors_.begin(); - data_it != config_data_->srdf_->end_effectors_.end(); ++data_it) + for (const auto& eef : config_data_->srdf_->end_effectors_) { - if (data_it->name_.compare(effector_name) == 0) // the names are the same + if (eef.name_ == effector_name) { // is this our existing effector? check if effector pointers are same - if (&(*data_it) != searched_data) + if (&eef != searched_data) { QMessageBox::warning( this, "Error Saving", @@ -516,7 +520,7 @@ void EndEffectorsWidget::doneEditing() return; } - const robot_model::JointModelGroup* jmg = + const moveit::core::JointModelGroup* jmg = config_data_->getRobotModel()->getJointModelGroup(group_name_field_->currentText().toStdString()); /* if (jmg->hasLinkModel(parent_name_field_->currentText().toStdString())) @@ -564,6 +568,7 @@ void EndEffectorsWidget::doneEditing() if (is_new) { config_data_->srdf_->end_effectors_.push_back(*searched_data); + delete searched_data; } // Finish up ------------------------------------------------------ @@ -572,7 +577,7 @@ void EndEffectorsWidget::doneEditing() loadDataTable(); // Switch to screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode Q_EMIT isModal(false); @@ -584,7 +589,7 @@ void EndEffectorsWidget::doneEditing() void EndEffectorsWidget::cancelEditing() { // Switch to screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Re-highlight the currently selected end effector group previewClicked(0, 0); // the number parameters are actually meaningless @@ -608,17 +613,16 @@ void EndEffectorsWidget::loadDataTable() // Loop through every end effector int row = 0; - for (std::vector::const_iterator data_it = config_data_->srdf_->end_effectors_.begin(); - data_it != config_data_->srdf_->end_effectors_.end(); ++data_it) + for (const auto& eef : config_data_->srdf_->end_effectors_) { // Create row elements - QTableWidgetItem* data_name = new QTableWidgetItem(data_it->name_.c_str()); + QTableWidgetItem* data_name = new QTableWidgetItem(eef.name_.c_str()); data_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* group_name = new QTableWidgetItem(data_it->component_group_.c_str()); + QTableWidgetItem* group_name = new QTableWidgetItem(eef.component_group_.c_str()); group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* parent_name = new QTableWidgetItem(data_it->parent_link_.c_str()); + QTableWidgetItem* parent_name = new QTableWidgetItem(eef.parent_link_.c_str()); group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* parent_group_name = new QTableWidgetItem(data_it->parent_group_.c_str()); + QTableWidgetItem* parent_group_name = new QTableWidgetItem(eef.parent_group_.c_str()); parent_group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); // Add to table @@ -652,7 +656,7 @@ void EndEffectorsWidget::loadDataTable() void EndEffectorsWidget::focusGiven() { // Show the current effectors screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Load the data to the tree loadDataTable(); diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.h b/moveit_setup_assistant/src/widgets/end_effectors_widget.h index 1c5b17247c..63403fea49 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.h +++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.h @@ -34,28 +34,20 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_END_EFFECTORS_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_END_EFFECTORS_WIDGET_ +#pragma once // Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +class QComboBox; +class QLineEdit; +class QPushButton; +class QStackedWidget; +class QTableWidget; // SA #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant @@ -82,7 +74,7 @@ class EndEffectorsWidget : public SetupScreenWidget QPushButton* btn_delete_; QPushButton* btn_save_; QPushButton* btn_cancel_; - QStackedLayout* stacked_layout_; + QStackedWidget* stacked_widget_; QLineEdit* effector_name_field_; QComboBox* parent_name_field_; QComboBox* parent_group_name_field_; @@ -183,6 +175,4 @@ private Q_SLOTS: void edit(const std::string& name); }; -} // namespace - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp index c822edd000..3382e95563 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp @@ -34,12 +34,18 @@ /* Author: Dave Coleman */ -#include +#include +#include +#include +#include #include +#include +#include #include -#include +#include #include -#include +#include + #include "group_edit_widget.h" #include // for loading all avail kinematic planners @@ -88,6 +94,20 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& con kinematics_timeout_field_->setMaximumWidth(400); form_layout->addRow("Kin. Search Timeout (sec):", kinematics_timeout_field_); + // file to load additional parameters from + kinematics_parameters_file_field_ = new QLineEdit(this); + kinematics_parameters_file_field_->setMaximumWidth(400); + QPushButton* kinematics_parameters_file_button = new QPushButton("...", this); + kinematics_parameters_file_button->setMaximumWidth(50); + connect(kinematics_parameters_file_button, SIGNAL(clicked()), this, SLOT(selectKinematicsFile())); + QBoxLayout* kinematics_parameters_file_layout = new QHBoxLayout(this); + kinematics_parameters_file_layout->addWidget(kinematics_parameters_file_field_); + kinematics_parameters_file_layout->addWidget(kinematics_parameters_file_button); + kinematics_parameters_file_layout->setContentsMargins(0, 0, 0, 0); + QWidget* container = new QWidget(this); + container->setLayout(kinematics_parameters_file_layout); + form_layout->addRow("Kin. parameters file:", container); + group1->setLayout(form_layout); // OMPL Planner form -------------------------------------------- @@ -127,6 +147,12 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& con add_subtitle->setFont(add_subtitle_font); recommended_options->addWidget(add_subtitle, 0, Qt::AlignLeft); + // Save and add chain + QPushButton* btn_save_chain = new QPushButton("Add Kin. Chain", this); + btn_save_chain->setMaximumWidth(200); + connect(btn_save_chain, SIGNAL(clicked()), this, SIGNAL(saveChain())); + recommended_options->addWidget(btn_save_chain); + // Save and add joints QPushButton* btn_save_joints = new QPushButton("Add Joints", this); btn_save_joints->setMaximumWidth(200); @@ -138,24 +164,18 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& con add_subtitle2->setFont(add_subtitle_font); advanced_options->addWidget(add_subtitle2, 0, Qt::AlignLeft); - // Save and add links - QPushButton* btn_save_links = new QPushButton("Add Links", this); - btn_save_links->setMaximumWidth(200); - connect(btn_save_links, SIGNAL(clicked()), this, SIGNAL(saveLinks())); - advanced_options->addWidget(btn_save_links); - - // Save and add chain - QPushButton* btn_save_chain = new QPushButton("Add Kin. Chain", this); - btn_save_chain->setMaximumWidth(200); - connect(btn_save_chain, SIGNAL(clicked()), this, SIGNAL(saveChain())); - advanced_options->addWidget(btn_save_chain); - // Save and add subgroups QPushButton* btn_save_subgroups = new QPushButton("Add Subgroups", this); btn_save_subgroups->setMaximumWidth(200); connect(btn_save_subgroups, SIGNAL(clicked()), this, SIGNAL(saveSubgroups())); advanced_options->addWidget(btn_save_subgroups); + // Save and add links + QPushButton* btn_save_links = new QPushButton("Add Links", this); + btn_save_links->setMaximumWidth(200); + connect(btn_save_links, SIGNAL(clicked()), this, SIGNAL(saveLinks())); + advanced_options->addWidget(btn_save_links); + // Add layouts new_buttons_layout_container->addLayout(label_layout); new_buttons_layout_container->addLayout(recommended_options); @@ -165,10 +185,8 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& con new_buttons_widget_->setLayout(new_buttons_layout_container); layout->addWidget(new_buttons_widget_); - // Verticle Spacer ----------------------------------------------------- - QWidget* vspacer = new QWidget(this); - vspacer->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); - layout->addWidget(vspacer); + // Vertical Spacer + layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Minimum, QSizePolicy::Expanding)); // Bottom Controls --------------------------------------------------------- QHBoxLayout* controls_layout = new QHBoxLayout(); @@ -181,9 +199,7 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& con controls_layout->setAlignment(btn_delete_, Qt::AlignRight); // Horizontal Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save btn_save_ = new QPushButton("&Save", this); @@ -220,7 +236,7 @@ void GroupEditWidget::setSelected(const std::string& group_name) if (*resolution == 0) { // Set default value - *resolution = DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION_; + *resolution = DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION; } kinematics_resolution_field_->setText(QString::number(*resolution)); @@ -229,7 +245,7 @@ void GroupEditWidget::setSelected(const std::string& group_name) if (*timeout == 0) { // Set default value - *timeout = DEFAULT_KIN_SOLVER_TIMEOUT_; + *timeout = DEFAULT_KIN_SOLVER_TIMEOUT; } kinematics_timeout_field_->setText(QString::number(*timeout)); @@ -250,7 +266,7 @@ void GroupEditWidget::setSelected(const std::string& group_name) QString("Unable to find the kinematic solver '") .append(kin_solver.c_str()) .append("'. Trying running rosmake for this package. Until fixed, this setting will be " - "lost the next time the MoveIt! configuration files are generated")); + "lost the next time the MoveIt configuration files are generated")); return; } else @@ -258,6 +274,9 @@ void GroupEditWidget::setSelected(const std::string& group_name) kinematics_solver_field_->setCurrentIndex(index); } + kinematics_parameters_file_field_->setText( + config_data_->group_meta_data_[group_name].kinematics_parameters_file_.c_str()); + // Set default planner std::string default_planner = config_data_->group_meta_data_[group_name].default_planner_; @@ -306,8 +325,9 @@ void GroupEditWidget::loadKinematicPlannersComboBox() } catch (pluginlib::PluginlibException& ex) { - QMessageBox::warning(this, "Missing Kinematic Solvers", "Exception while creating class loader for kinematic " - "solver plugins"); + QMessageBox::warning(this, "Missing Kinematic Solvers", + "Exception while creating class loader for kinematic " + "solver plugins"); ROS_ERROR_STREAM(ex.what()); return; } @@ -318,9 +338,10 @@ void GroupEditWidget::loadKinematicPlannersComboBox() // Warn if no plugins are found if (classes.empty()) { - QMessageBox::warning(this, "Missing Kinematic Solvers", "No MoveIt!-compatible kinematics solvers found. Try " - "installing moveit_kinematics (sudo apt-get install " - "ros-${ROS_DISTRO}-moveit-kinematics)"); + QMessageBox::warning(this, "Missing Kinematic Solvers", + "No MoveIt-compatible kinematics solvers found. Try " + "installing moveit_kinematics (sudo apt-get install " + "ros-${ROS_DISTRO}-moveit-kinematics)"); return; } @@ -338,4 +359,26 @@ void GroupEditWidget::loadKinematicPlannersComboBox() } } +void GroupEditWidget::selectKinematicsFile() +{ + QString filename = QFileDialog::getOpenFileName(this, "Select a parameter file", "", "YAML files (*.yaml)"); + + if (filename.isEmpty()) + { + return; + } + + std::string package_name; + std::string relative_filename; + bool package_found = + config_data_->extractPackageNameFromPath(filename.toStdString(), package_name, relative_filename); + + QString lookup_path = filename; + if (package_found) + { + lookup_path = QString("$(find %1)/%2").arg(package_name.c_str()).arg(relative_filename.c_str()); + } + kinematics_parameters_file_field_->setText(lookup_path); +} + } // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.h b/moveit_setup_assistant/src/widgets/group_edit_widget.h index e5ae0ec884..0cf37207c7 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.h +++ b/moveit_setup_assistant/src/widgets/group_edit_widget.h @@ -34,14 +34,13 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_GROUP_EDIT_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_GROUP_EDIT_WIDGET_ +#pragma once #include -#include -#include -#include -#include +class QComboBox; +class QLabel; +class QLineEdit; +class QPushButton; #ifndef Q_MOC_RUN #include @@ -76,6 +75,7 @@ class GroupEditWidget : public QWidget QComboBox* kinematics_solver_field_; QLineEdit* kinematics_resolution_field_; QLineEdit* kinematics_timeout_field_; + QLineEdit* kinematics_parameters_file_field_; QComboBox* default_planner_field_; QPushButton* btn_delete_; // this button is hidden for new groups QPushButton* btn_save_; // this button is hidden for new groups @@ -87,6 +87,9 @@ private Q_SLOTS: // Slot Event Functions // ****************************************************************************************** + /// Shows a file dialog to select an additional parameter file for kinematics + void selectKinematicsFile(); + Q_SIGNALS: // ****************************************************************************************** @@ -126,6 +129,4 @@ private Q_SLOTS: // Private Functions // ****************************************************************************************** }; -} - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/header_widget.cpp b/moveit_setup_assistant/src/widgets/header_widget.cpp index e42d06f5e1..695efbece0 100644 --- a/moveit_setup_assistant/src/widgets/header_widget.cpp +++ b/moveit_setup_assistant/src/widgets/header_widget.cpp @@ -34,11 +34,13 @@ /* Author: Dave Coleman */ -#include -#include +#include "header_widget.h" #include +#include +#include +#include +#include #include -#include "header_widget.h" namespace moveit_setup_assistant { diff --git a/moveit_setup_assistant/src/widgets/header_widget.h b/moveit_setup_assistant/src/widgets/header_widget.h index 40793d36a0..76bda444d7 100644 --- a/moveit_setup_assistant/src/widgets/header_widget.h +++ b/moveit_setup_assistant/src/widgets/header_widget.h @@ -34,12 +34,12 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_HEADER_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_HEADER_WIDGET_ +#pragma once #include -#include -#include +#include +class QLabel; +class QLineEdit; namespace moveit_setup_assistant { @@ -118,6 +118,4 @@ class LoadPathArgsWidget : public LoadPathWidget void setArgs(const QString& args); void setArgsEnabled(bool enabled = true); }; -} - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp index 926cbb18f4..88bda9853e 100644 --- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp +++ b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp @@ -41,7 +41,8 @@ #include #include #include -#include +#include +#include #include "kinematic_chain_widget.h" namespace moveit_setup_assistant @@ -108,9 +109,7 @@ KinematicChainWidget::KinematicChainWidget(QWidget* parent, const MoveItConfigDa controls_layout->addWidget(expand_controls); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save QPushButton* btn_save = new QPushButton("&Save", this); @@ -146,10 +145,10 @@ void KinematicChainWidget::setAvailable() return; // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel(); + const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); // Get the root joint - const robot_model::JointModel* root_joint = model->getRootJoint(); + const moveit::core::JointModel* root_joint = model->getRootJoint(); addLinktoTreeRecursive(root_joint->getChildLinkModel(), nullptr); @@ -160,8 +159,8 @@ void KinematicChainWidget::setAvailable() // ****************************************************************************************** // // ****************************************************************************************** -void KinematicChainWidget::addLinktoTreeRecursive(const robot_model::LinkModel* link, - const robot_model::LinkModel* parent) +void KinematicChainWidget::addLinktoTreeRecursive(const moveit::core::LinkModel* link, + const moveit::core::LinkModel* parent) { // Create new tree item QTreeWidgetItem* new_item = new QTreeWidgetItem(link_tree_); @@ -191,7 +190,7 @@ void KinematicChainWidget::addLinktoTreeRecursive(const robot_model::LinkModel* // ****************************************************************************************** // // ****************************************************************************************** -bool KinematicChainWidget::addLinkChildRecursive(QTreeWidgetItem* parent, const robot_model::LinkModel* link, +bool KinematicChainWidget::addLinkChildRecursive(QTreeWidgetItem* parent, const moveit::core::LinkModel* link, const std::string& parent_name) { if (parent->text(0).toStdString() == parent_name) diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h index da63182675..e5f004f222 100644 --- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h +++ b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h @@ -34,16 +34,16 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_KINEMATIC_CHAIN_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_KINEMATIC_CHAIN_WIDGET_ +#pragma once #include -#include -#include +class QLabel; +class QLineEdit; +class QTreeWidget; +class QTreeWidgetItem; #ifndef Q_MOC_RUN #include -#include #endif namespace moveit_setup_assistant @@ -69,9 +69,9 @@ class KinematicChainWidget : public QWidget /// Set the link field with previous value void setSelected(const std::string& base_link, const std::string& tip_link); - void addLinktoTreeRecursive(const robot_model::LinkModel* link, const robot_model::LinkModel* parent); + void addLinktoTreeRecursive(const moveit::core::LinkModel* link, const moveit::core::LinkModel* parent); - bool addLinkChildRecursive(QTreeWidgetItem* parent, const robot_model::LinkModel* link, + bool addLinkChildRecursive(QTreeWidgetItem* parent, const moveit::core::LinkModel* link, const std::string& parent_name); // ****************************************************************************************** @@ -134,6 +134,4 @@ private Q_SLOTS: // Private Functions // ****************************************************************************************** }; -} - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/navigation_widget.cpp b/moveit_setup_assistant/src/widgets/navigation_widget.cpp index 703e1936b1..56777454ef 100644 --- a/moveit_setup_assistant/src/widgets/navigation_widget.cpp +++ b/moveit_setup_assistant/src/widgets/navigation_widget.cpp @@ -36,6 +36,9 @@ #include "navigation_widget.h" #include +#include +#include +#include #include namespace moveit_setup_assistant @@ -116,7 +119,7 @@ NavDelegate::NavDelegate(QObject* parent) : QStyledItemDelegate(parent) { } -QSize NavDelegate::sizeHint(const QStyleOptionViewItem& option, const QModelIndex& index) const +QSize NavDelegate::sizeHint(const QStyleOptionViewItem& option, const QModelIndex& /*index*/) const { return QSize(option.rect.width(), 45); } diff --git a/moveit_setup_assistant/src/widgets/navigation_widget.h b/moveit_setup_assistant/src/widgets/navigation_widget.h index 962a7d1dc0..59bfba9e09 100644 --- a/moveit_setup_assistant/src/widgets/navigation_widget.h +++ b/moveit_setup_assistant/src/widgets/navigation_widget.h @@ -34,15 +34,11 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_NAVIGATION_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_NAVIGATION_WIDGET_ +#pragma once #include -#include -#include -#include #include -#include +class QStandardItemModel; namespace moveit_setup_assistant { @@ -57,7 +53,7 @@ class NavigationWidget : public QListView { Q_OBJECT public: - explicit NavigationWidget(QWidget* parent = 0); + explicit NavigationWidget(QWidget* parent = nullptr); void setNavs(const QList& navs); void setEnabled(const int& index, bool enabled); @@ -78,11 +74,9 @@ class NavDelegate : public QStyledItemDelegate { Q_OBJECT public: - explicit NavDelegate(QObject* parent = 0); + explicit NavDelegate(QObject* parent = nullptr); QSize sizeHint(const QStyleOptionViewItem& option, const QModelIndex& index) const override; void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override; }; -} - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp b/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp index f7aa81693d..85610b4404 100644 --- a/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp @@ -36,9 +36,14 @@ // SA #include "passive_joints_widget.h" +#include "header_widget.h" +#include "double_list_widget.h" + // Qt #include +#include #include +#include namespace moveit_setup_assistant { @@ -53,10 +58,10 @@ PassiveJointsWidget::PassiveJointsWidget(QWidget* parent, const MoveItConfigData // Top Header Area ------------------------------------------------ - HeaderWidget* header = - new HeaderWidget("Define Passive Joints", "Specify the set of passive joints (not actuated). Joint " - "state is not expected to be published for these joints.", - this); + HeaderWidget* header = new HeaderWidget("Define Passive Joints", + "Specify the set of passive joints (not actuated). Joint " + "state is not expected to be published for these joints.", + this); layout->addWidget(header); // Joints edit widget @@ -84,7 +89,7 @@ void PassiveJointsWidget::focusGiven() joints_widget_->clearContents(); // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel(); + const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); // Get the names of the all joints const std::vector& joints = model->getJointModelNames(); @@ -126,14 +131,14 @@ void PassiveJointsWidget::selectionUpdated() // ****************************************************************************************** // Called from Double List widget to highlight joints // ****************************************************************************************** -void PassiveJointsWidget::previewSelectedJoints(std::vector joints) +void PassiveJointsWidget::previewSelectedJoints(const std::vector& joints) { // Unhighlight all links Q_EMIT unhighlightAll(); for (const std::string& joint : joints) { - const robot_model::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); + const moveit::core::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); // Check that a joint model was found if (!joint_model) diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.h b/moveit_setup_assistant/src/widgets/passive_joints_widget.h index c7dc8dd1d5..91181744ce 100644 --- a/moveit_setup_assistant/src/widgets/passive_joints_widget.h +++ b/moveit_setup_assistant/src/widgets/passive_joints_widget.h @@ -34,31 +34,18 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PASSIVE_JOINTS_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PASSIVE_JOINTS_WIDGET_ - -// Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include +#pragma once // SA #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" -#include "double_list_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant { +class DoubleListWidget; class PassiveJointsWidget : public SetupScreenWidget { Q_OBJECT @@ -87,7 +74,7 @@ private Q_SLOTS: void selectionUpdated(); /// Called from Double List widget to highlight joints - void previewSelectedJoints(std::vector joints); + void previewSelectedJoints(const std::vector& joints); private: // ****************************************************************************************** @@ -101,6 +88,4 @@ private Q_SLOTS: std::string current_edit_vjoint_; }; -} // namespace - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/perception_widget.cpp b/moveit_setup_assistant/src/widgets/perception_widget.cpp index cc13ee2226..f9dc1c167d 100644 --- a/moveit_setup_assistant/src/widgets/perception_widget.cpp +++ b/moveit_setup_assistant/src/widgets/perception_widget.cpp @@ -35,11 +35,16 @@ // SA #include "perception_widget.h" +#include "header_widget.h" // Qt -#include -#include #include +#include +#include +#include +#include +#include +#include namespace moveit_setup_assistant { @@ -57,7 +62,7 @@ PerceptionWidget::PerceptionWidget(QWidget* parent, const MoveItConfigDataPtr& c HeaderWidget* header = new HeaderWidget("Setup 3D Perception Sensors", - "Configure your 3D sensors to work with Moveit! " + "Configure your 3D sensors to work with Moveit " "Please see Perception Documentation " @@ -208,8 +213,7 @@ bool PerceptionWidget::focusLost() "PointCloudOctomapUpdater"); config_data_->addGenericParameterToSensorPluginConfig("point_cloud_topic", point_cloud_topic_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("max_range", - max_range_field_->text().trimmed().toStdString()); + config_data_->addGenericParameterToSensorPluginConfig("max_range", max_range_field_->text().trimmed().toStdString()); config_data_->addGenericParameterToSensorPluginConfig("point_subsample", point_subsample_field_->text().trimmed().toStdString()); config_data_->addGenericParameterToSensorPluginConfig("padding_offset", diff --git a/moveit_setup_assistant/src/widgets/perception_widget.h b/moveit_setup_assistant/src/widgets/perception_widget.h index 336fe8a80e..6d23c0d5ba 100644 --- a/moveit_setup_assistant/src/widgets/perception_widget.h +++ b/moveit_setup_assistant/src/widgets/perception_widget.h @@ -33,22 +33,18 @@ /* Author: Mohamad Ayman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PERCEPTION_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PERCEPTION_WIDGET_ +#pragma once // Qt -#include -#include -#include -#include -#include +class QComboBox; +class QGroupBox; +class QLineEdit; // SA #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant @@ -125,5 +121,3 @@ private Q_SLOTS: }; } // namespace moveit_setup_assistant - -#endif diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp index a014914fc7..9c91095b42 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp @@ -47,7 +47,6 @@ */ // ****************************************************************************************** -#include "ros/ros.h" #include "header_widget.h" #include "planning_groups_widget.h" #include "double_list_widget.h" // for joints, links and subgroups pages @@ -55,13 +54,17 @@ #include "group_edit_widget.h" // for group rename page // Qt #include -#include +#include #include -#include -#include +#include +#include #include +#include +#include +#include #include #include +#include //// Cycle checking #include @@ -153,25 +156,20 @@ PlanningGroupsWidget::PlanningGroupsWidget(QWidget* parent, const MoveItConfigDa connect(group_edit_widget_, SIGNAL(saveChain()), this, SLOT(saveGroupScreenChain())); connect(group_edit_widget_, SIGNAL(saveSubgroups()), this, SLOT(saveGroupScreenSubgroups())); - // Combine into stack - stacked_layout_ = new QStackedLayout(this); - stacked_layout_->addWidget(groups_tree_widget_); // screen index 0 - stacked_layout_->addWidget(joints_widget_); // screen index 1 - stacked_layout_->addWidget(links_widget_); // screen index 2 - stacked_layout_->addWidget(chain_widget_); // screen index 3 - stacked_layout_->addWidget(subgroups_widget_); // screen index 4 - stacked_layout_->addWidget(group_edit_widget_); // screen index 5 + // Combine into stack: Note, order is same as GroupType! + stacked_widget_ = new QStackedWidget(this); + stacked_widget_->addWidget(groups_tree_widget_); // screen index 0 + stacked_widget_->addWidget(joints_widget_); // screen index 1 + stacked_widget_->addWidget(links_widget_); // screen index 2 + stacked_widget_->addWidget(chain_widget_); // screen index 3 + stacked_widget_->addWidget(subgroups_widget_); // screen index 4 + stacked_widget_->addWidget(group_edit_widget_); // screen index 5 showMainScreen(); // Finish GUI ----------------------------------------------------------- - // Create Widget wrapper for layout - QWidget* stacked_layout_widget = new QWidget(this); - stacked_layout_widget->setLayout(stacked_layout_); - - layout->addWidget(stacked_layout_widget); - + layout->addWidget(stacked_widget_); setLayout(layout); // process the gui @@ -208,9 +206,7 @@ QWidget* PlanningGroupsWidget::createContentsWidget() controls_layout->addWidget(expand_controls); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Delete Selected Button btn_delete_ = new QPushButton("&Delete Selected", this); @@ -318,7 +314,7 @@ void PlanningGroupsWidget::loadGroupsTreeRecursive(srdf::Model::Group& group_it, group->addChild(joints); // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel(); + const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); // Loop through all aval. joints for (std::vector::const_iterator joint_it = group_it.joints_.begin(); joint_it != group_it.joints_.end(); @@ -329,7 +325,7 @@ void PlanningGroupsWidget::loadGroupsTreeRecursive(srdf::Model::Group& group_it, std::string joint_name; // Get the type of joint this is - const robot_model::JointModel* jm = model->getJointModel(*joint_it); + const moveit::core::JointModel* jm = model->getJointModel(*joint_it); if (jm) // check if joint model was found { joint_name = *joint_it + " - " + jm->getTypeName(); @@ -374,7 +370,7 @@ void PlanningGroupsWidget::loadGroupsTreeRecursive(srdf::Model::Group& group_it, { warn_once = false; QMessageBox::warning(this, "Group with Multiple Kinematic Chains", - "Warning: this MoveIt! Setup Assistant is only designed to handle one kinematic chain per " + "Warning: this MoveIt Setup Assistant is only designed to handle one kinematic chain per " "group. The loaded SRDF has more than one kinematic chain for a group. A possible loss of " "data may occur."); } @@ -416,11 +412,12 @@ void PlanningGroupsWidget::loadGroupsTreeRecursive(srdf::Model::Group& group_it, // Check if subgroup was found if (searched_group == nullptr) // not found { - QMessageBox::critical(this, "Error Loading SRDF", QString("Subgroup '") - .append(subgroup_it->c_str()) - .append("' of group '") - .append(group_it.name_.c_str()) - .append("' not found. Your SRDF is invalid")); + QMessageBox::critical(this, "Error Loading SRDF", + QString("Subgroup '") + .append(subgroup_it->c_str()) + .append("' of group '") + .append(group_it.name_.c_str()) + .append("' not found. Your SRDF is invalid")); return; // TODO: something better for error handling? } @@ -468,50 +465,29 @@ void PlanningGroupsWidget::editSelected() // Get the user custom properties of the currently selected row PlanGroupType plan_group = item->data(0, Qt::UserRole).value(); - if (plan_group.type_ == JOINT) - { - // Load the data - loadJointsScreen(plan_group.group_); - - // Switch to screen - changeScreen(1); // 1 is index of joints - } - else if (plan_group.type_ == LINK) - { - // Load the data - loadLinksScreen(plan_group.group_); - - // Switch to screen - changeScreen(2); - } - else if (plan_group.type_ == CHAIN) - { - // Load the data - loadChainScreen(plan_group.group_); - - // Switch to screen - changeScreen(3); - } - else if (plan_group.type_ == SUBGROUP) - { - // Load the data - loadSubgroupsScreen(plan_group.group_); - - // Switch to screen - changeScreen(4); - } - else if (plan_group.type_ == GROUP) + switch (plan_group.type_) { - // Load the data - loadGroupScreen(plan_group.group_); - - // Switch to screen - changeScreen(5); - } - else - { - QMessageBox::critical(this, "Error Loading", "An internal error has occured while loading."); + case JOINT: + loadJointsScreen(plan_group.group_); + break; + case LINK: + loadLinksScreen(plan_group.group_); + break; + case CHAIN: + loadChainScreen(plan_group.group_); + break; + case SUBGROUP: + loadSubgroupsScreen(plan_group.group_); + break; + case GROUP: + loadGroupScreen(plan_group.group_); + break; + default: + QMessageBox::critical(this, "Error Loading", "An internal error has occured while loading."); + return; } + return_screen_ = 0; // return to main screen directly + changeScreen(plan_group.type_); } // ****************************************************************************************** @@ -520,7 +496,7 @@ void PlanningGroupsWidget::editSelected() void PlanningGroupsWidget::loadJointsScreen(srdf::Model::Group* this_group) { // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel(); + const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); // Get the names of the all joints const std::vector& joints = model->getJointModelNames(); @@ -543,7 +519,6 @@ void PlanningGroupsWidget::loadJointsScreen(srdf::Model::Group* this_group) // Remember what is currently being edited so we can later save changes current_edit_group_ = this_group->name_; - current_edit_element_ = JOINT; } // ****************************************************************************************** @@ -552,7 +527,7 @@ void PlanningGroupsWidget::loadJointsScreen(srdf::Model::Group* this_group) void PlanningGroupsWidget::loadLinksScreen(srdf::Model::Group* this_group) { // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel(); + const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); // Get the names of the all links const std::vector& links = model->getLinkModelNames(); @@ -575,7 +550,6 @@ void PlanningGroupsWidget::loadLinksScreen(srdf::Model::Group* this_group) // Remember what is currently being edited so we can later save changes current_edit_group_ = this_group->name_; - current_edit_element_ = LINK; } // ****************************************************************************************** @@ -589,10 +563,11 @@ void PlanningGroupsWidget::loadChainScreen(srdf::Model::Group* this_group) // Make sure there isn't more than 1 chain pair if (this_group->chains_.size() > 1) { - QMessageBox::warning(this, "Multiple Kinematic Chains", "Warning: This setup assistant is only designed to handle " - "one kinematic chain per group. The loaded SRDF has more " - "than one kinematic chain for a group. A possible loss of " - "data may occur."); + QMessageBox::warning(this, "Multiple Kinematic Chains", + "Warning: This setup assistant is only designed to handle " + "one kinematic chain per group. The loaded SRDF has more " + "than one kinematic chain for a group. A possible loss of " + "data may occur."); } // Set the selected tip and base of chain if one exists @@ -607,7 +582,6 @@ void PlanningGroupsWidget::loadChainScreen(srdf::Model::Group* this_group) // Remember what is currently being edited so we can later save changes current_edit_group_ = this_group->name_; - current_edit_element_ = CHAIN; } // ****************************************************************************************** @@ -640,7 +614,6 @@ void PlanningGroupsWidget::loadSubgroupsScreen(srdf::Model::Group* this_group) // Remember what is currently being edited so we can later save changes current_edit_group_ = this_group->name_; - current_edit_element_ = SUBGROUP; } // ****************************************************************************************** @@ -671,9 +644,6 @@ void PlanningGroupsWidget::loadGroupScreen(srdf::Model::Group* this_group) // Set the data in the edit box group_edit_widget_->setSelected(current_edit_group_); - - // Remember what is currently being edited so we can later save changes - current_edit_element_ = GROUP; } // ****************************************************************************************** @@ -702,12 +672,12 @@ void PlanningGroupsWidget::deleteGroup() srdf::Model::Group* searched_group = config_data_->findGroupByName(group); // Confirm user wants to delete group - if (QMessageBox::question( - this, "Confirm Group Deletion", - QString("Are you sure you want to delete the planning group '") - .append(searched_group->name_.c_str()) - .append("'? This will also delete all references in subgroups, robot poses and end effectors."), - QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) + if (QMessageBox::question(this, "Confirm Group Deletion", + QString("Are you sure you want to delete the planning group '") + .append(searched_group->name_.c_str()) + .append("'? This will also delete all references in subgroups, robot poses and end " + "effectors."), + QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) { return; } @@ -843,7 +813,7 @@ void PlanningGroupsWidget::addGroup() loadGroupScreen(nullptr); // NULL indicates this is a new group, not an existing one // Switch to screen - changeScreen(5); + changeScreen(GROUP); } // ****************************************************************************************** @@ -918,8 +888,9 @@ void PlanningGroupsWidget::saveChainScreen() // Check that box the tip and base, or neither, have text if ((!tip.empty() && base.empty()) || (tip.empty() && !base.empty())) { - QMessageBox::warning(this, "Error Saving", "You must specify a link for both the base and tip, or leave both " - "blank."); + QMessageBox::warning(this, "Error Saving", + "You must specify a link for both the base and tip, or leave both " + "blank."); return; } @@ -1084,6 +1055,8 @@ bool PlanningGroupsWidget::saveGroupScreen() const std::string& default_planner = group_edit_widget_->default_planner_field_->currentText().toStdString(); const std::string& kinematics_resolution = group_edit_widget_->kinematics_resolution_field_->text().toStdString(); const std::string& kinematics_timeout = group_edit_widget_->kinematics_timeout_field_->text().toStdString(); + const std::string& kinematics_parameters_file = + group_edit_widget_->kinematics_parameters_file_field_->text().toStdString(); // Used for editing existing groups srdf::Model::Group* searched_group = nullptr; @@ -1103,13 +1076,12 @@ bool PlanningGroupsWidget::saveGroupScreen() } // Check that the group name is unique - for (std::vector::const_iterator group_it = config_data_->srdf_->groups_.begin(); - group_it != config_data_->srdf_->groups_.end(); ++group_it) + for (const auto& group : config_data_->srdf_->groups_) { - if (group_it->name_.compare(group_name) == 0) // the names are the same + if (group.name_.compare(group_name) == 0) // the names are the same { // is this our existing group? check if group pointers are same - if (&(*group_it) != searched_group) + if (&group != searched_group) { QMessageBox::warning(this, "Error Saving", "A group already exists with that name!"); return false; @@ -1196,8 +1168,8 @@ bool PlanningGroupsWidget::saveGroupScreen() // Check if this eef's parent group references old group name. if so, update it if (eef_it->parent_group_.compare(old_group_name) == 0) // same name { - ROS_DEBUG_STREAM_NAMED("setup_assistant", "Changed eef '" << eef_it->name_ << "' to new parent group name " - << group_name); + ROS_DEBUG_STREAM_NAMED("setup_assistant", + "Changed eef '" << eef_it->name_ << "' to new parent group name " << group_name); eef_it->parent_group_ = group_name; // updated config_data_->changes |= MoveItConfigData::END_EFFECTORS; } @@ -1205,8 +1177,8 @@ bool PlanningGroupsWidget::saveGroupScreen() // Check if this eef's group references old group name. if so, update it if (eef_it->component_group_.compare(old_group_name) == 0) // same name { - ROS_DEBUG_STREAM_NAMED("setup_assistant", "Changed eef '" << eef_it->name_ << "' to new group name " - << group_name); + ROS_DEBUG_STREAM_NAMED("setup_assistant", + "Changed eef '" << eef_it->name_ << "' to new group name " << group_name); eef_it->component_group_ = group_name; // updated config_data_->changes |= MoveItConfigData::END_EFFECTORS; } @@ -1235,7 +1207,8 @@ bool PlanningGroupsWidget::saveGroupScreen() config_data_->group_meta_data_[group_name].kinematics_solver_ = kinematics_solver; config_data_->group_meta_data_[group_name].kinematics_solver_search_resolution_ = kinematics_resolution_double; config_data_->group_meta_data_[group_name].kinematics_solver_timeout_ = kinematics_timeout_double; - config_data_->group_meta_data_[group_name].default_planner_ = default_planner; + config_data_->group_meta_data_[group_name].kinematics_parameters_file_ = kinematics_parameters_file; + config_data_->group_meta_data_[group_name].default_planner_ = (default_planner == "None" ? "" : default_planner); config_data_->changes |= MoveItConfigData::GROUP_KINEMATICS; // Reload main screen table @@ -1271,9 +1244,10 @@ void PlanningGroupsWidget::saveGroupScreenJoints() // Find the group we are editing based on the goup name string loadJointsScreen(config_data_->findGroupByName(current_edit_group_)); + return_screen_ = GROUP; // Switch to screen - changeScreen(1); // 1 is index of joints + changeScreen(JOINT); } // ****************************************************************************************** @@ -1287,9 +1261,10 @@ void PlanningGroupsWidget::saveGroupScreenLinks() // Find the group we are editing based on the goup name string loadLinksScreen(config_data_->findGroupByName(current_edit_group_)); + return_screen_ = GROUP; // Switch to screen - changeScreen(2); // 2 is index of links + changeScreen(LINK); } // ****************************************************************************************** @@ -1303,9 +1278,10 @@ void PlanningGroupsWidget::saveGroupScreenChain() // Find the group we are editing based on the goup name string loadChainScreen(config_data_->findGroupByName(current_edit_group_)); + return_screen_ = GROUP; // Switch to screen - changeScreen(3); + changeScreen(CHAIN); } // ****************************************************************************************** @@ -1319,9 +1295,10 @@ void PlanningGroupsWidget::saveGroupScreenSubgroups() // Find the group we are editing based on the goup name string loadSubgroupsScreen(config_data_->findGroupByName(current_edit_group_)); + return_screen_ = GROUP; // Switch to screen - changeScreen(4); + changeScreen(SUBGROUP); } // ****************************************************************************************** @@ -1329,6 +1306,12 @@ void PlanningGroupsWidget::saveGroupScreenSubgroups() // ****************************************************************************************** void PlanningGroupsWidget::cancelEditing() { + if (return_screen_) + { + changeScreen(return_screen_); + return_screen_ = 0; + return; + } if (!current_edit_group_.empty() && adding_new_group_) { srdf::Model::Group* editing = config_data_->findGroupByName(current_edit_group_); @@ -1381,9 +1364,9 @@ void PlanningGroupsWidget::alterTree(const QString& link) // ****************************************************************************************** void PlanningGroupsWidget::showMainScreen() { - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); - // Announce that this widget is not in modal mode + // Announce that this widget is not in modal mode anymore Q_EMIT isModal(false); } @@ -1392,7 +1375,7 @@ void PlanningGroupsWidget::showMainScreen() // ****************************************************************************************** void PlanningGroupsWidget::changeScreen(int index) { - stacked_layout_->setCurrentIndex(index); + stacked_widget_->setCurrentIndex(index); // Announce this widget's mode Q_EMIT isModal(index != 0); @@ -1401,12 +1384,12 @@ void PlanningGroupsWidget::changeScreen(int index) // ****************************************************************************************** // Called from Double List widget to highlight a link // ****************************************************************************************** -void PlanningGroupsWidget::previewSelectedLink(std::vector links) +void PlanningGroupsWidget::previewSelectedLink(const std::vector& links) { // Unhighlight all links Q_EMIT unhighlightAll(); - for (std::string& link : links) + for (const std::string& link : links) { if (link.empty()) { @@ -1421,14 +1404,14 @@ void PlanningGroupsWidget::previewSelectedLink(std::vector links) // ****************************************************************************************** // Called from Double List widget to highlight joints // ****************************************************************************************** -void PlanningGroupsWidget::previewSelectedJoints(std::vector joints) +void PlanningGroupsWidget::previewSelectedJoints(const std::vector& joints) { // Unhighlight all links Q_EMIT unhighlightAll(); for (const std::string& joint : joints) { - const robot_model::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); + const moveit::core::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); // Check that a joint model was found if (!joint_model) @@ -1452,7 +1435,7 @@ void PlanningGroupsWidget::previewSelectedJoints(std::vector joints // ****************************************************************************************** // Called from Double List widget to highlight a subgroup // ****************************************************************************************** -void PlanningGroupsWidget::previewSelectedSubgroup(std::vector groups) +void PlanningGroupsWidget::previewSelectedSubgroup(const std::vector& groups) { // Unhighlight all links Q_EMIT unhighlightAll(); diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.h b/moveit_setup_assistant/src/widgets/planning_groups_widget.h index 85bcf64259..ef860eddbd 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.h +++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.h @@ -34,12 +34,11 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PLANNING_GROUPS_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PLANNING_GROUPS_WIDGET_ +#pragma once // Qt class QPushButton; -class QStackedLayout; +class QStackedWidget; class QTreeWidget; class QTreeWidgetItem; @@ -62,11 +61,11 @@ class GroupEditWidget; // Custom Type enum GroupType { - JOINT, - LINK, - CHAIN, - SUBGROUP, - GROUP + JOINT = 1, + LINK = 2, + CHAIN = 3, + SUBGROUP = 4, + GROUP = 5 }; // ****************************************************************************************** @@ -129,14 +128,14 @@ private Q_SLOTS: void alterTree(const QString& link); /// Called from Double List widget to highlight a link - void previewSelectedLink(std::vector links); + void previewSelectedLink(const std::vector& links); /// Called from Double List widget to highlight a joint // void previewClickedJoint( std::string name ); - void previewSelectedJoints(std::vector joints); + void previewSelectedJoints(const std::vector& joints); /// Called from Double List widget to highlight a subgroup - void previewSelectedSubgroup(std::vector groups); + void previewSelectedSubgroup(const std::vector& groups); private: // ****************************************************************************************** @@ -147,7 +146,7 @@ private Q_SLOTS: QTreeWidget* groups_tree_; /// For changing between table and different add/edit views - QStackedLayout* stacked_layout_; + QStackedWidget* stacked_widget_; /// Show and hide edit button QPushButton* btn_edit_; @@ -173,8 +172,8 @@ private Q_SLOTS: /// Remember what group we are editing when an edit screen is being shown std::string current_edit_group_; - /// Remember what group element we are editing when an edit screen is being shown - GroupType current_edit_element_; + /// Remember to which editing screen we should return on Cancel + int return_screen_; /// Remember whethere we're editing a group or adding a new one bool adding_new_group_; @@ -205,7 +204,7 @@ private Q_SLOTS: /// Switch to current groups view void showMainScreen(); }; -} +} // namespace moveit_setup_assistant // ****************************************************************************************** // ****************************************************************************************** @@ -232,5 +231,3 @@ class PlanGroupType }; Q_DECLARE_METATYPE(PlanGroupType); - -#endif diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp index 501cb4a87e..61a59d1602 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp @@ -36,20 +36,30 @@ // SA #include "robot_poses_widget.h" +#include "header_widget.h" #include // Qt +#include +#include +#include #include +#include +#include #include -#include -#include +#include +#include +#include +#include +#include #include #include +#include namespace moveit_setup_assistant { // ****************************************************************************************** -// Outer User Interface for MoveIt! Configuration Assistant +// Outer User Interface for MoveIt Configuration Assistant // ****************************************************************************************** RobotPosesWidget::RobotPosesWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) : SetupScreenWidget(parent), config_data_(config_data) @@ -62,10 +72,12 @@ RobotPosesWidget::RobotPosesWidget(QWidget* parent, const MoveItConfigDataPtr& c // Top Header Area ------------------------------------------------ - HeaderWidget* header = new HeaderWidget( - "Define Robot Poses", "Create poses for the robot. Poses are defined as sets of joint values for " - "particular planning groups. This is useful for things like home position.", - this); + HeaderWidget* header = + new HeaderWidget("Define Robot Poses", + "Create poses for the robot. Poses are defined as sets of joint values for " + "particular planning groups. This is useful for things like home position." + "The first pose for each robot will be its initial pose in simulation.", + this); layout->addWidget(header); // Create contents screens --------------------------------------- @@ -73,15 +85,10 @@ RobotPosesWidget::RobotPosesWidget(QWidget* parent, const MoveItConfigDataPtr& c pose_edit_widget_ = createEditWidget(); // Create stacked layout ----------------------------------------- - stacked_layout_ = new QStackedLayout(this); - stacked_layout_->addWidget(pose_list_widget_); // screen index 0 - stacked_layout_->addWidget(pose_edit_widget_); // screen index 1 - - // Create Widget wrapper for layout - QWidget* stacked_layout_widget = new QWidget(this); - stacked_layout_widget->setLayout(stacked_layout_); - - layout->addWidget(stacked_layout_widget); + stacked_widget_ = new QStackedWidget(this); + stacked_widget_->addWidget(pose_list_widget_); // screen index 0 + stacked_widget_->addWidget(pose_edit_widget_); // screen index 1 + layout->addWidget(stacked_widget_); // Finish Layout -------------------------------------------------- this->setLayout(layout); @@ -93,7 +100,7 @@ RobotPosesWidget::RobotPosesWidget(QWidget* parent, const MoveItConfigDataPtr& c pub_robot_state_ = nh.advertise(MOVEIT_ROBOT_STATE, 1); // Set the planning scene - config_data_->getPlanningScene()->setName("MoveIt! Planning Scene"); + config_data_->getPlanningScene()->setName("MoveIt Planning Scene"); // Collision Detection initializtion ------------------------------- @@ -122,7 +129,7 @@ QWidget* RobotPosesWidget::createContentsWidget() data_table_->setSortingEnabled(true); data_table_->setSelectionBehavior(QAbstractItemView::SelectRows); connect(data_table_, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(editDoubleClicked(int, int))); - connect(data_table_, SIGNAL(cellClicked(int, int)), this, SLOT(previewClicked(int, int))); + connect(data_table_, SIGNAL(currentCellChanged(int, int, int, int)), this, SLOT(previewClicked(int, int, int, int))); layout->addWidget(data_table_); // Set header labels @@ -144,7 +151,7 @@ QWidget* RobotPosesWidget::createContentsWidget() controls_layout->setAlignment(btn_default, Qt::AlignLeft); // Set play button - QPushButton* btn_play = new QPushButton("&MoveIt!", this); + QPushButton* btn_play = new QPushButton("&MoveIt", this); btn_play->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); btn_play->setMaximumWidth(300); connect(btn_play, SIGNAL(clicked()), this, SLOT(playPoses())); @@ -152,9 +159,7 @@ QWidget* RobotPosesWidget::createContentsWidget() controls_layout->setAlignment(btn_play, Qt::AlignLeft); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Edit Selected Button btn_edit_ = new QPushButton("&Edit Selected", this); @@ -256,9 +261,7 @@ QWidget* RobotPosesWidget::createEditWidget() controls_layout->setContentsMargins(0, 25, 0, 15); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save btn_save_ = new QPushButton("&Save", this); @@ -289,7 +292,7 @@ QWidget* RobotPosesWidget::createEditWidget() void RobotPosesWidget::showNewScreen() { // Switch to screen - do this before clearEditText() - stacked_layout_->setCurrentIndex(1); + stacked_widget_->setCurrentIndex(1); // Remember that this is a new pose current_edit_pose_ = nullptr; @@ -308,7 +311,7 @@ void RobotPosesWidget::showNewScreen() // ****************************************************************************************** // Edit whatever element is selected // ****************************************************************************************** -void RobotPosesWidget::editDoubleClicked(int row, int column) +void RobotPosesWidget::editDoubleClicked(int /*row*/, int /*column*/) { // We'll just base the edit on the selection highlight editSelected(); @@ -317,15 +320,19 @@ void RobotPosesWidget::editDoubleClicked(int row, int column) // ****************************************************************************************** // Preview whatever element is selected // ****************************************************************************************** -void RobotPosesWidget::previewClicked(int row, int column) +void RobotPosesWidget::previewClicked(int row, int /*column*/, int /*previous_row*/, int /*previous_column*/) { - const std::string& name = data_table_->item(row, 0)->text().toStdString(); - const std::string& group = data_table_->item(row, 1)->text().toStdString(); + QTableWidgetItem* name = data_table_->item(row, 0); + QTableWidgetItem* group = data_table_->item(row, 1); - // Find the selected in datastructure - srdf::Model::GroupState* pose = findPoseByName(name, group); + // nullptr check before dereferencing + if (name && group) + { + // Find the selected in datastructure + srdf::Model::GroupState* pose = findPoseByName(name->text().toStdString(), group->text().toStdString()); - showPose(pose); + showPose(pose); + } } // ****************************************************************************************** @@ -333,12 +340,12 @@ void RobotPosesWidget::previewClicked(int row, int column) // ****************************************************************************************** void RobotPosesWidget::showPose(srdf::Model::GroupState* pose) { - // Set pose joint values by adding them to the local joint state map + // Set the joints based on the SRDF pose + moveit::core::RobotState& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); for (std::map >::const_iterator value_it = pose->joint_values_.begin(); value_it != pose->joint_values_.end(); ++value_it) { - // Only copy the first joint value // TODO: add capability for multi-DOF joints? - joint_state_map_[value_it->first] = value_it->second[0]; + robot_state.setJointPositions(value_it->first, value_it->second); } // Update the joints @@ -356,25 +363,8 @@ void RobotPosesWidget::showPose(srdf::Model::GroupState* pose) // ****************************************************************************************** void RobotPosesWidget::showDefaultPose() { - // Get list of all joints for the robot - std::vector joint_models = config_data_->getRobotModel()->getJointModels(); - - // Iterate through the joints - for (std::vector::const_iterator joint_it = joint_models.begin(); - joint_it < joint_models.end(); ++joint_it) - { - // Check that this joint only represents 1 variable. - if ((*joint_it)->getVariableCount() == 1) - { - double init_value; - - // get the first joint value in its vector - (*joint_it)->getVariableDefaultPositions(&init_value); - - // Change joint's value in joint_state_map to the default - joint_state_map_[(*joint_it)->getName()] = init_value; - } - } + moveit::core::RobotState& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); + robot_state.setToDefaultValues(); // Update the joints publishJoints(); @@ -435,19 +425,10 @@ void RobotPosesWidget::edit(int row) } group_name_field_->setCurrentIndex(index); - // Set pose joint values by adding them to the local joint state map - for (std::map >::const_iterator value_it = pose->joint_values_.begin(); - value_it != pose->joint_values_.end(); ++value_it) - { - // Only copy the first joint value // TODO: add capability for multi-DOF joints? - joint_state_map_[value_it->first] = value_it->second[0]; - } - - // Update robot model in rviz - publishJoints(); + showPose(pose); // Switch to screen - do this before setCurrentIndex - stacked_layout_->setCurrentIndex(1); + stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode Q_EMIT isModal(true); @@ -478,7 +459,7 @@ void RobotPosesWidget::loadJointSliders(const QString& selected) { // Ignore this event if the combo box is empty. This occurs when clearing the combo box and reloading with the // newest groups. Also ignore if we are not on the edit screen - if (!group_name_field_->count() || selected.isEmpty() || stacked_layout_->currentIndex() == 0) + if (!group_name_field_->count() || selected.isEmpty() || stacked_widget_->currentIndex() == 0) return; // Get group name from input @@ -487,9 +468,10 @@ void RobotPosesWidget::loadJointSliders(const QString& selected) // Check that joint model exist if (!config_data_->getRobotModel()->hasJointModelGroup(group_name)) { - QMessageBox::critical(this, "Error Loading", QString("Unable to find joint model group for group: ") - .append(group_name.c_str()) - .append(" Are you sure this group has associated joints/links?")); + QMessageBox::critical(this, "Error Loading", + QString("Unable to find joint model group for group: ") + .append(group_name.c_str()) + .append(" Are you sure this group has associated joints/links?")); return; } @@ -508,31 +490,20 @@ void RobotPosesWidget::loadJointSliders(const QString& selected) joint_list_widget_->setMinimumSize(50, 50); // w, h // Get list of associated joints - const robot_model::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup(group_name); - joint_models_ = joint_model_group->getJointModels(); + const moveit::core::JointModelGroup* joint_model_group = + config_data_->getRobotModel()->getJointModelGroup(group_name); + const auto& robot_state = config_data_->getPlanningScene()->getCurrentState(); // Iterate through the joints int num_joints = 0; - for (const robot_model::JointModel* joint_model : joint_models_) + for (const moveit::core::JointModel* joint_model : joint_model_group->getJointModels()) { if (joint_model->getVariableCount() != 1 || // only consider 1-variable joints joint_model->isPassive() || // ignore passive joint_model->getMimic()) // and mimic joints continue; - double init_value; - - // Decide what this joint's initial value is - if (joint_state_map_.find(joint_model->getName()) == joint_state_map_.end()) - { - // The joint state map does not yet have an entry for this joint - // Get the first joint value in its vector - joint_model->getVariableDefaultPositions(&init_value); - } - else // There is already a value in the map - { - init_value = joint_state_map_[joint_model->getName()]; - } + double init_value = robot_state.getVariablePosition(joint_model->getVariableNames()[0]); // For each joint in group add slider SliderWidget* sw = new SliderWidget(this, joint_model, init_value); @@ -675,19 +646,22 @@ void RobotPosesWidget::doneEditing() // Clear the old values searched_data->joint_values_.clear(); - // Iterate through the current group's joints and add to SRDF - for (std::vector::const_iterator joint_it = joint_models_.begin(); - joint_it < joint_models_.end(); ++joint_it) + const moveit::core::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup(group); + const auto& robot_state = config_data_->getPlanningScene()->getCurrentState(); + + // Iterate through the current group's joints and add them to SRDF + for (const moveit::core::JointModel* jm : joint_model_group->getJointModels()) { // Check that this joint only represents 1 variable. - if ((*joint_it)->getVariableCount() == 1) + if (jm->getVariableCount() == 1 && !jm->isPassive() && !jm->getMimic()) { // Create vector for new joint values - std::vector joint_value; - joint_value.push_back(joint_state_map_[(*joint_it)->getName()]); + std::vector joint_values(jm->getVariableCount()); + const double* const first_variable = robot_state.getVariablePositions() + jm->getFirstVariableIndex(); + std::copy(first_variable, first_variable + joint_values.size(), joint_values.begin()); // Add joint vector to SRDF - searched_data->joint_values_[(*joint_it)->getName()] = joint_value; + searched_data->joint_values_[jm->getName()] = std::move(joint_values); } } @@ -704,7 +678,7 @@ void RobotPosesWidget::doneEditing() loadDataTable(); // Switch to screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is done with modal mode Q_EMIT isModal(false); @@ -716,7 +690,7 @@ void RobotPosesWidget::doneEditing() void RobotPosesWidget::cancelEditing() { // Switch to screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is done with modal mode Q_EMIT isModal(false); @@ -737,13 +711,12 @@ void RobotPosesWidget::loadDataTable() // Loop through every pose int row = 0; - for (std::vector::const_iterator data_it = config_data_->srdf_->group_states_.begin(); - data_it != config_data_->srdf_->group_states_.end(); ++data_it) + for (const auto& group_state : config_data_->srdf_->group_states_) { // Create row elements - QTableWidgetItem* data_name = new QTableWidgetItem(data_it->name_.c_str()); + QTableWidgetItem* data_name = new QTableWidgetItem(group_state.name_.c_str()); data_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* group_name = new QTableWidgetItem(data_it->group_.c_str()); + QTableWidgetItem* group_name = new QTableWidgetItem(group_state.group_.c_str()); group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); // Add to table @@ -773,7 +746,7 @@ void RobotPosesWidget::loadDataTable() void RobotPosesWidget::focusGiven() { // Show the current poses screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Load the data to the tree loadDataTable(); @@ -787,48 +760,33 @@ void RobotPosesWidget::focusGiven() // ****************************************************************************************** void RobotPosesWidget::updateRobotModel(const std::string& name, double value) { - // Save the new value - joint_state_map_[name] = value; + moveit::core::RobotState& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); + robot_state.setVariablePosition(name, value); // Update the robot model/rviz publishJoints(); } // ****************************************************************************************** -// Publish the joint values in the joint_state_map_ to Rviz +// Publish the current RobotState to Rviz // ****************************************************************************************** void RobotPosesWidget::publishJoints() { - // Change the scene - // scene.getCurrentState().setToDefaultValues();//set to default values of 0 OR half between low and high joint values - // config_data_->getPlanningScene()->getCurrentState().setToRandomValues(); - - // Set the joints based on the map - config_data_->getPlanningScene()->getCurrentStateNonConst().setVariablePositions(joint_state_map_); - + // Update link + collision transforms + auto& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); + robot_state.update(); // Create a planning scene message moveit_msgs::DisplayRobotState msg; - robot_state::robotStateToRobotStateMsg(config_data_->getPlanningScene()->getCurrentState(), msg.state); + moveit::core::robotStateToRobotStateMsg(robot_state, msg.state); // Publish! pub_robot_state_.publish(msg); - // Prevent dirty collision body transforms - config_data_->getPlanningScene()->getCurrentStateNonConst().update(); - // Decide if current state is in collision collision_detection::CollisionResult result; - config_data_->getPlanningScene()->checkSelfCollision( - request, result, config_data_->getPlanningScene()->getCurrentState(), config_data_->allowed_collision_matrix_); - // Show result notification - if (!result.contacts.empty()) - { - collision_warning_->show(); - } - else - { - collision_warning_->hide(); - } + config_data_->getPlanningScene()->checkSelfCollision(request, result, robot_state, + config_data_->allowed_collision_matrix_); + collision_warning_->setHidden(result.contacts.empty()); } // ****************************************************************************************** @@ -840,7 +798,7 @@ void RobotPosesWidget::publishJoints() // ****************************************************************************************** // Simple widget for adjusting joints of a robot // ****************************************************************************************** -SliderWidget::SliderWidget(QWidget* parent, const robot_model::JointModel* joint_model, double init_value) +SliderWidget::SliderWidget(QWidget* parent, const moveit::core::JointModel* joint_model, double init_value) : QWidget(parent), joint_model_(joint_model) { // Create layouts diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.h b/moveit_setup_assistant/src/widgets/robot_poses_widget.h index 6614273cca..dcc8377be4 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.h +++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.h @@ -34,30 +34,24 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROBOT_POSES_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROBOT_POSES_WIDGET_ +#pragma once // Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +class QComboBox; +class QLabel; +class QLineEdit; +class QPushButton; +class QScrollArea; +class QSlider; +class QStackedWidget; +class QTableWidget; +class QVBoxLayout; // SA #ifndef Q_MOC_RUN #include -#include // for collision stuff -#include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant @@ -84,7 +78,7 @@ class RobotPosesWidget : public SetupScreenWidget QPushButton* btn_delete_; QPushButton* btn_save_; QPushButton* btn_cancel_; - QStackedLayout* stacked_layout_; + QStackedWidget* stacked_widget_; QScrollArea* scroll_area_; QVBoxLayout* column2_; QLineEdit* pose_name_field_; @@ -111,7 +105,7 @@ private Q_SLOTS: void editDoubleClicked(int row, int column); /// Preview whatever element is selected - void previewClicked(int row, int column); + void previewClicked(int row, int column, int previous_row, int previous_column); /// Delete currently editing ite void deleteSelected(); @@ -153,12 +147,6 @@ private Q_SLOTS: /// Pointer to currently edited group state srdf::Model::GroupState* current_edit_pose_; - /// All the joint slider values that have thus far been seen. May contain more than just the current joints' values - std::map joint_state_map_; - - /// The joints currently in the selected planning group - std::vector joint_models_; - /// Remember the publisher for quick publishing later ros::Publisher pub_robot_state_; @@ -238,7 +226,7 @@ class SliderWidget : public QWidget * @param parent - parent QWidget * @param joint_model_ - a ptr reference to the joint this widget represents */ - SliderWidget(QWidget* parent, const robot_model::JointModel* joint_model, double init_value); + SliderWidget(QWidget* parent, const moveit::core::JointModel* joint_model, double init_value); /** * Deconstructor @@ -280,7 +268,7 @@ private Q_SLOTS: // ****************************************************************************************** // Ptr to the joint's data - const robot_model::JointModel* joint_model_; + const moveit::core::JointModel* joint_model_; // Max & min position double max_position_; @@ -291,9 +279,7 @@ private Q_SLOTS: // ****************************************************************************************** }; -} // namespace +} // namespace moveit_setup_assistant // Declare std::string as metatype so we can use it in a signal Q_DECLARE_METATYPE(std::string) - -#endif diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp index 05f27403bf..f29e71817a 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp @@ -35,13 +35,27 @@ // SA #include "ros_controllers_widget.h" +#include "double_list_widget.h" +#include "controller_edit_widget.h" +#include "header_widget.h" #include // Qt +#include +#include #include +#include +#include +#include +#include #include -#include +#include +#include +#include +#include +#include +#include #include -#include +#include #include #include @@ -49,7 +63,7 @@ namespace moveit_setup_assistant { // ****************************************************************************************** -// Outer User Interface for MoveIt! Configuration Assistant +// Outer User Interface for MoveIt Configuration Assistant // ****************************************************************************************** ROSControllersWidget::ROSControllersWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) : SetupScreenWidget(parent), config_data_(config_data) @@ -64,7 +78,7 @@ ROSControllersWidget::ROSControllersWidget(QWidget* parent, const MoveItConfigDa // Top Header Area ------------------------------------------------ moveit_setup_assistant::HeaderWidget* header = new moveit_setup_assistant::HeaderWidget( - "Setup ROS Controllers", "Configure MoveIt! to work with ROS Control to control the robot's physical hardware", + "Setup ROS Controllers", "Configure MoveIt to work with ROS Control to control the robot's physical hardware", this); layout->addWidget(header); @@ -95,19 +109,12 @@ ROSControllersWidget::ROSControllersWidget(QWidget* parent, const MoveItConfigDa connect(controller_edit_widget_, SIGNAL(saveJointsGroups()), this, SLOT(saveControllerScreenGroups())); // Combine into stack - stacked_layout_ = new QStackedLayout(this); - stacked_layout_->addWidget(controllers_tree_widget_); // screen index 0 - stacked_layout_->addWidget(joints_widget_); // screen index 1 - stacked_layout_->addWidget(controller_edit_widget_); // screen index 2 - stacked_layout_->addWidget(joint_groups_widget_); // screen index 3 - - // Finish GUI ----------------------------------------------------------- - - // Create Widget wrapper for layout - QWidget* stacked_layout_widget = new QWidget(this); - stacked_layout_widget->setLayout(stacked_layout_); - - layout->addWidget(stacked_layout_widget); + stacked_widget_ = new QStackedWidget(this); + stacked_widget_->addWidget(controllers_tree_widget_); // screen index 0 + stacked_widget_->addWidget(joints_widget_); // screen index 1 + stacked_widget_->addWidget(controller_edit_widget_); // screen index 2 + stacked_widget_->addWidget(joint_groups_widget_); // screen index 3 + layout->addWidget(stacked_widget_); this->setLayout(layout); } @@ -162,9 +169,7 @@ QWidget* ROSControllersWidget::createContentsWidget() controls_layout_->addWidget(expand_controls); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout_->addWidget(spacer); + controls_layout_->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Delete btn_delete_ = new QPushButton("&Delete Controller", this); @@ -283,7 +288,7 @@ void ROSControllersWidget::focusGiven() void ROSControllersWidget::loadJointsScreen(moveit_setup_assistant::ROSControlConfig* this_controller) { // Retrieve pointer to the shared kinematic model - const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel(); + const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); // Get the names of the all joints const std::vector& joints = model->getJointModelNames(); @@ -426,8 +431,7 @@ void ROSControllersWidget::loadControllerScreen(moveit_setup_assistant::ROSContr else // load the controller name into the widget { current_edit_controller_ = this_controller->name_; - controller_edit_widget_->setTitle( - QString("Edit Controller '").append(current_edit_controller_.c_str()).append("'")); + controller_edit_widget_->setTitle(QString("Edit Controller '").append(current_edit_controller_.c_str()).append("'")); controller_edit_widget_->showDelete(); controller_edit_widget_->hideNewButtonsWidget(); // not necessary for existing controllers controller_edit_widget_->showSave(); // this is only for edit mode @@ -466,14 +470,14 @@ void ROSControllersWidget::cancelEditing() // ****************************************************************************************** // Called from Double List widget to highlight joints // ****************************************************************************************** -void ROSControllersWidget::previewSelectedJoints(std::vector joints) +void ROSControllersWidget::previewSelectedJoints(const std::vector& joints) { // Unhighlight all links Q_EMIT unhighlightAll(); for (const std::string& joint : joints) { - const robot_model::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); + const moveit::core::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); // Check that a joint model was found if (!joint_model) @@ -497,7 +501,7 @@ void ROSControllersWidget::previewSelectedJoints(std::vector joints // ****************************************************************************************** // Called from Double List widget to highlight a group // ****************************************************************************************** -void ROSControllersWidget::previewSelectedGroup(std::vector groups) +void ROSControllersWidget::previewSelectedGroup(const std::vector& groups) { // Unhighlight all links Q_EMIT unhighlightAll(); @@ -512,7 +516,7 @@ void ROSControllersWidget::previewSelectedGroup(std::vector groups) // ****************************************************************************************** // Called when an item is seleceted from the controllers tree // ****************************************************************************************** -void ROSControllersWidget::previewSelected(QTreeWidgetItem* selected_item, int column) +void ROSControllersWidget::previewSelected(QTreeWidgetItem* selected_item, int /*column*/) { // Get the user custom properties of the currently selected row int type = selected_item->data(0, Qt::UserRole).value(); @@ -608,14 +612,14 @@ void ROSControllersWidget::saveJointsGroupsScreen() for (int i = 0; i < joint_groups_widget_->selected_data_table_->rowCount(); ++i) { // Get list of associated joints - const robot_model::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup( + const moveit::core::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup( joint_groups_widget_->selected_data_table_->item(i, 0)->text().toStdString()); - const std::vector& joint_models = joint_model_group->getActiveJointModels(); + const std::vector& joint_models = joint_model_group->getActiveJointModels(); // Iterate through the joints - for (const robot_model::JointModel* joint : joint_models) + for (const moveit::core::JointModel* joint : joint_models) { - if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == robot_model::JointModel::FIXED) + if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED) continue; searched_controller->joints_.push_back(joint->getName()); } @@ -671,14 +675,12 @@ bool ROSControllersWidget::saveControllerScreen() } // Check that the controller name is unique - for (std::vector::const_iterator controller_it = - config_data_->getROSControllers().begin(); - controller_it != config_data_->getROSControllers().end(); ++controller_it) + for (const auto& controller : config_data_->getROSControllers()) { - if (controller_it->name_.compare(controller_name) == 0) // the names are the same + if (controller.name_.compare(controller_name) == 0) // the names are the same { // is this our existing controller? check if controller pointers are same - if (&(*controller_it) != searched_controller) + if (&controller != searched_controller) { QMessageBox::warning(this, "Error Saving", "A controller already exists with that name!"); return false; @@ -802,7 +804,7 @@ void ROSControllersWidget::editController() // ****************************************************************************************** void ROSControllersWidget::showMainScreen() { - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode Q_EMIT isModal(false); @@ -813,7 +815,7 @@ void ROSControllersWidget::showMainScreen() // ****************************************************************************************** void ROSControllersWidget::changeScreen(int index) { - stacked_layout_->setCurrentIndex(index); + stacked_widget_->setCurrentIndex(index); // Announce this widget's mode Q_EMIT isModal(index != 0); diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h index 57fc7d3c8b..f38408d1c7 100644 --- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h +++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h @@ -33,31 +33,27 @@ /* Author: Mohamad Ayman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROS_CONTROLLERS_WIDGET_H -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROS_CONTROLLERS_WIDGET_H +#pragma once // Qt -#include -#include -#include -#include -#include -#include -#include -#include +class QHBoxLayout; +class QPushButton; +class QStackedWidget; +class QTreeWidget; +class QTreeWidgetItem; // SA #ifndef Q_MOC_RUN #include #endif -#include "double_list_widget.h" // for joints, links and subgroups pages -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant -#include "controller_edit_widget.h" namespace moveit_setup_assistant { +class DoubleListWidget; +class ControllerEditWidget; + class ROSControllersWidget : public SetupScreenWidget { Q_OBJECT @@ -108,10 +104,10 @@ private Q_SLOTS: void editSelected(); /// Called from Double List widget to highlight a joint - void previewSelectedJoints(std::vector joints); + void previewSelectedJoints(const std::vector& joints); /// Called from Double List widget to highlight a group - void previewSelectedGroup(std::vector groups); + void previewSelectedGroup(const std::vector& groups); /// Called when an item is seleceted from the controllers tree void previewSelected(QTreeWidgetItem* selected_item, int column); @@ -129,7 +125,7 @@ private Q_SLOTS: QWidget* controllers_tree_widget_; /// For changing between table and different add/edit views - QStackedLayout* stacked_layout_; + QStackedWidget* stacked_widget_; ControllerEditWidget* controller_edit_widget_; QPushButton* btn_delete_; @@ -160,5 +156,3 @@ private Q_SLOTS: }; } // namespace moveit_setup_assistant - -#endif diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp index 1f7f0595b4..40c880747e 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp @@ -37,15 +37,19 @@ // SA #include "setup_screen_widget.h" // a base class for screens in the setup assistant #include "setup_assistant_widget.h" +#include "header_widget.h" + // Qt -#include -#include -#include +#include +#include +#include #include +#include #include -#include -#include #include +#include +#include +#include #include #include // for loading all avail kinematic planners // Rviz @@ -58,7 +62,7 @@ namespace moveit_setup_assistant { // ****************************************************************************************** -// Outer User Interface for MoveIt! Configuration Assistant +// Outer User Interface for MoveIt Configuration Assistant // ****************************************************************************************** SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, const boost::program_options::variables_map& args) : QWidget(parent) @@ -66,7 +70,7 @@ SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, const boost::program rviz_manager_ = nullptr; rviz_render_panel_ = nullptr; - // Create object to hold all MoveIt! configuration data + // Create object to hold all MoveIt configuration data config_data_.reset(new MoveItConfigData()); // Set debug mode flag if necessary @@ -83,14 +87,9 @@ SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, const boost::program layout->setAlignment(Qt::AlignTop); // Create main content stack for various screens - main_content_ = new QStackedLayout(); + main_content_ = new QStackedWidget(); current_index_ = 0; - // Wrap main_content_ with a widget - middle_frame_ = new QWidget(this); - middle_frame_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); - middle_frame_->setLayout(main_content_); - // Screens -------------------------------------------------------- // Start Screen @@ -113,13 +112,7 @@ SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, const boost::program } else { - // Open the directory where the MSA was started from. - // cf. http://stackoverflow.com/a/7413516/577001 - QString pwdir(""); - char* pwd; - pwd = getenv("PWD"); - pwdir.append(pwd); - start_screen_widget_->stack_path_->setPath(pwdir); + start_screen_widget_->stack_path_->setPath(QString(getenv("PWD"))); } // Add Navigation Buttons (but do not load widgets yet except start screen) @@ -151,7 +144,7 @@ SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, const boost::program splitter_ = new QSplitter(Qt::Horizontal, this); splitter_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); splitter_->addWidget(navs_view_); - splitter_->addWidget(middle_frame_); + splitter_->addWidget(main_content_); splitter_->addWidget(rviz_container_); splitter_->setHandleWidth(6); // splitter_->setCollapsible( 0, false ); // don't let navigation collapse @@ -164,7 +157,7 @@ SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, const boost::program this->setLayout(layout); // Title - this->setWindowTitle("MoveIt! Setup Assistant"); // title of window + this->setWindowTitle("MoveIt Setup Assistant"); // title of window // Show screen before message QApplication::processEvents(); @@ -189,6 +182,7 @@ void SetupAssistantWidget::virtualJointReferenceFrameChanged() { rviz_manager_->setFixedFrame(QString::fromStdString(config_data_->getRobotModel()->getModelFrame())); robot_state_display_->reset(); + robot_state_display_->setVisible(true); } } @@ -391,7 +385,7 @@ void SetupAssistantWidget::loadRviz() // Set the fixed and target frame rviz_manager_->setFixedFrame(QString::fromStdString(config_data_->getRobotModel()->getModelFrame())); - // Create the MoveIt! Rviz Plugin and attach to display + // Create the MoveIt Rviz Plugin and attach to display robot_state_display_ = new moveit_rviz_plugin::RobotStateDisplay(); robot_state_display_->setName("Robot State"); @@ -402,16 +396,32 @@ void SetupAssistantWidget::loadRviz() // Set robot description robot_state_display_->subProp("Robot Description")->setValue(QString::fromStdString(ROBOT_DESCRIPTION)); + robot_state_display_->setVisible(true); // Zoom into robot rviz::ViewController* view = rviz_manager_->getViewManager()->getCurrent(); view->subProp("Distance")->setValue(4.0f); // Add Rviz to Planning Groups Widget - QHBoxLayout* rviz_layout = new QHBoxLayout(); + QVBoxLayout* rviz_layout = new QVBoxLayout(); rviz_layout->addWidget(rviz_render_panel_); rviz_container_->setLayout(rviz_layout); + // visual / collision buttons + auto btn_layout = new QHBoxLayout(); + rviz_layout->addLayout(btn_layout); + + QCheckBox* btn; + btn_layout->addWidget(btn = new QCheckBox("visual"), 0); + btn->setChecked(true); + connect(btn, &QCheckBox::toggled, + [this](bool checked) { robot_state_display_->subProp("Visual Enabled")->setValue(checked); }); + + btn_layout->addWidget(btn = new QCheckBox("collision"), 1); + btn->setChecked(false); + connect(btn, &QCheckBox::toggled, + [this](bool checked) { robot_state_display_->subProp("Collision Enabled")->setValue(checked); }); + rviz_container_->show(); } @@ -420,7 +430,7 @@ void SetupAssistantWidget::loadRviz() // ****************************************************************************************** void SetupAssistantWidget::highlightLink(const std::string& link_name, const QColor& color) { - const robot_model::LinkModel* lm = config_data_->getRobotModel()->getLinkModel(link_name); + const moveit::core::LinkModel* lm = config_data_->getRobotModel()->getLinkModel(link_name); if (!lm->getShapes().empty()) // skip links with no geometry robot_state_display_->setLinkColor(link_name, color); } @@ -434,12 +444,13 @@ void SetupAssistantWidget::highlightGroup(const std::string& group_name) if (!config_data_->getRobotModel()->hasJointModelGroup(group_name)) return; - const robot_model::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup(group_name); + const moveit::core::JointModelGroup* joint_model_group = + config_data_->getRobotModel()->getJointModelGroup(group_name); if (joint_model_group) { - const std::vector& link_models = joint_model_group->getLinkModels(); + const std::vector& link_models = joint_model_group->getLinkModels(); // Iterate through the links - for (std::vector::const_iterator link_it = link_models.begin(); + for (std::vector::const_iterator link_it = link_models.begin(); link_it < link_models.end(); ++link_it) highlightLink((*link_it)->getName(), QColor(255, 0, 0)); } @@ -484,7 +495,7 @@ void SetupAssistantWidget::closeEvent(QCloseEvent* event) if (!config_data_->debug_) { if (QMessageBox::question(this, "Exit Setup Assistant", - QString("Are you sure you want to exit the MoveIt! Setup Assistant?"), + QString("Are you sure you want to exit the MoveIt Setup Assistant?"), QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) { event->ignore(); @@ -499,7 +510,7 @@ void SetupAssistantWidget::closeEvent(QCloseEvent* event) // ****************************************************************************************** // Qt Error Handling - TODO // ****************************************************************************************** -bool SetupAssistantWidget::notify(QObject* reciever, QEvent* event) +bool SetupAssistantWidget::notify(QObject* /*receiver*/, QEvent* /*event*/) { QMessageBox::critical(this, "Error", "An error occurred and was caught by Qt notify event handler.", QMessageBox::Ok); diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h index 48ccf7d26a..154f2dc0b4 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h +++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h @@ -34,22 +34,12 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_SETUP_ASSISTANT_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_SETUP_ASSISTANT_WIDGET_ +#pragma once // Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -// Setup Asst +class QSplitter; + +// Setup Assistant #include "navigation_widget.h" #include "start_screen_widget.h" #include "default_collisions_widget.h" @@ -68,8 +58,7 @@ #include // Other -#include -#include // for parsing input arguments +#include // for parsing input arguments #include #endif @@ -79,7 +68,7 @@ namespace rviz class GridDisplay; class RenderPanel; class VisualizationManager; -} +} // namespace rviz namespace moveit_rviz_plugin { @@ -203,10 +192,9 @@ private Q_SLOTS: QList nav_name_list_; NavigationWidget* navs_view_; - QWidget* middle_frame_; QWidget* rviz_container_; QSplitter* splitter_; - QStackedLayout* main_content_; + QStackedWidget* main_content_; int current_index_; boost::mutex change_screen_lock_; @@ -236,6 +224,4 @@ private Q_SLOTS: // Private Functions // ****************************************************************************************** }; -} - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/setup_screen_widget.h b/moveit_setup_assistant/src/widgets/setup_screen_widget.h index 805350e9d5..7898b4f25e 100644 --- a/moveit_setup_assistant/src/widgets/setup_screen_widget.h +++ b/moveit_setup_assistant/src/widgets/setup_screen_widget.h @@ -34,8 +34,7 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_SETUP_SCREEN_WIDGET_ -#define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_SETUP_SCREEN_WIDGET_ +#pragma once #include @@ -75,5 +74,3 @@ class SetupScreenWidget : public QWidget /// Event for telling rviz to unhighlight all links of the robot void unhighlightAll(); }; - -#endif diff --git a/moveit_setup_assistant/src/widgets/simulation_widget.cpp b/moveit_setup_assistant/src/widgets/simulation_widget.cpp index abac044fb7..c86395a3c3 100644 --- a/moveit_setup_assistant/src/widgets/simulation_widget.cpp +++ b/moveit_setup_assistant/src/widgets/simulation_widget.cpp @@ -35,12 +35,15 @@ // SA #include "simulation_widget.h" +#include "header_widget.h" // Qt -#include +#include +#include #include #include -#include +#include +#include #include #include @@ -61,11 +64,11 @@ SimulationWidget::SimulationWidget(QWidget* parent, const MoveItConfigDataPtr& c // Top Header Area ------------------------------------------------ - HeaderWidget* header = - new HeaderWidget("Simulate With Gazebo", "The following tool will auto-generate the URDF changes needed " - "for Gazebo compatibility with ROSControl and MoveIt!. The " - "needed changes are shown in green.", - this); + HeaderWidget* header = new HeaderWidget("Simulate With Gazebo", + "The following tool will auto-generate the URDF changes needed " + "for Gazebo compatibility with ROSControl and MoveIt. The " + "needed changes are shown in green.", + this); layout->addWidget(header); // Spacing @@ -218,7 +221,7 @@ void SimulationWidget::generateURDFClick() // ****************************************************************************************** // Called the copy to clipboard button is clicked // ****************************************************************************************** -void SimulationWidget::copyURDF(const QString& link) +void SimulationWidget::copyURDF(const QString& /*link*/) { simulation_text_->selectAll(); simulation_text_->copy(); diff --git a/moveit_setup_assistant/src/widgets/simulation_widget.h b/moveit_setup_assistant/src/widgets/simulation_widget.h index 2b3ac58331..8fb5f79206 100644 --- a/moveit_setup_assistant/src/widgets/simulation_widget.h +++ b/moveit_setup_assistant/src/widgets/simulation_widget.h @@ -33,20 +33,17 @@ /* Author: Mohamad Ayman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_SIMULATION_WIDGET_H -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_SIMULATION_WIDGET_H +#pragma once // Qt -#include -#include -#include +class QLabel; +class QTextEdit; // SA #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant @@ -93,5 +90,3 @@ private Q_SLOTS: }; } // namespace moveit_setup_assistant - -#endif diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp index 8a8fadbd27..757db3252d 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp @@ -35,17 +35,21 @@ /* Author: Dave Coleman */ // Qt -#include -#include -#include +#include +#include +#include #include +#include +#include #include +#include +#include #include -#include -#include -#include +#include +#include +#include + // ROS -#include #include // for getting file path for loadng images // SA #include "header_widget.h" // title and instructions @@ -59,13 +63,20 @@ // MoveIt #include +// chdir +#ifdef _WIN32 +#include +#else +#include +#endif + namespace moveit_setup_assistant { // Boost file system namespace fs = boost::filesystem; // ****************************************************************************************** -// Start screen user interface for MoveIt! Configuration Assistant +// Start screen user interface for MoveIt Configuration Assistant // ****************************************************************************************** StartScreenWidget::StartScreenWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) : SetupScreenWidget(parent), config_data_(config_data) @@ -103,11 +114,11 @@ StartScreenWidget::StartScreenWidget(QWidget* parent, const MoveItConfigDataPtr& right_layout->setAlignment(right_image_label_, Qt::AlignRight | Qt::AlignTop); // Top Label Area --------------------------------------------------- - HeaderWidget* header = - new HeaderWidget("MoveIt! Setup Assistant", "These tools will assist you in creating a Semantic Robot " - "Description Format (SRDF) file, various yaml configuration and many " - "roslaunch files for utilizing all aspects of MoveIt! functionality.", - this); + HeaderWidget* header = new HeaderWidget("MoveIt Setup Assistant", + "These tools will assist you in creating a Semantic Robot " + "Description Format (SRDF) file, various yaml configuration and many " + "roslaunch files for utilizing all aspects of MoveIt functionality.", + this); layout->addWidget(header); // Select Mode Area ------------------------------------------------- @@ -120,8 +131,8 @@ StartScreenWidget::StartScreenWidget(QWidget* parent, const MoveItConfigDataPtr& // Stack Path Dialog stack_path_ = - new LoadPathArgsWidget("Load MoveIt! Configuration Package", - "Specify the package name or path of an existing MoveIt! configuration package to be " + new LoadPathArgsWidget("Load MoveIt Configuration Package", + "Specify the package name or path of an existing MoveIt configuration package to be " "edited for your robot. Example package name: panda_moveit_config", "optional xacro arguments:", this, true); // directory // user needs to select option before this is shown @@ -181,9 +192,7 @@ StartScreenWidget::StartScreenWidget(QWidget* parent, const MoveItConfigDataPtr& layout->addLayout(hlayout); // Verticle Spacer - QWidget* vspacer = new QWidget(this); - vspacer->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); - layout->addWidget(vspacer); + layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Minimum, QSizePolicy::Expanding)); // Attach bottom layout layout->addWidget(next_label_); @@ -291,7 +300,7 @@ void StartScreenWidget::loadFilesClick() } } -void StartScreenWidget::onPackagePathChanged(const QString& path) +void StartScreenWidget::onPackagePathChanged(const QString& /*path*/) { if (!loadPackageSettings(false)) return; @@ -330,11 +339,11 @@ bool StartScreenWidget::loadPackageSettings(bool show_warnings) if (!config_data_->getSetupAssistantYAMLPath(setup_assistant_path)) { if (show_warnings) - QMessageBox::warning( - this, "Incorrect Directory/Package", - QString("The chosen package location exists but was not created using MoveIt! Setup Assistant. " - "If this is a mistake, provide the missing file: ") - .append(setup_assistant_path.c_str())); + QMessageBox::warning(this, "Incorrect Directory/Package", + QString( + "The chosen package location exists but was not created using MoveIt Setup Assistant. " + "If this is a mistake, provide the missing file: ") + .append(setup_assistant_path.c_str())); return false; } @@ -400,14 +409,20 @@ bool StartScreenWidget::loadExistingFiles() fs::path kinematics_yaml_path = config_data_->config_pkg_path_; kinematics_yaml_path /= "config/kinematics.yaml"; - if (!config_data_->inputKinematicsYAML(kinematics_yaml_path.make_preferred().native())) + if (!config_data_->inputKinematicsYAML(kinematics_yaml_path.make_preferred().string())) { QMessageBox::warning(this, "No Kinematic YAML File", QString("Failed to parse kinematics yaml file. This file is not critical but any previous " "kinematic solver settings have been lost. To re-populate this file edit each " "existing planning group and choose a solver, then save each change. \n\nFile error " "at location ") - .append(kinematics_yaml_path.make_preferred().native().c_str())); + .append(kinematics_yaml_path.make_preferred().string().c_str())); + } + else + { + fs::path planning_context_launch_path = config_data_->config_pkg_path_; + planning_context_launch_path /= "launch/planning_context.launch"; + config_data_->inputPlanningContextLaunch(planning_context_launch_path.make_preferred().string()); } // Load 3d_sensors config file @@ -416,11 +431,11 @@ bool StartScreenWidget::loadExistingFiles() // Load ros controllers yaml file if available----------------------------------------------- fs::path ros_controllers_yaml_path = config_data_->config_pkg_path_; ros_controllers_yaml_path /= "config/ros_controllers.yaml"; - config_data_->inputROSControllersYAML(ros_controllers_yaml_path.make_preferred().native()); + config_data_->inputROSControllersYAML(ros_controllers_yaml_path.make_preferred().string()); fs::path ompl_yaml_path = config_data_->config_pkg_path_; ompl_yaml_path /= "config/ompl_planning.yaml"; - config_data_->inputOMPLYAML(ompl_yaml_path.make_preferred().native()); + config_data_->inputOMPLYAML(ompl_yaml_path.make_preferred().string()); // DONE LOADING -------------------------------------------------------------------------- @@ -489,8 +504,9 @@ bool StartScreenWidget::loadNewFiles() QApplication::processEvents(); // Create blank SRDF file - const std::string blank_srdf = ""; + const std::string blank_srdf = ""; // Load a blank SRDF file to the parameter server if (!setSRDFFile(blank_srdf)) @@ -561,7 +577,7 @@ bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const st while (!nh.ok() && steps < 4) { ROS_WARN("Waiting for node handle"); - sleep(1); + ros::Duration(1).sleep(); steps++; ros::spinOnce(); } @@ -610,7 +626,7 @@ bool StartScreenWidget::setSRDFFile(const std::string& srdf_string) while (!nh.ok() && steps < 4) { ROS_WARN("Waiting for node handle"); - sleep(1); + ros::Duration(1).sleep(); steps++; ros::spinOnce(); } @@ -629,65 +645,10 @@ bool StartScreenWidget::setSRDFFile(const std::string& srdf_string) // ****************************************************************************************** bool StartScreenWidget::extractPackageNameFromPath() { - // Get the path to urdf, save filename - fs::path urdf_path = config_data_->urdf_path_; - fs::path urdf_directory = urdf_path; - urdf_directory.remove_filename(); - - fs::path sub_path; // holds the directory less one folder - fs::path relative_path; // holds the path after the sub_path - std::string package_name; // result + std::string relative_path; // holds the path after the sub_path + std::string package_name; // result - // Paths for testing if files exist - fs::path package_path; - - std::vector path_parts; // holds each folder name in vector - - // Copy path into vector of parts - for (fs::path::iterator it = urdf_directory.begin(); it != urdf_directory.end(); ++it) - path_parts.push_back(it->native()); - - bool package_found = false; - - // reduce the generated directoy path's folder count by 1 each loop - for (int segment_length = path_parts.size(); segment_length > 0; --segment_length) - { - // Reset the sub_path - sub_path.clear(); - - // Create a subpath based on the outer loops length - for (int segment_count = 0; segment_count < segment_length; ++segment_count) - { - sub_path /= path_parts[segment_count]; - - // decide if we should remember this directory name because it is topmost, in case it is the package/stack name - if (segment_count == segment_length - 1) - { - package_name = path_parts[segment_count]; - } - } - - // check if this directory has a package.xml - package_path = sub_path; - package_path /= "package.xml"; - ROS_DEBUG_STREAM("Checking for " << package_path.make_preferred().native()); - - // Check if the files exist - if (fs::is_regular_file(package_path) || fs::is_regular_file(sub_path / "manifest.xml")) - { - // now generate the relative path - for (std::size_t relative_count = segment_length; relative_count < path_parts.size(); ++relative_count) - relative_path /= path_parts[relative_count]; - - // add the URDF filename at end of relative path - relative_path /= urdf_path.filename(); - - // end the search - segment_length = 0; - package_found = true; - break; - } - } + bool package_found = config_data_->extractPackageNameFromPath(config_data_->urdf_path_, package_name, relative_path); // Assign data to moveit_config_data if (!package_found) @@ -699,7 +660,7 @@ bool StartScreenWidget::extractPackageNameFromPath() else { // Check that ROS can find the package - const std::string robot_desc_pkg_path = ros::package::getPath(config_data_->urdf_pkg_name_) + "/"; + const std::string robot_desc_pkg_path = ros::package::getPath(package_name); if (robot_desc_pkg_path.empty()) { @@ -711,7 +672,7 @@ bool StartScreenWidget::extractPackageNameFromPath() // Success config_data_->urdf_pkg_name_ = package_name; - config_data_->urdf_pkg_relative_path_ = relative_path.make_preferred().native(); + config_data_->urdf_pkg_relative_path_ = relative_path; } ROS_DEBUG_STREAM("URDF Package Name: " << config_data_->urdf_pkg_name_); @@ -729,10 +690,11 @@ bool StartScreenWidget::createFullURDFPath() { if (config_data_->urdf_path_.empty()) // no path could be resolved { - QMessageBox::warning(this, "Error Loading Files", QString("ROS was unable to find the package name '") - .append(config_data_->urdf_pkg_name_.c_str()) - .append("'. Verify this package is inside your ROS " - "workspace and is a proper ROS package.")); + QMessageBox::warning(this, "Error Loading Files", + QString("ROS was unable to find the package name '") + .append(config_data_->urdf_pkg_name_.c_str()) + .append("'. Verify this package is inside your ROS " + "workspace and is a proper ROS package.")); } else { @@ -781,12 +743,12 @@ bool StartScreenWidget::load3DSensorsFile() if (!fs::is_regular_file(sensors_3d_yaml_path)) { - return config_data_->input3DSensorsYAML(default_sensors_3d_yaml_path.make_preferred().native()); + return config_data_->input3DSensorsYAML(default_sensors_3d_yaml_path.make_preferred().string()); } else { - return config_data_->input3DSensorsYAML(default_sensors_3d_yaml_path.make_preferred().native(), - sensors_3d_yaml_path.make_preferred().native()); + return config_data_->input3DSensorsYAML(default_sensors_3d_yaml_path.make_preferred().string(), + sensors_3d_yaml_path.make_preferred().string()); } } @@ -827,9 +789,9 @@ SelectModeWidget::SelectModeWidget(QWidget* parent) : QFrame(parent) widget_instructions_->setWordWrap(true); widget_instructions_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); widget_instructions_->setText( - "All settings for MoveIt! are stored in the MoveIt! configuration package. Here you have " + "All settings for MoveIt are stored in the MoveIt configuration package. Here you have " "the option to create a new configuration package or load an existing one. Note: " - "changes to a MoveIt! configuration package outside this Setup Assistant are likely to be " + "changes to a MoveIt configuration package outside this Setup Assistant are likely to be " "overwritten by this tool."); layout->addWidget(widget_instructions_); diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.h b/moveit_setup_assistant/src/widgets/start_screen_widget.h index 6e0a83a37f..b7c28ddef9 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.h +++ b/moveit_setup_assistant/src/widgets/start_screen_widget.h @@ -34,15 +34,11 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_START_SCREEN_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_START_SCREEN_WIDGET_ +#pragma once -#include -#include -#include -#include -#include -#include +class QLabel; +class QProgressBar; +class QPushButton; #ifndef Q_MOC_RUN #include // for testing a valid urdf is loaded @@ -59,7 +55,7 @@ class SelectModeWidget; class LoadPathArgsWidget; /** - * \brief Start screen user interface for MoveIt! Configuration Assistant + * \brief Start screen user interface for MoveIt Configuration Assistant */ class StartScreenWidget : public SetupScreenWidget { @@ -71,7 +67,7 @@ class StartScreenWidget : public SetupScreenWidget // ****************************************************************************************** /** - * \brief Start screen user interface for MoveIt! Configuration Assistant + * \brief Start screen user interface for MoveIt Configuration Assistant */ StartScreenWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); @@ -189,6 +185,4 @@ private Q_SLOTS: QPushButton* btn_exist_; QLabel* widget_instructions_; }; -} - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp index b07398daf8..b00d098865 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp @@ -36,10 +36,21 @@ // SA #include "virtual_joints_widget.h" +#include "header_widget.h" + // Qt +#include +#include #include +#include +#include +#include #include -#include +#include +#include +#include +#include +#include namespace moveit_setup_assistant { @@ -54,10 +65,10 @@ VirtualJointsWidget::VirtualJointsWidget(QWidget* parent, const MoveItConfigData // Top Header Area ------------------------------------------------ - HeaderWidget* header = - new HeaderWidget("Define Virtual Joints", "Create a virtual joint between a robot link and an external frame of " - "reference (considered fixed with respect to the robot).", - this); + HeaderWidget* header = new HeaderWidget("Define Virtual Joints", + "Create a virtual joint between a robot link and an external frame of " + "reference (considered fixed with respect to the robot).", + this); layout->addWidget(header); // Create contents screens --------------------------------------- @@ -65,16 +76,11 @@ VirtualJointsWidget::VirtualJointsWidget(QWidget* parent, const MoveItConfigData vjoint_list_widget_ = createContentsWidget(); vjoint_edit_widget_ = createEditWidget(); - // Create stacked layout ----------------------------------------- - stacked_layout_ = new QStackedLayout(this); - stacked_layout_->addWidget(vjoint_list_widget_); // screen index 0 - stacked_layout_->addWidget(vjoint_edit_widget_); // screen index 1 - // Create Widget wrapper for layout - QWidget* stacked_layout_widget = new QWidget(this); - stacked_layout_widget->setLayout(stacked_layout_); - - layout->addWidget(stacked_layout_widget); + stacked_widget_ = new QStackedWidget(this); + stacked_widget_->addWidget(vjoint_list_widget_); // screen index 0 + stacked_widget_->addWidget(vjoint_edit_widget_); // screen index 1 + layout->addWidget(stacked_widget_); // Finish Layout -------------------------------------------------- this->setLayout(layout); @@ -114,9 +120,7 @@ QWidget* VirtualJointsWidget::createContentsWidget() QHBoxLayout* controls_layout = new QHBoxLayout(); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Edit Selected Button btn_edit_ = new QPushButton("&Edit Selected", this); @@ -194,9 +198,7 @@ QWidget* VirtualJointsWidget::createEditWidget() controls_layout->setContentsMargins(0, 25, 0, 15); // Spacer - QWidget* spacer = new QWidget(this); - spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - controls_layout->addWidget(spacer); + controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum)); // Save QPushButton* btn_save = new QPushButton("&Save", this); @@ -236,7 +238,7 @@ void VirtualJointsWidget::showNewScreen() joint_type_field_->clearEditText(); // actually this just chooses first option // Switch to screen - stacked_layout_->setCurrentIndex(1); + stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode Q_EMIT isModal(true); @@ -245,7 +247,7 @@ void VirtualJointsWidget::showNewScreen() // ****************************************************************************************** // Edit whatever element is selected // ****************************************************************************************** -void VirtualJointsWidget::editDoubleClicked(int row, int column) +void VirtualJointsWidget::editDoubleClicked(int /*row*/, int /*column*/) { editSelected(); } @@ -253,31 +255,8 @@ void VirtualJointsWidget::editDoubleClicked(int row, int column) // ****************************************************************************************** // Preview whatever element is selected // ****************************************************************************************** -void VirtualJointsWidget::previewClicked(int row, int column) +void VirtualJointsWidget::previewClicked(int /*row*/, int /*column*/) { - // TODO: highlight the virtual joint? - - /* // Get list of all selected items - QList selected = data_table_->selectedItems(); - - // Check that an element was selected - if( !selected.size() ) - return; - - // Find the selected in datastructure - srdf::Model::GroupState *vjoint = findVjointByName( selected[0]->text().toStdString() ); - - // Set vjoint joint values by adding them to the local joint state map - for( std::map >::const_iterator value_it = vjoint->joint_values_.begin(); - value_it != vjoint->joint_values_.end(); ++value_it ) - { - // Only copy the first joint value - joint_state_map_[ value_it->first ] = value_it->second[0]; - } - - // Update the joints - publishJoints(); - */ } // ****************************************************************************************** @@ -330,7 +309,7 @@ void VirtualJointsWidget::edit(const std::string& name) joint_type_field_->setCurrentIndex(index); // Switch to screen - stacked_layout_->setCurrentIndex(1); + stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode Q_EMIT isModal(true); @@ -359,10 +338,10 @@ void VirtualJointsWidget::loadChildLinksComboBox() child_link_field_->clear(); // Get all links in robot model - std::vector link_models = config_data_->getRobotModel()->getLinkModels(); + std::vector link_models = config_data_->getRobotModel()->getLinkModels(); // Add all links to combo box - for (std::vector::const_iterator link_it = link_models.begin(); + for (std::vector::const_iterator link_it = link_models.begin(); link_it < link_models.end(); ++link_it) { child_link_field_->addItem((*link_it)->getName().c_str()); @@ -436,6 +415,7 @@ void VirtualJointsWidget::deleteSelected() // Reload main screen table loadDataTable(); config_data_->changes |= MoveItConfigData::VIRTUAL_JOINTS; + Q_EMIT referenceFrameChanged(); } // ****************************************************************************************** @@ -472,13 +452,12 @@ void VirtualJointsWidget::doneEditing() } // Check that the vjoint name is unique - for (std::vector::const_iterator data_it = config_data_->srdf_->virtual_joints_.begin(); - data_it != config_data_->srdf_->virtual_joints_.end(); ++data_it) + for (const auto& virtual_joint : config_data_->srdf_->virtual_joints_) { - if (data_it->name_.compare(vjoint_name) == 0) // the names are the same + if (virtual_joint.name_.compare(vjoint_name) == 0) // the names are the same { // is this our existing vjoint? check if vjoint pointers are same - if (&(*data_it) != searched_data) + if (&virtual_joint != searched_data) { QMessageBox::warning(this, "Error Saving", "A virtual joint already exists with that name!"); return; @@ -535,7 +514,7 @@ void VirtualJointsWidget::doneEditing() loadDataTable(); // Switch to screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode Q_EMIT isModal(false); @@ -553,7 +532,7 @@ void VirtualJointsWidget::doneEditing() void VirtualJointsWidget::cancelEditing() { // Switch to screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode Q_EMIT isModal(false); @@ -574,17 +553,16 @@ void VirtualJointsWidget::loadDataTable() // Loop through every virtual joint int row = 0; - for (std::vector::const_iterator data_it = config_data_->srdf_->virtual_joints_.begin(); - data_it != config_data_->srdf_->virtual_joints_.end(); ++data_it) + for (const auto& virtual_joint : config_data_->srdf_->virtual_joints_) { // Create row elements - QTableWidgetItem* data_name = new QTableWidgetItem(data_it->name_.c_str()); + QTableWidgetItem* data_name = new QTableWidgetItem(virtual_joint.name_.c_str()); data_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* child_link_name = new QTableWidgetItem(data_it->child_link_.c_str()); + QTableWidgetItem* child_link_name = new QTableWidgetItem(virtual_joint.child_link_.c_str()); child_link_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* parent_frame_name = new QTableWidgetItem(data_it->parent_frame_.c_str()); + QTableWidgetItem* parent_frame_name = new QTableWidgetItem(virtual_joint.parent_frame_.c_str()); parent_frame_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); - QTableWidgetItem* type_name = new QTableWidgetItem(data_it->type_.c_str()); + QTableWidgetItem* type_name = new QTableWidgetItem(virtual_joint.type_.c_str()); type_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable); // Add to table @@ -618,7 +596,7 @@ void VirtualJointsWidget::loadDataTable() void VirtualJointsWidget::focusGiven() { // Show the current vjoints screen - stacked_layout_->setCurrentIndex(0); + stacked_widget_->setCurrentIndex(0); // Load the data to the tree loadDataTable(); diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h b/moveit_setup_assistant/src/widgets/virtual_joints_widget.h index b1bb87a68b..f2d5ffaef0 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h +++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.h @@ -34,26 +34,21 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_VIRTUAL_JOINTS_WIDGET_ -#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_VIRTUAL_JOINTS_WIDGET_ +#pragma once // Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include +class QLabel; +class QLineEdit; +class QPushButton; +class QTableWidget; +class QStackedWidget; +class QComboBox; // SA #ifndef Q_MOC_RUN #include #endif -#include "header_widget.h" #include "setup_screen_widget.h" // a base class for screens in the setup assistant namespace moveit_setup_assistant @@ -80,7 +75,7 @@ class VirtualJointsWidget : public SetupScreenWidget QPushButton* btn_delete_; QPushButton* btn_save_; QPushButton* btn_cancel_; - QStackedLayout* stacked_layout_; + QStackedWidget* stacked_widget_; QLineEdit* vjoint_name_field_; QLineEdit* parent_name_field_; QComboBox* child_link_field_; @@ -187,6 +182,4 @@ private Q_SLOTS: void edit(const std::string& name); }; -} // namespace - -#endif +} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/CMakeLists.txt b/moveit_setup_assistant/templates/moveit_config_pkg_template/CMakeLists.txt index f95c9c1029..e0c28ee223 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/CMakeLists.txt +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project([GENERATED_PACKAGE_NAME]) find_package(catkin REQUIRED) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/config/cartesian_limits.yaml b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/cartesian_limits.yaml new file mode 100644 index 0000000000..7df72f693b --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml index 33d6ce4a57..f334bb4d57 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml @@ -1,10 +1,30 @@ - - - + + - - + + - + + + + + + + + + + + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch index a66b756773..192182a7f0 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch @@ -11,44 +11,46 @@ + + + - - - - - + - [VIRTUAL_JOINT_BROADCASTER] +[VIRTUAL_JOINT_BROADCASTER] - - + + [move_group/fake_controller_joint_states] + + [move_group/fake_controller_joint_states] - + + - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch index ec5d85541d..6d2427b16e 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch @@ -8,10 +8,13 @@ + + + - - - - [VIRTUAL_JOINT_BROADCASTER] - - + + [move_group/fake_controller_joint_states] + [/joint_states] + + + [move_group/fake_controller_joint_states] [/joint_states] - + + - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/edit_configuration_package.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/edit_configuration_package.launch index 6a313304a9..cce0611246 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/edit_configuration_package.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/edit_configuration_package.launch @@ -1,4 +1,4 @@ - + @@ -8,8 +8,8 @@ diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/fake_moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/fake_moveit_controller_manager.launch.xml index 06b3a93e74..f668745bb0 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/fake_moveit_controller_manager.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,12 @@ + + + - + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch index fb1959b048..6637d21031 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch @@ -4,7 +4,7 @@ + value="gdb -x $(find [GENERATED_PACKAGE_NAME])/launch/gdb_settings.gdb --ex run --args" /> @@ -15,6 +15,7 @@ + @@ -46,6 +47,8 @@ + + @@ -53,6 +56,7 @@ + @@ -68,8 +72,6 @@ - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch index 89078f2bc3..a4605c0374 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch @@ -9,8 +9,7 @@ - + args="$(arg command_args)" output="screen"> diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml index 7b2ad777a2..f85ed516e2 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml @@ -1,17 +1,23 @@ - + + + + + + + @@ -19,6 +25,8 @@ + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 0000000000..ac6272e235 --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch index c2f9b0b613..635179b3b8 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch @@ -14,11 +14,13 @@ + +[KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK] diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml index b58d3f4660..f1b5c7fcec 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml @@ -5,6 +5,15 @@ - + + + + + + + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch index f66253f1f3..f35d05fe05 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch @@ -15,7 +15,6 @@ - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch new file mode 100644 index 0000000000..50631869af --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml index 9354d7a7c1..2de6f893f6 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml @@ -2,7 +2,9 @@ - + + + @@ -15,6 +17,8 @@ - + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml new file mode 100644 index 0000000000..59470ce087 --- /dev/null +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template index ba305540fc..c0555ce40c 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template @@ -3,7 +3,7 @@ [GENERATED_PACKAGE_NAME] 0.3.0 - An automatically generated package with all the configuration and launch files for using the [ROBOT_NAME] with the MoveIt! Motion Planning Framework + An automatically generated package with all the configuration and launch files for using the [ROBOT_NAME] with the MoveIt Motion Planning Framework [AUTHOR_NAME] [AUTHOR_NAME] @@ -22,8 +22,11 @@ moveit_planners_ompl moveit_ros_visualization moveit_setup_assistant + moveit_simple_controller_manager joint_state_publisher + joint_state_publisher_gui robot_state_publisher + rviz tf2_ros xacro diff --git a/moveit_setup_assistant/test/moveit_config_data_test.cpp b/moveit_setup_assistant/test/moveit_config_data_test.cpp index 911be67575..658b22f785 100644 --- a/moveit_setup_assistant/test/moveit_config_data_test.cpp +++ b/moveit_setup_assistant/test/moveit_config_data_test.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2018, Mohamad Ayman. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Mohamad Ayman. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ /* Author: Mohamad Ayman */ #include @@ -43,6 +43,7 @@ #include #include #include +#include #include // This tests writing/parsing of ros_controller.yaml @@ -51,42 +52,26 @@ class MoveItConfigData : public testing::Test protected: void SetUp() override { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); - - srdf_model.reset(new srdf::Model()); - std::string xml_string; - std::fstream xml_file((res_path / "panda_description/urdf/panda.urdf").string().c_str(), std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - urdf_model = urdf::parseURDF(xml_string); - } - srdf_model->initFile(*urdf_model, (res_path / "panda_moveit_config/config/panda.srdf").string()); - robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model)); + std::string robot_name = "panda"; + urdf_model_ = moveit::core::loadModelInterface(robot_name); + srdf_model_ = moveit::core::loadSRDFModel(robot_name); + robot_model_ = std::make_shared(urdf_model_, srdf_model_); }; protected: - urdf::ModelInterfaceSharedPtr urdf_model; - srdf::ModelSharedPtr srdf_model; - moveit::core::RobotModelPtr robot_model; + urdf::ModelInterfaceSharedPtr urdf_model_; + srdf::ModelSharedPtr srdf_model_; + moveit::core::RobotModelPtr robot_model_; }; TEST_F(MoveItConfigData, ReadingControllers) { - boost::filesystem::path res_path(ros::package::getPath("moveit_resources")); - // Contains all the configuration data for the setup assistant moveit_setup_assistant::MoveItConfigDataPtr config_data; config_data.reset(new moveit_setup_assistant::MoveItConfigData()); - config_data->srdf_->srdf_model_ = srdf_model; - config_data->setRobotModel(robot_model); + config_data->srdf_->srdf_model_ = srdf_model_; + config_data->setRobotModel(robot_model_); // Initially no controllers EXPECT_EQ(config_data->getROSControllers().size(), 0u); @@ -106,7 +91,7 @@ TEST_F(MoveItConfigData, ReadingControllers) // ros_controller.yaml written correctly EXPECT_EQ(config_data->outputROSControllersYAML(test_file), true); - // Reset MoveIt! config MoveItConfigData + // Reset MoveIt config MoveItConfigData config_data.reset(new moveit_setup_assistant::MoveItConfigData()); // Initially no controllers
Package