diff --git a/.docker/README.md b/.docker/README.md
index a0cf726f20..6fb3ce6056 100644
--- a/.docker/README.md
+++ b/.docker/README.md
@@ -1,3 +1,3 @@
-# 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
deleted file mode 100644
index bc9107c99e..0000000000
--- a/.docker/ci-shadow-fixed/Dockerfile
+++ /dev/null
@@ -1,15 +0,0 @@
-# moveit/moveit:melodic-ci-shadow-fixed
-# Sets up a base image to use for running Continuous Integration on Travis
-
-FROM moveit/moveit:melodic-ci
-MAINTAINER Dave Coleman dave@picknik.ai
-
-# Switch to ros-shadow-fixed
-RUN echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main" | tee /etc/apt/sources.list.d/ros-latest.list
-
-# Upgrade packages to ros-shadow-fixed and clean apt-cache within one RUN command
-RUN apt-get -qq update && \
-    apt-get -qq dist-upgrade && \
-    #
-    # Clear apt-cache to reduce image size
-    rm -rf /var/lib/apt/lists/*
diff --git a/.docker/ci-testing/Dockerfile b/.docker/ci-testing/Dockerfile
new file mode 100644
index 0000000000..2d45c763c9
--- /dev/null
+++ b/.docker/ci-testing/Dockerfile
@@ -0,0 +1,15 @@
+# moveit/moveit:melodic-ci-testing
+# Sets up a base image to use for running Continuous Integration on Travis
+
+FROM moveit/moveit:melodic-ci
+MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de
+
+# Switch to ros-testing
+RUN echo "deb http://packages.ros.org/ros-testing/ubuntu `lsb_release -cs` main" | tee /etc/apt/sources.list.d/ros1-latest.list
+
+# Upgrade packages to ros-testing and clean apt-cache within one RUN command
+RUN apt-get -qq update && \
+    apt-get -qq -y dist-upgrade && \
+    #
+    # Clear apt-cache to reduce image size
+    rm -rf /var/lib/apt/lists/*
diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile
index 47819b3330..f58fbd81ca 100644
--- a/.docker/ci/Dockerfile
+++ b/.docker/ci/Dockerfile
@@ -2,48 +2,48 @@
 # Sets up a base image to use for running Continuous Integration on Travis
 
 FROM ros:melodic-ros-base
-MAINTAINER Dave Coleman dave@picknik.ai
+MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de
 
 ENV TERM xterm
 
 # Setup (temporary) ROS workspace
 WORKDIR /root/ws_moveit
 
+# Copy MoveIt sources from docker context
+COPY . src/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/${ROS_DISTRO}-devel/moveit.rosinstall && \
-    #
     # Update apt package list as previous containers clear the cache
-    apt-get -qq update && \
-    apt-get -qq dist-upgrade && \
+    apt-get -q update && \
+    apt-get -q -y 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 -q install --no-install-recommends -y \
+        # Some basic requirements
+        wget git sudo \
         # Preferred build tools
-        python-catkin-tools \
+        python$(test "${ROS_DISTRO}" = "noetic" && echo 3)-catkin-tools \
         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 sources, so that we can fetch all necessary dependencies
+    wstool init --shallow src src/moveit/.github/workflows/upstream.rosinstall && \
+    git clone --depth 1 --branch ${ROS_DISTRO}-devel https://github.com/ros-planning/moveit_resources src/moveit_resources && \
     #
+    # 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/*
+    rm -rf /var/lib/apt/lists/* && \
+    # Globally disable git security
+    # https://github.blog/2022-04-12-git-security-vulnerability-announced
+    git config --global --add safe.directory "*"
 
 # Continous Integration Setting
 ENV IN_DOCKER 1
diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile
index e06998b1a2..45cc49f311 100644
--- a/.docker/release/Dockerfile
+++ b/.docker/release/Dockerfile
@@ -1,10 +1,11 @@
 # 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
 MAINTAINER Dave Coleman dave@picknik.ai
 
 # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size
-RUN apt-get update && \
+RUN apt-get update -q && \
+    apt-get dist-upgrade -q -y && \
     apt-get install -y ros-${ROS_DISTRO}-moveit-* && \
     rm -rf /var/lib/apt/lists/*
diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile
index 5cd789d830..3c497d3a66 100644
--- a/.docker/source/Dockerfile
+++ b/.docker/source/Dockerfile
@@ -1,30 +1,35 @@
+# syntax = docker/dockerfile:1.3
+
 # moveit/moveit:melodic-source
 # Downloads the moveit source code and install remaining debian dependencies
 
-FROM moveit/moveit:melodic-ci-shadow-fixed
-MAINTAINER Dave Coleman dave@picknik.ai
+FROM moveit/moveit:melodic-ci-testing
+MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de
 
+ENV PYTHONIOENCODING UTF-8
+# Export ROS_UNDERLAY for downstream docker containers
 ENV ROS_UNDERLAY /root/ws_moveit/install
-WORKDIR $ROS_UNDERLAY/../src
+# Environment variable used in instructions on moveit.ros.org website for running clang-tidy
+ENV CATKIN_WS $(realpath $ROS_UNDERLAY/..)
+WORKDIR $ROS_UNDERLAY/..
+
+# Copy MoveIt sources from docker context
+COPY . src/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 \
-    # Download moveit source so that we can get necessary dependencies
-    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)
-    apt-get -qq update && \
-    apt-get -qq dist-upgrade && \
+RUN --mount=type=cache,target=/root/.ccache/ \
+    # Enable ccache
+    PATH=/usr/lib/ccache:$PATH && \
+    # Fetch required upstream sources for building
+    wstool init --shallow src src/moveit/.github/workflows/upstream.rosinstall && \
+    git clone --depth 1 --branch ${ROS_DISTRO}-devel https://github.com/ros-planning/moveit_resources src/moveit_resources && \
     #
-    rosdep update && \
-    rosdep install -y --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \
-    rm -rf /var/lib/apt/lists/*
-
-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
+    catkin build --limit-status-rate 0.001 --no-notify && \
+    ccache -s && \
+    #
+    # Update /ros_entrypoint.sh to source our new workspace
+    sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$ROS_UNDERLAY/setup.sh#g" /ros_entrypoint.sh
diff --git a/.dockerignore b/.dockerignore
new file mode 100644
index 0000000000..c586edd699
--- /dev/null
+++ b/.dockerignore
@@ -0,0 +1,37 @@
+# ignore everything
+*
+# but include these:
+!.github/workflows/upstream.rosinstall
+!**/package.xml
+!**/CATKIN_IGNORE
+
+# https://github.com/moby/moby/issues/42788
+!moveit_plugins/moveit_plugins/package.xml
+!moveit_plugins/moveit_ros_control_interface/package.xml
+!moveit_plugins/moveit_simple_controller_manager/package.xml
+!moveit_plugins/moveit_fake_controller_manager/package.xml
+!moveit_kinematics/package.xml
+!moveit_setup_assistant/package.xml
+!moveit_core/package.xml
+!moveit_commander/package.xml
+!moveit_planners/ompl/package.xml
+!moveit_planners/chomp/chomp_motion_planner/package.xml
+!moveit_planners/chomp/chomp_interface/package.xml
+!moveit_planners/chomp/chomp_optimizer_adapter/package.xml
+!moveit_planners/pilz_industrial_motion_planner_testutils/package.xml
+!moveit_planners/pilz_industrial_motion_planner/package.xml
+!moveit_planners/moveit_planners/package.xml
+!moveit_runtime/package.xml
+!moveit/package.xml
+!moveit_ros/warehouse/package.xml
+!moveit_ros/moveit_servo/package.xml
+!moveit_ros/occupancy_map_monitor/package.xml
+!moveit_ros/perception/package.xml
+!moveit_ros/move_group/package.xml
+!moveit_ros/robot_interaction/package.xml
+!moveit_ros/visualization/package.xml
+!moveit_ros/manipulation/package.xml
+!moveit_ros/planning/package.xml
+!moveit_ros/planning_interface/package.xml
+!moveit_ros/benchmarks/package.xml
+!moveit_ros/moveit_ros/package.xml
diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md
index 62f551be4a..2f81a5e828 100644
--- a/.github/ISSUE_TEMPLATE.md
+++ b/.github/ISSUE_TEMPLATE.md
@@ -3,8 +3,8 @@
 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?
diff --git a/.github/config.yml b/.github/config.yaml
similarity index 100%
rename from .github/config.yml
rename to .github/config.yaml
diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
new file mode 100644
index 0000000000..ff0d62095c
--- /dev/null
+++ b/.github/workflows/ci.yaml
@@ -0,0 +1,152 @@
+# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
+# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
+
+name: CI
+
+on:
+  workflow_dispatch:
+  pull_request:
+  push:
+    branches:
+      - melodic-devel
+
+jobs:
+  default:
+    strategy:
+      fail-fast: false
+      matrix:
+        env:
+          - IMAGE: melodic-ci
+            CCOV: true
+          - IMAGE: melodic-ci-testing
+            IKFAST_TEST: true
+            CATKIN_LINT: true
+            CLANG_TIDY: pedantic
+    env:
+      CXXFLAGS: >-
+        -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
+        -Wno-unused-parameter
+        ${{ matrix.env.CLANG_TIDY && '-Wno-c++17-compat -Wno-overloaded-virtual -Wno-unused-function' || '-Wno-unused-but-set-parameter' }}
+      CLANG_TIDY_ARGS: --fix --fix-errors
+      DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }}
+      UPSTREAM_WORKSPACE: .github/workflows/upstream.rosinstall
+      TARGET_WORKSPACE: $TARGET_REPO_PATH github:ros-planning/moveit_resources#master
+      DOWNSTREAM_WORKSPACE: .github/workflows/downstream.rosinstall
+      # Pull any updates to the upstream workspace (after restoring it from cache)
+      AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src
+      AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src
+      # Clear the ccache stats before and log the stats after the build
+      AFTER_SETUP_CCACHE: ccache --zero-stats --max-size=10.0G
+      AFTER_BUILD_TARGET_WORKSPACE: ccache --show-stats
+      AFTER_BUILD_DOWNSTREAM_WORKSPACE: ccache --show-stats
+      # Compile CCOV with Debug. Enable -Werror (except CLANG_TIDY=pedantic, which makes the clang-tidy step fail on warnings)
+      TARGET_CMAKE_ARGS: >
+        -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}}
+        -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}$CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}"
+      UPSTREAM_CMAKE_ARGS: "-DCMAKE_CXX_FLAGS=''"
+      DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Wall -Wextra"
+      CCACHE_DIR: ${{ github.workspace }}/.ccache
+      BASEDIR: ${{ github.workspace }}/.work
+      CLANG_TIDY_BASE_REF: ${{ github.event_name != 'workflow_dispatch' && (github.base_ref || github.ref) || '' }}
+      BEFORE_CLANG_TIDY_CHECKS: |
+        # Show list of applied checks
+        (cd $TARGET_REPO_PATH; clang-tidy --list-checks)
+        # Disable clang-tidy for ikfast plugins as we cannot fix the generated code
+        find $BASEDIR/target_ws/build -iwholename "*_ikfast_plugin/compile_commands.json" -exec rm {} \;
+      CC: ${{ matrix.env.CLANG_TIDY && 'clang' }}
+      CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }}
+
+    name: "${{ matrix.env.IMAGE }}${{ matrix.env.CATKIN_LINT && ' + catkin_lint' || ''}}${{ matrix.env.CCOV && ' + ccov' || ''}}${{ matrix.env.IKFAST_TEST && ' + ikfast' || ''}}${{ matrix.env.CLANG_TIDY && (github.event_name != 'workflow_dispatch' && ' + clang-tidy (delta)' || ' + clang-tidy (all)') || '' }}"
+    runs-on: ubuntu-latest
+    steps:
+      - name: "Free up disk space"
+        if: matrix.env.CCOV
+        run: |
+          sudo apt-get -qq purge build-essential "ghc*"
+          sudo apt-get clean
+          # cleanup docker images not used by us
+          docker system prune -af
+          # shift ccache folder to /mnt on a separate disk
+          sudo mkdir /mnt/ccache
+          mkdir ${{ env.CCACHE_DIR }}
+          sudo mount --bind ${{ env.CCACHE_DIR }} /mnt/ccache
+          # free up a lot of stuff from /usr/local
+          sudo rm -rf /usr/local
+          df -h
+      - uses: actions/checkout@v2
+      - name: Cache upstream workspace
+        uses: pat-s/always-upload-cache@v2.1.5
+        with:
+          path: ${{ env.BASEDIR }}/upstream_ws
+          key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
+          restore-keys: ${{ env.CACHE_PREFIX }}
+        env:
+          CACHE_PREFIX: upstream_ws-${{ matrix.env.IMAGE }}-${{ hashFiles('.github/workflows/upstream.rosinstall', '.github/workflows/ci.yaml') }}
+      - name: Cache downstream workspace
+        uses: pat-s/always-upload-cache@v2.1.5
+        with:
+          path: ${{ env.BASEDIR }}/downstream_ws
+          key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
+          restore-keys: ${{ env.CACHE_PREFIX }}
+        env:
+          CACHE_PREFIX: downstream_ws-${{ matrix.env.IMAGE }}-${{ hashFiles('.github/workflows/downstream.rosinstall', '.github/workflows/ci.yaml') }}
+      # The target directory cache doesn't include the source directory because
+      # that comes from the checkout.  See "prepare target_ws for cache" task below
+      - name: Cache target workspace
+        if: "!matrix.env.CCOV"
+        uses: pat-s/always-upload-cache@v2.1.5
+        with:
+          path: ${{ env.BASEDIR }}/target_ws
+          key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
+          restore-keys: ${{ env.CACHE_PREFIX }}
+        env:
+          CACHE_PREFIX: target_ws${{ matrix.env.CCOV && '-ccov' || '' }}-${{ matrix.env.IMAGE }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml', '.github/workflows/ci.yaml') }}
+      - name: Cache ccache
+        uses: pat-s/always-upload-cache@v2.1.5
+        with:
+          path: ${{ env.CCACHE_DIR }}
+          key: ${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
+          restore-keys: |
+            ${{ env.CACHE_PREFIX }}-${{ github.sha }}
+            ${{ env.CACHE_PREFIX }}
+        env:
+          CACHE_PREFIX: ccache-${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && '-ccov' || '' }}
+
+      - name: Generate ikfast packages
+        if: matrix.env.IKFAST_TEST
+        run: moveit_kinematics/test/test_ikfast_plugins.sh
+      - id: ici
+        name: Run industrial_ci
+        uses: ros-industrial/industrial_ci@master
+
+      - name: Upload test artifacts (on failure)
+        uses: actions/upload-artifact@v2
+        if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
+        with:
+          name: test-results-${{ matrix.env.IMAGE }}
+          path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml
+      - name: Generate codecov report
+        uses: rhaschke/lcov-action@main
+        if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0'
+        with:
+          docker: $DOCKER_IMAGE
+          workdir: ${{ env.BASEDIR }}/target_ws
+          ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
+      - name: Upload codecov report
+        uses: codecov/codecov-action@v2
+        if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0'
+        with:
+          files: ${{ env.BASEDIR }}/target_ws/coverage.info
+      - name: Upload clang-tidy changes
+        uses: rhaschke/upload-git-patch-action@main
+        if: always() && matrix.env.CLANG_TIDY
+        with:
+          name: clang-tidy
+          path: ${{ env.BASEDIR }}/target_ws/src/$(basename $(pwd))
+      - name: Prepare target_ws for cache
+        if: always() && !matrix.env.CCOV
+        run: |
+          du -sh ${{ env.BASEDIR }}/target_ws
+          sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete
+          sudo rm -rf ${{ env.BASEDIR }}/target_ws/src
+          du -sh ${{ env.BASEDIR }}/target_ws
diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml
new file mode 100644
index 0000000000..06e1fe7855
--- /dev/null
+++ b/.github/workflows/docker.yaml
@@ -0,0 +1,158 @@
+name: docker
+
+on:
+  schedule:
+    # 6 AM UTC every Sunday
+    - cron: "0 6 * * 6"
+  workflow_dispatch:
+  push:
+    branches:
+      - melodic-devel
+
+jobs:
+  release:
+    runs-on: ubuntu-latest
+    permissions:
+      packages: write
+      contents: read
+    env:
+      IMAGE: moveit/moveit:melodic-${{ github.job }}
+
+    steps:
+      - uses: addnab/docker-run-action@v3
+        name: Check for apt updates
+        continue-on-error: true
+        id: apt
+        with:
+          image: ${{ env.IMAGE }}
+          run: |
+            apt-get update
+            have_updates=$(apt-get --simulate upgrade | grep -q "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" && echo false || echo true)
+            echo "::set-output name=no_cache::$have_updates"
+      - name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v1
+        if: ${{ github.event_name == 'workflow_dispatch' || github.event_name != 'schedule' || steps.apt.outputs.no_cache }}
+      - name: Login to Container Registry
+        uses: docker/login-action@v1
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      - name: Build and Push
+        uses: docker/build-push-action@v2
+        if: ${{ github.event_name == 'workflow_dispatch' || github.event_name != 'schedule' || steps.apt.outputs.no_cache }}
+        with:
+          file: .docker/${{ github.job }}/Dockerfile
+          push: true
+          no-cache: ${{ steps.apt.outputs.no_cache || github.event_name == 'workflow_dispatch' }}
+          cache-from: type=registry,ref=${{ env.IMAGE }}
+          cache-to: type=inline
+          tags: ${{ env.IMAGE }}
+
+  ci:
+    runs-on: ubuntu-latest
+    permissions:
+      packages: write
+      contents: read
+    env:
+      IMAGE: moveit/moveit:melodic-${{ github.job }}
+
+    steps:
+      - uses: addnab/docker-run-action@v3
+        name: Check for apt updates
+        continue-on-error: true
+        id: apt
+        with:
+          image: ${{ env.IMAGE }}
+          run: |
+            apt-get update
+            have_updates=$(apt-get --simulate upgrade | grep -q "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" && echo false || echo true)
+            echo "::set-output name=no_cache::$have_updates"
+      - name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v1
+        if: ${{ github.event_name == 'workflow_dispatch' || github.event_name != 'schedule' || steps.apt.outputs.no_cache }}
+      - name: Login to Container Registry
+        uses: docker/login-action@v1
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      - name: Build and Push
+        uses: docker/build-push-action@v2
+        if: ${{ github.event_name == 'workflow_dispatch' || github.event_name != 'schedule' || steps.apt.outputs.no_cache }}
+        with:
+          file: .docker/${{ github.job }}/Dockerfile
+          push: true
+          no-cache: ${{ steps.apt.outputs.no_cache || github.event_name == 'workflow_dispatch' }}
+          cache-from: type=registry,ref=${{ env.IMAGE }}
+          cache-to: type=inline
+          tags: ${{ env.IMAGE }}
+
+  ci-testing:
+    needs: ci
+    runs-on: ubuntu-latest
+    permissions:
+      packages: write
+      contents: read
+    env:
+      IMAGE: moveit/moveit:melodic-${{ github.job }}
+
+    steps:
+      - uses: addnab/docker-run-action@v3
+        name: Check for apt updates
+        continue-on-error: true
+        id: apt
+        with:
+          image: ${{ env.IMAGE }}
+          run: |
+            apt-get update
+            have_updates=$(apt-get --simulate upgrade | grep -q "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" && echo false || echo true)
+            echo "::set-output name=no_cache::$have_updates"
+      - name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v1
+        if: ${{ github.event_name == 'workflow_dispatch' || github.event_name != 'schedule' || steps.apt.outputs.no_cache }}
+      - name: Login to Container Registry
+        uses: docker/login-action@v1
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      - name: Build and Push
+        uses: docker/build-push-action@v2
+        if: ${{ github.event_name == 'workflow_dispatch' || github.event_name != 'schedule' || steps.apt.outputs.no_cache }}
+        with:
+          file: .docker/${{ github.job }}/Dockerfile
+          push: true
+          no-cache: ${{ steps.apt.outputs.no_cache || github.event_name == 'workflow_dispatch' }}
+          cache-from: type=registry,ref=${{ env.IMAGE }}
+          cache-to: type=inline
+          tags: |
+            ${{ env.IMAGE }}
+            moveit/moveit:melodic-ci-shadow-fixed
+
+  source:
+    needs: ci-testing
+    runs-on: ubuntu-latest
+    permissions:
+      packages: write
+      contents: read
+    env:
+      IMAGE: moveit/moveit:melodic-${{ github.job }}
+
+    steps:
+      - uses: actions/checkout@v2
+      - name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v1
+      - name: Login to Container Registry
+        uses: docker/login-action@v1
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      - name: "Remove .dockerignore"
+        run: rm .dockerignore  # enforce full source context
+      - name: Build and Push
+        uses: docker/build-push-action@v2
+        with:
+          context: .
+          file: .docker/${{ github.job }}/Dockerfile
+          push: true
+          cache-from: type=registry,ref=${{ env.IMAGE }}
+          cache-to: type=inline
+          tags: ${{ env.IMAGE }}
diff --git a/.github/workflows/downstream.rosinstall b/.github/workflows/downstream.rosinstall
new file mode 100644
index 0000000000..d38088377a
--- /dev/null
+++ b/.github/workflows/downstream.rosinstall
@@ -0,0 +1,16 @@
+- git:
+    local-name: rviz_visual_tools
+    uri: https://github.com/PickNikRobotics/rviz_visual_tools
+    version: melodic-devel
+- git:
+    local-name: moveit_visual_tools
+    uri: https://github.com/ros-planning/moveit_visual_tools.git
+    version: melodic-devel
+- git:
+    local-name: moveit_tutorials
+    uri: https://github.com/ros-planning/moveit_tutorials.git
+    version: melodic-devel
+- git:
+    local-name: panda_moveit_config
+    uri: https://github.com/ros-planning/panda_moveit_config.git
+    version: melodic-devel
diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml
new file mode 100644
index 0000000000..5159c05ccc
--- /dev/null
+++ b/.github/workflows/format.yaml
@@ -0,0 +1,29 @@
+# This is a format job. Pre-commit has a first-party GitHub action, so we use
+# that: https://github.com/pre-commit/action
+
+name: Format
+
+on:
+  workflow_dispatch:
+  pull_request:
+  push:
+
+jobs:
+  pre-commit:
+    name: pre-commit
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v2
+    - uses: actions/setup-python@v2
+    - name: Install clang-format-10
+      run: sudo apt-get install clang-format-10
+    - uses: rhaschke/install-catkin_lint-action@v1.0
+      with:
+        distro: melodic
+    - uses: pre-commit/action@v2.0.3
+      id: precommit
+    - name: Upload pre-commit changes
+      if: failure() && steps.precommit.outcome == 'failure'
+      uses: rhaschke/upload-git-patch-action@main
+      with:
+        name: pre-commit
diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml
new file mode 100644
index 0000000000..d6316345a6
--- /dev/null
+++ b/.github/workflows/prerelease.yaml
@@ -0,0 +1,38 @@
+# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
+# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
+
+name: pre-release
+
+on:
+  workflow_dispatch:
+  push:
+
+jobs:
+  default:
+    strategy:
+      matrix:
+        distro: [melodic]
+
+    env:
+      # https://github.com/ros-industrial/industrial_ci/issues/666
+      BUILDER: catkin_make_isolated
+      ROS_DISTRO: ${{ matrix.distro }}
+      PRERELEASE: true
+      BASEDIR: ${{ github.workspace }}/.work
+
+    if: github.event_name == 'workflow_dispatch' # only allow manual triggering
+    name: "${{ matrix.distro }}"
+    runs-on: ubuntu-latest
+    steps:
+      - name: "Free up disk space"
+        run: |
+          sudo apt-get -qq purge build-essential "ghc*"
+          sudo apt-get clean
+          # cleanup docker images not used by us
+          docker system prune -af
+          # free up a lot of stuff from /usr/local
+          sudo rm -rf /usr/local
+          df -h
+      - uses: actions/checkout@v2
+      - name: industrial_ci
+        uses: ros-industrial/industrial_ci@master
diff --git a/.github/workflows/upstream.rosinstall b/.github/workflows/upstream.rosinstall
new file mode 100644
index 0000000000..4cd7b86ad0
--- /dev/null
+++ b/.github/workflows/upstream.rosinstall
@@ -0,0 +1,8 @@
+- git:
+    local-name: moveit_msgs
+    uri: https://github.com/ros-planning/moveit_msgs.git
+    version: melodic-devel
+- git:
+    local-name: geometric_shapes
+    uri: https://github.com/ros-planning/geometric_shapes.git
+    version: melodic-devel
diff --git a/.gitignore b/.gitignore
index 9877f9248d..477f567e18 100644
--- a/.gitignore
+++ b/.gitignore
@@ -58,3 +58,6 @@ CATKIN_IGNORE
 .moveit_ci
 
 *.pyc
+
+_build
+_autosummary
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
new file mode 100644
index 0000000000..f89df73fa0
--- /dev/null
+++ b/.pre-commit-config.yaml
@@ -0,0 +1,58 @@
+# To use:
+#
+#     pre-commit run -a
+#
+# Or:
+#
+#     pre-commit install  # (runs every time you commit in git)
+#
+# To update this file:
+#
+#     pre-commit autoupdate
+#
+# See https://github.com/pre-commit/pre-commit
+
+repos:
+  # Standard hooks
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.3.0
+    hooks:
+      - id: check-added-large-files
+      - id: check-case-conflict
+      - id: check-merge-conflict
+      - id: check-symlinks
+      - id: check-yaml
+      - id: debug-statements
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+
+  - repo: https://github.com/psf/black
+    rev: 22.6.0
+    hooks:
+      - id: black
+        # https://github.com/psf/black/issues/406#issuecomment-1101449515
+        args: [ "--skip-string-normalization" ]
+
+  - repo: https://github.com/Lucas-C/pre-commit-hooks-markup
+    rev: v1.0.1
+    hooks:
+      - id: rst-linter
+        exclude: .*/doc/.*
+
+  - repo: local
+    hooks:
+      - id: clang-format
+        name: clang-format
+        description: Format files with ClangFormat.
+        entry: clang-format-10
+        language: system
+        files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
+        args: ["-fallback-style=none", "-i"]
+      - id: catkin_lint
+        name: catkin_lint
+        description: Check package.xml and cmake files
+        entry: catkin_lint --rosdistro melodic --explain .
+        language: system
+        always_run: true
+        pass_filenames: false
diff --git a/.travis.yml b/.travis.yml
deleted file mode 100644
index 4a79fcd05d..0000000000
--- a/.travis.yml
+++ /dev/null
@@ -1,39 +0,0 @@
-# This config file for Travis CI utilizes https://github.com/ros-planning/moveit_ci/ package.
-os: linux
-dist: bionic  # distro used by Travis, moveit_ci uses the docker image's distro
-services:
-  - docker
-language: cpp
-cache: ccache
-compiler: gcc
-
-notifications:
-  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
-    - UPSTREAM_WORKSPACE=moveit.rosinstall
-    - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-unused-function"
-    - WARNINGS_OK=false
-
-jobs:
-  fast_finish: true
-  include:
-    - env: TEST="clang-format catkin_lint"
-    - env: TEST=code-coverage
-    - env: ROS_DISTRO=melodic
-    - env: ROS_DISTRO=kinetic
-    - env: BEFORE_DOCKER_SCRIPT="source moveit_kinematics/test/test_ikfast_plugins.sh"
-    - 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
-
-script:
-  - .moveit_ci/travis.sh
diff --git a/MIGRATION.md b/MIGRATION.md
index e64ea7d37a..c703307e1d 100644
--- a/MIGRATION.md
+++ b/MIGRATION.md
@@ -2,8 +2,6 @@
 
 API changes in MoveIt! releases
 
-## ROS Noetic (upcoming changes in master)
-
 ## ROS Melodic
 
 - Migration to ``tf2`` API.
@@ -24,6 +22,8 @@ API changes in MoveIt! releases
 - Deprecated `EndEffectorInteractionStyle` got removed from `RobotInteraction` (https://github.com/ros-planning/moveit/pull/1287)
   Use [the corresponding `InteractionStyle` definitions](https://github.com/ros-planning/moveit/pull/1287/files#diff-24e57a8ea7f2f2d8a63cfc31580d09ddL240) instead
 - `moveit_ros_plannning` no longer depends on `moveit_ros_perception`
+- The joint states of `passive` joints must be published in ROS and the CurrentStateMonitor will now wait for them as well. Their semantics dictate that they cannot be actively controlled, but they must be known to use the full robot state in collision checks. (https://github.com/ros-planning/moveit/pull/2663)
+- In release 1.0.9 the macro `MOVEIT_VERSION` was renamed to `MOVEIT_VERSION_STR` in favour of `MOVEIT_VERSION` becoming a numeric version identifier suitable for a version check via: `#if MOVEIT_VERSION >= MOVEIT_VERSION_CHECK(1, 0, 9)`.
 
 ## ROS Kinetic
 
diff --git a/README.md b/README.md
index bfcc3bad5d..1283b78954 100644
--- a/README.md
+++ b/README.md
@@ -11,20 +11,21 @@ Currently we support ROS Indigo, Kinetic, and Melodic.
 
 ## 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 occasionally get backported to these released versions of MoveIt.
+- 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).
 
 ## Continuous Integration Status
 
-service    | Indigo | Kinetic | Melodic | Master
----------- | ------ | ------- | ------- | ------
-Travis     | [![Build Status](https://travis-ci.com/ros-planning/moveit.svg?branch=indigo-devel)](https://travis-ci.com/ros-planning/moveit/branches) | [![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=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) | N/A | N/A |
+GitHub | N/A | [![Format](https://github.com/ros-planning/moveit/actions/workflows/format.yaml/badge.svg?branch=melodic-devel)](https://github.com/ros-planning/moveit/actions/workflows/format.yaml?query=branch%3Amelodic-devel) [![CI](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml/badge.svg?branch=melodic-devel)](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml?query=branch%3Amelodic-devel) | [![Format](https://github.com/ros-planning/moveit/actions/workflows/format.yaml/badge.svg?branch=master)](https://github.com/ros-planning/moveit/actions/workflows/format.yaml?query=branch%3Amaster) [![CI](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml/badge.svg?branch=master)](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml?query=branch%3Amaster) |
+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 |
+CodeCov | N/A | [![codecov](https://codecov.io/gh/ros-planning/moveit/branch/melodic-devel/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit) | [![codecov](https://codecov.io/gh/ros-planning/moveit/branch/master/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit) |
 
 ## Docker Containers
 
@@ -32,32 +33,32 @@ build farm | [![Build Status](http://build.ros.org/buildStatus/icon?job=Idev__mo
 [![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_controller_manager_example | [![Build Status](http://build.ros.org/buildStatus/icon?job=Isrc_uT__moveit_controller_manager_example__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_controller_manager_example__ubuntu_trusty__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__moveit_controller_manager_example__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_controller_manager_example__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_controller_manager_example__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_controller_manager_example__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_controller_manager_example__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_controller_manager_example__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_controller_manager_example__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_controller_manager_example__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_controller_manager_example__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_controller_manager_example__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)
+MoveIt! Package | Kinetic Source | Kinetic Debian | Melodic Source | Melodic 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)
+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=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_controller_manager_example | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_controller_manager_example__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_controller_manager_example__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_controller_manager_example__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_controller_manager_example__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_controller_manager_example__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_controller_manager_example__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_controller_manager_example__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_controller_manager_example__ubuntu_bionic_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)
+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=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=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=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=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=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=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=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=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=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=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=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=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=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=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=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=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=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=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=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)
diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst
index 5156491c08..b5be0b191a 100644
--- a/moveit/CHANGELOG.rst
+++ b/moveit/CHANGELOG.rst
@@ -2,6 +2,29 @@
 Changelog for package moveit
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+* [feature][moveit_setup_assistant] Fix group editing (`#2350 <https://github.com/ros-planning/moveit/issues/2350>`_)
+* [feature][moveit_setup_assistant] Allow showing both, visual and collision geometry (`#2352 <https://github.com/ros-planning/moveit/issues/2352>`_)
+* [feature][moveit_setup_assistant] Start new joint_state_publisher_gui on param use_gui (`#2257 <https://github.com/ros-planning/moveit/issues/2257>`_)
+* [feature][moveit_servo] A library for servoing toward a moving pose (`#2203 <https://github.com/ros-planning/moveit/issues/2203>`_)
+* [fix][moveit_setup_assistant] Segfault when editing pose in moveit_setup_assistant (`#2340 <https://github.com/ros-planning/moveit/issues/2340>`_)
+* [fix][moveit_setup_assistant] only write default_planner_config field if any is selected (`#2293 <https://github.com/ros-planning/moveit/issues/2293>`_)
+* [fix][moveit_setup_assistant] Fix disappearing robot on change of reference frame (`#2335 <https://github.com/ros-planning/moveit/issues/2335>`_)
+* Contributors: AdamPettinger, AndyZe, Jere Liukkonen, David V. Lu!!, Felix von Drigalski, Michael Görner, Robert Haschke, Tyler Weaver, Yoan Mollard
+
 1.0.6 (2020-08-19)
 ------------------
 
@@ -43,7 +66,7 @@ Changelog for package moveit
 * [improve][moveit_kinematics] KDL IK solver improvements (`#1321 <https://github.com/ros-planning/moveit/issues/1321>`_)
 * [improve][moveit_setup_assistant] support dark themes (`#1173 <https://github.com/ros-planning/moveit/issues/1173>`_)
 * [improve][moveit_ros_robot_interaction] cleanup RobotInteraction (`#1287 <https://github.com/ros-planning/moveit/issues/1287>`_)
-* [improve][moveit_ros_robot_interaction] limit IK timeout to 0.1s for a responsive interaction behaviour (`#1291 <https://github.com/ros-planning/moveit/issues/1291>`_)  
+* [improve][moveit_ros_robot_interaction] limit IK timeout to 0.1s for a responsive interaction behaviour (`#1291 <https://github.com/ros-planning/moveit/issues/1291>`_)
 * [maintenance] cleanup SimpleControllerManager https://github.com/ros-planning/moveit/pull/1352
 * [maintenance][moveit_core]  Add coverage analysis for moveit_core (`#1133 <https://github.com/ros-planning/moveit/issues/1133>`_)
 * Contributors: Alexander Gutenkunst, Dave Coleman, Jonathan Binney, Keerthana Subramanian Manivannan, Martin Oehler, Michael Görner, Mike Lautman, Robert Haschke, Simon Schmeisser
@@ -177,7 +200,7 @@ Changelog for package moveit
   * [fix][moveit_ros_visualization] RobotStateVisualization: clear before load to avoid segfault `#572 <https://github.com/ros-planning/moveit/pull/572>`_
   * [fix][setup_assistant] Fix for lunar (`#542 <https://github.com/ros-planning/moveit/issues/542>`_) (fix `#506 <https://github.com/ros-planning/moveit/issues/506>`_)
   * [fix][moveit_core] segfault due to missing string format parameter. (`#547 <https://github.com/ros-planning/moveit/issues/547>`_)
-  * [fix][moveit_core] doc-comment for robot_state::computeAABB (`#516 <https://github.com/ros-planning/moveit/issues/516>`_) 
+  * [fix][moveit_core] doc-comment for robot_state::computeAABB (`#516 <https://github.com/ros-planning/moveit/issues/516>`_)
 * Improvement in the contained packages:
 
   * [improve][moveit_ros_planning] Chomp use PlanningScene (`#546 <https://github.com/ros-planning/moveit/issues/546>`_) to partially address `#305 <https://github.com/ros-planning/moveit/issues/305>`_
@@ -231,12 +254,12 @@ Changelog for package moveit
 
 0.9.5 (2017-03-08)
 ------------------
-* [fix] correct "simplify widget handling" `#452 <https://github.com/ros-planning/moveit/pull/452>`_ This reverts "simplify widget handling (`#442 <https://github.com/ros-planning/moveit/issues/442>`_)" 
-* [fix][moveit_ros_warehouse] gcc6 build error `#423 <https://github.com/ros-planning/moveit/pull/423>`_ 
+* [fix] correct "simplify widget handling" `#452 <https://github.com/ros-planning/moveit/pull/452>`_ This reverts "simplify widget handling (`#442 <https://github.com/ros-planning/moveit/issues/442>`_)"
+* [fix][moveit_ros_warehouse] gcc6 build error `#423 <https://github.com/ros-planning/moveit/pull/423>`_
 * [fix] Regression on Ubuntu Xenial; numpy.ndarray indices bug (from `#86 <https://github.com/ros-planning/moveit/issues/86>`_) (`#450 <https://github.com/ros-planning/moveit/issues/450>`_).
 * [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 <https://github.com/ros-planning/moveit/issues/445>`_)
 * [enhancement][MoveGroup] Add getLinkNames function (`#440 <https://github.com/ros-planning/moveit/issues/440>`_)
-* [doc][moveit_commander] added description for set_start_state (`#447 <https://github.com/ros-planning/moveit/issues/447>`_) 
+* [doc][moveit_commander] added description for set_start_state (`#447 <https://github.com/ros-planning/moveit/issues/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/package.xml b/moveit/package.xml
index fc1e8a92ca..6a29c51f85 100644
--- a/moveit/package.xml
+++ b/moveit/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>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 <a href = "http://discourse.ros.org/t/migration-to-one-github-repo-for-moveit/266/34">the detailed discussion for the merge of several repositories</a>.</description>
   <maintainer email="dave@picknik.ai">Dave Coleman</maintainer>
   <maintainer email="mferguson@fetchrobotics.com">Michael Ferguson</maintainer>
diff --git a/moveit/scripts/create_maintainer_table.py b/moveit/scripts/create_maintainer_table.py
index af7615c2a9..3f3878443d 100644
--- a/moveit/scripts/create_maintainer_table.py
+++ b/moveit/scripts/create_maintainer_table.py
@@ -8,80 +8,129 @@
 import webbrowser
 from catkin_pkg.packages import find_packages
 
-maintainers_dict = {"Ioan Sucan" : "isucan",
-                    "Michael Ferguson" : "mikeferguson",
-                    "Sachin Chitta" : "sachinchitta",
-                    "G.A. vd. Hoorn" : "gavanderhoorn",
-                    "Dave Coleman" : "davetcoleman",
-                    "Acorn Pooley" : "acorn",
-                    "Jon Binney" : "jonbinney",
-                    "Matei Ciocarlie" : "mateiciocarlie",
-                    "Michael Görner".decode('utf8') : "v4hn",
-                    "Robert Haschke" : "rhaschke",
-                    "Ian McMahon" : "IanTheEngineer",
-                    "Isaac I. Y. Saito" : "130s",
-                    "Mathias Lüdtke".decode('utf8') : "ipa-mdl",
-                    "Ryan Luna" : "ryanluna",
-                    "Chittaranjan Srinivas Swaminathan" : "ksatyaki",
-                    "Chittaranjan S Srinivas" : "ksatyaki"
+maintainers_dict = {
+    "Ioan Sucan": "isucan",
+    "Michael Ferguson": "mikeferguson",
+    "Sachin Chitta": "sachinchitta",
+    "G.A. vd. Hoorn": "gavanderhoorn",
+    "Dave Coleman": "davetcoleman",
+    "Acorn Pooley": "acorn",
+    "Jon Binney": "jonbinney",
+    "Matei Ciocarlie": "mateiciocarlie",
+    "Michael Görner".decode("utf8"): "v4hn",
+    "Robert Haschke": "rhaschke",
+    "Ian McMahon": "IanTheEngineer",
+    "Isaac I. Y. Saito": "130s",
+    "Mathias Lüdtke".decode("utf8"): "ipa-mdl",
+    "Ryan Luna": "ryanluna",
+    "Chittaranjan Srinivas Swaminathan": "ksatyaki",
+    "Chittaranjan S Srinivas": "ksatyaki",
 }
 
+
 def eprint(*args, **kwargs):
     print(*args, file=sys.stderr, **kwargs)
 
+
 def author_to_github(maintainer):
     try:
         name = maintainers_dict[maintainer.name]
     except KeyError:
-        eprint( "Missing maintainer: ",  maintainer.name)
+        eprint("Missing maintainer: ", maintainer.name)
         name = maintainer.email
     return name
 
+
 def template_file(src, dst, subs):
     print("++ Templating '{0}'".format(src))
-    with open(src, 'r') as f:
+    with open(src, "r") as f:
         data = f.read()
     for k, v in subs.items():
         data = data.replace(k, v)
-    with open(dst, 'w+') as f:
+    with open(dst, "w+") as f:
         f.write(data)
     print("++ Webpage ready at '{0}'".format(dst))
     webbrowser.open(dst)
 
+
 def create_travis_badge(package_name):
-    output = ''
+    output = ""
     # Indigo
     output += "<td>"
-    output += "<a href='http://build.ros.org/view/Isrc_uT/job/Isrc_uT__" + package_name + "__ubuntu_trusty__source/'><img src='http://build.ros.org/buildStatus/icon?job=Isrc_uT__" + package_name + "__ubuntu_trusty__source'></a>"
+    output += (
+        "<a href='http://build.ros.org/view/Isrc_uT/job/Isrc_uT__"
+        + package_name
+        + "__ubuntu_trusty__source/'><img src='http://build.ros.org/buildStatus/icon?job=Isrc_uT__"
+        + package_name
+        + "__ubuntu_trusty__source'></a>"
+    )
     output += "</td><td>"
-    output += "<a href='http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__" + package_name + "__ubuntu_trusty_amd64__binary/'><img src='http://build.ros.org/buildStatus/icon?job=Ibin_uT64__" + package_name + "__ubuntu_trusty_amd64__binary'></a>"
+    output += (
+        "<a href='http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__"
+        + package_name
+        + "__ubuntu_trusty_amd64__binary/'><img src='http://build.ros.org/buildStatus/icon?job=Ibin_uT64__"
+        + package_name
+        + "__ubuntu_trusty_amd64__binary'></a>"
+    )
     output += "</td>"
 
     # Jade
     output += "<td>"
-    output += "<a href='http://build.ros.org/view/Jsrc_uT/job/Jsrc_uT__" + package_name + "__ubuntu_trusty__source/'><img src='http://build.ros.org/buildStatus/icon?job=Jsrc_uT__" + package_name + "__ubuntu_trusty__source'></a>"
+    output += (
+        "<a href='http://build.ros.org/view/Jsrc_uT/job/Jsrc_uT__"
+        + package_name
+        + "__ubuntu_trusty__source/'><img src='http://build.ros.org/buildStatus/icon?job=Jsrc_uT__"
+        + package_name
+        + "__ubuntu_trusty__source'></a>"
+    )
     output += "</td><td>"
-    output += "<a href='http://build.ros.org/view/Jbin_uT64/job/Jbin_uT64__" + package_name + "__ubuntu_trusty_amd64__binary/'><img src='http://build.ros.org/buildStatus/icon?job=Jbin_uT64__" + package_name + "__ubuntu_trusty_amd64__binary'></a>"
+    output += (
+        "<a href='http://build.ros.org/view/Jbin_uT64/job/Jbin_uT64__"
+        + package_name
+        + "__ubuntu_trusty_amd64__binary/'><img src='http://build.ros.org/buildStatus/icon?job=Jbin_uT64__"
+        + package_name
+        + "__ubuntu_trusty_amd64__binary'></a>"
+    )
     output += "</td>"
 
     # Kinetic
     output += "<td>"
-    output += "<a href='http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__" + package_name + "__ubuntu_xenial__source/'><img src='http://build.ros.org/buildStatus/icon?job=Ksrc_uX__" + package_name + "__ubuntu_xenial__source'></a>"
+    output += (
+        "<a href='http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__"
+        + package_name
+        + "__ubuntu_xenial__source/'><img src='http://build.ros.org/buildStatus/icon?job=Ksrc_uX__"
+        + package_name
+        + "__ubuntu_xenial__source'></a>"
+    )
     output += "</td><td>"
-    output += "<a href='http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__" + package_name + "__ubuntu_xenial_amd64__binary/'><img src='http://build.ros.org/buildStatus/icon?job=Kbin_uX64__" + package_name + "__ubuntu_xenial_amd64__binary'></a>"
+    output += (
+        "<a href='http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__"
+        + package_name
+        + "__ubuntu_xenial_amd64__binary/'><img src='http://build.ros.org/buildStatus/icon?job=Kbin_uX64__"
+        + package_name
+        + "__ubuntu_xenial_amd64__binary'></a>"
+    )
     output += "</td>"
     return output
 
-def get_first_folder(path) :
-    head,tail = os.path.split(path)
+
+def get_first_folder(path):
+    head, tail = os.path.split(path)
     components = []
-    while len(tail)>0:
-        components.insert(0,tail)
-        head,tail = os.path.split(head)
+    while len(tail) > 0:
+        components.insert(0, tail)
+        head, tail = os.path.split(head)
     return components[0]
 
+
 def populate_package_data(path, package):
-    output = "<td><a href='https://github.com/ros-planning/" + get_first_folder(path) + "'>" + package.name + "</a></td>"
+    output = (
+        "<td><a href='https://github.com/ros-planning/"
+        + get_first_folder(path)
+        + "'>"
+        + package.name
+        + "</a></td>"
+    )
     output += "<td>" + package.version + "</td>"
     output += "<td>"
     first = True
@@ -96,11 +145,12 @@ def populate_package_data(path, package):
     output += create_travis_badge(package.name)
     return output
 
+
 def list_moveit_packages():
     """
     Creates MoveIt! List
     """
-    output = ''
+    output = ""
     packages = find_packages(os.getcwd())
 
     for path, package in packages.items():
@@ -110,9 +160,11 @@ def list_moveit_packages():
 
     # Save to file
     basepath = os.path.dirname(os.path.realpath(__file__))
-    template_file(os.path.join(basepath, 'maintainer_table_template.html'),
-                  os.path.join(basepath, 'index.html'),
-                  {'CONTENTS' : output})
+    template_file(
+        os.path.join(basepath, "maintainer_table_template.html"),
+        os.path.join(basepath, "index.html"),
+        {"CONTENTS": output},
+    )
 
 
 if __name__ == "__main__":
diff --git a/moveit/scripts/create_readme_table.py b/moveit/scripts/create_readme_table.py
index 9a5af08554..db67b94c85 100644
--- a/moveit/scripts/create_readme_table.py
+++ b/moveit/scripts/create_readme_table.py
@@ -8,45 +8,62 @@
 import catkin_pkg
 from catkin_pkg.packages import find_packages
 
+
 def create_header(ros_ubuntu_dict):
-  ros_distros =  sorted(ros_ubuntu_dict.keys())
-  section_header = "## ROS Buildfarm\n"
-  header="MoveIt! Package"
-  header_lines = '-'*len(header)
-  for ros in ros_distros:
-      source = ' '.join([ros.capitalize(), "Source"])
-      debian = ' '.join([ros.capitalize(), "Debian"])
-      header = ' | '.join([header, source, debian])
-      header_lines = ' | '.join([header_lines, '-'*len(source), '-'*len(debian)])
-  return '\n'.join([section_header, header, header_lines])
+    ros_distros = sorted(ros_ubuntu_dict.keys())
+    section_header = "## ROS Buildfarm\n"
+    header = "MoveIt! Package"
+    header_lines = "-" * len(header)
+    for ros in ros_distros:
+        source = " ".join([ros.capitalize(), "Source"])
+        debian = " ".join([ros.capitalize(), "Debian"])
+        header = " | ".join([header, source, debian])
+        header_lines = " | ".join([header_lines, "-" * len(source), "-" * len(debian)])
+    return "\n".join([section_header, header, header_lines])
+
 
 def define_urls(target, params):
-    if target == 'src':
-        params['job'] = "{R}src_u{U}__{package}__ubuntu_{ubuntu}__source".format(**params)
-        params['url'] = "{base_url}/view/{R}src_u{U}/job/{job}".format(**params)
-    elif target == 'bin':
-        params['job'] = "{R}bin_u{U}64__{package}__ubuntu_{ubuntu}_amd64__binary".format(**params)
-        params['url'] = "{base_url}/view/{R}bin_u{U}64/job/{job}".format(**params)
+    if target == "src":
+        params["job"] = "{R}src_u{U}__{package}__ubuntu_{ubuntu}__source".format(
+            **params
+        )
+        params["url"] = "{base_url}/view/{R}src_u{U}/job/{job}".format(**params)
+    elif target == "bin":
+        params[
+            "job"
+        ] = "{R}bin_u{U}64__{package}__ubuntu_{ubuntu}_amd64__binary".format(**params)
+        params["url"] = "{base_url}/view/{R}bin_u{U}64/job/{job}".format(**params)
+
 
 def create_line(package, ros_ubuntu_dict):
     ros_distros = sorted(ros_ubuntu_dict.keys())
-    line = '\n' + package
+    line = "\n" + package
     print(package, file=sys.stderr)
     for ros in ros_distros:
         ubuntu = ros_ubuntu_dict[ros]
-        params = dict(R=ros[0].upper(), U=ubuntu[0].upper(), ubuntu=ubuntu.lower(),
-                      package=package, base_url="http://build.ros.org")
-        for target in ['src', 'bin']:
+        params = dict(
+            R=ros[0].upper(),
+            U=ubuntu[0].upper(),
+            ubuntu=ubuntu.lower(),
+            package=package,
+            base_url="http://build.ros.org",
+        )
+        for target in ["src", "bin"]:
             define_urls(target, params)
-            response = requests.get(params['url']).status_code
+            response = requests.get(params["url"]).status_code
             if response < 400:  # success
-                line += ' | [![Build Status]({base_url}/buildStatus/icon?job={job})]({url})'.format(**params)
+                line += " | [![Build Status]({base_url}/buildStatus/icon?job={job})]({url})".format(
+                    **params
+                )
             else:  # error
-                line += ' | '
-                print('  {}: {} {}'.format(ros, response, params['url']), file=sys.stderr)
+                line += " | "
+                print(
+                    "  {}: {} {}".format(ros, response, params["url"]), file=sys.stderr
+                )
 
     return line
 
+
 def create_moveit_buildfarm_table():
     """
     Creates MoveIt! buildfarm badge table
@@ -55,13 +72,19 @@ def create_moveit_buildfarm_table():
     # 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 = {
+        "indigo": "trusty",
+        "kinetic": "xenial",
+        "melodic": "bionic",
+    }
 
-    all_packages = sorted([package.name for _, package in find_packages(os.getcwd()).items()])
+    all_packages = sorted(
+        [package.name for _, package in find_packages(os.getcwd()).items()]
+    )
     moveit_packages = list()
     other_packages = list()
     for package in all_packages:
-        if package.startswith('moveit'):
+        if package.startswith("moveit"):
             moveit_packages.append(package)
         else:
             other_packages.append(package)
@@ -69,8 +92,9 @@ def create_moveit_buildfarm_table():
 
     buildfarm_table = create_header(supported_distro_ubuntu_dict)
     for package in moveit_packages:
-      buildfarm_table += create_line(package, supported_distro_ubuntu_dict)
+        buildfarm_table += create_line(package, supported_distro_ubuntu_dict)
     print(buildfarm_table)
 
+
 if __name__ == "__main__":
     sys.exit(create_moveit_buildfarm_table())
diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst
index 7df74415a1..901128c64f 100644
--- a/moveit_commander/CHANGELOG.rst
+++ b/moveit_commander/CHANGELOG.rst
@@ -2,6 +2,30 @@
 Changelog for package moveit_commander
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+* Add clear() to Python PSI + Allow empty call to remove_attached_object() (`#2609 <https://github.com/ros-planning/moveit/issues/2609>`_)
+* Add utility functions to Python PSI (`#2532 <https://github.com/ros-planning/moveit/issues/2532>`_)
+* Add get_active_joint_names (`#2533 <https://github.com/ros-planning/moveit/issues/2533>`_)
+* Contributors: Felix von Drigalski, Peter Mitrano, Tyler Weaver
+
+1.0.7 (2020-11-20)
+------------------
+* [feature] Python interface improvements.  (`#2356 <https://github.com/ros-planning/moveit/issues/2356>`_)
+* [feature] Fix ROS namespacing in moveit_commander's PSI (`#2347 <https://github.com/ros-planning/moveit/issues/2347>`_)
+* [feature] MGC: Improve exception messages (`#2318 <https://github.com/ros-planning/moveit/issues/2318>`_)
+* [feature] Add missing variants of place from list of PlaceLocations and Poses in the python interface (`#2231 <https://github.com/ros-planning/moveit/issues/2231>`_)
+* Contributors: Gerard Canal, Michael Görner, Peter Mitrano, Robert Haschke
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint]   Adapt repository for splitted moveit_resources layout (`#2199 <https://github.com/ros-planning/moveit/issues/2199>`_)
diff --git a/moveit_commander/CMakeLists.txt b/moveit_commander/CMakeLists.txt
index 74a0c315af..cb0618035e 100644
--- a/moveit_commander/CMakeLists.txt
+++ b/moveit_commander/CMakeLists.txt
@@ -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/bin/moveit_commander_cmdline.py b/moveit_commander/bin/moveit_commander_cmdline.py
index fd425d3a82..d045270fee 100755
--- a/moveit_commander/bin/moveit_commander_cmdline.py
+++ b/moveit_commander/bin/moveit_commander_cmdline.py
@@ -4,35 +4,43 @@
 
 import roslib
 import rospy
+
 try:
     import readline
 except ImportError:
-    import pyreadline as readline # for Windows
+    import pyreadline as readline  # for Windows
 import sys
 import os
 import signal
 
 import argparse
-from moveit_commander import MoveGroupCommandInterpreter, MoveGroupInfoLevel, roscpp_initialize, roscpp_shutdown
+from moveit_commander import (
+    MoveGroupCommandInterpreter,
+    MoveGroupInfoLevel,
+    roscpp_initialize,
+    roscpp_shutdown,
+)
 
 # python3 has renamed raw_input to input: https://www.python.org/dev/peps/pep-3111
 # Here, we use the new input(). Hence, for python2, we redirect raw_input to input
 try:
     import __builtin__  # This is named builtin in python3
-    input = getattr(__builtin__, 'raw_input')
+
+    input = getattr(__builtin__, "raw_input")
 except (ImportError, AttributeError):
     pass
 
+
 class bcolors:
-    HEADER = '\033[95m'
-    OKBLUE = '\033[94m'
-    OKGREEN = '\033[92m'
-    WARNING = '\033[93m'
-    FAIL = '\033[91m'
-    ENDC = '\033[0m'
+    HEADER = "\033[95m"
+    OKBLUE = "\033[94m"
+    OKGREEN = "\033[92m"
+    WARNING = "\033[93m"
+    FAIL = "\033[91m"
+    ENDC = "\033[0m"
 
-class SimpleCompleter(object):
 
+class SimpleCompleter(object):
     def __init__(self, options):
         self.options = options
 
@@ -52,13 +60,13 @@ def complete(self, text, state):
             # This is the first time for this text, so build a match list.
             if text:
                 if len(prefix) == 0:
-                    self.matches = sorted([s
-                                           for s in self.options.keys()
-                                           if s and s.startswith(text)])
+                    self.matches = sorted(
+                        [s for s in self.options.keys() if s and s.startswith(text)]
+                    )
                 else:
-                    self.matches = sorted([s
-                                           for s in self.options[prefix]
-                                           if s and s.startswith(text)])
+                    self.matches = sorted(
+                        [s for s in self.options[prefix] if s and s.startswith(text)]
+                    )
             else:
                 if len(prefix) == 0:
                     self.matches = sorted(self.options.keys())
@@ -73,6 +81,7 @@ def complete(self, text, state):
             response = None
         return response
 
+
 def print_message(level, msg):
     if level == MoveGroupInfoLevel.FAIL:
         print(bcolors.FAIL + msg + bcolors.ENDC)
@@ -85,11 +94,13 @@ def print_message(level, msg):
     else:
         print(msg)
 
+
 def get_context_keywords(interpreter):
     kw = interpreter.get_keywords()
     kw["quit"] = []
     return kw
 
+
 def run_interactive(group_name):
     c = MoveGroupCommandInterpreter()
     if len(group_name) > 0:
@@ -98,9 +109,13 @@ def run_interactive(group_name):
     readline.set_completer(completer.complete)
 
     print()
-    print(bcolors.HEADER + "Waiting for commands. Type 'help' to get a list of known commands." + bcolors.ENDC)
+    print(
+        bcolors.HEADER
+        + "Waiting for commands. Type 'help' to get a list of known commands."
+        + bcolors.ENDC
+    )
     print()
-    readline.parse_and_bind('tab: complete')
+    readline.parse_and_bind("tab: complete")
 
     while not rospy.is_shutdown():
         cmd = ""
@@ -109,7 +124,7 @@ def run_interactive(group_name):
             ag = c.get_active_group()
             if ag != None:
                 name = ag.get_name()
-            cmd = input(bcolors.OKBLUE + name + '> ' + bcolors.ENDC)
+            cmd = input(bcolors.OKBLUE + name + "> " + bcolors.ENDC)
         except:
             break
         cmdorig = cmd.strip()
@@ -120,7 +135,10 @@ def run_interactive(group_name):
         if cmd == "q" or cmd == "quit" or cmd == "exit":
             break
         if cmd == "host":
-            print_message(MoveGroupInfoLevel.INFO, "Master is '" + os.environ['ROS_MASTER_URI'] + "'")
+            print_message(
+                MoveGroupInfoLevel.INFO,
+                "Master is '" + os.environ["ROS_MASTER_URI"] + "'",
+            )
             continue
 
         (level, msg) = c.execute(cmdorig)
@@ -128,6 +146,7 @@ def run_interactive(group_name):
         # update the set of keywords
         completer.set_options(get_context_keywords(c))
 
+
 def run_service(group_name):
     c = MoveGroupCommandInterpreter()
     if len(group_name) > 0:
@@ -136,30 +155,55 @@ def run_service(group_name):
     print("Running ROS service")
     rospy.spin()
 
+
 def stop_ros(reason):
     rospy.signal_shutdown(reason)
     roscpp_shutdown()
 
+
 def sigint_handler(signal, frame):
     stop_ros("Ctrl+C pressed")
     # this won't actually exit, but trigger an exception to terminate input
     sys.exit(0)
 
-if __name__=='__main__':
+
+if __name__ == "__main__":
 
     signal.signal(signal.SIGINT, sigint_handler)
 
     roscpp_initialize(sys.argv)
 
-    rospy.init_node('move_group_interface_cmdline', anonymous=True, disable_signals=True)
-
-    parser = argparse.ArgumentParser(usage = """%(prog)s [options] [<group_name>]""",
-                                     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,
-                        help="Run the command processing script as a ROS service")
-    parser.add_argument("group_name", type=str, default="", nargs='?', help="Group name to initialize the CLI for.")
+    rospy.init_node(
+        "move_group_interface_cmdline", anonymous=True, disable_signals=True
+    )
+
+    parser = argparse.ArgumentParser(
+        usage="""%(prog)s [options] [<group_name>]""",
+        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,
+        help="Run the command processing script as a ROS service",
+    )
+    parser.add_argument(
+        "group_name",
+        type=str,
+        default="",
+        nargs="?",
+        help="Group name to initialize the CLI for.",
+    )
 
     opt = parser.parse_args(rospy.myargv()[1:])
 
diff --git a/moveit_commander/demos/pick.py b/moveit_commander/demos/pick.py
index 8912ca6067..175400bbae 100755
--- a/moveit_commander/demos/pick.py
+++ b/moveit_commander/demos/pick.py
@@ -36,13 +36,18 @@
 
 import sys
 import rospy
-from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
+from moveit_commander import (
+    RobotCommander,
+    PlanningSceneInterface,
+    roscpp_initialize,
+    roscpp_shutdown,
+)
 from geometry_msgs.msg import PoseStamped
 
-if __name__=='__main__':
+if __name__ == "__main__":
 
     roscpp_initialize(sys.argv)
-    rospy.init_node('moveit_py_demo', anonymous=True)
+    rospy.init_node("moveit_py_demo", anonymous=True)
 
     scene = PlanningSceneInterface()
     robot = RobotCommander()
diff --git a/moveit_commander/demos/plan.py b/moveit_commander/demos/plan.py
index a65a6ae0af..6eee209f0c 100755
--- a/moveit_commander/demos/plan.py
+++ b/moveit_commander/demos/plan.py
@@ -39,25 +39,25 @@
 from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown
 from moveit_msgs.msg import RobotState
 
-if __name__=='__main__':
+if __name__ == "__main__":
 
     roscpp_initialize(sys.argv)
-    rospy.init_node('moveit_py_demo', anonymous=True)
+    rospy.init_node("moveit_py_demo", anonymous=True)
 
     robot = RobotCommander()
     rospy.sleep(1)
 
-    print "Current state:"
-    print robot.get_current_state()
+    print("Current state:")
+    print(robot.get_current_state())
 
     # plan to a random location
     a = robot.right_arm
     a.set_start_state(RobotState())
     r = a.get_random_joint_values()
-    print "Planning to random joint position: "
-    print r
+    print("Planning to random joint position: ")
+    print(r)
     p = a.plan(r)
-    print "Solution:"
-    print p
+    print("Solution:")
+    print(p)
 
     roscpp_shutdown()
diff --git a/moveit_commander/demos/plan_with_constraints.py b/moveit_commander/demos/plan_with_constraints.py
index 8f169ae43a..01b69ed1a1 100755
--- a/moveit_commander/demos/plan_with_constraints.py
+++ b/moveit_commander/demos/plan_with_constraints.py
@@ -40,10 +40,10 @@
 from moveit_msgs.msg import RobotState, Constraints
 from geometry_msgs.msg import Pose
 
-if __name__=='__main__':
+if __name__ == "__main__":
 
     roscpp_initialize(sys.argv)
-    rospy.init_node('moveit_py_demo', anonymous=True)
+    rospy.init_node("moveit_py_demo", anonymous=True)
 
     robot = RobotCommander()
     rospy.sleep(1)
@@ -51,8 +51,8 @@
     a = robot.right_arm
     a.set_start_state(RobotState())
 
-    print "current pose:"
-    print a.get_current_pose()
+    print("current pose:")
+    print(a.get_current_pose())
     c = Constraints()
 
     waypoints = []
@@ -72,4 +72,4 @@
     waypoints.append(wpose)
 
     plan, fraction = a.compute_cartesian_path(waypoints, 0.01, 0.0, path_constraints=c)
-    print 'Plan success percent: ', fraction
+    print("Plan success percent: ", fraction)
diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml
index 852af669d4..5d7e5c2312 100644
--- a/moveit_commander/package.xml
+++ b/moveit_commander/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_commander</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Python interfaces to MoveIt</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_commander/setup.py b/moveit_commander/setup.py
index 531c7291ac..8e1007e775 100644
--- a/moveit_commander/setup.py
+++ b/moveit_commander/setup.py
@@ -2,7 +2,7 @@
 from catkin_pkg.python_setup import generate_distutils_setup
 
 d = generate_distutils_setup()
-d['packages'] = ['moveit_commander']
-d['package_dir'] = {'': 'src'}
+d["packages"] = ["moveit_commander"]
+d["package_dir"] = {"": "src"}
 
 setup(**d)
diff --git a/moveit_commander/src/moveit_commander/conversions.py b/moveit_commander/src/moveit_commander/conversions.py
index f9129ffd8f..dcf884aa47 100644
--- a/moveit_commander/src/moveit_commander/conversions.py
+++ b/moveit_commander/src/moveit_commander/conversions.py
@@ -44,14 +44,17 @@
 import rospy
 import tf
 
+
 def msg_to_string(msg):
     buf = StringIO()
     msg.serialize(buf)
     return buf.getvalue()
 
+
 def msg_from_string(msg, data):
     msg.deserialize(data)
 
+
 def pose_to_list(pose_msg):
     pose = []
     pose.append(pose_msg.position.x)
@@ -63,6 +66,7 @@ def pose_to_list(pose_msg):
     pose.append(pose_msg.orientation.w)
     return pose
 
+
 def list_to_pose(pose_list):
     pose_msg = Pose()
     if len(pose_list) == 7:
@@ -77,15 +81,20 @@ def list_to_pose(pose_list):
         pose_msg.position.x = pose_list[0]
         pose_msg.position.y = pose_list[1]
         pose_msg.position.z = pose_list[2]
-        q = tf.transformations.quaternion_from_euler(pose_list[3], pose_list[4], pose_list[5])
+        q = tf.transformations.quaternion_from_euler(
+            pose_list[3], pose_list[4], pose_list[5]
+        )
         pose_msg.orientation.x = q[0]
         pose_msg.orientation.y = q[1]
         pose_msg.orientation.z = q[2]
         pose_msg.orientation.w = q[3]
     else:
-        raise MoveItCommanderException("Expected either 6 or 7 elements in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)")
+        raise MoveItCommanderException(
+            "Expected either 6 or 7 elements in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)"
+        )
     return pose_msg
 
+
 def list_to_pose_stamped(pose_list, target_frame):
     pose_msg = PoseStamped()
     pose_msg.pose = list_to_pose(pose_list)
@@ -93,6 +102,7 @@ def list_to_pose_stamped(pose_list, target_frame):
     pose_msg.header.stamp = rospy.Time.now()
     return pose_msg
 
+
 def transform_to_list(trf_msg):
     trf = []
     trf.append(trf_msg.translation.x)
@@ -104,6 +114,7 @@ def transform_to_list(trf_msg):
     trf.append(trf_msg.rotation.w)
     return trf
 
+
 def list_to_transform(trf_list):
     trf_msg = Transform()
     trf_msg.translation.x = trf_list[0]
diff --git a/moveit_commander/src/moveit_commander/exception.py b/moveit_commander/src/moveit_commander/exception.py
index cbd7fd1e43..8b48333897 100644
--- a/moveit_commander/src/moveit_commander/exception.py
+++ b/moveit_commander/src/moveit_commander/exception.py
@@ -32,4 +32,6 @@
 #
 # Author: Ioan Sucan
 
-class MoveItCommanderException(Exception): pass
+
+class MoveItCommanderException(Exception):
+    pass
diff --git a/moveit_commander/src/moveit_commander/interpreter.py b/moveit_commander/src/moveit_commander/interpreter.py
index a7f6027b2a..d1eaf9355e 100644
--- a/moveit_commander/src/moveit_commander/interpreter.py
+++ b/moveit_commander/src/moveit_commander/interpreter.py
@@ -33,13 +33,19 @@
 # Author: Ioan Sucan
 
 import rospy
-from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface, MoveItCommanderException
+from moveit_commander import (
+    RobotCommander,
+    MoveGroupCommander,
+    PlanningSceneInterface,
+    MoveItCommanderException,
+)
 from geometry_msgs.msg import Pose, PoseStamped
 import tf
 import re
 import time
 import os.path
 
+
 class MoveGroupInfoLevel:
     FAIL = 1
     WARN = 2
@@ -47,15 +53,25 @@ class MoveGroupInfoLevel:
     INFO = 4
     DEBUG = 5
 
+
 class MoveGroupCommandInterpreter(object):
     """
     Interpreter for simple commands
     """
 
     DEFAULT_FILENAME = "move_group.cfg"
-    GO_DIRS = {"up" : (2,1), "down" : (2, -1), "z" : (2, 1),
-               "left" : (1, 1), "right" : (1, -1), "y" : (1, 1),
-               "forward" : (0, 1), "backward" : (0, -1), "back" : (0, -1), "x" : (0, 1) }
+    GO_DIRS = {
+        "up": (2, 1),
+        "down": (2, -1),
+        "z": (2, 1),
+        "left": (1, 1),
+        "right": (1, -1),
+        "y": (1, 1),
+        "forward": (0, 1),
+        "backward": (0, -1),
+        "back": (0, -1),
+        "x": (0, 1),
+    }
 
     def __init__(self):
         self._gdict = {}
@@ -86,7 +102,11 @@ def execute(self, cmd):
             if len(self._group_name) > 0:
                 return self.execute_group_command(self._gdict[self._group_name], cmd)
             else:
-                return (MoveGroupInfoLevel.INFO, self.get_help() + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n")
+                return (
+                    MoveGroupInfoLevel.INFO,
+                    self.get_help()
+                    + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n",
+                )
 
     def execute_generic_command(self, cmd):
         if os.path.isfile("cmd/" + cmd):
@@ -114,12 +134,21 @@ def execute_generic_command(self, cmd):
                             self._group_name = clist[1]
                             return (MoveGroupInfoLevel.DEBUG, "OK")
                         except MoveItCommanderException as e:
-                            return (MoveGroupInfoLevel.FAIL, "Initializing " + clist[1] + ": " + str(e))
+                            return (
+                                MoveGroupInfoLevel.FAIL,
+                                "Initializing " + clist[1] + ": " + str(e),
+                            )
                         except:
-                            return (MoveGroupInfoLevel.FAIL, "Unable to initialize " + clist[1])
+                            return (
+                                MoveGroupInfoLevel.FAIL,
+                                "Unable to initialize " + clist[1],
+                            )
         elif cmdlow.startswith("trace"):
             if cmdlow == "trace":
-                return (MoveGroupInfoLevel.INFO, "trace is on" if self._trace else "trace is off")
+                return (
+                    MoveGroupInfoLevel.INFO,
+                    "trace is on" if self._trace else "trace is off",
+                )
             clist = cmdlow.split()
             if clist[0] == "trace" and clist[1] == "on":
                 self._trace = True
@@ -139,28 +168,38 @@ def execute_generic_command(self, cmd):
             line_num = 0
             line_content = ""
             try:
-                f = open(filename, 'r')
+                f = open(filename, "r")
                 for line in f:
                     line_num += 1
                     line = line.rstrip()
                     line_content = line
                     if self._trace:
-                        print ("%s:%d:  %s" % (filename, line_num, line_content))
+                        print("%s:%d:  %s" % (filename, line_num, line_content))
                     comment = line.find("#")
                     if comment != -1:
-                      line = line[0:comment].rstrip()
+                        line = line[0:comment].rstrip()
                     if line != "":
-                      self.execute(line.rstrip())
+                        self.execute(line.rstrip())
                     line_content = ""
                 return (MoveGroupInfoLevel.DEBUG, "OK")
             except MoveItCommanderException as e:
                 if line_num > 0:
-                    return (MoveGroupInfoLevel.WARN, "Error at %s:%d:  %s\n%s" % (filename, line_num, line_content, str(e)))
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Error at %s:%d:  %s\n%s"
+                        % (filename, line_num, line_content, str(e)),
+                    )
                 else:
-                    return (MoveGroupInfoLevel.WARN, "Processing " + filename + ": " + str(e))
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Processing " + filename + ": " + str(e),
+                    )
             except:
                 if line_num > 0:
-                    return (MoveGroupInfoLevel.WARN, "Error at %s:%d:  %s" % (filename, line_num, line_content))
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Error at %s:%d:  %s" % (filename, line_num, line_content),
+                    )
                 else:
                     return (MoveGroupInfoLevel.WARN, "Unable to load " + filename)
         elif cmdlow.startswith("save"):
@@ -171,14 +210,18 @@ def execute_generic_command(self, cmd):
             if len(clist) == 2:
                 filename = clist[1]
             try:
-                f = open(filename, 'w')
+                f = open(filename, "w")
                 for gr in self._gdict.keys():
                     f.write("use " + gr + "\n")
                     known = self._gdict[gr].get_remembered_joint_values()
                     for v in known.keys():
-                        f.write(v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n")
+                        f.write(
+                            v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n"
+                        )
                 if self._db_host != None:
-                    f.write("database " + self._db_host + " " + str(self._db_port) + "\n")
+                    f.write(
+                        "database " + self._db_host + " " + str(self._db_port) + "\n"
+                    )
                 return (MoveGroupInfoLevel.DEBUG, "OK")
             except:
                 return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
@@ -204,7 +247,12 @@ def execute_group_command(self, g, cmdorig):
 
         if cmd == "joints":
             joints = g.get_joints()
-            return (MoveGroupInfoLevel.INFO, "\n" + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))]) + "\n")
+            return (
+                MoveGroupInfoLevel.INFO,
+                "\n"
+                + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))])
+                + "\n",
+            )
 
         if cmd == "show":
             return self.command_show(g)
@@ -224,7 +272,9 @@ def execute_group_command(self, g, cmdorig):
             pose.pose.orientation.w = 1
             pose.header.stamp = rospy.get_rostime()
             pose.header.frame_id = self._robot.get_root_link()
-            self._planning_scene_interface.attach_box(self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
+            self._planning_scene_interface.attach_box(
+                self._robot.get_root_link(), "ground", pose, (3, 3, 0.1)
+            )
             return (MoveGroupInfoLevel.INFO, "Added ground")
 
         if cmd == "eef":
@@ -237,7 +287,10 @@ def execute_group_command(self, g, cmdorig):
             if self._db_host == None:
                 return (MoveGroupInfoLevel.INFO, "Not connected to a database")
             else:
-                return (MoveGroupInfoLevel.INFO, "Connected to " + self._db_host + ":" + str(self._db_port))
+                return (
+                    MoveGroupInfoLevel.INFO,
+                    "Connected to " + self._db_host + ":" + str(self._db_port),
+                )
         if cmd == "plan":
             if self._last_plan != None:
                 return (MoveGroupInfoLevel.INFO, str(self._last_plan))
@@ -249,7 +302,11 @@ def execute_group_command(self, g, cmdorig):
             return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")
 
         if cmd == "tol" or cmd == "tolerance":
-            return (MoveGroupInfoLevel.INFO, "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" % g.get_goal_tolerance())
+            return (
+                MoveGroupInfoLevel.INFO,
+                "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g"
+                % g.get_goal_tolerance(),
+            )
 
         if cmd == "time":
             return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))
@@ -259,7 +316,10 @@ def execute_group_command(self, g, cmdorig):
                 if g.execute(self._last_plan):
                     return (MoveGroupInfoLevel.SUCCESS, "Plan submitted for execution")
                 else:
-                    return (MoveGroupInfoLevel.WARN, "Failed to submit plan for execution")
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Failed to submit plan for execution",
+                    )
             else:
                 return (MoveGroupInfoLevel.WARN, "No motion plan computed")
 
@@ -268,8 +328,15 @@ def execute_group_command(self, g, cmdorig):
         if assign_match:
             known = g.get_remembered_joint_values()
             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))
+                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:
                 return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
 
@@ -277,13 +344,26 @@ def execute_group_command(self, g, cmdorig):
         set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
         if set_match:
             try:
-                g.remember_joint_values(set_match.group(1), [float(x) for x in set_match.group(2).split()])
-                return (MoveGroupInfoLevel.SUCCESS, "Remembered joint values [" + set_match.group(2) + "] under the name " + set_match.group(1))
+                g.remember_joint_values(
+                    set_match.group(1), [float(x) for x in set_match.group(2).split()]
+                )
+                return (
+                    MoveGroupInfoLevel.SUCCESS,
+                    "Remembered joint values ["
+                    + set_match.group(2)
+                    + "] under the name "
+                    + set_match.group(1),
+                )
             except:
-                return (MoveGroupInfoLevel.WARN, "Unable to parse joint value [" + set_match.group(2) + "]")
+                return (
+                    MoveGroupInfoLevel.WARN,
+                    "Unable to parse joint value [" + set_match.group(2) + "]",
+                )
 
         # see if we have assignment of matlab-like element update
-        component_match = re.match(r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd)
+        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 component_match.group(1) in known:
@@ -291,9 +371,19 @@ def execute_group_command(self, g, cmdorig):
                     val = known[component_match.group(1)]
                     val[int(component_match.group(2))] = float(component_match.group(3))
                     g.remember_joint_values(component_match.group(1), val)
-                    return (MoveGroupInfoLevel.SUCCESS, "Updated " + component_match.group(1) + "[" + component_match.group(2) + "]")
+                    return (
+                        MoveGroupInfoLevel.SUCCESS,
+                        "Updated "
+                        + component_match.group(1)
+                        + "["
+                        + component_match.group(2)
+                        + "]",
+                    )
                 except:
-                    return (MoveGroupInfoLevel.WARN, "Unable to parse index or value in '" + cmd +"'")
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Unable to parse index or value in '" + cmd + "'",
+                    )
             else:
                 return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
 
@@ -304,7 +394,10 @@ def execute_group_command(self, g, cmdorig):
         if len(clist) == 1:
             known = g.get_remembered_joint_values()
             if cmd in known:
-                return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[cmd]]) + "]")
+                return (
+                    MoveGroupInfoLevel.INFO,
+                    "[" + " ".join([str(x) for x in known[cmd]]) + "]",
+                )
             else:
                 return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
 
@@ -316,18 +409,34 @@ def execute_group_command(self, g, cmdorig):
                     vals = g.get_random_joint_values()
                     g.set_joint_value_target(vals)
                     if g.go():
-                        return (MoveGroupInfoLevel.SUCCESS, "Moved to random target [" + " ".join([str(x) for x in vals]) + "]")
+                        return (
+                            MoveGroupInfoLevel.SUCCESS,
+                            "Moved to random target ["
+                            + " ".join([str(x) for x in vals])
+                            + "]",
+                        )
                     else:
-                        return (MoveGroupInfoLevel.FAIL, "Failed while moving to random target [" + " ".join([str(x) for x in vals]) + "]")
+                        return (
+                            MoveGroupInfoLevel.FAIL,
+                            "Failed while moving to random target ["
+                            + " ".join([str(x) for x in vals])
+                            + "]",
+                        )
                 else:
                     try:
                         g.set_named_target(clist[1])
                         if g.go():
                             return (MoveGroupInfoLevel.SUCCESS, "Moved to " + clist[1])
                         else:
-                            return (MoveGroupInfoLevel.FAIL, "Failed while moving to " + clist[1])
+                            return (
+                                MoveGroupInfoLevel.FAIL,
+                                "Failed while moving to " + clist[1],
+                            )
                     except MoveItCommanderException as e:
-                        return (MoveGroupInfoLevel.WARN, "Going to " + clist[1] + ": " + str(e))
+                        return (
+                            MoveGroupInfoLevel.WARN,
+                            "Going to " + clist[1] + ": " + str(e),
+                        )
                     except:
                         return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
             if clist[0] == "plan":
@@ -342,11 +451,17 @@ def execute_group_command(self, g, cmdorig):
                         g.set_named_target(clist[1])
                         self._last_plan = g.plan()
                     except MoveItCommanderException as e:
-                        return (MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e))
+                        return (
+                            MoveGroupInfoLevel.WARN,
+                            "Planning to " + clist[1] + ": " + str(e),
+                        )
                     except:
                         return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
                 if self._last_plan != None:
-                    if len(self._last_plan.joint_trajectory.points) == 0 and len(self._last_plan.multi_dof_joint_trajectory.points) == 0:
+                    if (
+                        len(self._last_plan.joint_trajectory.points) == 0
+                        and len(self._last_plan.multi_dof_joint_trajectory.points) == 0
+                    ):
                         self._last_plan = None
                 dest_str = clist[1]
                 if vals != None:
@@ -354,56 +469,89 @@ def execute_group_command(self, g, cmdorig):
                 if self._last_plan != None:
                     return (MoveGroupInfoLevel.SUCCESS, "Planned to " + dest_str)
                 else:
-                    return (MoveGroupInfoLevel.FAIL, "Failed while planning to " + dest_str)
+                    return (
+                        MoveGroupInfoLevel.FAIL,
+                        "Failed while planning to " + dest_str,
+                    )
             elif clist[0] == "pick":
                 self._last_plan = None
                 if g.pick(clist[1]):
                     return (MoveGroupInfoLevel.SUCCESS, "Picked object " + clist[1])
                 else:
-                    return (MoveGroupInfoLevel.FAIL, "Failed while trying to pick object " + clist[1])
+                    return (
+                        MoveGroupInfoLevel.FAIL,
+                        "Failed while trying to pick object " + clist[1],
+                    )
             elif clist[0] == "place":
                 self._last_plan = None
                 if g.place(clist[1]):
                     return (MoveGroupInfoLevel.SUCCESS, "Placed object " + clist[1])
                 else:
-                    return (MoveGroupInfoLevel.FAIL, "Failed while trying to place object " + clist[1])
+                    return (
+                        MoveGroupInfoLevel.FAIL,
+                        "Failed while trying to place object " + clist[1],
+                    )
             elif clist[0] == "planner":
                 g.set_planner_id(clist[1])
                 return (MoveGroupInfoLevel.SUCCESS, "Planner is now " + clist[1])
             elif clist[0] == "record" or clist[0] == "rec":
                 g.remember_joint_values(clist[1])
-                return (MoveGroupInfoLevel.SUCCESS, "Remembered current joint values under the name " + clist[1])
+                return (
+                    MoveGroupInfoLevel.SUCCESS,
+                    "Remembered current joint values under the name " + clist[1],
+                )
             elif clist[0] == "rand" or clist[0] == "random":
                 g.remember_joint_values(clist[1], g.get_random_joint_values())
-                return (MoveGroupInfoLevel.SUCCESS, "Remembered random joint values under the name " + clist[1])
+                return (
+                    MoveGroupInfoLevel.SUCCESS,
+                    "Remembered random joint values under the name " + clist[1],
+                )
             elif clist[0] == "del" or clist[0] == "delete":
                 g.forget_joint_values(clist[1])
-                return (MoveGroupInfoLevel.SUCCESS, "Forgot joint values under the name " + clist[1])
+                return (
+                    MoveGroupInfoLevel.SUCCESS,
+                    "Forgot joint values under the name " + clist[1],
+                )
             elif clist[0] == "show":
                 known = g.get_remembered_joint_values()
                 if clist[1] in known:
-                    return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[clist[1]]]) + "]")
+                    return (
+                        MoveGroupInfoLevel.INFO,
+                        "[" + " ".join([str(x) for x in known[clist[1]]]) + "]",
+                    )
                 else:
-                    return (MoveGroupInfoLevel.WARN, "Joint values for " + clist[1] + " are not known")
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Joint values for " + clist[1] + " are not known",
+                    )
             elif clist[0] == "tol" or clist[0] == "tolerance":
                 try:
                     g.set_goal_tolerance(float(clist[1]))
                     return (MoveGroupInfoLevel.SUCCESS, "OK")
                 except:
-                    return (MoveGroupInfoLevel.WARN, "Unable to parse tolerance value '" + clist[1] + "'")
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Unable to parse tolerance value '" + clist[1] + "'",
+                    )
             elif clist[0] == "time":
                 try:
                     g.set_planning_time(float(clist[1]))
                     return (MoveGroupInfoLevel.SUCCESS, "OK")
                 except:
-                    return (MoveGroupInfoLevel.WARN, "Unable to parse planning duration value '" + clist[1] + "'")
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Unable to parse planning duration value '" + clist[1] + "'",
+                    )
             elif clist[0] == "constrain":
                 try:
                     g.set_path_constraints(clist[1])
                     return (MoveGroupInfoLevel.SUCCESS, "OK")
                 except:
                     if self._db_host != None:
-                        return (MoveGroupInfoLevel.WARN, "Constraint " + clist[1] + " is not known.")
+                        return (
+                            MoveGroupInfoLevel.WARN,
+                            "Constraint " + clist[1] + " is not known.",
+                        )
                     else:
                         return (MoveGroupInfoLevel.WARN, "Not connected to a database.")
             elif clist[0] == "wait":
@@ -411,14 +559,27 @@ def execute_group_command(self, g, cmdorig):
                     time.sleep(float(clist[1]))
                     return (MoveGroupInfoLevel.SUCCESS, clist[1] + " seconds passed")
                 except:
-                    return (MoveGroupInfoLevel.WARN, "Unable to wait '" + clist[1] + "' seconds")
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Unable to wait '" + clist[1] + "' seconds",
+                    )
             elif clist[0] == "database":
                 try:
                     g.set_constraints_database(clist[1], self._db_port)
                     self._db_host = clist[1]
-                    return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
+                    return (
+                        MoveGroupInfoLevel.SUCCESS,
+                        "Connected to " + self._db_host + ":" + str(self._db_port),
+                    )
                 except:
-                    return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + str(self._db_port) + "'")
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Unable to connect to '"
+                        + clist[1]
+                        + ":"
+                        + str(self._db_port)
+                        + "'",
+                    )
             else:
                 return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
 
@@ -430,17 +591,35 @@ def execute_group_command(self, g, cmdorig):
                     (axis, factor) = self.GO_DIRS[clist[1]]
                     return self.command_go_offset(g, offset, factor, axis, clist[1])
                 except MoveItCommanderException as e:
-                    return (MoveGroupInfoLevel.WARN, "Going " + clist[1] + ": " + str(e))
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Going " + clist[1] + ": " + str(e),
+                    )
                 except:
-                    return (MoveGroupInfoLevel.WARN, "Unable to parse distance '" + clist[2] + "'")
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Unable to parse distance '" + clist[2] + "'",
+                    )
             elif clist[0] == "allow" and (clist[1] == "look" or clist[1] == "looking"):
-                if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
+                if (
+                    clist[2] == "1"
+                    or clist[2] == "true"
+                    or clist[2] == "T"
+                    or clist[2] == "True"
+                ):
                     g.allow_looking(True)
                 else:
                     g.allow_looking(False)
                 return (MoveGroupInfoLevel.DEBUG, "OK")
-            elif clist[0] == "allow" and (clist[1] == "replan" or clist[1] == "replanning"):
-                if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
+            elif clist[0] == "allow" and (
+                clist[1] == "replan" or clist[1] == "replanning"
+            ):
+                if (
+                    clist[2] == "1"
+                    or clist[2] == "true"
+                    or clist[2] == "T"
+                    or clist[2] == "True"
+                ):
                     g.allow_replanning(True)
                 else:
                     g.allow_replanning(False)
@@ -450,10 +629,16 @@ def execute_group_command(self, g, cmdorig):
                     g.set_constraints_database(clist[1], int(clist[2]))
                     self._db_host = clist[1]
                     self._db_port = int(clist[2])
-                    return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
+                    return (
+                        MoveGroupInfoLevel.SUCCESS,
+                        "Connected to " + self._db_host + ":" + str(self._db_port),
+                    )
                 except:
                     self._db_host = None
-                    return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + clist[2] + "'")
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Unable to connect to '" + clist[1] + ":" + clist[2] + "'",
+                    )
         if len(clist) == 4:
             if clist[0] == "rotate":
                 try:
@@ -461,24 +646,34 @@ def execute_group_command(self, g, cmdorig):
                     if g.go():
                         return (MoveGroupInfoLevel.SUCCESS, "Rotation complete")
                     else:
-                        return (MoveGroupInfoLevel.FAIL, "Failed while rotating to " + " ".join(clist[1:]))
+                        return (
+                            MoveGroupInfoLevel.FAIL,
+                            "Failed while rotating to " + " ".join(clist[1:]),
+                        )
                 except MoveItCommanderException as e:
                     return (MoveGroupInfoLevel.WARN, str(e))
                 except:
-                    return (MoveGroupInfoLevel.WARN, "Unable to parse X-Y-Z rotation  values '" + " ".join(clist[1:]) + "'")
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Unable to parse X-Y-Z rotation  values '"
+                        + " ".join(clist[1:])
+                        + "'",
+                    )
         if len(clist) >= 7:
             if clist[0] == "go":
                 self._last_plan = None
                 approx = False
-                if (len(clist) > 7):
-                    if (clist[7] == "approx" or clist[7] == "approximate"):
+                if len(clist) > 7:
+                    if clist[7] == "approx" or clist[7] == "approximate":
                         approx = True
                 try:
                     p = Pose()
                     p.position.x = float(clist[1])
                     p.position.y = float(clist[2])
                     p.position.z = float(clist[3])
-                    q = tf.transformations.quaternion_from_euler(float(clist[4]), float(clist[5]), float(clist[6]))
+                    q = tf.transformations.quaternion_from_euler(
+                        float(clist[4]), float(clist[5]), float(clist[6])
+                    )
                     p.orientation.x = q[0]
                     p.orientation.y = q[1]
                     p.orientation.z = q[2]
@@ -488,13 +683,25 @@ def execute_group_command(self, g, cmdorig):
                     else:
                         g.set_pose_target(p)
                     if g.go():
-                        return (MoveGroupInfoLevel.SUCCESS, "Moved to pose target\n%s\n" % (str(p)))
+                        return (
+                            MoveGroupInfoLevel.SUCCESS,
+                            "Moved to pose target\n%s\n" % (str(p)),
+                        )
                     else:
-                        return (MoveGroupInfoLevel.FAIL, "Failed while moving to pose \n%s\n" % (str(p)))
+                        return (
+                            MoveGroupInfoLevel.FAIL,
+                            "Failed while moving to pose \n%s\n" % (str(p)),
+                        )
                 except MoveItCommanderException as e:
-                    return (MoveGroupInfoLevel.WARN, "Going to pose target: %s" % (str(e)))
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Going to pose target: %s" % (str(e)),
+                    )
                 except:
-                    return (MoveGroupInfoLevel.WARN, "Unable to parse pose '" + " ".join(clist[1:]) + "'")
+                    return (
+                        MoveGroupInfoLevel.WARN,
+                        "Unable to parse pose '" + " ".join(clist[1:]) + "'",
+                    )
 
         return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
 
@@ -506,10 +713,27 @@ def command_show(self, g):
         return (MoveGroupInfoLevel.INFO, "\n".join(res))
 
     def command_current(self, g):
-        res = "joints = [" + " ".join([str(x) for x in g.get_current_joint_values()]) + "]"
+        res = (
+            "joints = ["
+            + " ".join([str(x) for x in g.get_current_joint_values()])
+            + "]"
+        )
         if len(g.get_end_effector_link()) > 0:
-            res = res + "\n" + g.get_end_effector_link() + " pose = [\n" + str(g.get_current_pose()) + " ]"
-            res = res + "\n" + g.get_end_effector_link() + " RPY = " + str(g.get_current_rpy())
+            res = (
+                res
+                + "\n"
+                + g.get_end_effector_link()
+                + " pose = [\n"
+                + str(g.get_current_pose())
+                + " ]"
+            )
+            res = (
+                res
+                + "\n"
+                + g.get_end_effector_link()
+                + " RPY = "
+                + str(g.get_current_rpy())
+            )
         return (MoveGroupInfoLevel.INFO, res)
 
     def command_go_offset(self, g, offset, factor, dimension_index, direction_name):
@@ -517,15 +741,24 @@ def command_go_offset(self, g, offset, factor, dimension_index, direction_name):
             try:
                 g.shift_pose_target(dimension_index, factor * offset)
                 if g.go():
-                    return (MoveGroupInfoLevel.SUCCESS, "Moved " + direction_name + " by " + str(offset) + " m")
+                    return (
+                        MoveGroupInfoLevel.SUCCESS,
+                        "Moved " + direction_name + " by " + str(offset) + " m",
+                    )
                 else:
-                    return (MoveGroupInfoLevel.FAIL, "Failed while moving " + direction_name)
+                    return (
+                        MoveGroupInfoLevel.FAIL,
+                        "Failed while moving " + direction_name,
+                    )
             except MoveItCommanderException as e:
                 return (MoveGroupInfoLevel.WARN, str(e))
             except:
                 return (MoveGroupInfoLevel.WARN, "Unable to process pose update")
         else:
-            return (MoveGroupInfoLevel.WARN, "No known end effector. Cannot move " + direction_name)
+            return (
+                MoveGroupInfoLevel.WARN,
+                "No known end effector. Cannot move " + direction_name,
+            )
 
     def resolve_command_alias(self, cmdorig):
         cmd = cmdorig.lower()
@@ -542,36 +775,68 @@ def get_help(self):
         res.append("  allow looking <true|false>       enable/disable looking around")
         res.append("  allow replanning <true|false>    enable/disable replanning")
         res.append("  constrain           clear path constraints")
-        res.append("  constrain <name>    use the constraint <name> as a path constraint")
+        res.append(
+            "  constrain <name>    use the constraint <name> as a path constraint"
+        )
         res.append("  current             show the current state of the active group")
-        res.append("  database            display the current database connection (if any)")
-        res.append("  delete <name>       forget the joint values under the name <name>")
-        res.append("  eef                 print the name of the end effector attached to the current group")
+        res.append(
+            "  database            display the current database connection (if any)"
+        )
+        res.append(
+            "  delete <name>       forget the joint values under the name <name>"
+        )
+        res.append(
+            "  eef                 print the name of the end effector attached to the current group"
+        )
         res.append("  execute             execute a previously computed motion plan")
-        res.append("  go <name>           plan and execute a motion to the state <name>")
+        res.append(
+            "  go <name>           plan and execute a motion to the state <name>"
+        )
         res.append("  go rand             plan and execute a motion to a random state")
-        res.append("  go <dir> <dx>|      plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>")
+        res.append(
+            "  go <dir> <dx>|      plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>"
+        )
         res.append("  ground              add a ground plane to the planning scene")
-        res.append("  id|which            display the name of the group that is operated on")
-        res.append("  joints              display names of the joints in the active group")
-        res.append("  load [<file>]       load a set of interpreted commands from a file")
+        res.append(
+            "  id|which            display the name of the group that is operated on"
+        )
+        res.append(
+            "  joints              display names of the joints in the active group"
+        )
+        res.append(
+            "  load [<file>]       load a set of interpreted commands from a file"
+        )
         res.append("  pick <name>         pick up object <name>")
         res.append("  place <name>        place object <name>")
         res.append("  plan <name>         plan a motion to the state <name>")
         res.append("  plan rand           plan a motion to a random state")
         res.append("  planner <name>      use planner <name> to plan next motion")
-        res.append("  record <name>       record the current joint values under the name <name>")
-        res.append("  rotate <x> <y> <z>  plan and execute a motion to a specified orientation (about the X,Y,Z axes)")
-        res.append("  save [<file>]       save the currently known variables as a set of commands")
-        res.append("  show                display the names and values of the known states")
+        res.append(
+            "  record <name>       record the current joint values under the name <name>"
+        )
+        res.append(
+            "  rotate <x> <y> <z>  plan and execute a motion to a specified orientation (about the X,Y,Z axes)"
+        )
+        res.append(
+            "  save [<file>]       save the currently known variables as a set of commands"
+        )
+        res.append(
+            "  show                display the names and values of the known states"
+        )
         res.append("  show <name>         display the value of a state")
         res.append("  stop                stop the active group")
         res.append("  time                show the configured allowed planning time")
         res.append("  time <val>          set the allowed planning time")
-        res.append("  tolerance           show the tolerance for reaching the goal region")
-        res.append("  tolerance <val>     set the tolerance for reaching the goal region")
+        res.append(
+            "  tolerance           show the tolerance for reaching the goal region"
+        )
+        res.append(
+            "  tolerance <val>     set the tolerance for reaching the goal region"
+        )
         res.append("  trace <on|off>      enable/disable replanning or looking around")
-        res.append("  use <name>          switch to using the group named <name> (and load it if necessary)")
+        res.append(
+            "  use <name>          switch to using the group named <name> (and load it if necessary)"
+        )
         res.append("  use|groups          show the group names that are already loaded")
         res.append("  vars                display the names of the known states")
         res.append("  wait <dt>           sleep for <dt> seconds")
@@ -587,27 +852,30 @@ 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'] + list(known_vars),
-                'help':[],
-                'record':known_vars,
-                'show':known_vars,
-                'wait':[],
-                'delete':known_vars,
-                'database': [],
-                'current':[],
-                'use':groups,
-                'load':[],
-                'save':[],
-                'pick':[],
-                'place':[],
-                'plan':known_vars,
-                'allow':['replanning', 'looking'],
-                'constrain':known_constr,
-                'vars':[],
-                'joints':[],
-                'tolerance':[],
-                'time':[],
-                'eef':[],
-                'execute':[],
-                'ground':[],
-                'id':[]}
+        return {
+            "go": ["up", "down", "left", "right", "backward", "forward", "random"]
+            + list(known_vars),
+            "help": [],
+            "record": known_vars,
+            "show": known_vars,
+            "wait": [],
+            "delete": known_vars,
+            "database": [],
+            "current": [],
+            "use": groups,
+            "load": [],
+            "save": [],
+            "pick": [],
+            "place": [],
+            "plan": known_vars,
+            "allow": ["replanning", "looking"],
+            "constrain": known_constr,
+            "vars": [],
+            "joints": [],
+            "tolerance": [],
+            "time": [],
+            "eef": [],
+            "execute": [],
+            "ground": [],
+            "id": [],
+        }
diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py
index d10bebab17..7a7f95a21e 100644
--- a/moveit_commander/src/moveit_commander/move_group.py
+++ b/moveit_commander/src/moveit_commander/move_group.py
@@ -33,8 +33,18 @@
 # Author: Ioan Sucan, William Baker
 
 from geometry_msgs.msg import Pose, PoseStamped
-from moveit_msgs.msg import RobotTrajectory, Grasp, PlaceLocation, Constraints, RobotState
-from moveit_msgs.msg import MoveItErrorCodes, TrajectoryConstraints, PlannerInterfaceDescription
+from moveit_msgs.msg import (
+    RobotTrajectory,
+    Grasp,
+    PlaceLocation,
+    Constraints,
+    RobotState,
+)
+from moveit_msgs.msg import (
+    MoveItErrorCodes,
+    TrajectoryConstraints,
+    PlannerInterfaceDescription,
+)
 from sensor_msgs.msg import JointState
 import rospy
 import tf
@@ -42,79 +52,88 @@
 from .exception import MoveItCommanderException
 import moveit_commander.conversions as conversions
 
+
 class MoveGroupCommander(object):
     """
     Execution of simple commands for a particular group
     """
 
-    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, wait_for_servers)
+    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, wait_for_servers
+        )
 
     def get_name(self):
-        """ Get the name of the group this instance was initialized for """
+        """Get the name of the group this instance was initialized for"""
         return self._g.get_name()
 
     def stop(self):
-        """ Stop the current execution, if any """
+        """Stop the current execution, if any"""
         self._g.stop()
 
     def get_active_joints(self):
-        """ Get the active joints of this group """
+        """Get the active joints of this group"""
         return self._g.get_active_joints()
 
     def get_joints(self):
-        """ Get the joints of this group """
+        """Get the joints of this group"""
         return self._g.get_joints()
 
     def get_variable_count(self):
-        """ Return the number of variables used to parameterize a state in this group (larger or equal to number of DOF)"""
+        """Return the number of variables used to parameterize a state in this group (larger or equal to number of DOF)"""
         return self._g.get_variable_count()
 
     def has_end_effector_link(self):
-        """ Check if this group has a link that is considered to be an end effector """
+        """Check if this group has a link that is considered to be an end effector"""
         return len(self._g.get_end_effector_link()) > 0
 
     def get_end_effector_link(self):
-        """ Get the name of the link that is considered to be an end-effector. Return an empty string if there is no end-effector. """
+        """Get the name of the link that is considered to be an end-effector. Return an empty string if there is no end-effector."""
         return self._g.get_end_effector_link()
 
     def set_end_effector_link(self, link_name):
-        """ Set the name of the link to be considered as an end effector """
+        """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) """
+        """Get the description of the planner interface (list of planner ids)"""
         desc = PlannerInterfaceDescription()
         conversions.msg_from_string(desc, self._g.get_interface_description())
         return desc
 
     def get_pose_reference_frame(self):
-        """ Get the reference frame assumed for poses of end-effectors """
+        """Get the reference frame assumed for poses of end-effectors"""
         return self._g.get_pose_reference_frame()
 
     def set_pose_reference_frame(self, reference_frame):
-        """ Set the reference frame to assume for poses of end-effectors """
+        """Set the reference frame to assume for poses of end-effectors"""
         self._g.set_pose_reference_frame(reference_frame)
 
     def get_planning_frame(self):
-        """ Get the name of the frame where all planning is performed """
+        """Get the name of the frame where all planning is performed"""
         return self._g.get_planning_frame()
 
     def get_current_joint_values(self):
-        """ Get the current configuration of the group as a list (these are values published on /joint_states) """
+        """Get the current configuration of the group as a list (these are values published on /joint_states)"""
         return self._g.get_current_joint_values()
 
-    def get_current_pose(self, end_effector_link = ""):
-        """ Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector. """
+    def get_current_pose(self, end_effector_link=""):
+        """Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector."""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
-            return conversions.list_to_pose_stamped(self._g.get_current_pose(end_effector_link), self.get_planning_frame())
+            return conversions.list_to_pose_stamped(
+                self._g.get_current_pose(end_effector_link), self.get_planning_frame()
+            )
         else:
-            raise MoveItCommanderException("There is no end effector to get the pose of")
+            raise MoveItCommanderException(
+                "There is no end effector to get the pose of"
+            )
 
-    def get_current_rpy(self, end_effector_link = ""):
-        """ Get a list of 3 elements defining the [roll, pitch, yaw] of the end-effector. Throws an exception if there is not end-effector. """
+    def get_current_rpy(self, end_effector_link=""):
+        """Get a list of 3 elements defining the [roll, pitch, yaw] of the end-effector. Throws an exception if there is not end-effector."""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
             return self._g.get_current_rpy(end_effector_link)
         else:
@@ -123,11 +142,15 @@ def get_current_rpy(self, end_effector_link = ""):
     def get_random_joint_values(self):
         return self._g.get_random_joint_values()
 
-    def get_random_pose(self, end_effector_link = ""):
+    def get_random_pose(self, end_effector_link=""):
         if len(end_effector_link) > 0 or self.has_end_effector_link():
-            return conversions.list_to_pose_stamped(self._g.get_random_pose(end_effector_link), self.get_planning_frame())
+            return conversions.list_to_pose_stamped(
+                self._g.get_random_pose(end_effector_link), self.get_planning_frame()
+            )
         else:
-            raise MoveItCommanderException("There is no end effector to get the pose of")
+            raise MoveItCommanderException(
+                "There is no end effector to get the pose of"
+            )
 
     def set_start_state_to_current_state(self):
         self._g.set_start_state_to_current_state()
@@ -155,10 +178,24 @@ 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()
 
-    def set_joint_value_target(self, arg1, arg2 = None, arg3 = None):
+    def set_joint_value_target(self, arg1, arg2=None, arg3=None):
         """
         Specify a target joint configuration for the group.
         - if the type of arg1 is one of the following: dict, list, JointState message, then no other arguments should be provided.
@@ -174,26 +211,36 @@ def set_joint_value_target(self, arg1, arg2 = None, arg3 = None):
         """
         if isinstance(arg1, RobotState):
             if not self._g.set_state_value_target(conversions.msg_to_string(arg1)):
-                raise MoveItCommanderException("Error setting state target. Is the target state within bounds?")
+                raise MoveItCommanderException(
+                    "Error setting state target. Is the target state within bounds?"
+                )
 
         elif isinstance(arg1, JointState):
-            if (arg2 is not None or arg3 is not None):
+            if arg2 is not None or arg3 is not None:
                 raise MoveItCommanderException("Too many arguments specified")
-            if not self._g.set_joint_value_target_from_joint_state_message(conversions.msg_to_string(arg1)):
-                raise MoveItCommanderException("Error setting joint target. Is the target within bounds?")
+            if not self._g.set_joint_value_target_from_joint_state_message(
+                conversions.msg_to_string(arg1)
+            ):
+                raise MoveItCommanderException(
+                    "Error setting joint target. Is the target within bounds?"
+                )
 
         elif isinstance(arg1, str):
-            if (arg2 is None):
-                raise MoveItCommanderException("Joint value expected when joint name specified")
-            if (arg3 is not None):
+            if arg2 is None:
+                raise MoveItCommanderException(
+                    "Joint value expected when joint name specified"
+                )
+            if arg3 is not None:
                 raise MoveItCommanderException("Too many arguments specified")
             if not self._g.set_joint_value_target(arg1, arg2):
-                raise MoveItCommanderException("Error setting joint target. Is the target within bounds?")
+                raise MoveItCommanderException(
+                    "Error setting joint target. Is the target within bounds?"
+                )
 
         elif isinstance(arg1, (Pose, PoseStamped)):
             approx = False
             eef = ""
-            if (arg2 is not None):
+            if arg2 is not None:
                 if type(arg2) is str:
                     eef = arg2
                 else:
@@ -201,7 +248,7 @@ def set_joint_value_target(self, arg1, arg2 = None, arg3 = None):
                         approx = arg2
                     else:
                         raise MoveItCommanderException("Unexpected type")
-            if (arg3 is not None):
+            if arg3 is not None:
                 if type(arg3) is str:
                     eef = arg3
                 else:
@@ -211,84 +258,116 @@ def set_joint_value_target(self, arg1, arg2 = None, arg3 = None):
                         raise MoveItCommanderException("Unexpected type")
             r = False
             if type(arg1) is PoseStamped:
-                r = self._g.set_joint_value_target_from_pose_stamped(conversions.msg_to_string(arg1), eef, approx)
+                r = self._g.set_joint_value_target_from_pose_stamped(
+                    conversions.msg_to_string(arg1), eef, approx
+                )
             else:
-                r = self._g.set_joint_value_target_from_pose(conversions.msg_to_string(arg1), eef, approx)
+                r = self._g.set_joint_value_target_from_pose(
+                    conversions.msg_to_string(arg1), eef, approx
+                )
             if not r:
                 if approx:
-                    raise MoveItCommanderException("Error setting joint target. Does your IK solver support approximate IK?")
+                    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):
+        elif hasattr(arg1, "__iter__"):
+            if arg2 is not None or arg3 is not None:
                 raise MoveItCommanderException("Too many arguments specified")
             if not self._g.set_joint_value_target(arg1):
-                raise MoveItCommanderException("Error setting joint target. Is the target within bounds?")
+                raise MoveItCommanderException(
+                    "Error setting joint target. Is the target within bounds?"
+                )
 
         else:
-            raise MoveItCommanderException("Unsupported argument of type %s" % type(arg1))
+            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."""
+    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():
             if len(rpy) == 3:
-                if not self._g.set_rpy_target(rpy[0], rpy[1], rpy[2], end_effector_link):
+                if not self._g.set_rpy_target(
+                    rpy[0], rpy[1], rpy[2], end_effector_link
+                ):
                     raise MoveItCommanderException("Unable to set orientation target")
             else:
                 raise MoveItCommanderException("Expected [roll, pitch, yaw]")
         else:
-            raise MoveItCommanderException("There is no end effector to set the pose for")
+            raise MoveItCommanderException(
+                "There is no end effector to set the pose for"
+            )
 
-    def set_orientation_target(self, q, end_effector_link = ""):
-        """ Specify a target orientation for the end-effector. Any position of the end-effector is acceptable."""
+    def set_orientation_target(self, q, 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():
             if len(q) == 4:
-                if not self._g.set_orientation_target(q[0], q[1], q[2], q[3], end_effector_link):
+                if not self._g.set_orientation_target(
+                    q[0], q[1], q[2], q[3], end_effector_link
+                ):
                     raise MoveItCommanderException("Unable to set orientation target")
             else:
                 raise MoveItCommanderException("Expected [qx, qy, qz, qw]")
         else:
-            raise MoveItCommanderException("There is no end effector to set the pose for")
+            raise MoveItCommanderException(
+                "There is no end effector to set the pose for"
+            )
 
-    def set_position_target(self, xyz, end_effector_link = ""):
-        """ Specify a target position for the end-effector. Any orientation of the end-effector is acceptable."""
+    def set_position_target(self, xyz, end_effector_link=""):
+        """Specify a target position for the end-effector. Any orientation of the end-effector is acceptable."""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
-            if not self._g.set_position_target(xyz[0], xyz[1], xyz[2], end_effector_link):
+            if not self._g.set_position_target(
+                xyz[0], xyz[1], xyz[2], end_effector_link
+            ):
                 raise MoveItCommanderException("Unable to set position target")
         else:
-            raise MoveItCommanderException("There is no end effector to set the pose for")
+            raise MoveItCommanderException(
+                "There is no end effector to set the pose for"
+            )
 
-    def set_pose_target(self, pose, end_effector_link = ""):
-        """ Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:"""
+    def set_pose_target(self, pose, end_effector_link=""):
+        """Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:"""
         """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
         if len(end_effector_link) > 0 or self.has_end_effector_link():
             ok = False
             if type(pose) is PoseStamped:
                 old = self.get_pose_reference_frame()
                 self.set_pose_reference_frame(pose.header.frame_id)
-                ok = self._g.set_pose_target(conversions.pose_to_list(pose.pose), end_effector_link)
+                ok = self._g.set_pose_target(
+                    conversions.pose_to_list(pose.pose), end_effector_link
+                )
                 self.set_pose_reference_frame(old)
             elif type(pose) is Pose:
-                ok = self._g.set_pose_target(conversions.pose_to_list(pose), end_effector_link)
+                ok = self._g.set_pose_target(
+                    conversions.pose_to_list(pose), end_effector_link
+                )
             else:
                 ok = self._g.set_pose_target(pose, end_effector_link)
             if not ok:
                 raise MoveItCommanderException("Unable to set target pose")
         else:
-            raise MoveItCommanderException("There is no end effector to set the pose for")
+            raise MoveItCommanderException(
+                "There is no end effector to set the pose for"
+            )
 
-    def set_pose_targets(self, poses, end_effector_link = ""):
-        """ Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
+    def set_pose_targets(self, poses, end_effector_link=""):
+        """Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw]"""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
-            if not self._g.set_pose_targets([conversions.pose_to_list(p) if type(p) is Pose else p for p in poses], end_effector_link):
+            if not self._g.set_pose_targets(
+                [conversions.pose_to_list(p) if type(p) is Pose else p for p in poses],
+                end_effector_link,
+            ):
                 raise MoveItCommanderException("Unable to set target poses")
         else:
             raise MoveItCommanderException("There is no end effector to set poses for")
 
-    def shift_pose_target(self, axis, value, end_effector_link = ""):
-        """ Get the current pose of the end effector, add value to the corresponding axis (0..5: X, Y, Z, R, P, Y) and set the new pose as the pose target """
+    def shift_pose_target(self, axis, value, end_effector_link=""):
+        """Get the current pose of the end effector, add value to the corresponding axis (0..5: X, Y, Z, R, P, Y) and set the new pose as the pose target"""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
             pose = self._g.get_current_pose(end_effector_link)
             # by default we get orientation as a quaternion list
@@ -305,156 +384,168 @@ def shift_pose_target(self, axis, value, end_effector_link = ""):
             raise MoveItCommanderException("There is no end effector to set poses for")
 
     def clear_pose_target(self, end_effector_link):
-        """ Clear the pose target for a particular end-effector """
+        """Clear the pose target for a particular end-effector"""
         self._g.clear_pose_target(end_effector_link)
 
     def clear_pose_targets(self):
-        """ Clear all known pose targets """
+        """Clear all known pose targets"""
         self._g.clear_pose_targets()
 
     def set_random_target(self):
-        """ Set a random joint configuration target """
+        """Set a random joint configuration target"""
         self._g.set_random_target()
 
     def get_named_targets(self):
-        """ Get a list of all the names of joint configurations."""
+        """Get a list of all the names of joint configurations."""
         return self._g.get_named_targets()
 
     def set_named_target(self, name):
-        """ Set a joint configuration by name. The name can be a name previlusy remembered with remember_joint_values() or a configuration specified in the SRDF. """
+        """Set a joint configuration by name. The name can be a name previlusy remembered with remember_joint_values() or a configuration specified in the SRDF."""
         if not self._g.set_named_target(name):
-            raise MoveItCommanderException("Unable to set target %s. Is the target within bounds?" % name)
+            raise MoveItCommanderException(
+                "Unable to set target %s. Is the target within bounds?" % name
+            )
 
     def get_named_target_values(self, target):
         """Get a dictionary of joint values of a named target"""
         return self._g.get_named_target_values(target)
 
-    def remember_joint_values(self, name, values = None):
-        """ Record the specified joint configuration of the group under the specified name. If no values are specified, the current state of the group is recorded. """
+    def remember_joint_values(self, name, values=None):
+        """Record the specified joint configuration of the group under the specified name. If no values are specified, the current state of the group is recorded."""
         if values is None:
             values = self.get_current_joint_values()
         self._g.remember_joint_values(name, values)
 
     def get_remembered_joint_values(self):
-        """ Get a dictionary that maps names to joint configurations for the group """
+        """Get a dictionary that maps names to joint configurations for the group"""
         return self._g.get_remembered_joint_values()
 
     def forget_joint_values(self, name):
-        """ Forget a stored joint configuration """
+        """Forget a stored joint configuration"""
         self._g.forget_joint_values(name)
 
     def get_goal_tolerance(self):
-        """ Return a tuple of goal tolerances: joint, position and orientation. """
-        return (self.get_goal_joint_tolerance(), self.get_goal_position_tolerance(), self.get_goal_orientation_tolerance())
+        """Return a tuple of goal tolerances: joint, position and orientation."""
+        return (
+            self.get_goal_joint_tolerance(),
+            self.get_goal_position_tolerance(),
+            self.get_goal_orientation_tolerance(),
+        )
 
     def get_goal_joint_tolerance(self):
-        """ Get the tolerance for achieving a joint goal (distance for each joint variable) """
+        """Get the tolerance for achieving a joint goal (distance for each joint variable)"""
         return self._g.get_goal_joint_tolerance()
 
     def get_goal_position_tolerance(self):
-        """ When moving to a position goal or to a pose goal, the tolerance for the goal position is specified as the radius a sphere around the target origin of the end-effector """
+        """When moving to a position goal or to a pose goal, the tolerance for the goal position is specified as the radius a sphere around the target origin of the end-effector"""
         return self._g.get_goal_position_tolerance()
 
     def get_goal_orientation_tolerance(self):
-        """ When moving to an orientation goal or to a pose goal, the tolerance for the goal orientation is specified as the distance (roll, pitch, yaw) to the target origin of the end-effector """
+        """When moving to an orientation goal or to a pose goal, the tolerance for the goal orientation is specified as the distance (roll, pitch, yaw) to the target origin of the end-effector"""
         return self._g.get_goal_orientation_tolerance()
 
     def set_goal_tolerance(self, value):
-        """ Set the joint, position and orientation goal tolerances simultaneously """
+        """Set the joint, position and orientation goal tolerances simultaneously"""
         self._g.set_goal_tolerance(value)
 
     def set_goal_joint_tolerance(self, value):
-        """ Set the tolerance for a target joint configuration """
+        """Set the tolerance for a target joint configuration"""
         self._g.set_goal_joint_tolerance(value)
 
     def set_goal_position_tolerance(self, value):
-        """ Set the tolerance for a target end-effector position """
+        """Set the tolerance for a target end-effector position"""
         self._g.set_goal_position_tolerance(value)
 
     def set_goal_orientation_tolerance(self, value):
-        """ Set the tolerance for a target end-effector orientation """
+        """Set the tolerance for a target end-effector orientation"""
         self._g.set_goal_orientation_tolerance(value)
 
     def allow_looking(self, value):
-        """ Enable/disable looking around for motion planning """
+        """Enable/disable looking around for motion planning"""
         self._g.allow_looking(value)
 
     def allow_replanning(self, value):
-        """ Enable/disable replanning """
+        """Enable/disable replanning"""
         self._g.allow_replanning(value)
 
     def get_known_constraints(self):
-        """ Get a list of names for the constraints specific for this group, as read from the warehouse """
+        """Get a list of names for the constraints specific for this group, as read from the warehouse"""
         return self._g.get_known_constraints()
 
     def get_path_constraints(self):
-        """ Get the acutal path constraints in form of a moveit_msgs.msgs.Constraints """
+        """Get the acutal path constraints in form of a moveit_msgs.msgs.Constraints"""
         c = Constraints()
         c_str = self._g.get_path_constraints()
-        conversions.msg_from_string(c,c_str)
+        conversions.msg_from_string(c, c_str)
         return c
 
     def set_path_constraints(self, value):
-        """ Specify the path constraints to be used (as read from the database) """
+        """Specify the path constraints to be used (as read from the database)"""
         if value is None:
             self.clear_path_constraints()
         else:
             if type(value) is Constraints:
                 self._g.set_path_constraints_from_msg(conversions.msg_to_string(value))
             elif not self._g.set_path_constraints(value):
-                raise MoveItCommanderException("Unable to set path constraints " + value)
+                raise MoveItCommanderException(
+                    "Unable to set path constraints " + value
+                )
 
     def clear_path_constraints(self):
-        """ Specify that no path constraints are to be used during motion planning """
+        """Specify that no path constraints are to be used during motion planning"""
         self._g.clear_path_constraints()
 
     def get_trajectory_constraints(self):
-        """ Get the actual trajectory constraints in form of a moveit_msgs.msgs.Constraints """
+        """Get the actual trajectory constraints in form of a moveit_msgs.msgs.Constraints"""
         c = Constraints()
         c_str = self._g.get_trajectory_constraints()
         conversions.msg_from_string(c, c_str)
         return c
 
     def set_trajectory_constraints(self, value):
-        """ Specify the trajectory constraints to be used """
+        """Specify the trajectory constraints to be used"""
         if value is None:
             self.clear_trajectory_constraints()
         else:
             if type(value) is TrajectoryConstraints:
-                self._g.set_trajectory_constraints_from_msg(conversions.msg_to_string(value))
+                self._g.set_trajectory_constraints_from_msg(
+                    conversions.msg_to_string(value)
+                )
             elif not self._g.set_trajectory_constraints(value):
-                raise MoveItCommanderException("Unable to set trajectory constraints " + value)
+                raise MoveItCommanderException(
+                    "Unable to set trajectory constraints " + value
+                )
 
     def clear_trajectory_constraints(self):
-        """ Specify that no trajectory constraints are to be used during motion planning """
+        """Specify that no trajectory constraints are to be used during motion planning"""
         self._g.clear_trajectory_constraints()
 
     def set_constraints_database(self, host, port):
-        """ Specify which database to connect to for loading possible path constraints """
+        """Specify which database to connect to for loading possible path constraints"""
         self._g.set_constraints_database(host, port)
 
     def set_planning_time(self, seconds):
-        """ Specify the amount of time to be used for motion planning. """
+        """Specify the amount of time to be used for motion planning."""
         self._g.set_planning_time(seconds)
 
     def get_planning_time(self):
-        """ Specify the amount of time to be used for motion planning. """
+        """Specify the amount of time to be used for motion planning."""
         return self._g.get_planning_time()
 
     def set_planner_id(self, planner_id):
-        """ Specify which planner to use when motion planning """
+        """Specify which planner to use when motion planning"""
         self._g.set_planner_id(planner_id)
 
     def get_planner_id(self):
-        """ Get the current planner_id """
+        """Get the current 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. """
+        """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."""
         self._g.set_num_planning_attempts(num_planning_attempts)
 
     def set_workspace(self, ws):
-        """ Set the workspace for the robot as either [], [minX, minY, maxX, maxY] or [minX, minY, minZ, maxX, maxY, maxZ] """
+        """Set the workspace for the robot as either [], [minX, minY, maxX, maxY] or [minX, minY, minZ, maxX, maxY, maxZ]"""
         if len(ws) == 0:
             self._g.set_workspace(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
         else:
@@ -464,24 +555,30 @@ def set_workspace(self, ws):
                 if len(ws) == 6:
                     self._g.set_workspace(ws[0], ws[1], ws[2], ws[3], ws[4], ws[5])
                 else:
-                    raise MoveItCommanderException("Expected 0, 4 or 6 values in list specifying workspace")
+                    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 for optionally reducing the maximum joint velocity. Allowed values are in (0,1]."""
         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" )
+            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 for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]."""
         if value > 0 and value <= 1:
             self._g.set_max_acceleration_scaling_factor(value)
         else:
-            raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor" )
+            raise MoveItCommanderException(
+                "Expected value in the range from 0 to 1 for scaling factor"
+            )
 
-    def go(self, joints = None, wait = True):
-        """ Set the target of the group and then move the group to the specified target """
+    def go(self, joints=None, wait=True):
+        """Set the target of the group and then move the group to the specified target"""
         if type(joints) is bool:
             wait = joints
             joints = None
@@ -502,8 +599,8 @@ def go(self, joints = None, wait = True):
         else:
             return self._g.async_move()
 
-    def plan(self, joints = None):
-        """ Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """
+    def plan(self, joints=None):
+        """Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument)"""
         if type(joints) is JointState:
             self.set_joint_value_target(joints)
 
@@ -519,42 +616,67 @@ def plan(self, joints = None):
         plan.deserialize(self._g.compute_plan())
         return plan
 
-    def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions = True, path_constraints = None):
-        """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
+    def compute_cartesian_path(
+        self,
+        waypoints,
+        eef_step,
+        jump_threshold,
+        avoid_collisions=True,
+        path_constraints=None,
+    ):
+        """Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory."""
         if path_constraints:
             if type(path_constraints) is Constraints:
                 constraints_str = conversions.msg_to_string(path_constraints)
             else:
-                raise MoveItCommanderException("Unable to set path constraints, unknown constraint type " + type(path_constraints))
-            (ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions, constraints_str)
+                raise MoveItCommanderException(
+                    "Unable to set path constraints, unknown constraint type "
+                    + type(path_constraints)
+                )
+            (ser_path, fraction) = self._g.compute_cartesian_path(
+                [conversions.pose_to_list(p) for p in waypoints],
+                eef_step,
+                jump_threshold,
+                avoid_collisions,
+                constraints_str,
+            )
         else:
-            (ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions)
+            (ser_path, fraction) = self._g.compute_cartesian_path(
+                [conversions.pose_to_list(p) for p in waypoints],
+                eef_step,
+                jump_threshold,
+                avoid_collisions,
+            )
 
         path = RobotTrajectory()
         path.deserialize(ser_path)
         return (path, fraction)
 
-    def execute(self, plan_msg, wait = True):
+    def execute(self, plan_msg, wait=True):
         """Execute a previously planned path"""
         if wait:
             return self._g.execute(conversions.msg_to_string(plan_msg))
         else:
             return self._g.async_execute(conversions.msg_to_string(plan_msg))
 
-    def attach_object(self, object_name, link_name = "", touch_links = []):
-        """ Given the name of an object existing in the planning scene, attach it to a link. The link used is specified by the second argument. If left unspecified, the end-effector link is used, if one is known. If there is no end-effector link, the first link in the group is used. If no link is identified, failure is reported. True is returned if an attach request was succesfully sent to the move_group node. This does not verify that the attach request also was successfuly applied by move_group."""
+    def attach_object(self, object_name, link_name="", touch_links=[]):
+        """Given the name of an object existing in the planning scene, attach it to a link. The link used is specified by the second argument. If left unspecified, the end-effector link is used, if one is known. If there is no end-effector link, the first link in the group is used. If no link is identified, failure is reported. True is returned if an attach request was succesfully sent to the move_group node. This does not verify that the attach request also was successfuly applied by move_group."""
         return self._g.attach_object(object_name, link_name, touch_links)
 
-    def detach_object(self, name = ""):
-        """ Given the name of a link, detach the object(s) from that link. If no such link exists, the name is interpreted as an object name. If there is no name specified, an attempt is made to detach all objects attached to any link in the group."""
+    def detach_object(self, name=""):
+        """Given the name of a link, detach the object(s) from that link. If no such link exists, the name is interpreted as an object name. If there is no name specified, an attempt is made to detach all objects attached to any link in the group."""
         return self._g.detach_object(name)
 
-    def pick(self, object_name, grasp = [], plan_only=False):
+    def pick(self, object_name, grasp=[], plan_only=False):
         """Pick the named object. A grasp message, or a list of Grasp messages can also be specified as argument."""
         if type(grasp) is Grasp:
-            return self._g.pick(object_name, conversions.msg_to_string(grasp), plan_only)
+            return self._g.pick(
+                object_name, conversions.msg_to_string(grasp), plan_only
+            )
         else:
-            return self._g.pick(object_name, [conversions.msg_to_string(x) for x in grasp], plan_only)
+            return self._g.pick(
+                object_name, [conversions.msg_to_string(x) for x in grasp], plan_only
+            )
 
     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"""
@@ -564,37 +686,77 @@ def place(self, object_name, location=None, plan_only=False):
         elif type(location) is PoseStamped:
             old = self.get_pose_reference_frame()
             self.set_pose_reference_frame(location.header.frame_id)
-            result = self._g.place(object_name, conversions.pose_to_list(location.pose), plan_only)
+            result = self._g.place(
+                object_name, conversions.pose_to_list(location.pose), plan_only
+            )
             self.set_pose_reference_frame(old)
         elif type(location) is Pose:
-            result = self._g.place(object_name, conversions.pose_to_list(location), plan_only)
+            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)
+            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)
+                    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)
+                    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")
+                    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, PlaceLocation, list of PoseStamped or list of 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):
-        """ Set the support surface name for a place operation """
+        """Set the support surface name for a place operation"""
         self._g.set_support_surface_name(value)
 
-    def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor=1.0, acceleration_scaling_factor=1.0, algorithm="iterative_time_parameterization"):
+    def retime_trajectory(
+        self,
+        ref_state_in,
+        traj_in,
+        velocity_scaling_factor=1.0,
+        acceleration_scaling_factor=1.0,
+        algorithm="iterative_time_parameterization",
+    ):
         ser_ref_state_in = conversions.msg_to_string(ref_state_in)
         ser_traj_in = conversions.msg_to_string(traj_in)
-        ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor, acceleration_scaling_factor, algorithm)
+        ser_traj_out = self._g.retime_trajectory(
+            ser_ref_state_in,
+            ser_traj_in,
+            velocity_scaling_factor,
+            acceleration_scaling_factor,
+            algorithm,
+        )
         traj_out = RobotTrajectory()
         traj_out.deserialize(ser_traj_out)
         return traj_out
 
     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, [0.0, 0.0, 0.0] if reference_point is None else reference_point)
-
+        """Get the jacobian matrix of the group as a list"""
+        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 cbc3c72021..acd442e77e 100644
--- a/moveit_commander/src/moveit_commander/planning_scene_interface.py
+++ b/moveit_commander/src/moveit_commander/planning_scene_interface.py
@@ -33,6 +33,7 @@
 # Author: Ioan Sucan, Felix Messmer
 
 import rospy
+from rosgraph.names import ns_join
 from . import conversions
 
 from moveit_msgs.msg import PlanningScene, CollisionObject, AttachedCollisionObject
@@ -50,21 +51,36 @@
         import pyassimp
     except:
         pyassimp = False
-        print("Failed to import pyassimp, see https://github.com/ros-planning/moveit/issues/86 for more info")
+        print(
+            "Failed to import pyassimp, see https://github.com/ros-planning/moveit/issues/86 for more info"
+        )
 
 
 class PlanningSceneInterface(object):
-    """ Simple interface to making updates to a planning scene """
+    """
+    Python interface for a C++ PlanningSceneInterface.
+    Uses both C++ wrapped methods and scene manipulation topics
+    to manipulate the PlanningScene managed by the PlanningSceneMonitor.
+    See wrap_python_planning_scene_interface.cpp for the wrapped methods.
+    """
 
-    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. """
+    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(ns + '/collision_object', CollisionObject, queue_size=100)
-        self._pub_aco = rospy.Publisher(ns + '/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(ns + '/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):
@@ -77,36 +93,32 @@ def __submit(self, collision_object, attach=False):
             else:
                 self._pub_co.publish(collision_object)
 
+    def add_object(self, collision_object):
+        """Add an object to the planning scene"""
+        self.__submit(collision_object, attach=False)
+
     def add_sphere(self, name, pose, radius=1):
-        """
-        Add a sphere to the planning scene
-        """
+        """Add a sphere to the planning scene"""
         co = self.__make_sphere(name, pose, radius)
         self.__submit(co, attach=False)
 
     def add_cylinder(self, name, pose, height, radius):
-        """
-        Add a cylinder to the planning scene
-        """
+        """Add a cylinder to the planning scene"""
         co = self.__make_cylinder(name, pose, height, radius)
         self.__submit(co, attach=False)
 
     def add_mesh(self, name, pose, filename, size=(1, 1, 1)):
-        """
-        Add a mesh to the planning scene
-        """
+        """Add a mesh to the planning scene"""
         co = self.__make_mesh(name, pose, filename, size)
         self.__submit(co, attach=False)
 
     def add_box(self, name, pose, size=(1, 1, 1)):
-        """
-        Add a box to the planning scene
-        """
+        """Add a box to the planning scene"""
         co = self.__make_box(name, pose, size)
         self.__submit(co, attach=False)
 
     def add_plane(self, name, pose, normal=(0, 0, 1), offset=0):
-        """ Add a plane to the planning scene """
+        """Add a plane to the planning scene"""
         co = CollisionObject()
         co.operation = CollisionObject.ADD
         co.id = name
@@ -118,7 +130,13 @@ def add_plane(self, name, pose, normal=(0, 0, 1), offset=0):
         co.plane_poses = [pose.pose]
         self.__submit(co, attach=False)
 
-    def attach_mesh(self, link, name, pose=None, filename='', size=(1, 1, 1), touch_links=[]):
+    def attach_object(self, attached_collision_object):
+        """Attach an object in the planning scene"""
+        self.__submit(attached_collision_object, attach=True)
+
+    def attach_mesh(
+        self, link, name, pose=None, filename="", size=(1, 1, 1), touch_links=[]
+    ):
         aco = AttachedCollisionObject()
         if (pose is not None) and filename:
             aco.object = self.__make_mesh(name, pose, filename, size)
@@ -143,6 +161,11 @@ def attach_box(self, link, name, pose=None, size=(1, 1, 1), touch_links=[]):
             aco.touch_links = [link]
         self.__submit(aco, attach=True)
 
+    def clear(self):
+        """Remove all objects from the planning scene"""
+        self.remove_attached_object()
+        self.remove_world_object()
+
     def remove_world_object(self, name=None):
         """
         Remove an object from planning scene, or all if no name is provided
@@ -153,13 +176,18 @@ def remove_world_object(self, name=None):
             co.id = name
         self.__submit(co, attach=False)
 
-    def remove_attached_object(self, link, name=None):
+    def remove_attached_object(self, link=None, name=None):
         """
-        Remove an attached object from planning scene, or all objects attached to this link if no name is provided
+        Remove an attached object from the robot, or all objects attached to the link if no name is provided,
+        or all attached objects in the scene if neither link nor name are provided.
+
+        Removed attached objects remain in the scene as world objects.
+        Call remove_world_object afterwards to remove them from the scene.
         """
         aco = AttachedCollisionObject()
         aco.object.operation = CollisionObject.REMOVE
-        aco.link_name = link
+        if link is not None:
+            aco.link_name = link
         if name is not None:
             aco.object.id = name
         self.__submit(aco, attach=True)
@@ -170,12 +198,16 @@ def get_known_object_names(self, with_type=False):
         """
         return self._psi.get_known_object_names(with_type)
 
-    def get_known_object_names_in_roi(self, minx, miny, minz, maxx, maxy, maxz, with_type=False):
+    def get_known_object_names_in_roi(
+        self, minx, miny, minz, maxx, maxy, maxz, with_type=False
+    ):
         """
         Get the names of known objects in the world that are located within a bounding region (specified in the frame reported by
         get_planning_frame()). If with_type is set to true, only return objects that have a known type.
         """
-        return self._psi.get_known_object_names_in_roi(minx, miny, minz, maxx, maxy, maxz, with_type)
+        return self._psi.get_known_object_names_in_roi(
+            minx, miny, minz, maxx, maxy, maxz, with_type
+        )
 
     def get_object_poses(self, object_ids):
         """
@@ -213,10 +245,18 @@ def get_attached_objects(self, object_ids=[]):
             aobjs[key] = msg
         return aobjs
 
+    def apply_planning_scene(self, planning_scene_message):
+        """
+        Applies the planning scene message.
+        """
+        return self._psi.apply_planning_scene(
+            conversions.msg_to_string(planning_scene_message)
+        )
+
     @staticmethod
     def __make_existing(name):
         """
-        Create an empty Collision Object, used when the object already exists
+        Create an empty Collision Object. Used when the object already exists
         """
         co = CollisionObject()
         co.id = name
@@ -240,7 +280,8 @@ def __make_mesh(name, pose, filename, scale=(1, 1, 1)):
         co = CollisionObject()
         if pyassimp is False:
             raise MoveItCommanderException(
-                "Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt")
+                "Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt"
+            )
         scene = pyassimp.load(filename)
         if not scene.meshes or len(scene.meshes) == 0:
             raise MoveItCommanderException("There are no meshes in the file")
@@ -252,22 +293,26 @@ def __make_mesh(name, pose, filename, scale=(1, 1, 1)):
 
         mesh = Mesh()
         first_face = scene.meshes[0].faces[0]
-        if hasattr(first_face, '__len__'):
+        if hasattr(first_face, "__len__"):
             for face in scene.meshes[0].faces:
                 if len(face) == 3:
                     triangle = MeshTriangle()
                     triangle.vertex_indices = [face[0], face[1], face[2]]
                     mesh.triangles.append(triangle)
-        elif hasattr(first_face, 'indices'):
+        elif hasattr(first_face, "indices"):
             for face in scene.meshes[0].faces:
                 if len(face.indices) == 3:
                     triangle = MeshTriangle()
-                    triangle.vertex_indices = [face.indices[0],
-                                               face.indices[1],
-                                               face.indices[2]]
+                    triangle.vertex_indices = [
+                        face.indices[0],
+                        face.indices[1],
+                        face.indices[2],
+                    ]
                     mesh.triangles.append(triangle)
         else:
-            raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure")
+            raise MoveItCommanderException(
+                "Unable to build triangles from mesh due to mesh object structure"
+            )
         for vertex in scene.meshes[0].vertices:
             point = Point()
             point.x = vertex[0] * scale[0]
diff --git a/moveit_commander/src/moveit_commander/robot.py b/moveit_commander/src/moveit_commander/robot.py
index 137fe4b4b4..7b9b51848e 100644
--- a/moveit_commander/src/moveit_commander/robot.py
+++ b/moveit_commander/src/moveit_commander/robot.py
@@ -40,7 +40,6 @@
 
 
 class RobotCommander(object):
-
     class Joint(object):
         def __init__(self, robot, name):
             self._robot = robot
@@ -116,7 +115,10 @@ def move(self, position, wait=True):
             """
             group = self._robot.get_default_owner_group(self.name())
             if group is None:
-                raise MoveItCommanderException("There is no known group containing joint %s. Cannot move." % self._name)
+                raise MoveItCommanderException(
+                    "There is no known group containing joint %s. Cannot move."
+                    % self._name
+                )
             gc = self._robot.get_group(group)
             if gc is not None:
                 gc.set_joint_value_target(gc.get_current_joint_values())
@@ -143,7 +145,10 @@ def pose(self):
             """
             @rtype: geometry_msgs.Pose
             """
-            return conversions.list_to_pose_stamped(self._robot._r.get_link_pose(self._name), self._robot.get_planning_frame())
+            return conversions.list_to_pose_stamped(
+                self._robot._r.get_link_pose(self._name),
+                self._robot.get_planning_frame(),
+            )
 
     def __init__(self, robot_description="robot_description", ns=""):
         self._robot_description = robot_description
@@ -186,15 +191,28 @@ def get_robot_markers(self, *args):
         return mrkr
 
     def get_root_link(self):
-        """Get the name of the root link of the robot model """
+        """Get the name of the root link of the robot model"""
         return self._r.get_robot_root_link()
 
+    def get_active_joint_names(self, group=None):
+        """
+        Get the names of all the movable joints that make up a group.
+        If no group name is specified, all joints in the robot model are returned.
+        Excludes fixed and mimic joints.
+        """
+        if group is not None:
+            if self.has_group(group):
+                return self._r.get_group_active_joint_names(group)
+            else:
+                raise MoveItCommanderException("There is no group named %s" % group)
+        else:
+            return self._r.get_active_joint_names()
+
     def get_joint_names(self, group=None):
         """
-        Get the names of all the movable joints that make up a group
-        (mimic joints and fixed joints are excluded). If no group name is
-        specified, all joints in the robot model are returned, including
-        fixed and mimic joints.
+        Get the names of all the movable joints that make up a group.
+        If no group name is specified, all joints in the robot model are returned.
+        Includes fixed and mimic joints.
         """
         if group is not None:
             if self.has_group(group):
@@ -222,7 +240,7 @@ def get_group_names(self):
         return self._r.get_group_names()
 
     def get_current_state(self):
-        """ Get a RobotState message describing the current state of the robot"""
+        """Get a RobotState message describing the current state of the robot"""
         s = RobotState()
         s.deserialize(self._r.get_current_state())
         return s
@@ -264,7 +282,9 @@ def get_group(self, 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)
+            self._groups[name] = MoveGroupCommander(
+                name, self._robot_description, self._ns
+            )
         return self._groups[name]
 
     def has_group(self, name):
@@ -286,7 +306,9 @@ def get_default_owner_group(self, joint_name):
                     if group is None:
                         group = g
                     else:
-                        if len(self.get_link_names(g)) < len(self.get_link_names(group)):
+                        if len(self.get_link_names(g)) < len(
+                            self.get_link_names(group)
+                        ):
                             group = g
             self._joint_owner_groups[joint_name] = group
         return self._joint_owner_groups[joint_name]
diff --git a/moveit_commander/src/moveit_commander/roscpp_initializer.py b/moveit_commander/src/moveit_commander/roscpp_initializer.py
index 383571ee7e..8f7f5145b3 100644
--- a/moveit_commander/src/moveit_commander/roscpp_initializer.py
+++ b/moveit_commander/src/moveit_commander/roscpp_initializer.py
@@ -34,10 +34,12 @@
 
 from moveit_ros_planning_interface import _moveit_roscpp_initializer
 
+
 def roscpp_initialize(args):
     # remove __name:= argument
-    args2 = [ a for a in args if not a.startswith("__name:=") ]
+    args2 = [a for a in args if not a.startswith("__name:=")]
     _moveit_roscpp_initializer.roscpp_init("move_group_commander_wrappers", args2)
 
+
 def roscpp_shutdown():
     _moveit_roscpp_initializer.roscpp_shutdown()
diff --git a/moveit_commander/test/python_moveit_commander.py b/moveit_commander/test/python_moveit_commander.py
index 41d77dc922..40637d3dca 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,21 +59,64 @@ 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]
         self.group.set_joint_value_target(*args)
         res = self.group.get_joint_value_target()
-        self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)),
-                        "Setting failed for %s, values: %s" % (type(args[0]), res))
+        self.assertTrue(
+            np.all(np.asarray(res) == np.asarray(expect)),
+            "Setting failed for %s, values: %s" % (type(args[0]), res),
+        )
 
     def test_target_setting(self):
         n = self.group.get_variable_count()
         self.check_target_setting([0.1] * n)
         self.check_target_setting((0.2,) * n)
         self.check_target_setting(np.zeros(n))
-        self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()})
-        self.check_target_setting([0.5] + [0.3]*(n-1), "joint_1", 0.5)
+        self.check_target_setting(
+            [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}
+        )
+        self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5)
 
     def plan(self, target):
         self.group.set_joint_value_target(target)
@@ -95,9 +142,9 @@ def test_planning_scene_interface(self):
         planning_scene = PlanningSceneInterface()
 
 
-if __name__ == '__main__':
-    PKGNAME = 'moveit_ros_planning_interface'
-    NODENAME = 'moveit_test_python_moveit_commander'
+if __name__ == "__main__":
+    PKGNAME = "moveit_ros_planning_interface"
+    NODENAME = "moveit_test_python_moveit_commander"
     rospy.init_node(NODENAME)
     rostest.rosrun(PKGNAME, NODENAME, PythonMoveitCommanderTest)
 
diff --git a/moveit_commander/test/python_moveit_commander_ns.py b/moveit_commander/test/python_moveit_commander_ns.py
index efaa6f03d3..4b73802841 100755
--- a/moveit_commander/test/python_moveit_commander_ns.py
+++ b/moveit_commander/test/python_moveit_commander_ns.py
@@ -52,7 +52,9 @@ class PythonMoveitCommanderNsTest(unittest.TestCase):
 
     @classmethod
     def setUpClass(self):
-        self.commander = RobotCommander("%srobot_description"%self.PLANNING_NS, self.PLANNING_NS)
+        self.commander = RobotCommander(
+            "%srobot_description" % self.PLANNING_NS, self.PLANNING_NS
+        )
         self.group = self.commander.get_group(self.PLANNING_GROUP)
 
     @classmethod
@@ -64,16 +66,20 @@ def check_target_setting(self, expect, *args):
             args = [expect]
         self.group.set_joint_value_target(*args)
         res = self.group.get_joint_value_target()
-        self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)),
-                        "Setting failed for %s, values: %s" % (type(args[0]), res))
+        self.assertTrue(
+            np.all(np.asarray(res) == np.asarray(expect)),
+            "Setting failed for %s, values: %s" % (type(args[0]), res),
+        )
 
     def test_target_setting(self):
         n = self.group.get_variable_count()
         self.check_target_setting([0.1] * n)
         self.check_target_setting((0.2,) * n)
         self.check_target_setting(np.zeros(n))
-        self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()})
-        self.check_target_setting([0.5] + [0.3]*(n-1), "joint_1", 0.5)
+        self.check_target_setting(
+            [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}
+        )
+        self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5)
 
     def plan(self, target):
         self.group.set_joint_value_target(target)
@@ -96,9 +102,9 @@ def test_validation(self):
         self.assertTrue(self.group.execute(plan3))
 
 
-if __name__ == '__main__':
-    PKGNAME = 'moveit_ros_planning_interface'
-    NODENAME = 'moveit_test_python_moveit_commander_ns'
+if __name__ == "__main__":
+    PKGNAME = "moveit_ros_planning_interface"
+    NODENAME = "moveit_test_python_moveit_commander_ns"
     rospy.init_node(NODENAME)
     rostest.rosrun(PKGNAME, NODENAME, PythonMoveitCommanderNsTest)
 
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..b7de1f059e
--- /dev/null
+++ b/moveit_commander/test/python_moveit_commander_ros_namespace.py
@@ -0,0 +1,72 @@
+#!/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 @@
+<launch>
+    <!-- PlanningSceneInterface can connect to namespaced moveit configuration -->
+    <group ns="test_ros_namespace">
+        <include file="$(find moveit_resources_fanuc_moveit_config)/launch/test_environment.launch"/>
+        <test pkg="moveit_commander" type="python_moveit_commander_ros_namespace.py"
+              test-name="python_moveit_commander_ros_namespace"
+              time-limit="300" args=""/>
+    </group>
+</launch>
diff --git a/moveit_commander/test/python_time_parameterization.py b/moveit_commander/test/python_time_parameterization.py
index 3fe1a9809f..1cf587d6af 100755
--- a/moveit_commander/test/python_time_parameterization.py
+++ b/moveit_commander/test/python_time_parameterization.py
@@ -59,34 +59,52 @@ def plan(self):
         start_pose = self.group.get_current_pose().pose
         goal_pose = self.group.get_current_pose().pose
         goal_pose.position.z -= 0.1
-        (plan, fraction) = self.group.compute_cartesian_path([start_pose, goal_pose], 0.005, 0.0)
+        (plan, fraction) = self.group.compute_cartesian_path(
+            [start_pose, goal_pose], 0.005, 0.0
+        )
         self.assertEqual(fraction, 1.0, "Cartesian path plan failed")
         return plan
 
     def time_parameterization(self, plan, algorithm):
         ref_state = self.commander.get_current_state()
         retimed_plan = self.group.retime_trajectory(
-            ref_state, plan,
+            ref_state,
+            plan,
             velocity_scaling_factor=0.1,
             acceleration_scaling_factor=0.1,
-            algorithm=algorithm)
+            algorithm=algorithm,
+        )
         return retimed_plan
 
-
     def test_plan_and_time_parameterization(self):
         plan = self.plan()
-        retimed_plan = self.time_parameterization(plan, "iterative_time_parameterization")
-        self.assertTrue(len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid")
-        retimed_plan = self.time_parameterization(plan, "iterative_spline_parameterization")
-        self.assertTrue(len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid")
-        retimed_plan = self.time_parameterization(plan, "time_optimal_trajectory_generation")
-        self.assertTrue(len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid")
+        retimed_plan = self.time_parameterization(
+            plan, "iterative_time_parameterization"
+        )
+        self.assertTrue(
+            len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid"
+        )
+        retimed_plan = self.time_parameterization(
+            plan, "iterative_spline_parameterization"
+        )
+        self.assertTrue(
+            len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid"
+        )
+        retimed_plan = self.time_parameterization(
+            plan, "time_optimal_trajectory_generation"
+        )
+        self.assertTrue(
+            len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid"
+        )
         retimed_plan = self.time_parameterization(plan, "")
-        self.assertTrue(len(retimed_plan.joint_trajectory.points) == 0, "Invalid retime algorithm")
+        self.assertTrue(
+            len(retimed_plan.joint_trajectory.points) == 0, "Invalid retime algorithm"
+        )
+
 
-if __name__ == '__main__':
-    PKGNAME = 'moveit_ros_planning_interface'
-    NODENAME = 'moveit_test_python_time_parameterization'
+if __name__ == "__main__":
+    PKGNAME = "moveit_ros_planning_interface"
+    NODENAME = "moveit_test_python_time_parameterization"
     rospy.init_node(NODENAME)
     rostest.rosrun(PKGNAME, NODENAME, PythonTimeParameterizationTest)
 
diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst
index c2f2a9bd5f..b133b5cc0c 100644
--- a/moveit_core/CHANGELOG.rst
+++ b/moveit_core/CHANGELOG.rst
@@ -2,6 +2,55 @@
 Changelog for package moveit_core
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+* Add Ptr definitions for TimeParameterization classes (`#3078 <https://github.com/ros-planning/moveit/issues/3078>`_)
+* Contributors: Michael Görner
+
+1.0.9 (2022-01-09)
+------------------
+* Use moveit-resources@master (`#2951 <https://github.com/ros-planning/moveit/issues/2951>`_)
+
+  - Simplify launch files to use the test_environment.launch files from moveit_resources@master
+  - Provide compatibility to the Noetic-style configuration of (multiple) planning pipelines
+    Only a single pipeline can be used at a time, specified via the ~default_planning_pipeline parameter.
+* Make TimeParameterization classes polymorphic (`#3023 <https://github.com/ros-planning/moveit/issues/3023>`_)
+* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 <https://github.com/ros-planning/moveit/issues/3009>`_)
+* Provide ``MOVEIT_VERSION_CHECK`` macro (`#2997 <https://github.com/ros-planning/moveit/issues/2997>`_)
+* Fix padding collision attached objects (`#2721 <https://github.com/ros-planning/moveit/issues/2721>`_)
+* Contributors: Michael Görner, Robert Haschke, Toru Kuga, Andrea Pupa
+
+1.0.8 (2021-05-23)
+------------------
+* RobotState interpolation: warn if interpolation parameter is out of range [0, 1] (`#2664 <https://github.com/ros-planning/moveit/issues/2664>`_)
+* Python bindings for moveit_core (`#2547 <https://github.com/ros-planning/moveit/issues/2547>`_ / `#2651 <https://github.com/ros-planning/moveit/issues/2651>`_)
+* New function setJointGroupActivePositions sets the position of *active* joints (`#2456 <https://github.com/ros-planning/moveit/issues/2456>`_)
+* Set rotation value of Cartesian MaxEEFStep by default (`#2614 <https://github.com/ros-planning/moveit/issues/2614>`_)
+* Make setToIKSolverFrame accessible again (`#2580 <https://github.com/ros-planning/moveit/issues/2580>`_)
+* Add get_active_joint_names() (`#2533 <https://github.com/ros-planning/moveit/issues/2533>`_)
+* Update doxygen comments for distance() and interpolate() (`#2528 <https://github.com/ros-planning/moveit/issues/2528>`_)
+* Clean up collision-related log statements (`#2480 <https://github.com/ros-planning/moveit/issues/2480>`_)
+* Fix RobotState::dropAccelerations/dropEffort to not drop velocities (`#2478 <https://github.com/ros-planning/moveit/issues/2478>`_)
+* Fix doxygen documentation for setToIKSolverFrame (`#2461 <https://github.com/ros-planning/moveit/issues/2461>`_)
+* Fix validation of orientation constraints (`#2434 <https://github.com/ros-planning/moveit/issues/2434>`_)
+* Fix OrientationConstraint::decide (`#2414 <https://github.com/ros-planning/moveit/issues/2414>`_)
+* Changed processing_thread\_ spin to use std::make_unique instead of new (`#2412 <https://github.com/ros-planning/moveit/issues/2412>`_)
+* RobotModelBuilder: Add parameter to specify the joint rotation axis
+* RobotModelBuilder: Allow adding end effectors (`#2454 <https://github.com/ros-planning/moveit/issues/2454>`_)
+* Contributors: AndyZe, JafarAbdi, Michael Görner, Peter Mitrano, Robert Haschke, Simon Schmeisser, Stuart Anderson, Thomas G, Tyler Weaver, sevangelatos, John Stechschulte
+
+1.0.7 (2020-11-20)
+------------------
+* [feature] Handle multiple link libraries for FCL (`#2325 <https://github.com/ros-planning/moveit/issues/2325>`_)
+* [feature] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* [feature] Add a utility to print collision pairs (`#2278 <https://github.com/ros-planning/moveit/issues/2278>`_) (`#2275 <https://github.com/ros-planning/moveit/issues/2275>`_)
+* [maint] Move constraint representation dox to moveit_tutorials (`#2147 <https://github.com/ros-planning/moveit/issues/2147>`_)
+* [maint] Update collision-related comments (`#2382 <https://github.com/ros-planning/moveit/issues/2382>`_)
+* Contributors: AndyZe, Dave Coleman, Felix von Drigalski, Ghenohenomohe, Robert Haschke
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Adapt repository for splitted moveit_resources layout (`#2199 <https://github.com/ros-planning/moveit/issues/2199>`_)
diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt
index 53a8acb171..a6bfb89d15 100644
--- a/moveit_core/CMakeLists.txt
+++ b/moveit_core/CMakeLists.txt
@@ -35,9 +35,13 @@ find_package(Eigen3 REQUIRED)
 
 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)
@@ -65,8 +69,11 @@ COMPONENTS
   urdf
   visualization_msgs
   xmlrpcpp
+  pybind11_catkin
 )
 
+catkin_python_setup()
+
 set(VERSION_FILE_PATH "${CATKIN_DEVEL_PREFIX}/include")
 # Pass the folder of the generated version.h to catkin_package() for export in devel-space
 # This is how gencpp adds the folder of generated message code to the include dirs, see:
@@ -97,6 +104,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS
     planning_request_adapter/include
     planning_scene/include
     profiler/include
+    python/tools/include
     sensor_manager/include
     trajectory_processing/include
     utils/include
@@ -121,6 +129,7 @@ catkin_package(
     moveit_constraint_samplers
     moveit_planning_request_adapter
     moveit_profiler
+    moveit_python_tools
     moveit_trajectory_processing
     moveit_distance_field
     moveit_collision_distance_field
@@ -215,3 +224,29 @@ add_subdirectory(distance_field)
 add_subdirectory(collision_distance_field)
 add_subdirectory(kinematics_metrics)
 add_subdirectory(dynamics_solver)
+
+add_subdirectory(python)
+set(pymoveit_libs
+  moveit_collision_detection
+  moveit_kinematic_constraints
+  moveit_planning_scene
+  moveit_python_tools
+  moveit_robot_model
+  moveit_robot_state
+  moveit_transforms
+)
+
+pybind_add_module(pymoveit_core
+    python/pymoveit_core.cpp
+    collision_detection/src/pycollision_detection.cpp
+    robot_model/src/pyrobot_model.cpp
+    robot_state/src/pyrobot_state.cpp
+    transforms/src/pytransforms.cpp
+    planning_scene/src/pyplanning_scene.cpp
+    kinematic_constraints/src/pykinematic_constraint.cpp
+)
+target_include_directories(pymoveit_core SYSTEM PRIVATE ${catkin_INCLUDE_DIRS})
+target_link_libraries(pymoveit_core PRIVATE ${pymoveit_libs} ${catkin_LIBRARIES})
+
+#catkin_lint: ignore_once undefined_target (pymoveit_core is defined by pybind_add_module)
+install(TARGETS pymoveit_core LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION})
diff --git a/moveit_core/background_processing/src/background_processing.cpp b/moveit_core/background_processing/src/background_processing.cpp
index f86c879260..f1fcba275e 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::thread>(boost::bind(&BackgroundProcessing::processingThread, this));
 }
 
 BackgroundProcessing::~BackgroundProcessing()
@@ -139,4 +139,4 @@ void BackgroundProcessing::clearJobUpdateEvent()
 }
 
 }  // end of namespace tools
-}  // end of namespace moveit
\ No newline at end of file
+}  // end of namespace moveit
diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt
index 21d3bcb5d3..9249588be5 100644
--- a/moveit_core/collision_detection/CMakeLists.txt
+++ b/moveit_core/collision_detection/CMakeLists.txt
@@ -3,6 +3,7 @@ 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/collision_common.cpp
   src/collision_matrix.cpp
   src/collision_octomap_filter.cpp
   src/collision_robot.cpp
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 edf7914d2e..76f60eb401 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
@@ -48,7 +48,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
@@ -153,6 +153,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;
 
@@ -331,19 +334,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)
   {
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 01335bc465..ac01f9a34d 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
@@ -43,7 +43,7 @@
 
 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
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 20ec95acae..83943db290 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
@@ -66,13 +66,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<bool(collision_detection::Contact&)> DecideContactFn;
 
-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
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..9c419b31e5 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
@@ -42,7 +42,7 @@
 
 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.
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h
index dcd1445c0d..ce49caf9ff 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h
@@ -45,7 +45,7 @@
 
 namespace collision_detection
 {
-MOVEIT_CLASS_FORWARD(CollisionRobot);
+MOVEIT_CLASS_FORWARD(CollisionRobot);  // Defines CollisionRobotPtr, 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
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 9166a9bfb8..bc7f121d89 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h
@@ -48,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
@@ -217,7 +217,7 @@ class World
   class ObserverHandle
   {
   public:
-    ObserverHandle() : observer_(NULL)
+    ObserverHandle() : observer_(nullptr)
     {
     }
 
@@ -246,7 +246,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);
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 620a9efd6d..38863fec14 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
@@ -42,7 +42,7 @@
 
 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
@@ -112,7 +112,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<std::string, World::Action> changes_;
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 <moveit/collision_detection/collision_common.h>
+
+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_world.cpp b/moveit_core/collision_detection/src/collision_world.cpp
index ec2f3abcf9..68bd11235f 100644
--- a/moveit_core/collision_detection/src/collision_world.cpp
+++ b/moveit_core/collision_detection/src/collision_world.cpp
@@ -93,4 +93,4 @@ void CollisionWorld::setWorld(const WorldPtr& world)
   world_const_ = world;
 }
 
-}  // end of namespace collision_detection
\ No newline at end of file
+}  // end of namespace collision_detection
diff --git a/moveit_core/collision_detection/src/pycollision_detection.cpp b/moveit_core/collision_detection/src/pycollision_detection.cpp
new file mode 100644
index 0000000000..f63e839d96
--- /dev/null
+++ b/moveit_core/collision_detection/src/pycollision_detection.cpp
@@ -0,0 +1,98 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, Peter Mitrano
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * The name of Peter Mitrano may not be used to endorse or promote
+ *     products derived from this software without specific prior
+ *     written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (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 */
+
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <pybind11/eigen.h>
+#include <moveit/collision_detection/world.h>
+#include <moveit/collision_detection/collision_matrix.h>
+#include <moveit/collision_detection/collision_common.h>
+#include <moveit/python/pybind_rosmsg_typecasters.h>
+
+namespace py = pybind11;
+
+using namespace collision_detection;
+
+void def_collision_detection_bindings(py::module& m)
+{
+  m.doc() = "contains collision detection, the world, and allowed collision matrices";
+  py::enum_<BodyType>(m, "BodyType")
+      .value("ROBOT_ATTACHED", BodyType::ROBOT_ATTACHED)
+      .value("ROBOT_LINK", BodyType::ROBOT_LINK)
+      .value("WORLD_OBJECT", BodyType::WORLD_OBJECT)
+      .export_values();
+  py::class_<Contact>(m, "Contact")
+      .def(py::init<>())
+      .def_readwrite("body_name_1", &Contact::body_name_1)
+      .def_readwrite("body_name_2", &Contact::body_name_2)
+      .def_readwrite("body_type_1", &Contact::body_type_1)
+      .def_readwrite("body_type_2", &Contact::body_type_2)
+      .def_readwrite("depth", &Contact::depth)
+      .def_readwrite("normal", &Contact::normal)
+      .def_readwrite("pos", &Contact::pos)
+      //
+      ;
+  py::class_<CollisionRequest>(m, "CollisionRequest")
+      .def(py::init<>())
+      .def_readwrite("contacts", &CollisionRequest::contacts)
+      .def_readwrite("cost", &CollisionRequest::cost)
+      .def_readwrite("distance", &CollisionRequest::distance)
+      .def_readwrite("group_name", &CollisionRequest::group_name)
+      .def_readwrite("is_done", &CollisionRequest::is_done)
+      .def_readwrite("max_contacts", &CollisionRequest::max_contacts)
+      .def_readwrite("max_contacts_per_pair", &CollisionRequest::max_contacts_per_pair)
+      .def_readwrite("max_cost_sources", &CollisionRequest::max_cost_sources)
+      .def_readwrite("verbose", &CollisionRequest::verbose)
+      //
+      ;
+  py::class_<CollisionResult>(m, "CollisionResult")
+      .def(py::init<>())
+      .def_readwrite("collision", &CollisionResult::collision)
+      .def_readwrite("contact_count", &CollisionResult::contact_count)
+      .def_readwrite("contacts", &CollisionResult::contacts)
+      .def_readwrite("cost_sources", &CollisionResult::cost_sources)
+      .def_readwrite("distance", &CollisionResult::distance)
+      .def("clear", &CollisionResult::clear)
+      //
+      ;
+  py::class_<AllowedCollisionMatrix>(m, "AllowedCollisionMatrix")
+      .def(py::init<>())
+      .def("setEntry",
+           py::overload_cast<const std::string&, const std::string&, bool>(&AllowedCollisionMatrix::setEntry))
+      //
+      ;
+  py::class_<World, WorldPtr>(m, "World").def(py::init<>());
+}
diff --git a/moveit_core/collision_detection/test/test_all_valid.cpp b/moveit_core/collision_detection/test/test_all_valid.cpp
index 47a33a81cf..16aaab8a64 100644
--- a/moveit_core/collision_detection/test/test_all_valid.cpp
+++ b/moveit_core/collision_detection/test/test_all_valid.cpp
@@ -58,4 +58,4 @@ 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/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 <moveit/collision_detection_bullet/collision_env_bullet.h>
+#include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>
+#include <moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h>
+#include <moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h>
+#include <boost/bind.hpp>
+#include <bullet/btBulletCollisionCommon.h>
+
+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<const std::string, urdf::LinkSharedPtr>& 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<const std::string, urdf::LinkSharedPtr>& 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<const std::string, urdf::LinkSharedPtr>& 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<collision_detection_bullet::CollisionObjectWrapperPtr> 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<collision_detection_bullet::CollisionObjectWrapperPtr> 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<collision_detection_bullet::CollisionObjectWrapperPtr> 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_detection_bullet::CollisionObjectType> 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<collision_detection_bullet::CollisionObjectWrapperPtr>& cows) const
+{
+  std::vector<const moveit::core::AttachedBody*> 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_detection_bullet::CollisionObjectType> 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<std::string>& 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<urdf::CollisionSharedPtr>& col_array =
+        link->collision_array.empty() ? std::vector<urdf::CollisionSharedPtr>(1, link->collision) :
+                                        link->collision_array;
+
+    std::vector<shapes::ShapeConstPtr> shapes;
+    collision_detection_bullet::AlignedVector<Eigen::Isometry3d> shape_poses;
+    std::vector<collision_detection_bullet::CollisionObjectType> 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<double>::epsilon() ||
+              fabs(getLinkPadding(link->name)) >= std::numeric_limits<double>::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_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt
index 51415e69b0..7ebf674b9d 100644
--- a/moveit_core/collision_detection_fcl/CMakeLists.txt
+++ b/moveit_core/collision_detection_fcl/CMakeLists.txt
@@ -27,4 +27,7 @@ install(FILES ../collision_detector_fcl_description.xml DESTINATION ${CATKIN_PAC
 if(CATKIN_ENABLE_TESTING)
   catkin_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection.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)
+
 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 f7dff1dec5..1eeaa2a3aa 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
@@ -139,12 +139,12 @@ struct CollisionGeometryData
 /** \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)
   {
   }
 
@@ -273,7 +273,7 @@ struct FCLManager
  *   \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 */
+ *   \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
@@ -282,7 +282,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
  *   \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 */
+ *   \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);
 
 /** \brief Create new FCLGeometry object out of robot link model. */
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 81e3995bab..2248d94ca0 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
@@ -13,7 +13,7 @@ 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;
 };
 }  // namespace collision_detection
-#endif  // MOVEIT_COLLISION_DETECTION_FCL_COLLISION_DETECTOR_FCL_PLUGIN_LOADER_H_
\ No newline at end of file
+#endif  // MOVEIT_COLLISION_DETECTION_FCL_COLLISION_DETECTOR_FCL_PLUGIN_LOADER_H_
diff --git a/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp
index 9b8a4cea68..9165e379c0 100644
--- a/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp
+++ b/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp
@@ -85,7 +85,8 @@ void CollisionRobotFCL::getAttachedBodyObjects(const robot_state::AttachedBody*
   const std::vector<shapes::ShapeConstPtr>& shapes = ab->getShapes();
   for (std::size_t i = 0; i < shapes.size(); ++i)
   {
-    FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], ab, i);
+    FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], getLinkScale(ab->getAttachedLinkName()),
+                                                     getLinkPadding(ab->getAttachedLinkName()), ab, i);
     if (co)
       geoms.push_back(co);
   }
diff --git a/moveit_core/collision_detector_fcl_description.xml b/moveit_core/collision_detector_fcl_description.xml
index 0bd2cfbdb1..f96b06c05e 100644
--- a/moveit_core/collision_detector_fcl_description.xml
+++ b/moveit_core/collision_detector_fcl_description.xml
@@ -5,4 +5,4 @@
       FCL Collision Detector, default for MoveIt.
     </description>
   </class>
-</library>
\ No newline at end of file
+</library>
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 7f5e957a2e..fbae8af8a7 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
@@ -106,7 +106,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)
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_robot_distance_field.h
index 4ecc0fec48..7b292f157b 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_robot_distance_field.h
@@ -54,7 +54,7 @@ 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(CollisionRobotDistanceField);  // Defines CollisionRobotDistanceFieldPtr, ConstPtr, WeakPtr... etc
 
 class CollisionRobotDistanceField : public CollisionRobot
 {
diff --git a/moveit_core/constraint_samplers/dox/constraint_samplers.dox b/moveit_core/constraint_samplers/dox/constraint_samplers.dox
index 45205f8b6e..86a82fb560 100644
--- a/moveit_core/constraint_samplers/dox/constraint_samplers.dox
+++ b/moveit_core/constraint_samplers/dox/constraint_samplers.dox
@@ -4,4 +4,4 @@
 See constraint_samplers namespace
 
 
-*/
\ No newline at end of file
+*/
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 d41541f025..3b1ed03da2 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
@@ -51,7 +51,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
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 1f5d56ad15..65dd147eb5 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
@@ -42,7 +42,7 @@
 
 namespace constraint_samplers
 {
-MOVEIT_CLASS_FORWARD(ConstraintSamplerAllocator);
+MOVEIT_CLASS_FORWARD(ConstraintSamplerAllocator);  // Defines ConstraintSamplerAllocatorPtr, ConstPtr, WeakPtr... etc
 
 class ConstraintSamplerAllocator
 {
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 8ebcd6a48c..69f1eb8b0e 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
@@ -42,7 +42,7 @@
 
 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
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 e6068653e0..6d424493be 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
@@ -43,7 +43,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
@@ -280,7 +280,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.
diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
index 643d4d3418..49e35a6710 100644
--- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
+++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
@@ -647,4 +647,4 @@ bool IKConstraintSampler::callIK(const geometry_msgs::Pose& ik_query,
   return false;
 }
 
-}  // end of namespace constraint_samplers
\ No newline at end of file
+}  // end of namespace constraint_samplers
diff --git a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp
index 84dbc233f9..ceb6801279 100644
--- a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp
+++ b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp
@@ -161,4 +161,4 @@ bool UnionConstraintSampler::project(robot_state::RobotState& state, unsigned in
   return true;
 }
 
-}  // end of namespace constraint_samplers
\ No newline at end of file
+}  // end of namespace constraint_samplers
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 c5db73b02e..f25288a81b 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
@@ -99,7 +99,7 @@ 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. */
 class MoveItControllerHandle
@@ -147,7 +147,7 @@ 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.
     To make your controllers usable by MoveIt, this interface needs to be implemented.
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 569cc64015..69340a33d3 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
@@ -48,7 +48,7 @@
 
 namespace shapes
 {
-MOVEIT_CLASS_FORWARD(Shape);
+MOVEIT_CLASS_FORWARD(Shape);  // Defines ShapePtr, ConstPtr, WeakPtr... etc
 }
 namespace octomap
 {
@@ -72,7 +72,7 @@ enum PlaneVisualizationType
   YZPlane
 };
 
-MOVEIT_CLASS_FORWARD(DistanceField);
+MOVEIT_CLASS_FORWARD(DistanceField);  // Defines DistanceFieldPtr, ConstPtr, WeakPtr... etc
 
 /**
  * \brief DistanceField is an abstract base class for computing
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 128eb925db..c4f50782a0 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
@@ -417,20 +417,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;
   }
 
   /**
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..34ed83c18d 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
@@ -342,7 +342,7 @@ class VoxelGrid
 template <typename T>
 VoxelGrid<T>::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 +369,7 @@ void VoxelGrid<T>::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;
diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp
index 071777706b..ed9133c34e 100644
--- a/moveit_core/distance_field/src/distance_field.cpp
+++ b/moveit_core/distance_field/src/distance_field.cpp
@@ -557,4 +557,4 @@ void DistanceField::getProjectionPlanes(const std::string& frame_id, const ros::
     delete[] z_projection;
 }
 
-}  // end of namespace distance_field
\ No newline at end of file
+}  // end of namespace distance_field
diff --git a/moveit_core/doc/_templates/custom-class-template.rst b/moveit_core/doc/_templates/custom-class-template.rst
new file mode 100644
index 0000000000..f73eda50ec
--- /dev/null
+++ b/moveit_core/doc/_templates/custom-class-template.rst
@@ -0,0 +1,34 @@
+{{ fullname | escape | underline}}
+
+.. currentmodule:: {{ module }}
+
+.. autoclass:: {{ objname }}
+   :members:
+   :show-inheritance:
+   :inherited-members:
+   :special-members: __call__, __add__, __mul__
+
+   {% block methods %}
+   {% if methods %}
+   .. rubric:: {{ _('Methods') }}
+
+   .. autosummary::
+      :nosignatures:
+   {% for item in methods %}
+      {%- if not item.startswith('_') %}
+      ~{{ name }}.{{ item }}
+      {%- endif -%}
+   {%- endfor %}
+   {% endif %}
+   {% endblock %}
+
+   {% block attributes %}
+   {% if attributes %}
+   .. rubric:: {{ _('Attributes') }}
+
+   .. autosummary::
+   {% for item in attributes %}
+      ~{{ name }}.{{ item }}
+   {%- endfor %}
+   {% endif %}
+   {% endblock %}
diff --git a/moveit_core/doc/_templates/custom-module-template.rst b/moveit_core/doc/_templates/custom-module-template.rst
new file mode 100644
index 0000000000..d066d0e4dc
--- /dev/null
+++ b/moveit_core/doc/_templates/custom-module-template.rst
@@ -0,0 +1,66 @@
+{{ fullname | escape | underline}}
+
+.. automodule:: {{ fullname }}
+
+   {% block attributes %}
+   {% if attributes %}
+   .. rubric:: Module attributes
+
+   .. autosummary::
+      :toctree:
+   {% for item in attributes %}
+      {{ item }}
+   {%- endfor %}
+   {% endif %}
+   {% endblock %}
+
+   {% block functions %}
+   {% if functions %}
+   .. rubric:: {{ _('Functions') }}
+
+   .. autosummary::
+      :toctree:
+      :nosignatures:
+   {% for item in functions %}
+      {{ item }}
+   {%- endfor %}
+   {% endif %}
+   {% endblock %}
+
+   {% block classes %}
+   {% if classes %}
+   .. rubric:: {{ _('Classes') }}
+
+   .. autosummary::
+      :toctree:
+      :template: custom-class-template.rst
+      :nosignatures:
+   {% for item in classes %}
+      {{ item }}
+   {%- endfor %}
+   {% endif %}
+   {% endblock %}
+
+   {% block exceptions %}
+   {% if exceptions %}
+   .. rubric:: {{ _('Exceptions') }}
+
+   .. autosummary::
+      :toctree:
+   {% for item in exceptions %}
+      {{ item }}
+   {%- endfor %}
+   {% endif %}
+   {% endblock %}
+
+{% block modules %}
+{% if modules %}
+.. autosummary::
+   :toctree:
+   :template: custom-module-template.rst
+   :recursive:
+{% for item in modules %}
+   {{ item }}
+{%- endfor %}
+{% endif %}
+{% endblock %}
diff --git a/moveit_core/doc/api.rst b/moveit_core/doc/api.rst
new file mode 100644
index 0000000000..58c6bd1760
--- /dev/null
+++ b/moveit_core/doc/api.rst
@@ -0,0 +1,6 @@
+.. autosummary::
+   :toctree: _autosummary
+   :template: custom-module-template.rst
+   :recursive:
+
+   moveit.core
diff --git a/moveit_core/doc/conf.py b/moveit_core/doc/conf.py
new file mode 100644
index 0000000000..0ae380d607
--- /dev/null
+++ b/moveit_core/doc/conf.py
@@ -0,0 +1,200 @@
+# -*- coding: utf-8 -*-
+#
+# Configuration file for the Sphinx documentation builder.
+#
+# This file does only contain a selection of the most common options. For a
+# full list see the documentation:
+# http://www.sphinx-doc.org/en/master/config
+
+# -- Path setup --------------------------------------------------------------
+
+# If extensions (or modules to document with autodoc) are in another directory,
+# add these directories to sys.path here. If the directory is relative to the
+# documentation root, use os.path.abspath to make it absolute, like shown here.
+#
+import os
+import sys
+import sphinx_rtd_theme
+
+
+# -- Project information -----------------------------------------------------
+
+project = "moveit.core"
+copyright = "2021, MoveIt maintainer team"
+author = "MoveIt maintainer team"
+
+# The short X.Y version
+version = ""
+# The full version, including alpha/beta/rc tags
+release = ""
+
+
+# -- General configuration ---------------------------------------------------
+
+# If your documentation needs a minimal Sphinx version, state it here.
+#
+# needs_sphinx = '1.0'
+
+# Add any Sphinx extension module names here, as strings. They can be
+# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
+# ones.
+extensions = [
+    "sphinx.ext.autodoc",
+    "sphinx.ext.autosummary",
+    "sphinx.ext.intersphinx",
+    "sphinx_rtd_theme",
+]
+
+# NOTE: Important variables that make auto-generation work,
+#  see https://www.sphinx-doc.org/en/master/usage/extensions/autodoc.html
+autosummary_generate = True
+autosummary_imported_members = True
+autoclass_content = "both"  # Add __init__ doc (ie. params) to class summaries
+
+# Customization, not as important
+html_show_sourcelink = (
+    True  # Remove 'view source code' from top of page (for html, not python)
+)
+autodoc_inherit_docstrings = True  # If no docstring, inherit from base class
+set_type_checking_flag = True  # Enable 'expensive' imports for sphinx_autodoc_typehints
+add_module_names = False
+
+
+# Add any paths that contain templates here, relative to this directory.
+templates_path = ["_templates"]
+
+# The suffix(es) of source filenames.
+# You can specify multiple suffix as a list of string:
+#
+# source_suffix = ['.rst', '.md']
+source_suffix = ".rst"
+
+# The master toctree document.
+master_doc = "index"
+
+# The language for content autogenerated by Sphinx. Refer to documentation
+# for a list of supported languages.
+#
+# This is also used if you do content translation via gettext catalogs.
+# Usually you set "language" from the command line for these cases.
+language = None
+
+# List of patterns, relative to source directory, that match files and
+# directories to ignore when looking for source files.
+# This pattern also affects html_static_path and html_extra_path.
+exclude_patterns = ["_build", "Thumbs.db", ".DS_Store", "_templates"]
+
+# The name of the Pygments (syntax highlighting) style to use.
+pygments_style = None
+
+
+# -- Options for HTML output -------------------------------------------------
+
+# The theme to use for HTML and HTML Help pages.  See the documentation for
+# a list of builtin themes.
+#
+html_theme = "sphinx_rtd_theme"
+
+
+# Theme options are theme-specific and customize the look and feel of a theme
+# further.  For a list of options available for each theme, see the
+# documentation.
+#
+# html_theme_options = {}
+
+# Add any paths that contain custom static files (such as style sheets) here,
+# relative to this directory. They are copied after the builtin static files,
+# so a file named "default.css" will overwrite the builtin "default.css".
+html_static_path = ["_static"]
+
+# Custom sidebar templates, must be a dictionary that maps document names
+# to template names.
+#
+# The default sidebars (for documents that don't match any pattern) are
+# defined by theme itself.  Builtin themes are using these templates by
+# default: ``['localtoc.html', 'relations.html', 'sourcelink.html',
+# 'searchbox.html']``.
+#
+# html_sidebars = {}
+
+
+# -- Options for HTMLHelp output ---------------------------------------------
+
+# Output file base name for HTML help builder.
+htmlhelp_basename = "moveit_coredoc"
+
+
+# -- Options for LaTeX output ------------------------------------------------
+
+latex_elements = {
+    # The paper size ('letterpaper' or 'a4paper').
+    #
+    # 'papersize': 'letterpaper',
+    # The font size ('10pt', '11pt' or '12pt').
+    #
+    # 'pointsize': '10pt',
+    # Additional stuff for the LaTeX preamble.
+    #
+    # 'preamble': '',
+    # Latex figure (float) alignment
+    #
+    # 'figure_align': 'htbp',
+}
+
+# Grouping the document tree into LaTeX files. List of tuples
+# (source start file, target name, title,
+#  author, documentclass [howto, manual, or own class]).
+latex_documents = [
+    (master_doc, "moveit_core.tex", "moveit\\_core Documentation", "peter", "manual"),
+]
+
+
+# -- Options for manual page output ------------------------------------------
+
+# One entry per manual page. List of tuples
+# (source start file, name, description, authors, manual section).
+man_pages = [(master_doc, "moveit_core", "moveit_core Documentation", [author], 1)]
+
+
+# -- Options for Texinfo output ----------------------------------------------
+
+# Grouping the document tree into Texinfo files. List of tuples
+# (source start file, target name, title, author,
+#  dir menu entry, description, category)
+texinfo_documents = [
+    (
+        master_doc,
+        "moveit_core",
+        "moveit_core Documentation",
+        author,
+        "moveit_core",
+        "One line description of project.",
+        "Miscellaneous",
+    ),
+]
+
+
+# -- Options for Epub output -------------------------------------------------
+
+# Bibliographic Dublin Core info.
+epub_title = project
+
+# The unique identifier of the text. This can be a ISBN number
+# or the project homepage.
+#
+# epub_identifier = ''
+
+# A unique identification for the text.
+#
+# epub_uid = ''
+
+# A list of files that should not be packed into the epub file.
+epub_exclude_files = ["search.html"]
+
+
+# -- Extension configuration -------------------------------------------------
+
+# -- Options for intersphinx extension ---------------------------------------
+
+# Example configuration for intersphinx: refer to the Python standard library.
+intersphinx_mapping = {"https://docs.python.org/3": None}
diff --git a/moveit_core/doc/index.rst b/moveit_core/doc/index.rst
new file mode 100644
index 0000000000..9c719062c5
--- /dev/null
+++ b/moveit_core/doc/index.rst
@@ -0,0 +1,13 @@
+Python API Reference: moveit_core
+=======================================
+
+.. note::
+    These docs are autogenerated. You may see reference to module names like pymoveit_core,
+    but you should not use these in your code. Instead, use the modules under the `moveit` namespace.
+    For example, you should write ``import moveit.core`` instead of ``import pymoveit_core``.
+    The latter will still work, but will not contain the full API that ``import moveit.core`` does.
+
+.. toctree::
+
+    Home page <self>
+    API reference <api.rst>
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 a454c99749..54bfaa2a19 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
@@ -50,7 +50,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
diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp
index 0ff30f9dec..530d5d0799 100644
--- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp
+++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp
@@ -313,4 +313,4 @@ const std::vector<double>& DynamicsSolver::getMaxTorques() const
   return max_torques_;
 }
 
-}  // end of namespace dynamics_solver
\ No newline at end of file
+}  // end of namespace dynamics_solver
diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt
index 619f7a4229..31198de4aa 100644
--- a/moveit_core/kinematic_constraints/CMakeLists.txt
+++ b/moveit_core/kinematic_constraints/CMakeLists.txt
@@ -20,4 +20,7 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
 if(CATKIN_ENABLE_TESTING)
   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 a197e00c41..0698d71480 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
@@ -72,7 +72,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
@@ -145,7 +145,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
   {
   }
 
@@ -179,7 +179,7 @@ class KinematicConstraint
                                 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.
@@ -209,7 +209,7 @@ 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)
+    : KinematicConstraint(model), joint_model_(nullptr), joint_variable_index_(-1)
   {
     type_ = JOINT_CONSTRAINT;
   }
@@ -332,7 +332,7 @@ class JointConstraint : public KinematicConstraint
   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
@@ -356,7 +356,7 @@ 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 robot_model::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(nullptr)
   {
     type_ = ORIENTATION_CONSTRAINT;
   }
@@ -487,7 +487,7 @@ class OrientationConstraint : public KinematicConstraint
       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 +513,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 robot_model::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(nullptr)
   {
     type_ = POSITION_CONSTRAINT;
   }
@@ -648,7 +648,7 @@ class PositionConstraint : public KinematicConstraint
   const robot_model::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
@@ -848,7 +848,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
diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
index 847cb84b73..749cc61e35 100644
--- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
+++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
@@ -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 <typename Derived>
+std::tuple<Eigen::Matrix<typename Eigen::MatrixBase<Derived>::Scalar, 3, 1>, bool>
+CalcEulerAngles(const Eigen::MatrixBase<Derived>& 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<Derived>::Scalar;
+  const Index i = 0;
+  const Index j = 1;
+  const Index k = 2;
+  Eigen::Matrix<Scalar, 3, 1> 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<Scalar>::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<double>::epsilon())
 {
 }
@@ -600,24 +652,41 @@ ConstraintEvaluationResult OrientationConstraint::decide(const robot_state::Robo
   if (!link_model_)
     return ConstraintEvaluationResult(true, 0.0);
 
-  Eigen::Vector3d xyz;
+  std::tuple<Eigen::Vector3d, bool> 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<Eigen::Vector3d>(euler_angles_error);
+  if (!std::get<bool>(euler_angles_error))
+  {
+    if (normalizeAbsoluteAngle(xyz(0)) > absolute_z_axis_tolerance_ + std::numeric_limits<double>::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<double>() - fabs(xyz(0)));
-  xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
-  xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - 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<double>::epsilon() &&
                 xyz(1) < absolute_y_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
                 xyz(0) < absolute_x_axis_tolerance_ + std::numeric_limits<double>::epsilon();
diff --git a/moveit_core/kinematic_constraints/src/pykinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/pykinematic_constraint.cpp
new file mode 100644
index 0000000000..ef3e20710e
--- /dev/null
+++ b/moveit_core/kinematic_constraints/src/pykinematic_constraint.cpp
@@ -0,0 +1,77 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, Peter Mitrano
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * The name of Peter Mitrano may not be used to endorse or promote
+ *     products derived from this software without specific prior
+ *     written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (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 */
+
+#include <pybind11/pybind11.h>
+#include <pybind11/numpy.h>
+#include <pybind11/eigen.h>
+#include <pybind11/stl.h>
+#include <moveit/python/pybind_rosmsg_typecasters.h>
+
+#include <moveit/kinematic_constraints/kinematic_constraint.h>
+#include <moveit/kinematic_constraints/utils.h>
+
+namespace py = pybind11;
+
+using namespace kinematic_constraints;
+
+void def_kinematic_constraints_bindings(py::module& m)
+{
+  m.doc() = "Class for joint, position, visibility, and other constraints";
+
+  py::class_<ConstraintEvaluationResult, std::shared_ptr<ConstraintEvaluationResult>>(m, "ConstraintEvaluationResult")
+      .def(py::init<>())
+      .def_readwrite("satisfied", &ConstraintEvaluationResult::satisfied)
+      .def_readwrite("distance", &ConstraintEvaluationResult::distance)
+      //
+      ;
+
+  py::class_<KinematicConstraintSet, KinematicConstraintSetPtr>(m, "KinematicConstraintSet")
+      .def(py::init<robot_model::RobotModelConstPtr>(), py::arg("robot_model"))
+      .def("add", py::overload_cast<const moveit_msgs::Constraints&, const moveit::core::Transforms&>(
+                      &KinematicConstraintSet::add))
+      .def("decide",
+           py::overload_cast<const moveit::core::RobotState&, bool>(&KinematicConstraintSet::decide, py::const_),
+           py::arg("state"), py::arg("verbose") = false)
+      //
+      ;
+
+  m.def("constructGoalConstraints",
+        py::overload_cast<const std::string&, const geometry_msgs::PoseStamped&, double, double>(
+            &constructGoalConstraints),
+        py::arg("link_name"), py::arg("pose"), py::arg("tolerance_pos") = 1e-3, py::arg("tolerance_angle") = 1e-2)
+      //
+      ;
+}
diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp
index ee12c491a2..a7add9a709 100644
--- a/moveit_core/kinematic_constraints/src/utils.cpp
+++ b/moveit_core/kinematic_constraints/src/utils.cpp
@@ -144,8 +144,8 @@ moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState&
   {
     goal.joint_constraints[i].joint_name = jmg->getVariableNames()[i];
     goal.joint_constraints[i].position = vals[i];
-    goal.joint_constraints[i].tolerance_above = tolerance_below;
-    goal.joint_constraints[i].tolerance_below = tolerance_above;
+    goal.joint_constraints[i].tolerance_above = tolerance_above;
+    goal.joint_constraints[i].tolerance_below = tolerance_below;
     goal.joint_constraints[i].weight = 1.0;
   }
 
diff --git a/moveit_core/kinematic_constraints/test/test_constraints.cpp b/moveit_core/kinematic_constraints/test/test_constraints.cpp
index 4c6cd14eba..1817847bb3 100644
--- a/moveit_core/kinematic_constraints/test/test_constraints.cpp
+++ b/moveit_core/kinematic_constraints/test/test_constraints.cpp
@@ -40,6 +40,7 @@
 #include <fstream>
 #include <tf2_eigen/tf2_eigen.h>
 #include <moveit/utils/robot_model_test_utils.h>
+#include <boost/math/constants/constants.hpp>
 
 class LoadPlanningModelsPr2 : public testing::Test
 {
@@ -652,6 +653,12 @@ 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<double>();
+  robot_state.setVariablePositions(jvals);
+  robot_state.update();
+  EXPECT_FALSE(oc.decide(robot_state).satisfied);
 }
 
 TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple)
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 <moveit/kinematic_constraints/kinematic_constraint.h>
+#include <gtest/gtest.h>
+#include <urdf_parser/urdf_parser.h>
+#include <tf2_eigen/tf2_eigen.h>
+#include <moveit/utils/robot_model_test_utils.h>
+#include <boost/math/constants/constants.hpp>
+
+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<std::string, double> getJointValues(const double roll, const double pitch, const double yaw)
+  {
+    std::map<std::string, double> 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<double>(), 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<double>(), 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<double>(), 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<double>(), 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<double>(), 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<double>(), 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<double>(), 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<double>(), 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<double>(), 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<double>(), 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<double>(), 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<double>(), 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<double>(), 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<double>(), 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<double>(), 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/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
index fb8cc98f7f..9b7e68be53 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
@@ -76,7 +76,7 @@ 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. */
 };
-}
+}  // namespace DiscretizationMethods
 typedef DiscretizationMethods::DiscretizationMethod DiscretizationMethod;
 
 /*
@@ -98,7 +98,7 @@ enum KinematicError
   NO_SOLUTION                          /**< A valid joint solution that can reach this pose(s) could not be found */
 
 };
-}
+}  // namespace KinematicErrors
 typedef KinematicErrors::KinematicError KinematicError;
 
 /**
@@ -136,7 +136,7 @@ struct KinematicsResult
                                        of solutions explored. */
 };
 
-MOVEIT_CLASS_FORWARD(KinematicsBase);
+MOVEIT_CLASS_FORWARD(KinematicsBase);  // Defines KinematicsBasePtr, ConstPtr, WeakPtr... etc
 
 /**
  * @class KinematicsBase
@@ -298,7 +298,7 @@ class KinematicsBase
                    double timeout, const std::vector<double>& consistency_limits, std::vector<double>& 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
   {
     // For IK solvers that do not support multiple poses, fall back to single pose call
     if (ik_poses.size() == 1)
@@ -508,7 +508,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
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 280099eb26..5147b10c98 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
@@ -42,7 +42,7 @@
 /** @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
diff --git a/moveit_core/package.xml b/moveit_core/package.xml
index 04ae1a6a24..63df6e1741 100644
--- a/moveit_core/package.xml
+++ b/moveit_core/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_core</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Core libraries used by MoveIt!</description>
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="robot.moveit@gmail.com">Sachin Chitta</author>
@@ -34,6 +34,7 @@
   <depend>moveit_msgs</depend>
   <depend>octomap</depend>
   <depend>octomap_msgs</depend>
+  <depend>pybind11_catkin</depend>
   <depend>random_numbers</depend>
   <depend>roslib</depend>
   <depend>rostime</depend>
@@ -48,6 +49,8 @@
   <depend>visualization_msgs</depend>
   <depend>xmlrpcpp</depend>
 
+  <doc_depend>python3-sphinx-rtd-theme</doc_depend>
+
   <test_depend>moveit_resources_panda_moveit_config</test_depend>
   <test_depend>moveit_resources_pr2_description</test_depend>
   <test_depend>angles</test_depend>
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 cc09b7d424..d4f6fd7711 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
@@ -45,12 +45,19 @@
 
 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 */
 namespace planning_interface
 {
+/** \brief Retrieve NodeHandle/namespace defining the PlanningPipeline parameters
+ * Traditionally, these were directly defined in the private namespace of the move_group node.
+ * Since MoveIt 1.1.2 multiple pipeline configs are supported in parallel.
+ * In Melodic we support this new scheme by allowing to choose the default pipeline,
+ * specified via the parameter ~default_planning_pipeline. */
+ros::NodeHandle getConfigNodeHandle(const ros::NodeHandle& nh = ros::NodeHandle("~"));
+
 /**
    \brief Specify the settings for a particular planning algorithm, for a particular group. The Planner plugin uses
    these settings to configure the algorithm.
@@ -74,7 +81,7 @@ struct PlannerConfigurationSettings
 /** \brief Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings */
 typedef std::map<std::string, PlannerConfigurationSettings> 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,7 +152,7 @@ class PlanningContext
   MotionPlanRequest request_;
 };
 
-MOVEIT_CLASS_FORWARD(PlannerManager);
+MOVEIT_CLASS_FORWARD(PlannerManager);  // Defines PlannerManagerPtr, ConstPtr, WeakPtr... etc
 
 /** \brief Base class for a MoveIt! planner */
 class PlannerManager
diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp
index 26c393c997..65a706ed19 100644
--- a/moveit_core/planning_interface/src/planning_interface.cpp
+++ b/moveit_core/planning_interface/src/planning_interface.cpp
@@ -56,6 +56,16 @@ static ActiveContexts& getActiveContexts()
 }
 }  // namespace
 
+ros::NodeHandle getConfigNodeHandle(const ros::NodeHandle& nh)
+{
+  // Handle new configuration scheme with multiple pipelines configured in namespaces planning_pipelines/*
+  std::string default_pipeline;
+  if (nh.getParam("default_planning_pipeline", default_pipeline) &&
+      nh.hasParam("planning_pipelines/" + default_pipeline))
+    return ros::NodeHandle(nh, "planning_pipelines/" + default_pipeline);
+  return nh;
+}
+
 PlanningContext::PlanningContext(const std::string& name, const std::string& group) : name_(name), group_(group)
 {
   ActiveContexts& ac = getActiveContexts();
@@ -127,4 +137,4 @@ void PlannerManager::terminate() const
     (*it)->terminate();
 }
 
-}  // end of namespace planning_interface
\ No newline at end of file
+}  // end of namespace planning_interface
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 19c95e396c..58396b4e72 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
@@ -45,7 +45,7 @@
 /** \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
 {
diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp
index a62ac0acb3..37b942b2cf 100644
--- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp
+++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp
@@ -166,4 +166,4 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner
   }
 }
 
-}  // end of namespace planning_request_adapter
\ No newline at end of file
+}  // end of namespace planning_request_adapter
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 8cfc019030..e40907e7ed 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
@@ -59,7 +59,7 @@
 /** \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).
@@ -873,14 +873,14 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
   /** \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<std::size_t>* invalid_index = NULL) const;
+                   std::vector<std::size_t>* 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<std::size_t>* invalid_index = NULL) const;
+                   bool verbose = false, std::vector<std::size_t>* 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
@@ -888,7 +888,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<std::size_t>* invalid_index = NULL) const;
+                   std::vector<std::size_t>* 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
@@ -896,7 +896,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<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
-                   bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
+                   bool verbose = false, std::vector<std::size_t>* 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
@@ -904,7 +904,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<moveit_msgs::Constraints>& goal_constraints, const std::string& group = "",
-                   bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;
+                   bool verbose = false, std::vector<std::size_t>* 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
@@ -912,17 +912,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<std::size_t>* invalid_index = NULL) const;
+                   std::vector<std::size_t>* 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<std::size_t>* invalid_index = NULL) const;
+                   bool verbose = false, std::vector<std::size_t>* 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<std::size_t>* invalid_index = NULL) const;
+                   bool verbose = false, std::vector<std::size_t>* 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 */
diff --git a/moveit_core/planning_scene/src/pyplanning_scene.cpp b/moveit_core/planning_scene/src/pyplanning_scene.cpp
new file mode 100644
index 0000000000..a9e5ebe2ad
--- /dev/null
+++ b/moveit_core/planning_scene/src/pyplanning_scene.cpp
@@ -0,0 +1,115 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, Peter Mitrano
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * The name of Peter Mitrano may not be used to endorse or promote
+ *     products derived from this software without specific prior
+ *     written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (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 */
+
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <moveit/python/pybind_rosmsg_typecasters.h>
+
+#include <moveit/planning_scene/planning_scene.h>
+
+namespace py = pybind11;
+using namespace planning_scene;
+
+void def_planning_scene_bindings(py::module& m)
+{
+  m.doc() = "The planning scene represents the state of the world and the robot, "
+            "and can be used for collision checking";
+
+  py::class_<PlanningScene, PlanningScenePtr>(m, "PlanningScene")
+      .def(py::init<const moveit::core::RobotModelConstPtr&, const collision_detection::WorldPtr&>(),
+           py::arg("robot_model"), py::arg("world") = collision_detection::WorldPtr(new collision_detection::World()))
+      .def("checkSelfCollision",
+           py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&>(
+               &PlanningScene::checkSelfCollision, py::const_))
+      .def("checkSelfCollision",
+           py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&,
+                             const robot_state::RobotState&, const collision_detection::AllowedCollisionMatrix&>(
+               &PlanningScene::checkSelfCollision, py::const_))
+      .def("checkCollision",
+           py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&>(
+               &PlanningScene::checkCollision, py::const_))
+      .def("checkCollision",
+           py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&,
+                             const robot_state::RobotState&, const collision_detection::AllowedCollisionMatrix&>(
+               &PlanningScene::checkCollision, py::const_))
+      .def("getCurrentStateNonConst", &PlanningScene::getCurrentStateNonConst)
+      .def("getCurrentState", &PlanningScene::getCurrentState)
+      .def("getAllowedCollisionMatrix", &PlanningScene::getAllowedCollisionMatrix)
+      .def("isStateConstrained",
+           py::overload_cast<const moveit_msgs::RobotState&, const kinematic_constraints::KinematicConstraintSet&, bool>(
+               &PlanningScene::isStateConstrained, py::const_),
+           py::arg("state"), py::arg("constr"), py::arg("verbose") = false)
+      .def(
+          "isStateConstrained",
+          py::overload_cast<const moveit::core::RobotState&, const kinematic_constraints::KinematicConstraintSet&, bool>(
+              &PlanningScene::isStateConstrained, py::const_),
+          py::arg("state"), py::arg("constr"), py::arg("verbose") = false)
+      .def("isStateConstrained",
+           py::overload_cast<const moveit_msgs::RobotState&, const moveit_msgs::Constraints&, bool>(
+               &PlanningScene::isStateConstrained, py::const_),
+           py::arg("state"), py::arg("constr"), py::arg("verbose") = false)
+      .def("isStateConstrained",
+           py::overload_cast<const moveit::core::RobotState&, const moveit_msgs::Constraints&, bool>(
+               &PlanningScene::isStateConstrained, py::const_),
+           py::arg("state"), py::arg("constr"), py::arg("verbose") = false)
+      .def("getTransforms", py::overload_cast<>(&PlanningScene::getTransforms, py::const_),
+           py::return_value_policy::reference)
+      //      .def("setStateFeasibilityPredicate", &PlanningScene::setStateFeasibilityPredicate)
+      .def("isStateValid",
+           py::overload_cast<const moveit_msgs::RobotState&, const std::string&, bool>(&PlanningScene::isStateValid,
+                                                                                       py::const_),
+           py::arg("state"), py::arg("group"), py::arg("verbose") = false)
+      .def("isStateValid",
+           py::overload_cast<const moveit::core::RobotState&, const std::string&, bool>(&PlanningScene::isStateValid,
+                                                                                        py::const_),
+           py::arg("state"), py::arg("group"), py::arg("verbose") = false)
+      .def("isStateValid",
+           py::overload_cast<const moveit_msgs::RobotState&, const moveit_msgs::Constraints&, const std::string&, bool>(
+               &PlanningScene::isStateValid, py::const_),
+           py::arg("state"), py::arg("constr"), py::arg("group"), py::arg("verbose") = false)
+      .def("isStateValid",
+           py::overload_cast<const moveit::core::RobotState&, const moveit_msgs::Constraints&, const std::string&, bool>(
+               &PlanningScene::isStateValid, py::const_),
+           py::arg("state"), py::arg("constr"), py::arg("group"), py::arg("verbose") = false)
+      .def("isStateValid",
+           py::overload_cast<const moveit::core::RobotState&, const kinematic_constraints::KinematicConstraintSet&,
+                             const std::string&, bool>(&PlanningScene::isStateValid, py::const_),
+           py::arg("state"), py::arg("constr"), py::arg("group"), py::arg("verbose") = false)
+      .def("setCurrentState", py::overload_cast<const moveit_msgs::RobotState&>(&PlanningScene::setCurrentState))
+      .def("setCurrentState", py::overload_cast<const robot_state::RobotState&>(&PlanningScene::setCurrentState))
+      //
+      ;
+}
diff --git a/moveit_core/profiler/include/moveit/profiler/profiler.h b/moveit_core/profiler/include/moveit/profiler/profiler.h
index 43645088d9..9ed619e5e8 100644
--- a/moveit_core/profiler/include/moveit/profiler/profiler.h
+++ b/moveit_core/profiler/include/moveit/profiler/profiler.h
@@ -84,7 +84,7 @@ class Profiler : private boost::noncopyable
       prof_.begin(name);
     }
 
-    ~ScopedBlock(void)
+    ~ScopedBlock()
     {
       prof_.end(name_);
     }
@@ -106,7 +106,7 @@ class Profiler : private boost::noncopyable
         prof_.start();
     }
 
-    ~ScopedStart(void)
+    ~ScopedStart()
     {
       if (!wasRunning_)
         prof_.stop();
@@ -118,7 +118,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 +129,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 +172,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 +181,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 +201,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 +213,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 +238,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 +259,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)
diff --git a/moveit_core/python/CMakeLists.txt b/moveit_core/python/CMakeLists.txt
new file mode 100644
index 0000000000..f7c6b56eb7
--- /dev/null
+++ b/moveit_core/python/CMakeLists.txt
@@ -0,0 +1 @@
+add_subdirectory(tools)
diff --git a/moveit_core/python/pymoveit_core.cpp b/moveit_core/python/pymoveit_core.cpp
new file mode 100644
index 0000000000..3179656d7e
--- /dev/null
+++ b/moveit_core/python/pymoveit_core.cpp
@@ -0,0 +1,84 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, Peter Mitrano
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * The name of Peter Mitrano may not be used to endorse or promote
+ *     products derived from this software without specific prior
+ *     written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (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 */
+
+#include <pybind11/pybind11.h>
+#include <urdf_parser/urdf_parser.h>
+#include <moveit/robot_model/robot_model.h>
+#include <srdfdom/model.h>
+
+namespace py = pybind11;
+
+// Each of these functions defines the bindings for a namespace/subfolder within moveit_core
+void def_collision_detection_bindings(py::module& contact);
+
+void def_robot_model_bindings(py::module& m);
+
+void def_robot_state_bindings(py::module& m);
+
+void def_transforms_bindings(py::module& m);
+
+void def_planning_scene_bindings(py::module& m);
+
+void def_kinematic_constraints_bindings(py::module& m);
+
+auto load_robot_model(const std::string& urdf_path, const std::string& srdf_path)
+{
+  auto urdf_model = urdf::parseURDFFile(urdf_path);
+  auto srdf_model = std::make_shared<srdf::Model>();
+  srdf_model->initFile(*urdf_model, srdf_path);
+  auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
+  return robot_model;
+}
+
+PYBIND11_MODULE(pymoveit_core, m)
+{
+  auto collision_detection_m = m.def_submodule("collision_detection");
+  auto planning_scene_m = m.def_submodule("planning_scene");
+  auto robot_model_m = m.def_submodule("robot_model");
+  auto robot_state_m = m.def_submodule("robot_state");
+  auto kinematic_constraints_m = m.def_submodule("kinematic_constraints");
+  auto transforms_m = m.def_submodule("transforms");
+
+  def_collision_detection_bindings(collision_detection_m);
+  def_robot_model_bindings(robot_model_m);
+  def_robot_state_bindings(robot_state_m);
+  def_planning_scene_bindings(planning_scene_m);
+  def_transforms_bindings(transforms_m);
+  def_kinematic_constraints_bindings(kinematic_constraints_m);
+
+  // convenience function
+  m.def("load_robot_model", &load_robot_model);
+}
diff --git a/moveit_core/python/src/moveit/__init__.py b/moveit_core/python/src/moveit/__init__.py
new file mode 100644
index 0000000000..1e2408706e
--- /dev/null
+++ b/moveit_core/python/src/moveit/__init__.py
@@ -0,0 +1,2 @@
+# https://packaging.python.org/guides/packaging-namespace-packages/#pkgutil-style-namespace-packages
+__path__ = __import__("pkgutil").extend_path(__path__, __name__)
diff --git a/moveit_core/python/src/moveit/core/__init__.py b/moveit_core/python/src/moveit/core/__init__.py
new file mode 100644
index 0000000000..3903855dbd
--- /dev/null
+++ b/moveit_core/python/src/moveit/core/__init__.py
@@ -0,0 +1,12 @@
+# load symbols from C++ extension lib
+from pymoveit_core import load_robot_model
+
+# and augment with symbols from python modules (order is important to allow overriding!)
+from . import (
+    collision_detection,
+    kinematic_constraints,
+    planning_scene,
+    robot_model,
+    robot_state,
+    transforms,
+)
diff --git a/moveit_core/python/src/moveit/core/collision_detection/__init__.py b/moveit_core/python/src/moveit/core/collision_detection/__init__.py
new file mode 100644
index 0000000000..7c4f406f41
--- /dev/null
+++ b/moveit_core/python/src/moveit/core/collision_detection/__init__.py
@@ -0,0 +1 @@
+from pymoveit_core.collision_detection import *
diff --git a/moveit_core/python/src/moveit/core/kinematic_constraints/__init__.py b/moveit_core/python/src/moveit/core/kinematic_constraints/__init__.py
new file mode 100644
index 0000000000..739336ede6
--- /dev/null
+++ b/moveit_core/python/src/moveit/core/kinematic_constraints/__init__.py
@@ -0,0 +1 @@
+from pymoveit_core.kinematic_constraints import *
diff --git a/moveit_core/python/src/moveit/core/planning_scene/__init__.py b/moveit_core/python/src/moveit/core/planning_scene/__init__.py
new file mode 100644
index 0000000000..e233204063
--- /dev/null
+++ b/moveit_core/python/src/moveit/core/planning_scene/__init__.py
@@ -0,0 +1 @@
+from pymoveit_core.planning_scene import *
diff --git a/moveit_core/python/src/moveit/core/robot_model/__init__.py b/moveit_core/python/src/moveit/core/robot_model/__init__.py
new file mode 100644
index 0000000000..0b265fd59c
--- /dev/null
+++ b/moveit_core/python/src/moveit/core/robot_model/__init__.py
@@ -0,0 +1 @@
+from pymoveit_core.robot_model import *
diff --git a/moveit_core/python/src/moveit/core/robot_state/__init__.py b/moveit_core/python/src/moveit/core/robot_state/__init__.py
new file mode 100644
index 0000000000..70599f4aed
--- /dev/null
+++ b/moveit_core/python/src/moveit/core/robot_state/__init__.py
@@ -0,0 +1 @@
+from pymoveit_core.robot_state import *
diff --git a/moveit_core/python/src/moveit/core/transforms/__init__.py b/moveit_core/python/src/moveit/core/transforms/__init__.py
new file mode 100644
index 0000000000..508e97d204
--- /dev/null
+++ b/moveit_core/python/src/moveit/core/transforms/__init__.py
@@ -0,0 +1 @@
+from pymoveit_core.transforms import *
diff --git a/moveit_core/python/tools/CMakeLists.txt b/moveit_core/python/tools/CMakeLists.txt
new file mode 100644
index 0000000000..6a42a03fe9
--- /dev/null
+++ b/moveit_core/python/tools/CMakeLists.txt
@@ -0,0 +1,17 @@
+set(MOVEIT_LIB_NAME moveit_python_tools)
+
+add_library(${MOVEIT_LIB_NAME}
+  src/pybind_rosmsg_typecasters.cpp
+)
+add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES})
+set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+
+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_core/python/tools/include/moveit/python/pybind_rosmsg_typecasters.h b/moveit_core/python/tools/include/moveit/python/pybind_rosmsg_typecasters.h
new file mode 100644
index 0000000000..e613144421
--- /dev/null
+++ b/moveit_core/python/tools/include/moveit/python/pybind_rosmsg_typecasters.h
@@ -0,0 +1,137 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, Robert Haschke
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * The name of Robert Haschke may not be used to endorse or promote
+ *     products derived from this software without specific prior
+ *     written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (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 <pybind11/pybind11.h>
+#include <ros/duration.h>
+#include <ros/serialization.h>
+
+/** Provide pybind11 type converters for ROS types */
+
+namespace moveit
+{
+namespace python
+{
+PYBIND11_EXPORT pybind11::object createMessage(const std::string& ros_msg_name);
+PYBIND11_EXPORT bool convertible(const pybind11::handle& h, const char* ros_msg_name);
+
+}  // namespace python
+}  // namespace moveit
+
+namespace pybind11
+{
+namespace detail
+{
+/// Convert ros::Duration / ros::WallDuration into a float
+template <typename T>
+struct DurationCaster
+{
+  // C++ -> Python
+  static handle cast(T&& src, return_value_policy /* policy */, handle /* parent */)
+  {
+    return PyFloat_FromDouble(src.toSec());
+  }
+
+  // Python -> C++
+  bool load(handle src, bool convert)
+  {
+    if (hasattr(src, "to_sec"))
+    {
+      value = T(src.attr("to_sec")().cast<double>());
+    }
+    else if (convert)
+    {
+      value = T(src.cast<double>());
+    }
+    else
+      return false;
+    return true;
+  }
+
+  PYBIND11_TYPE_CASTER(T, _("Duration"));
+};
+
+template <>
+struct type_caster<ros::Duration> : DurationCaster<ros::Duration>
+{
+};
+
+template <>
+struct type_caster<ros::WallDuration> : DurationCaster<ros::WallDuration>
+{
+};
+
+/// Convert ROS message types (C++ <-> python)
+template <typename T>
+struct type_caster<T, enable_if_t<ros::message_traits::IsMessage<T>::value>>
+{
+  // C++ -> Python
+  static handle cast(const T& src, return_value_policy /* policy */, handle /* parent */)
+  {
+    // serialize src into (python) buffer
+    std::size_t size = ros::serialization::serializationLength(src);
+    object pbuffer = reinterpret_steal<object>(PyBytes_FromStringAndSize(nullptr, size));
+    ros::serialization::OStream stream(reinterpret_cast<uint8_t*>(PyBytes_AsString(pbuffer.ptr())), size);
+    ros::serialization::serialize(stream, src);
+    // deserialize python type from buffer
+    object msg = moveit::python::createMessage(ros::message_traits::DataType<T>::value());
+    msg.attr("deserialize")(pbuffer);
+    return msg.release();
+  }
+
+  // Python -> C++
+  bool load(handle src, bool convert)
+  {
+    if (!moveit::python::convertible(src, ros::message_traits::DataType<T>::value()))
+      return false;
+    // serialize src into (python) buffer
+    object pstream = module::import("io").attr("BytesIO")();
+    src.attr("serialize")(pstream);
+    object pbuffer = pstream.attr("getvalue")();
+    // deserialize C++ type from buffer
+    char* cbuffer = nullptr;
+    Py_ssize_t size;
+    PyBytes_AsStringAndSize(pbuffer.ptr(), &cbuffer, &size);
+    ros::serialization::IStream cstream(const_cast<uint8_t*>(reinterpret_cast<const uint8_t*>(cbuffer)), size);
+    ros::serialization::deserialize(cstream, value);
+    return true;
+  }
+
+  PYBIND11_TYPE_CASTER(T, _<T>());
+};
+}  // namespace detail
+}  // namespace pybind11
diff --git a/moveit_core/python/tools/src/pybind_rosmsg_typecasters.cpp b/moveit_core/python/tools/src/pybind_rosmsg_typecasters.cpp
new file mode 100644
index 0000000000..c94c57f85b
--- /dev/null
+++ b/moveit_core/python/tools/src/pybind_rosmsg_typecasters.cpp
@@ -0,0 +1,69 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, Robert Haschke
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * The name of Robert Haschke may not be used to endorse or promote
+ *     products derived from this software without specific prior
+ *     written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (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 <moveit/python/pybind_rosmsg_typecasters.h>
+
+namespace py = pybind11;
+namespace moveit
+{
+namespace python
+{
+py::object createMessage(const std::string& ros_msg_name)
+{
+  // find delimiting '/' in ros msg name
+  std::size_t pos = ros_msg_name.find('/');
+  // import module
+  py::module m = py::module::import((ros_msg_name.substr(0, pos) + ".msg").c_str());
+  // retrieve type instance
+  py::object cls = m.attr(ros_msg_name.substr(pos + 1).c_str());
+  // create message instance
+  return cls();
+}
+
+bool convertible(const pybind11::handle& h, const char* ros_msg_name)
+{
+  try
+  {
+    PyObject* o = h.attr("_type").ptr();
+    return py::cast<std::string>(o) == ros_msg_name;
+  }
+  catch (const std::exception& e)
+  {
+    return false;
+  }
+}
+}  // namespace python
+}  // 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 f2f44d44f4..5da9599903 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
@@ -412,6 +412,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<std::string>& subgroups);
 
@@ -695,6 +702,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_;
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 fb4ea46413..7c40c43fd3 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
@@ -47,7 +47,7 @@
 
 namespace shapes
 {
-MOVEIT_CLASS_FORWARD(Shape);
+MOVEIT_CLASS_FORWARD(Shape);  // Defines ShapePtr, ConstPtr, WeakPtr... etc
 }
 
 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 53f5c558dc..32155e62a5 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
@@ -60,7 +60,16 @@ namespace moveit
 /** \brief Core components of MoveIt! */
 namespace core
 {
-MOVEIT_CLASS_FORWARD(RobotModel);
+MOVEIT_CLASS_FORWARD(RobotModel);  // Defines RobotModelPtr, ConstPtr, WeakPtr... etc
+
+static inline void checkInterpolationParamBounds(const char LOGNAME[], double t)
+{
+  if (std::isnan(t) || std::isinf(t))
+  {
+    throw Exception("Interpolation parameter is NaN or inf.");
+  }
+  ROS_WARN_STREAM_COND_NAMED(t < 0. || t > 1., LOGNAME, "Interpolation parameter is not in the range [0, 1]: " << t);
+}
 
 /** \brief Definition of a kinematic model. This class is not thread
     safe, however multiple instances can be created */
@@ -91,7 +100,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 */
@@ -164,6 +173,12 @@ class RobotModel
     return active_joint_model_vector_const_;
   }
 
+  /** \brief Get the array of active joint names, in the order they appear in the robot state. */
+  const std::vector<std::string>& getActiveJointModelNames() const
+  {
+    return active_joint_model_names_vector_;
+  }
+
   /** \brief Get the array of joints that are active (not fixed, not mimic) in this model */
   const std::vector<JointModel*>& getActiveJointModels()
   {
@@ -337,7 +352,17 @@ class RobotModel
   }
   double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const;
 
+  /** \brief Return the sum of joint distances between two states. Only considers active joints. */
   double distance(const double* state1, const double* state2) const;
+
+  /**
+   * Interpolate between "from" state, to "to" state. Mimic joints are correctly updated.
+   *
+   * @param from interpolate from this state
+   * @param to to this state
+   * @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly.
+   * @param state holds the result
+   */
   void interpolate(const double* from, const double* to, double t, double* state) const;
 
   /** \name Access to joint groups
@@ -504,6 +529,9 @@ class RobotModel
   /** \brief The vector of joints in the model, in the order they appear in the state vector */
   std::vector<JointModel*> active_joint_model_vector_;
 
+  /** \brief The vector of joint names that corresponds to active_joint_model_vector_ */
+  std::vector<std::string> active_joint_model_names_vector_;
+
   /** \brief The vector of joints in the model, in the order they appear in the state vector */
   std::vector<const JointModel*> active_joint_model_vector_const_;
 
diff --git a/moveit_core/robot_model/src/fixed_joint_model.cpp b/moveit_core/robot_model/src/fixed_joint_model.cpp
index a5c8d288c3..1bb2fd8bb1 100644
--- a/moveit_core/robot_model/src/fixed_joint_model.cpp
+++ b/moveit_core/robot_model/src/fixed_joint_model.cpp
@@ -100,4 +100,4 @@ void FixedJointModel::computeVariablePositions(const Eigen::Isometry3d& /* trans
 }
 
 }  // end of namespace core
-}  // end of namespace moveit
\ No newline at end of file
+}  // end of namespace moveit
diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp
index 664805c565..3b165460aa 100644
--- a/moveit_core/robot_model/src/joint_model_group.cpp
+++ b/moveit_core/robot_model/src/joint_model_group.cpp
@@ -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_vector_[i]->getName());
         active_joint_model_start_index_.push_back(variable_count_);
         active_joint_models_bounds_.push_back(&joint_model_vector_[i]->getVariableBounds());
+        active_variable_count_ += vc;
       }
       else
         mimic_joints_.push_back(joint_model_vector_[i]);
diff --git a/moveit_core/robot_model/src/link_model.cpp b/moveit_core/robot_model/src/link_model.cpp
index 9c8dfcfa3b..32e9590d9d 100644
--- a/moveit_core/robot_model/src/link_model.cpp
+++ b/moveit_core/robot_model/src/link_model.cpp
@@ -121,4 +121,4 @@ void LinkModel::setVisualMesh(const std::string& visual_mesh, const Eigen::Isome
 }
 
 }  // end of namespace core
-}  // end of namespace moveit
\ No newline at end of file
+}  // end of namespace moveit
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..640808b7f4 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
diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp
index 10a8ac5879..aca66d5ba0 100644
--- a/moveit_core/robot_model/src/planar_joint_model.cpp
+++ b/moveit_core/robot_model/src/planar_joint_model.cpp
@@ -237,4 +237,4 @@ void PlanarJointModel::computeVariablePositions(const Eigen::Isometry3d& transf,
 }
 
 }  // end of namespace core
-}  // end of namespace moveit
\ No newline at end of file
+}  // end of namespace moveit
diff --git a/moveit_core/robot_model/src/prismatic_joint_model.cpp b/moveit_core/robot_model/src/prismatic_joint_model.cpp
index 2f2166edb5..65a95ad507 100644
--- a/moveit_core/robot_model/src/prismatic_joint_model.cpp
+++ b/moveit_core/robot_model/src/prismatic_joint_model.cpp
@@ -149,4 +149,4 @@ void PrismaticJointModel::computeVariablePositions(const Eigen::Isometry3d& tran
 }
 
 }  // end of namespace core
-}  // end of namespace moveit
\ No newline at end of file
+}  // end of namespace moveit
diff --git a/moveit_core/robot_model/src/pyrobot_model.cpp b/moveit_core/robot_model/src/pyrobot_model.cpp
new file mode 100644
index 0000000000..73d8d23f21
--- /dev/null
+++ b/moveit_core/robot_model/src/pyrobot_model.cpp
@@ -0,0 +1,63 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, Peter Mitrano
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * The name of Peter Mitrano may not be used to endorse or promote
+ *     products derived from this software without specific prior
+ *     written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (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 */
+
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <moveit/robot_model/robot_model.h>
+#include <urdf_model/model.h>
+#include <moveit/python/pybind_rosmsg_typecasters.h>
+
+namespace py = pybind11;
+using namespace robot_model;
+
+void def_robot_model_bindings(py::module& m)
+{
+  m.doc() = "Definition of a kinematic model. Not thread safe, however multiple instances can be created.";
+  py::class_<RobotModel, RobotModelPtr>(m, "RobotModel")
+      .def(py::init<const urdf::ModelInterfaceSharedPtr&, const srdf::ModelConstSharedPtr&>(), py::arg("urdf_model"),
+           py::arg("srdf_model"))
+      .def("getName", &RobotModel::getName)
+      .def("getLinkModelNames", &RobotModel::getLinkModelNames)
+      .def("getJointModelNames", &RobotModel::getJointModelNames)
+      //
+      ;
+
+  py::class_<JointModelGroup, JointModelGroupPtr>(m, "JointModelGroup")
+      .def("getLinkModelNames", &JointModelGroup::getLinkModelNames)
+      .def("getJointModelNames", &JointModelGroup::getJointModelNames)
+      //
+      ;
+}
diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp
index 2835352d83..c28c4d1c0c 100644
--- a/moveit_core/robot_model/src/robot_model.cpp
+++ b/moveit_core/robot_model/src/robot_model.cpp
@@ -49,7 +49,10 @@ namespace moveit
 {
 namespace core
 {
-const std::string LOGNAME = "robot_model";
+namespace
+{
+constexpr char LOGNAME[] = "robot_model";
+}  // namespace
 
 RobotModel::RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model)
 {
@@ -271,6 +274,7 @@ void RobotModel::buildJointInfo()
       {
         active_joint_model_start_index_.push_back(variable_count_);
         active_joint_model_vector_.push_back(joint_model_vector_[i]);
+        active_joint_model_names_vector_.push_back(joint_model_vector_[i]->getName());
         active_joint_model_vector_const_.push_back(joint_model_vector_[i]);
         active_joint_models_bounds_.push_back(&joint_model_vector_[i]->getVariableBounds());
       }
@@ -1274,6 +1278,7 @@ double RobotModel::distance(const double* state1, const double* state2) const
 
 void RobotModel::interpolate(const double* from, const double* to, double t, double* state) const
 {
+  moveit::core::checkInterpolationParamBounds(LOGNAME, t);
   // we interpolate values only for active joint models (non-mimic)
   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
     active_joint_model_vector_[i]->interpolate(from + active_joint_model_start_index_[i],
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 869ceaa8ef..beb943a18b 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
@@ -52,7 +52,7 @@ 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
@@ -91,10 +91,16 @@ struct JumpThreshold
 
 /** \brief Struct for containing max_step for computeCartesianPath
 
-    Setting translation to zero will disable checking for translations and the same goes for rotation */
+    Setting translation to zero will disable checking for translations. The same goes for rotation.
+    Initializing with only one value (translation) sets the rotation such that
+    1 cm of allowed translation = 2 degrees of allowed rotation. */
 struct MaxEEFStep
 {
-  MaxEEFStep(double translation = 0.0, double rotation = 0.0) : translation(translation), rotation(rotation)
+  MaxEEFStep(double translation, double rotation) : translation(translation), rotation(rotation)
+  {
+  }
+
+  MaxEEFStep(double step_size) : translation(step_size), rotation(3.5 * step_size)  // 0.035 rad = 2 deg
   {
   }
 
@@ -650,7 +656,10 @@ as the new values that correspond to the group */
   {
     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
@@ -658,6 +667,7 @@ of mimic joints), set those
 as the new values that correspond to the group */
   void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>& gstate)
   {
+    assert(gstate.size() == group->getVariableCount());
     setJointGroupPositions(group, &gstate[0]);
   }
 
@@ -673,7 +683,10 @@ as the new values that correspond to the group */
   {
     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
@@ -681,6 +694,42 @@ 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<double>& 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<double>& 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.*/
@@ -951,22 +1000,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.
@@ -1537,32 +1570,52 @@ as the new values that correspond to the group */
    *  @{
    */
 
+  /** \brief Return the sum of joint distances to "other" state. Only considers active joints. */
   double distance(const RobotState& other) const
   {
     return robot_model_->distance(position_, other.getVariablePositions());
   }
 
+  /** \brief Return the sum of joint distances to "other" state. Only considers active joints. */
   double distance(const RobotState& other, const JointModelGroup* joint_group) const;
 
+  /** \brief Return the sum of joint distances to "other" state. Only considers active joints. */
   double distance(const RobotState& other, const JointModel* joint) const
   {
     const int idx = joint->getFirstVariableIndex();
     return joint->distance(position_ + idx, other.position_ + idx);
   }
 
-  /** \brief Interpolate from this state towards state \e to, at time \e t in [0,1].
-      The result is stored in \e state, mimic joints are correctly updated and flags are set
-      so that FK is recomputed when needed. */
+  /**
+   * Interpolate towards "to" state. Mimic joints are correctly updated and flags are set so that FK is recomputed
+   * when needed.
+   *
+   * @param to interpolate to this state
+   * @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly.
+   * @param state holds the result
+   */
   void interpolate(const RobotState& to, double t, RobotState& state) const;
 
-  /** \brief Interpolate from this state towards \e to, at time \e t in [0,1], but only for the joints in the
-      specified group. If mimic joints need to be updated, they are updated accordingly. Flags are set so that FK
-      computation is triggered when needed. */
+  /**
+   * Interpolate towards "to" state, but only for the joints in the specified group. Mimic joints are correctly updated
+   * and flags are set so that FK is recomputed when needed.
+   *
+   * @param to interpolate to this state
+   * @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly.
+   * @param state holds the result
+   * @param joint_group interpolate only for the joints in this group
+   */
   void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;
 
-  /** \brief Update \e state by interpolating form this state towards \e to, at time \e t in [0,1] but only for
-      the joint \e joint. If there are joints that mimic this joint, they are updated. Flags are set so that
-      FK computation is triggered as needed. */
+  /**
+   * Interpolate towards "to" state, but only for a single joint. Mimic joints are correctly updated
+   * and flags are set so that FK is recomputed when needed.
+   *
+   * @param to interpolate to this state
+   * @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly.
+   * @param state holds the result
+   * @param joint interpolate only for this joint
+   */
   void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
   {
     const int idx = joint->getFirstVariableIndex();
@@ -1827,6 +1880,22 @@ as the new values that correspond to the group */
 
   std::string getStateTreeString(const std::string& prefix = "") const;
 
+  /**
+   * \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
+   */
+  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);
+
 private:
   void allocMemory();
   void initTransforms();
@@ -1836,7 +1905,7 @@ as the new values that correspond to the group */
   {
     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)
@@ -1844,7 +1913,7 @@ as the new values that correspond to the group */
     const std::vector<const JointModel*>& 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 ?
+    dirty_link_transforms_ = dirty_link_transforms_ == nullptr ?
                                  group->getCommonRoot() :
                                  robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
   }
diff --git a/moveit_core/robot_state/src/pyrobot_state.cpp b/moveit_core/robot_state/src/pyrobot_state.cpp
new file mode 100644
index 0000000000..9dea327e18
--- /dev/null
+++ b/moveit_core/robot_state/src/pyrobot_state.cpp
@@ -0,0 +1,101 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, Peter Mitrano
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * The name of Peter Mitrano may not be used to endorse or promote
+ *     products derived from this software without specific prior
+ *     written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (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 */
+
+#include <pybind11/pybind11.h>
+#include <pybind11/numpy.h>
+#include <pybind11/stl.h>
+#include <moveit/python/pybind_rosmsg_typecasters.h>
+
+#include <moveit/robot_state/robot_state.h>
+#include <moveit/robot_state/conversions.h>
+
+namespace py = pybind11;
+using namespace robot_state;
+
+void def_robot_state_bindings(py::module& m)
+{
+  m.doc() = "Representation of a robot's state. This includes position, velocity, acceleration and effort.";
+  py::class_<RobotState, RobotStatePtr>(m, "RobotState")
+      .def(py::init<const robot_model::RobotModelConstPtr&>(), py::arg("robot_model"))
+      .def("setToRandomPositions", py::overload_cast<>(&RobotState::setToRandomPositions))
+      .def("setToRandomPositions", py::overload_cast<const JointModelGroup*>(&RobotState::setToRandomPositions))
+      .def("getJointModelGroup", &RobotState::getJointModelGroup, py::return_value_policy::reference)
+      .def("getJointModel", &RobotState::getJointModel, py::return_value_policy::reference)
+      .def("getLinkModel", &RobotState::getLinkModel, py::return_value_policy::reference)
+      .def("getVariableNames", &RobotState::getVariableNames)
+      .def("getVariablePositions", py::overload_cast<>(&RobotState::getVariablePositions, py::const_),
+           py::return_value_policy::reference)
+      .def("getVariableCount", &RobotState::getVariableCount)
+      .def("hasVelocities", &RobotState::hasVelocities)
+      .def("setJointGroupPositions",
+           py::overload_cast<const std::string&, const std::vector<double>&>(&RobotState::setJointGroupPositions))
+      .def("setJointGroupPositions",
+           py::overload_cast<const JointModelGroup*, const std::vector<double>&>(&RobotState::setJointGroupPositions))
+      .def("satisfiesBounds",
+           py::overload_cast<const JointModelGroup*, double>(&RobotState::satisfiesBounds, py::const_),
+           py::arg("joint_model_group"), py::arg("margin") = 0.0)
+      .def("update", &RobotState::update, py::arg("force") = false)
+      .def("printStateInfo",
+           [](const RobotState& s) {
+             std::stringstream ss;
+             s.printStateInfo(ss);
+             return ss.str();
+           })
+      .def("printStatePositions",
+           [](const RobotState& s) {
+             std::stringstream ss;
+             s.printStatePositions(ss);
+             return ss.str();
+           })
+      .def("__repr__",
+           [](const RobotState& s) {
+             std::stringstream ss;
+             s.printStatePositions(ss);
+             return ss.str();
+           })
+      //
+      ;
+
+  m.def("jointStateToRobotState", &jointStateToRobotState);
+  m.def(
+      "robotStateToRobotStateMsg",
+      [](const RobotState& state, bool copy_attached_bodies) {
+        moveit_msgs::RobotState state_msg;
+        robotStateToRobotStateMsg(state, state_msg, copy_attached_bodies);
+        return state_msg;
+      },
+      py::arg("state"), py::arg("copy_attached_bodies") = true);
+}
diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
index e2b3f05feb..748f63bfce 100644
--- a/moveit_core/robot_state/src/robot_state.cpp
+++ b/moveit_core/robot_state/src/robot_state.cpp
@@ -55,7 +55,10 @@ namespace core
  * valid paths from paths with large joint space jumps. */
 static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10;
 
-const std::string LOGNAME = "robot_state";
+namespace
+{
+constexpr char LOGNAME[] = "robot_state";
+}  // namespace
 
 RobotState::RobotState(const RobotModelConstPtr& robot_model)
   : robot_model_(robot_model)
@@ -261,12 +264,12 @@ void RobotState::dropVelocities()
 
 void RobotState::dropAccelerations()
 {
-  has_velocity_ = false;
+  has_acceleration_ = false;
 }
 
 void RobotState::dropEffort()
 {
-  has_velocity_ = false;
+  has_effort_ = false;
 }
 
 void RobotState::dropDynamics()
@@ -519,6 +522,30 @@ void RobotState::setJointGroupPositions(const JointModelGroup* group, const Eige
   updateMimicJoints(group);
 }
 
+void RobotState::setJointGroupActivePositions(const JointModelGroup* group, const std::vector<double>& 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<int>& il = group->getVariableIndexList();
@@ -886,6 +913,7 @@ double RobotState::distance(const RobotState& other, const JointModelGroup* join
 
 void RobotState::interpolate(const RobotState& to, double t, RobotState& state) const
 {
+  moveit::core::checkInterpolationParamBounds(LOGNAME, t);
   robot_model_->interpolate(getVariablePositions(), to.getVariablePositions(), t, state.getVariablePositions());
 
   memset(state.dirty_joint_transforms_, 1, state.robot_model_->getJointModelCount() * sizeof(unsigned char));
@@ -894,6 +922,7 @@ void RobotState::interpolate(const RobotState& to, double t, RobotState& state)
 
 void RobotState::interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const
 {
+  moveit::core::checkInterpolationParamBounds(LOGNAME, t);
   const std::vector<const JointModel*>& jm = joint_group->getActiveJointModels();
   for (std::size_t i = 0; i < jm.size(); ++i)
   {
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 ba890a160c..6c93b91312 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
@@ -45,7 +45,7 @@
 
 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 */
diff --git a/moveit_core/rosdoc.yaml b/moveit_core/rosdoc.yaml
new file mode 100644
index 0000000000..d6d104d5e6
--- /dev/null
+++ b/moveit_core/rosdoc.yaml
@@ -0,0 +1,7 @@
+- builder: sphinx
+  output_dir: python
+  sphinx_root_dir: doc
+- builder: doxygen
+  name: C++ API
+  output_dir: cpp
+  file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
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 d3dca71b52..b274d374c7 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
@@ -71,7 +71,7 @@ struct SensorInfo
   double y_angle;
 };
 
-MOVEIT_CLASS_FORWARD(MoveItSensorManager);
+MOVEIT_CLASS_FORWARD(MoveItSensorManager);  // Defines MoveItSensorManagerPtr, ConstPtr, WeakPtr... etc
 
 class MoveItSensorManager
 {
diff --git a/moveit_core/setup.py b/moveit_core/setup.py
new file mode 100644
index 0000000000..0da9425595
--- /dev/null
+++ b/moveit_core/setup.py
@@ -0,0 +1,10 @@
+# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from catkin_pkg.python_setup import generate_distutils_setup
+from setuptools import setup, find_packages
+
+packages = find_packages("python/src/")
+
+d = generate_distutils_setup(packages=packages, package_dir={"": "python/src"})
+
+setup(**d)
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 bc287eccef..b4ecb04f25 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
@@ -39,6 +39,7 @@
 #define MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_SPLINE_PARAMETERIZATION__
 
 #include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/trajectory_processing/time_parameterization.h>
 
 namespace trajectory_processing
 {
@@ -68,14 +69,13 @@ namespace trajectory_processing
 /// velocity and acceleration limits, will result in a longer trajectory.
 /// If this is a problem, try retuning (increasing) the limits.
 ///
-class IterativeSplineParameterization
+class IterativeSplineParameterization : public TimeParameterization
 {
 public:
   IterativeSplineParameterization(bool add_points = true);
-  ~IterativeSplineParameterization();
 
   bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0,
-                         const double max_acceleration_scaling_factor = 1.0) const;
+                         const double max_acceleration_scaling_factor = 1.0) const override;
 
 private:
   bool add_points_;  /// @brief If true, add two points to trajectory (first and last segments).
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 29a9bcf348..8229b684c9 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
@@ -38,19 +38,20 @@
 #define MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_PARABOLIC_SMOOTHER_
 
 #include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/trajectory_processing/time_parameterization.h>
 
 namespace trajectory_processing
 {
 /// \brief This class  modifies the timestamps of a trajectory to respect
 /// velocity and acceleration constraints.
-class IterativeParabolicTimeParameterization
+MOVEIT_CLASS_FORWARD(IterativeParabolicTimeParameterization);
+class IterativeParabolicTimeParameterization : public TimeParameterization
 {
 public:
   IterativeParabolicTimeParameterization(unsigned int max_iterations = 100, double max_time_change_per_it = .01);
-  ~IterativeParabolicTimeParameterization();
 
   bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0,
-                         const double max_acceleration_scaling_factor = 1.0) const;
+                         const double max_acceleration_scaling_factor = 1.0) const override;
 
 private:
   unsigned int max_iterations_;    /// @brief maximum number of iterations to find solution
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 8c41aafe05..8d8e8b76a5 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
@@ -42,6 +42,7 @@
 #include <Eigen/Core>
 #include <list>
 #include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/trajectory_processing/time_parameterization.h>
 
 namespace trajectory_processing
 {
@@ -96,7 +97,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
@@ -160,15 +161,15 @@ class Trajectory
   mutable std::list<TrajectoryStep>::const_iterator cached_trajectory_segment_;
 };
 
-class TimeOptimalTrajectoryGeneration
+MOVEIT_CLASS_FORWARD(TimeOptimalTrajectoryGeneration);
+class TimeOptimalTrajectoryGeneration : public TimeParameterization
 {
 public:
   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,
-                         const double max_acceleration_scaling_factor = 1.0) const;
+                         const double max_acceleration_scaling_factor = 1.0) const override;
 
 private:
   const double path_tolerance_;
diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h
new file mode 100644
index 0000000000..2cbc29121f
--- /dev/null
+++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <moveit/robot_trajectory/robot_trajectory.h>
+
+namespace trajectory_processing
+{
+/**
+ * @brief Base class for trajectory parameterization algorithms
+ */
+MOVEIT_CLASS_FORWARD(TimeParameterization);
+class TimeParameterization
+{
+public:
+  virtual ~TimeParameterization() = default;
+  virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
+                                 const double max_velocity_scaling_factor = 1.0,
+                                 const double max_acceleration_scaling_factor = 1.0) const = 0;
+};
+}  // 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 498957f6ab..a026985cc2 100644
--- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp
+++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp
@@ -76,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
diff --git a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp
index 1b86b56918..4f4072a2ec 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)
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 63f8512989..4ae1806481 100644
--- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
+++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -867,10 +867,6 @@ TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(const double pa
 {
 }
 
-TimeOptimalTrajectoryGeneration::~TimeOptimalTrajectoryGeneration()
-{
-}
-
 bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
                                                         const double max_velocity_scaling_factor,
                                                         const double max_acceleration_scaling_factor) const
diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h
index 720e16135e..4b6cbe4e97 100644
--- a/moveit_core/transforms/include/moveit/transforms/transforms.h
+++ b/moveit_core/transforms/include/moveit/transforms/transforms.h
@@ -46,7 +46,7 @@ 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
diff --git a/moveit_core/transforms/src/pytransforms.cpp b/moveit_core/transforms/src/pytransforms.cpp
new file mode 100644
index 0000000000..3699147720
--- /dev/null
+++ b/moveit_core/transforms/src/pytransforms.cpp
@@ -0,0 +1,68 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, Peter Mitrano
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * The name of Peter Mitrano may not be used to endorse or promote
+ *     products derived from this software without specific prior
+ *     written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (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 */
+
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <moveit/python/pybind_rosmsg_typecasters.h>
+
+#include <moveit/transforms/transforms.h>
+
+namespace py = pybind11;
+using namespace moveit::core;
+
+void def_transforms_bindings(py::module& m)
+{
+  m.doc() = "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. All stored "
+            "transforms are considered fixed.";
+  py::class_<Transforms, TransformsPtr>(m, "Transforms")
+      .def(py::init<const std::string&>())
+      .def("canTransform", &Transforms::canTransform)
+      .def("getTargetFrame", &Transforms::getTargetFrame)
+      .def("getTransform", &Transforms::getTransform)
+      .def("isFixedFrame", &Transforms::isFixedFrame)
+      .def("getAllTransforms", &Transforms::getAllTransforms)
+      .def("setTransform", py::overload_cast<const Eigen::Isometry3d&, const std::string&>(&Transforms::setTransform))
+      .def("setTransform", py::overload_cast<const geometry_msgs::TransformStamped&>(&Transforms::setTransform))
+      .def("setTransforms", &Transforms::setTransforms)
+      .def("setAllTransforms", &Transforms::setAllTransforms)
+      .def("transformVector3", &Transforms::transformVector3)
+      .def("transformQuaternion", &Transforms::transformQuaternion)
+      .def("transformRotationMatrix", &Transforms::transformRotationMatrix)
+      .def("transformPose", &Transforms::transformPose)
+      //
+      ;
+}
diff --git a/moveit_core/utils/include/moveit/utils/moveit_error_code.h b/moveit_core/utils/include/moveit/utils/moveit_error_code.h
new file mode 100644
index 0000000000..5e9aa4bb11
--- /dev/null
+++ b/moveit_core/utils/include/moveit/utils/moveit_error_code.h
@@ -0,0 +1,76 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, PickNik 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.
+ *********************************************************************/
+
+#pragma once
+
+#include <moveit_msgs/MoveItErrorCodes.h>
+
+namespace moveit
+{
+namespace core
+{
+/**
+ * @brief a wrapper around moveit_msgs::MoveItErrorCodes to make it easier to return an error code message from a function
+ */
+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 c) const
+  {
+    return val == c;
+  }
+  bool operator!=(const int c) const
+  {
+    return val != c;
+  }
+};
+
+}  // 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 28489d53f1..bb267f2a09 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
@@ -102,9 +102,11 @@ 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<geometry_msgs::Pose>& joint_origins = {});
+                const std::vector<geometry_msgs::Pose>& 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
@@ -165,6 +167,9 @@ class RobotModelBuilder
    */
   void addGroup(const std::vector<std::string>& links, const std::vector<std::string>& 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. */
diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp
index 71f8d358b9..ff3238baad 100644
--- a/moveit_core/utils/src/robot_model_test_utils.cpp
+++ b/moveit_core/utils/src/robot_model_test_utils.cpp
@@ -110,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<geometry_msgs::Pose>& joint_origins)
+                                 const std::vector<geometry_msgs::Pose>& joint_origins, urdf::Vector3 joint_axis)
 {
   std::vector<std::string> link_names;
   boost::split_regex(link_names, section, boost::regex("->"));
@@ -182,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);
@@ -342,6 +342,17 @@ void RobotModelBuilder::addGroup(const std::vector<std::string>& 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_core/version/version.cpp b/moveit_core/version/version.cpp
index 0bbb8638dc..42b83e7d56 100644
--- a/moveit_core/version/version.cpp
+++ b/moveit_core/version/version.cpp
@@ -39,6 +39,6 @@
 
 int main(int argc, char** argv)
 {
-  printf("%s\n", MOVEIT_VERSION);
+  printf("%s\n", MOVEIT_VERSION_STR);
   return 0;
 }
diff --git a/moveit_core/version/version.h.in b/moveit_core/version/version.h.in
index 573a2e72b5..8e596d8f9f 100644
--- a/moveit_core/version/version.h.in
+++ b/moveit_core/version/version.h.in
@@ -35,11 +35,17 @@
 #ifndef MOVEIT_VERSION_
 #define MOVEIT_VERSION_
 
-#define MOVEIT_VERSION "@MOVEIT_VERSION@"
+#define MOVEIT_VERSION_STR "@MOVEIT_VERSION@"
 
 #define MOVEIT_VERSION_MAJOR @MOVEIT_VERSION_MAJOR@
 #define MOVEIT_VERSION_MINOR @MOVEIT_VERSION_MINOR@
 #define MOVEIT_VERSION_PATCH @MOVEIT_VERSION_PATCH@
 #define MOVEIT_VERSION_EXTRA "@MOVEIT_VERSION_EXTRA@"
 
+/// MOVEIT_VERSION is (major << 16) + (minor << 8) + patch.
+#define MOVEIT_VERSION MOVEIT_VERSION_CHECK(MOVEIT_VERSION_MAJOR, MOVEIT_VERSION_MINOR, MOVEIT_VERSION_PATCH)
+
+/// Use like: #if MOVEIT_VERSION >= MOVEIT_VERSION_CHECK(1, 0, 0)
+#define MOVEIT_VERSION_CHECK(major, minor, patch) ((major << 16) | (minor << 8) | (patch))
+
 #endif
diff --git a/moveit_experimental/collision_distance_field_ros/Makefile b/moveit_experimental/collision_distance_field_ros/Makefile
index b75b928f20..bbd3fc6049 100644
--- a/moveit_experimental/collision_distance_field_ros/Makefile
+++ b/moveit_experimental/collision_distance_field_ros/Makefile
@@ -1 +1 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
+include $(shell rospack find mk)/cmake.mk
diff --git a/moveit_experimental/collision_distance_field_ros/mainpage.dox b/moveit_experimental/collision_distance_field_ros/mainpage.dox
index 5aa6d661cd..a8ca7e0512 100644
--- a/moveit_experimental/collision_distance_field_ros/mainpage.dox
+++ b/moveit_experimental/collision_distance_field_ros/mainpage.dox
@@ -2,9 +2,9 @@
 \mainpage
 \htmlinclude manifest.html
 
-\b collision_distance_field_ros 
+\b collision_distance_field_ros
 
-<!-- 
+<!--
 Provide an overview of your package.
 -->
 
diff --git a/moveit_experimental/collision_distance_field_ros/manifest.xml b/moveit_experimental/collision_distance_field_ros/manifest.xml
index d29dffb155..c612633e9c 100644
--- a/moveit_experimental/collision_distance_field_ros/manifest.xml
+++ b/moveit_experimental/collision_distance_field_ros/manifest.xml
@@ -16,5 +16,3 @@
   </export>
 
 </package>
-
-
diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache/Makefile b/moveit_experimental/kinematics_cache/v1/kinematics_cache/Makefile
index b75b928f20..bbd3fc6049 100644
--- a/moveit_experimental/kinematics_cache/v1/kinematics_cache/Makefile
+++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache/Makefile
@@ -1 +1 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
+include $(shell rospack find mk)/cmake.mk
diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache/mainpage.dox b/moveit_experimental/kinematics_cache/v1/kinematics_cache/mainpage.dox
index 29c7b2feff..a96ba936ff 100644
--- a/moveit_experimental/kinematics_cache/v1/kinematics_cache/mainpage.dox
+++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache/mainpage.dox
@@ -2,9 +2,9 @@
 \mainpage
 \htmlinclude manifest.html
 
-\b kinematics_cache 
+\b kinematics_cache
 
-<!-- 
+<!--
 Provide an overview of your package.
 -->
 
diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/Makefile b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/Makefile
index b75b928f20..bbd3fc6049 100644
--- a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/Makefile
+++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/Makefile
@@ -1 +1 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
+include $(shell rospack find mk)/cmake.mk
diff --git a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/manifest.xml b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/manifest.xml
index 4eb7facc5b..cf86451d79 100644
--- a/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/manifest.xml
+++ b/moveit_experimental/kinematics_cache/v1/kinematics_cache_ros/manifest.xml
@@ -17,5 +17,3 @@
   <depend package="urdf"/>
 
 </package>
-
-
diff --git a/moveit_experimental/kinematics_cache/v2/kinematics_cache/CMakeLists.txt b/moveit_experimental/kinematics_cache/v2/kinematics_cache/CMakeLists.txt
index 6dcd3680b0..6667751f5b 100644
--- a/moveit_experimental/kinematics_cache/v2/kinematics_cache/CMakeLists.txt
+++ b/moveit_experimental/kinematics_cache/v2/kinematics_cache/CMakeLists.txt
@@ -14,4 +14,3 @@ install(TARGETS ${MOVEIT_LIB_NAME}
         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/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst
index 1a3e0b4ef4..d99b8e9f99 100644
--- a/moveit_kinematics/CHANGELOG.rst
+++ b/moveit_kinematics/CHANGELOG.rst
@@ -2,6 +2,27 @@
 Changelog for package moveit_kinematics
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+* Improve ikfast QUIET handling (`#2685 <https://github.com/ros-planning/moveit/issues/2685>`_)
+* ikfast script: install sympy 0.7.1 from git (`#2650 <https://github.com/ros-planning/moveit/issues/2650>`_)
+* Python3 compatibility for ikfast's round_collada_numbers.py (`#2473 <https://github.com/ros-planning/moveit/issues/2473>`_)
+* Contributors: Tobias Fischer, ags-dy, Robert Haschke
+
+1.0.7 (2020-11-20)
+------------------
+* [fix] Fix IKfast tests (`#2331 <https://github.com/ros-planning/moveit/issues/2331>`_)
+* Contributors: Robert Haschke
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Adapt repository for splitted moveit_resources layout (`#2199 <https://github.com/ros-planning/moveit/issues/2199>`_)
diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt
index b295d3eb50..15f08b94a8 100644
--- a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt
+++ b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt
@@ -9,7 +9,7 @@ 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} 
+install(TARGETS ${MOVEIT_LIB_NAME}
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
@@ -30,7 +30,7 @@ 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} 
+install(TARGETS ${MOVEIT_LIB_NAME}
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
@@ -48,7 +48,7 @@ if(ur_kinematics_FOUND)
             moveit_cached_ik_kinematics_base
             ${ur${UR}_pluginlib}
             ${catkin_LIBRARIES})
-        install(TARGETS ${MOVEIT_LIB_NAME} 
+        install(TARGETS ${MOVEIT_LIB_NAME}
                 LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
                 ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
                 RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
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 cb2b041f50..c0f71f041f 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
@@ -286,8 +286,8 @@ class CachedIKKinematicsPlugin : public KinematicsPlugin
 
   template <class T = KinematicsPlugin>
   typename std::enable_if<!hasRobotModelAPI<T>::value, bool>::type
-  initializeImpl(const moveit::core::RobotModel&, const std::string&, const std::string&,
-                 const std::vector<std::string>&, double)
+  initializeImpl(const moveit::core::RobotModel& /*unused*/, const std::string& /*unused*/,
+                 const std::string& /*unused*/, const std::vector<std::string>& /*unused*/, double /*unused*/)
   {
     return false;  // API not supported
   }
@@ -306,7 +306,8 @@ class CachedIKKinematicsPlugin : public KinematicsPlugin
 
   template <class T = KinematicsPlugin>
   typename std::enable_if<!hasRobotDescAPI<T>::value, bool>::type
-  initializeImpl(const std::string&, const std::string&, const std::string&, const std::string&, double)
+  initializeImpl(const std::string& /*unused*/, const std::string& /*unused*/, const std::string& /*unused*/,
+                 const std::string& /*unused*/, double /*unused*/)
   {
     return false;  // API not supported
   }
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/scripts/auto_create_ikfast_moveit_plugin.sh b/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh
index a04e2077c7..055c2f7365 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
@@ -84,6 +84,32 @@ function cleanup {
    rm -rf "$TMP_DIR"
 }
 
+function run_quiet {
+   # When running in quiet mode, save stdout as 3, then redirect stdout to $TMP_DIR/ikfast.log
+   if [ "$QUIET" == "1" ] ; then
+      local STDOUT=3
+      local STDERR=4
+      exec 3>&1 1>"$TMP_DIR/ikfast.log"
+      exec 4>&2 2>&1
+   fi
+
+   set +e
+   "$@"
+   ret=$?
+   set -e
+
+   # Restore stdout + stderr
+   exec 1>&${STDOUT:-1}  # restore stdout
+   exec 2>&${STDERR:-2}  # restore stderr
+
+   if [ $ret != 0 ] ; then
+      echo "'$*' failed with exec code $ret:"
+      test "$QUIET" == "1" && cat "$TMP_DIR/ikfast.log"
+   fi
+
+   return $ret
+}
+
 function build_docker_image {
    test "$__DOCKER_BUILT" == "1" && return
 
@@ -97,12 +123,9 @@ RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E
     apt-get install -y --no-install-recommends python-pip build-essential liblapack-dev ros-indigo-collada-urdf && \
     apt-get clean && rm -rf /var/lib/apt/lists/*
 # enforce a specific version of sympy, which is known to work with OpenRave
-RUN pip install sympy==0.7.1
+RUN pip install git+https://github.com/sympy/sympy.git@sympy-0.7.1
 EOF
-   # When running in quiet mode, save stdout as 3, then redirect stdout to /dev/null
-   test "$QUIET" == "1" && STDOUT=3 && exec 3>&1 1>/dev/null
-   docker build -t fixed-openrave $TMP_DIR || exit 1
-   exec 1>&${STDOUT:-1}  # restore stdout
+   run_quiet docker build -t fixed-openrave $TMP_DIR
    echo "Successfully built docker image."
    __DOCKER_BUILT=1
 }
@@ -119,16 +142,14 @@ function extract_robot_name {
 }
 
 function create_dae_file {
-   echo "Converting urdf to Collada"
+   echo "Try converting urdf to Collada directly"
    if ! rosrun collada_urdf urdf_to_collada "$INPUT" "$DAE_FILE" 2> /dev/null ; then
       # When this failed, run docker
+      echo "Failed. Converting urdf to Collada (in docker)"
       build_docker_image
-      echo "Converting urdf to Collada"
       cp "$INPUT" "$TMP_DIR/robot.urdf"
-      test "$QUIET" == "1" && exec 3>&2 2>/dev/null  # save stderr as 3, then redirect to /dev/null
-      docker run --rm --user $(id -u):$(id -g) -v $TMP_DIR:/input --workdir /input -e HOME=/input \
+      run_quiet docker run --rm --user $(id -u):$(id -g) -v $TMP_DIR:/input --workdir /input -e HOME=/input \
              fixed-openrave:latest rosrun collada_urdf urdf_to_collada robot.urdf robot.dae
-      test "$QUIET" == "1" && exec 2>&3  # restore stderr
    fi
 }
 
@@ -149,16 +170,11 @@ EOF
    cmd="openrave0.9.py --database inversekinematics --robot=/input/wrapper.xml --iktype=$IK_TYPE --iktests=1000"
    echo "Running $cmd"
 
-   # save stdout as 3, then redirect to /dev/null
-   test "$QUIET" == "1" && exec 3>&1 1>/dev/null
    # run $cmd in docker as current user, outputting files to $TMP_DIR/.openrave
-   docker run --rm --user $(id -u):$(id -g) \
+   run_quiet docker run --rm --user $(id -u):$(id -g) \
       -v $TMP_DIR:/input --workdir /input -e HOME=/input \
       fixed-openrave:latest $cmd
 
-   # restore stdout
-   test "$QUIET" == "1" && exec 1>&3
-
    # update INPUT to generated .cpp
    INPUT=$(ls -1 $TMP_DIR/.openrave/*/*.cpp 2> /dev/null)
    if [ -n "$INPUT" ] ; then
@@ -194,6 +210,11 @@ while true ; do
    filename=$(basename -- "$INPUT")
    extension=$(echo "${filename##*.}" | tr '[:upper:]' '[:lower:]')
    case "$extension" in
+      xacro)
+         URDF="$TMP_DIR/${filename%.*}.urdf"
+         xacro "$INPUT" > "$URDF"
+         INPUT="$URDF"
+         ;;
       urdf)  # create .dae from .urdf
          extract_robot_name "$INPUT"
          create_dae_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 0617eeb17a..ff80dcdc2d 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,6 +1,7 @@
 #! /usr/bin/env python
 from __future__ import print_function
-'''
+
+"""
 IKFast Plugin Generator for MoveIt!
 
 Creates a kinematics plugin using the output of IKFast from OpenRAVE.
@@ -15,8 +16,8 @@
 
 Date: March 2013
 
-'''
-'''
+"""
+"""
 Copyright (c) 2013, Jeremy Zoss, SwRI
 All rights reserved.
 
@@ -43,7 +44,7 @@
 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 POSSIBILITY OF SUCH DAMAGE.
-'''
+"""
 
 import re
 import os
@@ -54,252 +55,360 @@
 import argparse
 
 try:
-  from roslib.packages import get_pkg_dir, InvalidROSPkgException
+    from roslib.packages import get_pkg_dir, InvalidROSPkgException
 except ImportError:
-  print("Failed to import roslib. No ROS environment available? Trying without.")
-  # define stubs
-  class InvalidROSPkgException(Exception):
-    pass
-  def get_pkg_dir(pkg_name):
-    raise InvalidROSPkgException('Failed to locate ROS package {}'.format(pkg_name))
+    print("Failed to import roslib. No ROS environment available? Trying without.")
+    # define stubs
+    class InvalidROSPkgException(Exception):
+        pass
+
+    def get_pkg_dir(pkg_name):
+        raise InvalidROSPkgException("Failed to locate ROS package {}".format(pkg_name))
+
 
 # Package containing this file
-plugin_gen_pkg = 'moveit_kinematics'
+plugin_gen_pkg = "moveit_kinematics"
 # Allowed search modes, see SEARCH_MODE enum in template file
-search_modes = ['OPTIMIZE_MAX_JOINT', 'OPTIMIZE_FREE_JOINT']
+search_modes = ["OPTIMIZE_MAX_JOINT", "OPTIMIZE_FREE_JOINT"]
 
 
 def create_parser():
-  parser = argparse.ArgumentParser(
-      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")
-  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",
-                      help="The name of the end effector link that was used when generating your IKFast solution")
-  parser.add_argument("ikfast_output_path",
-                      help="The full path to the analytic IK solution output by IKFast")
-  parser.add_argument("--search_mode",
-                      default=search_modes[0],
-                      help="The search mode used to solve IK for robots with more than 6DOF")
-  parser.add_argument("--srdf_filename",
-                      help="The name of your robot. Defaults to <robot_name>.srdf")
-  parser.add_argument("--robot_name_in_srdf",
-                      help="The name of your robot as defined in the srdf. Defaults to <robot_name>")
-  parser.add_argument("--moveit_config_pkg",
-                      help="The robot moveit_config package. Defaults to <robot_name>_moveit_config")
-  return parser
+    parser = argparse.ArgumentParser(
+        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",
+    )
+    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",
+        help="The name of the end effector link that was used when generating your IKFast solution",
+    )
+    parser.add_argument(
+        "ikfast_output_path",
+        help="The full path to the analytic IK solution output by IKFast",
+    )
+    parser.add_argument(
+        "--search_mode",
+        default=search_modes[0],
+        help="The search mode used to solve IK for robots with more than 6DOF",
+    )
+    parser.add_argument(
+        "--srdf_filename", help="The name of your robot. Defaults to <robot_name>.srdf"
+    )
+    parser.add_argument(
+        "--robot_name_in_srdf",
+        help="The name of your robot as defined in the srdf. Defaults to <robot_name>",
+    )
+    parser.add_argument(
+        "--moveit_config_pkg",
+        help="The robot moveit_config package. Defaults to <robot_name>_moveit_config",
+    )
+    return parser
 
 
 def populate_optional(args):
-  if args.srdf_filename is None:
-    args.srdf_filename = args.robot_name + ".srdf"
-  if args.robot_name_in_srdf is None:
-    args.robot_name_in_srdf = args.robot_name
-  if args.moveit_config_pkg is None:
-    args.moveit_config_pkg = args.robot_name + "_moveit_config"
+    if args.srdf_filename is None:
+        args.srdf_filename = args.robot_name + ".srdf"
+    if args.robot_name_in_srdf is None:
+        args.robot_name_in_srdf = args.robot_name
+    if args.moveit_config_pkg is None:
+        args.moveit_config_pkg = args.robot_name + "_moveit_config"
 
 
 def print_args(args):
-  print("Creating IKFastKinematicsPlugin with parameters: ")
-  print(" robot_name:           %s" % args.robot_name)
-  print(" base_link_name:       %s" % args.base_link_name)
-  print(" eef_link_name:        %s" % args.eef_link_name)
-  print(" planning_group_name:  %s" % args.planning_group_name)
-  print(" ikfast_plugin_pkg:    %s" % args.ikfast_plugin_pkg)
-  print(" ikfast_output_path:   %s" % args.ikfast_output_path)
-  print(" search_mode:          %s" % args.search_mode)
-  print(" srdf_filename:        %s" % args.srdf_filename)
-  print(" robot_name_in_srdf:   %s" % args.robot_name_in_srdf)
-  print(" moveit_config_pkg:    %s" % args.moveit_config_pkg)
-  print("")
+    print("Creating IKFastKinematicsPlugin with parameters: ")
+    print(" robot_name:           %s" % args.robot_name)
+    print(" base_link_name:       %s" % args.base_link_name)
+    print(" eef_link_name:        %s" % args.eef_link_name)
+    print(" planning_group_name:  %s" % args.planning_group_name)
+    print(" ikfast_plugin_pkg:    %s" % args.ikfast_plugin_pkg)
+    print(" ikfast_output_path:   %s" % args.ikfast_output_path)
+    print(" search_mode:          %s" % args.search_mode)
+    print(" srdf_filename:        %s" % args.srdf_filename)
+    print(" robot_name_in_srdf:   %s" % args.robot_name_in_srdf)
+    print(" moveit_config_pkg:    %s" % args.moveit_config_pkg)
+    print("")
 
 
 def update_deps(reqd_deps, req_type, e_parent):
-  curr_deps = [e.text for e in e_parent.findall(req_type)]
-  missing_deps = set(reqd_deps) - set(curr_deps)
-  for dep in missing_deps:
-     etree.SubElement(e_parent, req_type).text = dep
+    curr_deps = [e.text for e in e_parent.findall(req_type)]
+    missing_deps = set(reqd_deps) - set(curr_deps)
+    for dep in missing_deps:
+        etree.SubElement(e_parent, req_type).text = dep
 
 
 def validate_openrave_version(args):
-  if not os.path.exists(args.ikfast_output_path):
-    raise Exception("Can't find IKFast source code at " + args.ikfast_output_path)
-
-  # Detect version of IKFast used to generate solver code
-  solver_version = 0
-  with open(args.ikfast_output_path, 'r') as src:
-    for line in src:
-       if line.startswith('/// ikfast version'):
-          line_search = re.search('ikfast version (.*) generated', line)
-          if line_search:
-             solver_version = int(line_search.group(1), 0) & ~0x10000000
-          break
-  print('Found source code generated by IKFast version %s' % str(solver_version))
-
-  # Chose template depending on IKFast version
-  if solver_version >= 56:
-    setattr(args, 'template_version', 61)
-  else:
-    raise Exception('This converter requires IKFast 0.5.6 or newer.')
+    if not os.path.exists(args.ikfast_output_path):
+        raise Exception("Can't find IKFast source code at " + args.ikfast_output_path)
+
+    # Detect version of IKFast used to generate solver code
+    solver_version = 0
+    with open(args.ikfast_output_path, "r") as src:
+        for line in src:
+            if line.startswith("/// ikfast version"):
+                line_search = re.search("ikfast version (.*) generated", line)
+                if line_search:
+                    solver_version = int(line_search.group(1), 0) & ~0x10000000
+                break
+    print("Found source code generated by IKFast version %s" % str(solver_version))
+
+    # Chose template depending on IKFast version
+    if solver_version >= 56:
+        setattr(args, "template_version", 61)
+    else:
+        raise Exception("This converter requires IKFast 0.5.6 or newer.")
+
 
 def xmlElement(name, text=None, **attributes):
-   e = etree.Element(name, **attributes)
-   e.text = text
-   return e
+    e = etree.Element(name, **attributes)
+    e.text = text
+    return e
+
 
 def create_ikfast_package(args):
-  try:
-    setattr(args, 'ikfast_plugin_pkg_path', get_pkg_dir(args.ikfast_plugin_pkg))
-  except InvalidROSPkgException:
-    args.ikfast_plugin_pkg_path = os.path.abspath(args.ikfast_plugin_pkg)
-    print("Failed to find package: %s. Will create it in %s." %
-          (args.ikfast_plugin_pkg, args.ikfast_plugin_pkg_path))
-    # update pkg name to basename of path
-    args.ikfast_plugin_pkg=os.path.basename(args.ikfast_plugin_pkg_path)
-
-  src_path = args.ikfast_plugin_pkg_path + '/src/'
-  if not os.path.exists(src_path):
-     os.makedirs(src_path)
-
-  include_path = args.ikfast_plugin_pkg_path + '/include/'
-  if not os.path.exists(include_path):
-     os.makedirs(include_path)
-
-  # Create package.xml
-  pkg_xml_path = args.ikfast_plugin_pkg_path + '/package.xml'
-  if not os.path.exists(pkg_xml_path):
-    root = xmlElement("package", format="2")
-    root.append(xmlElement("name", text=args.ikfast_plugin_pkg))
-    root.append(xmlElement("version", text="0.0.0"))
-    root.append(xmlElement("description", text="IKFast plugin for " + args.robot_name))
-    root.append(xmlElement("license", text="BSD"))
-    user_name = getuser()
-    root.append(xmlElement("maintainer", email="%s@todo.todo" % user_name, text=user_name))
-    root.append(xmlElement("buildtool_depend", text="catkin"))
-    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)
+    try:
+        setattr(args, "ikfast_plugin_pkg_path", get_pkg_dir(args.ikfast_plugin_pkg))
+    except InvalidROSPkgException:
+        args.ikfast_plugin_pkg_path = os.path.abspath(args.ikfast_plugin_pkg)
+        print(
+            "Failed to find package: %s. Will create it in %s."
+            % (args.ikfast_plugin_pkg, args.ikfast_plugin_pkg_path)
+        )
+        # update pkg name to basename of path
+        args.ikfast_plugin_pkg = os.path.basename(args.ikfast_plugin_pkg_path)
+
+    src_path = args.ikfast_plugin_pkg_path + "/src/"
+    if not os.path.exists(src_path):
+        os.makedirs(src_path)
+
+    include_path = args.ikfast_plugin_pkg_path + "/include/"
+    if not os.path.exists(include_path):
+        os.makedirs(include_path)
+
+    # Create package.xml
+    pkg_xml_path = args.ikfast_plugin_pkg_path + "/package.xml"
+    if not os.path.exists(pkg_xml_path):
+        root = xmlElement("package", format="2")
+        root.append(xmlElement("name", text=args.ikfast_plugin_pkg))
+        root.append(xmlElement("version", text="0.0.0"))
+        root.append(
+            xmlElement("description", text="IKFast plugin for " + args.robot_name)
+        )
+        root.append(xmlElement("license", text="BSD"))
+        user_name = getuser()
+        root.append(
+            xmlElement("maintainer", email="%s@todo.todo" % user_name, text=user_name)
+        )
+        root.append(xmlElement("buildtool_depend", text="catkin"))
+        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)
 
 
 def find_template_dir():
-  for candidate in [os.path.dirname(__file__) + '/../templates']:
-    if os.path.exists(candidate) and os.path.exists(candidate + '/ikfast.h'):
-      return os.path.realpath(candidate)
-  try:
-    return os.path.join(get_pkg_dir(plugin_gen_pkg), 'ikfast_kinematics_plugin/templates')
-  except InvalidROSPkgException:
-    raise Exception("Can't find package %s" % plugin_gen_pkg)
+    for candidate in [os.path.dirname(__file__) + "/../templates"]:
+        if os.path.exists(candidate) and os.path.exists(candidate + "/ikfast.h"):
+            return os.path.realpath(candidate)
+    try:
+        return os.path.join(
+            get_pkg_dir(plugin_gen_pkg), "ikfast_kinematics_plugin/templates"
+        )
+    except InvalidROSPkgException:
+        raise Exception("Can't find package %s" % plugin_gen_pkg)
 
 
 def update_ikfast_package(args):
-  # Copy the source code generated by IKFast into our src folder
-  src_path = args.ikfast_plugin_pkg_path + '/src/'
-  solver_file_path = src_path + args.robot_name + '_' + args.planning_group_name + '_ikfast_solver.cpp'
-  if not os.path.exists(solver_file_path) or not os.path.samefile(args.ikfast_output_path, solver_file_path):
-    shutil.copy2(args.ikfast_output_path, solver_file_path)
-
-  if not os.path.exists(solver_file_path):
-    raise Exception("Failed to copy IKFast source code from '%s' to '%s'\n"
-                    "Manually copy the source file generated by IKFast to this location and re-run" %
-                    (args.ikfast_output_path, solver_file_path))
-  # Remember ikfast solver file for update of MoveIt package
-  args.ikfast_output_path = solver_file_path
-
-  # Get template folder location
-  template_dir = find_template_dir()
-
-  # namespace for the plugin
-  setattr(args, 'namespace', args.robot_name + "_" + args.planning_group_name)
-  replacements = dict(_ROBOT_NAME_= args.robot_name,
-                      _GROUP_NAME_ = args.planning_group_name,
-                      _SEARCH_MODE_ = args.search_mode,
-                      _EEF_LINK_ = args.eef_link_name,
-                      _BASE_LINK_ = args.base_link_name,
-                      _PACKAGE_NAME_ = args.ikfast_plugin_pkg,
-                      _NAMESPACE_ = args.namespace
-  )
-
-  # Copy ikfast header file
-  copy_file(template_dir + '/ikfast.h', args.ikfast_plugin_pkg_path + "/include/ikfast.h",
-            "ikfast header file")
-  # Create ikfast plugin template
-  copy_file(template_dir + '/ikfast' + str(args.template_version) + '_moveit_plugin_template.cpp',
-            args.ikfast_plugin_pkg_path + "/src/" + args.robot_name + '_' + args.planning_group_name + "_ikfast_moveit_plugin.cpp",
-            "ikfast plugin file", replacements)
-
-  # Create plugin definition .xml file
-  ik_library_name = args.namespace + "_moveit_ikfast_plugin"
-  plugin_def = etree.Element("library", path="lib/lib" + ik_library_name)
-  setattr(args, 'plugin_name', args.namespace + '/IKFastKinematicsPlugin')
-  cl = etree.SubElement(plugin_def, "class", name=args.plugin_name,
-                        type=args.namespace + "::IKFastKinematicsPlugin",
-                        base_class_type="kinematics::KinematicsBase")
-  desc = etree.SubElement(cl, "description")
-  desc.text = 'IKFast{template} plugin for closed-form kinematics of {robot} {group}' \
-      .format(template=args.template_version, robot=args.robot_name, group=args.planning_group_name)
-
-  # Write plugin definition to file
-  plugin_file_name = ik_library_name + "_description.xml"
-  plugin_file_path = args.ikfast_plugin_pkg_path + "/" + plugin_file_name
-  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
-  replacements.update(dict(_LIBRARY_NAME_ = ik_library_name))
-  copy_file(template_dir + "/CMakeLists.txt", args.ikfast_plugin_pkg_path + '/CMakeLists.txt',
-            "cmake file", replacements)
-
-  # Add plugin export to package manifest
-  parser = etree.XMLParser(remove_blank_text=True)
-  package_file_name = args.ikfast_plugin_pkg_path + "/package.xml"
-  package_xml = etree.parse(package_file_name, parser).getroot()
-
-  # Make sure at least all required dependencies are in the depends lists
-  build_deps = ["liblapack-dev", "moveit_core", "pluginlib", "roscpp", "tf2_kdl", "tf2_eigen", "eigen_conversions"]
-  run_deps = ["liblapack-dev", "moveit_core", "pluginlib", "roscpp", "eigen_conversions"]
-
-  update_deps(build_deps, "build_depend", package_xml)
-  update_deps(run_deps, "exec_depend", package_xml)
-
-  # Check that plugin definition file is in the export list
-  new_export = etree.Element("moveit_core", plugin="${prefix}/" + plugin_file_name)
-
-  export_element = package_xml.find("export")
-  if export_element is None:
-    export_element = etree.SubElement(package_xml, "export")
-
-  found = False
-  for el in export_element.findall("moveit_core"):
-    found = (etree.tostring(new_export) == etree.tostring(el))
-    if found:
-      break
-
-  if not found:
-    export_element.append(new_export)
-
-  # Always write the package xml file, even if there are no changes, to ensure
-  # proper encodings are used in the future (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
-  easy_script_file_path = args.ikfast_plugin_pkg_path + "/update_ikfast_plugin.sh"
-  with open(easy_script_file_path, 'w') as f:
-    f.write("search_mode=" + args.search_mode + "\n"
-            + "srdf_filename=" + args.srdf_filename + "\n"
-            + "robot_name_in_srdf=" + args.robot_name_in_srdf + "\n"
-            + "moveit_config_pkg=" + args.moveit_config_pkg + "\n"
-            + "robot_name=" + args.robot_name + "\n"
-            + "planning_group_name=" + args.planning_group_name + "\n"
-            + "ikfast_plugin_pkg=" + args.ikfast_plugin_pkg + "\n"
-            + "base_link_name=" + args.base_link_name + "\n"
-            + "eef_link_name=" + args.eef_link_name + "\n"
-            + "ikfast_output_path=" + args.ikfast_output_path + "\n\n"
+    # Copy the source code generated by IKFast into our src folder
+    src_path = args.ikfast_plugin_pkg_path + "/src/"
+    solver_file_path = (
+        src_path
+        + args.robot_name
+        + "_"
+        + args.planning_group_name
+        + "_ikfast_solver.cpp"
+    )
+    if not os.path.exists(solver_file_path) or not os.path.samefile(
+        args.ikfast_output_path, solver_file_path
+    ):
+        shutil.copy2(args.ikfast_output_path, solver_file_path)
+
+    if not os.path.exists(solver_file_path):
+        raise Exception(
+            "Failed to copy IKFast source code from '%s' to '%s'\n"
+            "Manually copy the source file generated by IKFast to this location and re-run"
+            % (args.ikfast_output_path, solver_file_path)
+        )
+    # Remember ikfast solver file for update of MoveIt package
+    args.ikfast_output_path = solver_file_path
+
+    # Get template folder location
+    template_dir = find_template_dir()
+
+    # namespace for the plugin
+    setattr(args, "namespace", args.robot_name + "_" + args.planning_group_name)
+    replacements = dict(
+        _ROBOT_NAME_=args.robot_name,
+        _GROUP_NAME_=args.planning_group_name,
+        _SEARCH_MODE_=args.search_mode,
+        _EEF_LINK_=args.eef_link_name,
+        _BASE_LINK_=args.base_link_name,
+        _PACKAGE_NAME_=args.ikfast_plugin_pkg,
+        _NAMESPACE_=args.namespace,
+    )
+
+    # Copy ikfast header file
+    copy_file(
+        template_dir + "/ikfast.h",
+        args.ikfast_plugin_pkg_path + "/include/ikfast.h",
+        "ikfast header file",
+    )
+    # Create ikfast plugin template
+    copy_file(
+        template_dir
+        + "/ikfast"
+        + str(args.template_version)
+        + "_moveit_plugin_template.cpp",
+        args.ikfast_plugin_pkg_path
+        + "/src/"
+        + args.robot_name
+        + "_"
+        + args.planning_group_name
+        + "_ikfast_moveit_plugin.cpp",
+        "ikfast plugin file",
+        replacements,
+    )
+
+    # Create plugin definition .xml file
+    ik_library_name = args.namespace + "_moveit_ikfast_plugin"
+    plugin_def = etree.Element("library", path="lib/lib" + ik_library_name)
+    setattr(args, "plugin_name", args.namespace + "/IKFastKinematicsPlugin")
+    cl = etree.SubElement(
+        plugin_def,
+        "class",
+        name=args.plugin_name,
+        type=args.namespace + "::IKFastKinematicsPlugin",
+        base_class_type="kinematics::KinematicsBase",
+    )
+    desc = etree.SubElement(cl, "description")
+    desc.text = (
+        "IKFast{template} plugin for closed-form kinematics of {robot} {group}".format(
+            template=args.template_version,
+            robot=args.robot_name,
+            group=args.planning_group_name,
+        )
+    )
+
+    # Write plugin definition to file
+    plugin_file_name = ik_library_name + "_description.xml"
+    plugin_file_path = args.ikfast_plugin_pkg_path + "/" + plugin_file_name
+    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
+    replacements.update(dict(_LIBRARY_NAME_=ik_library_name))
+    copy_file(
+        template_dir + "/CMakeLists.txt",
+        args.ikfast_plugin_pkg_path + "/CMakeLists.txt",
+        "cmake file",
+        replacements,
+    )
+
+    # Add plugin export to package manifest
+    parser = etree.XMLParser(remove_blank_text=True)
+    package_file_name = args.ikfast_plugin_pkg_path + "/package.xml"
+    package_xml = etree.parse(package_file_name, parser).getroot()
+
+    # Make sure at least all required dependencies are in the depends lists
+    build_deps = [
+        "liblapack-dev",
+        "moveit_core",
+        "pluginlib",
+        "roscpp",
+        "tf2_kdl",
+        "tf2_eigen",
+        "eigen_conversions",
+    ]
+    run_deps = [
+        "liblapack-dev",
+        "moveit_core",
+        "pluginlib",
+        "roscpp",
+        "eigen_conversions",
+    ]
+
+    update_deps(build_deps, "build_depend", package_xml)
+    update_deps(run_deps, "exec_depend", package_xml)
+
+    # Check that plugin definition file is in the export list
+    new_export = etree.Element("moveit_core", plugin="${prefix}/" + plugin_file_name)
+
+    export_element = package_xml.find("export")
+    if export_element is None:
+        export_element = etree.SubElement(package_xml, "export")
+
+    found = False
+    for el in export_element.findall("moveit_core"):
+        found = etree.tostring(new_export) == etree.tostring(el)
+        if found:
+            break
+
+    if not found:
+        export_element.append(new_export)
+
+    # Always write the package xml file, even if there are no changes, to ensure
+    # proper encodings are used in the future (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
+    easy_script_file_path = args.ikfast_plugin_pkg_path + "/update_ikfast_plugin.sh"
+    with open(easy_script_file_path, "w") as f:
+        f.write(
+            "search_mode="
+            + args.search_mode
+            + "\n"
+            + "srdf_filename="
+            + args.srdf_filename
+            + "\n"
+            + "robot_name_in_srdf="
+            + args.robot_name_in_srdf
+            + "\n"
+            + "moveit_config_pkg="
+            + args.moveit_config_pkg
+            + "\n"
+            + "robot_name="
+            + args.robot_name
+            + "\n"
+            + "planning_group_name="
+            + args.planning_group_name
+            + "\n"
+            + "ikfast_plugin_pkg="
+            + args.ikfast_plugin_pkg
+            + "\n"
+            + "base_link_name="
+            + args.base_link_name
+            + "\n"
+            + "eef_link_name="
+            + args.eef_link_name
+            + "\n"
+            + "ikfast_output_path="
+            + args.ikfast_output_path
+            + "\n\n"
             + "rosrun moveit_kinematics create_ikfast_moveit_plugin.py\\\n"
             + "  --search_mode=$search_mode\\\n"
             + "  --srdf_filename=$srdf_filename\\\n"
@@ -310,87 +419,97 @@ def update_ikfast_package(args):
             + "  $ikfast_plugin_pkg\\\n"
             + "  $base_link_name\\\n"
             + "  $eef_link_name\\\n"
-            + "  $ikfast_output_path\n")
+            + "  $ikfast_output_path\n"
+        )
 
-  print("Created update plugin script at '%s'" % easy_script_file_path)
+    print("Created update plugin script at '%s'" % easy_script_file_path)
 
 
 def update_moveit_package(args):
-  try:
-    moveit_config_pkg_path = get_pkg_dir(args.moveit_config_pkg)
-  except InvalidROSPkgException:
-    raise Exception("Failed to find package: " + args.moveit_config_pkg)
-
-  try:
-    srdf_file_name = moveit_config_pkg_path + '/config/' + args.srdf_filename
-    srdf = etree.parse(srdf_file_name).getroot()
-  except IOError:
-    raise Exception("Failed to find SRDF file: " + srdf_file_name)
-  except etree.XMLSyntaxError as err:
-    raise Exception("Failed to parse xml in file: %s\n%s" % (srdf_file_name, err.msg))
-
-  if args.robot_name_in_srdf != srdf.get('name'):
-    raise Exception("Robot name in srdf ('%s') doesn't match expected name ('%s')" %
-                    (srdf.get('name'), args.robot_name_in_srdf))
-
-  groups = srdf.findall('group')
-  if len(groups) < 1:
-    raise Exception("No planning groups are defined in the SRDF")
-
-  planning_group = None
-  for group in groups:
-    if group.get('name').lower() == args.planning_group_name.lower():
-      planning_group = group
-
-  if planning_group is None:
-    raise Exception("Planning group '%s' not defined in the SRDF. Available groups: \n%s" % (
-        args.planning_group_name, ", ".join([group_name.get('name') for group_name in groups])))
-
-  # Modify kinematics.yaml file
-  kin_yaml_file_name = moveit_config_pkg_path + "/config/kinematics.yaml"
-  with open(kin_yaml_file_name, 'r') as f:
-    kin_yaml_data = yaml.safe_load(f)
-
-  kin_yaml_data[args.planning_group_name]["kinematics_solver"] = args.plugin_name
-  with open(kin_yaml_file_name, 'w') as f:
-    yaml.dump(kin_yaml_data, f, default_flow_style=False)
-
-  print("Modified kinematics.yaml at '%s'" % kin_yaml_file_name)
+    try:
+        moveit_config_pkg_path = get_pkg_dir(args.moveit_config_pkg)
+    except InvalidROSPkgException:
+        raise Exception("Failed to find package: " + args.moveit_config_pkg)
+
+    try:
+        srdf_file_name = moveit_config_pkg_path + "/config/" + args.srdf_filename
+        srdf = etree.parse(srdf_file_name).getroot()
+    except IOError:
+        raise Exception("Failed to find SRDF file: " + srdf_file_name)
+    except etree.XMLSyntaxError as err:
+        raise Exception(
+            "Failed to parse xml in file: %s\n%s" % (srdf_file_name, err.msg)
+        )
+
+    if args.robot_name_in_srdf != srdf.get("name"):
+        raise Exception(
+            "Robot name in srdf ('%s') doesn't match expected name ('%s')"
+            % (srdf.get("name"), args.robot_name_in_srdf)
+        )
+
+    groups = srdf.findall("group")
+    if len(groups) < 1:
+        raise Exception("No planning groups are defined in the SRDF")
+
+    planning_group = None
+    for group in groups:
+        if group.get("name").lower() == args.planning_group_name.lower():
+            planning_group = group
+
+    if planning_group is None:
+        raise Exception(
+            "Planning group '%s' not defined in the SRDF. Available groups: \n%s"
+            % (
+                args.planning_group_name,
+                ", ".join([group_name.get("name") for group_name in groups]),
+            )
+        )
+
+    # Modify kinematics.yaml file
+    kin_yaml_file_name = moveit_config_pkg_path + "/config/kinematics.yaml"
+    with open(kin_yaml_file_name, "r") as f:
+        kin_yaml_data = yaml.safe_load(f)
+
+    kin_yaml_data[args.planning_group_name]["kinematics_solver"] = args.plugin_name
+    with open(kin_yaml_file_name, "w") as f:
+        yaml.dump(kin_yaml_data, f, default_flow_style=False)
+
+    print("Modified kinematics.yaml at '%s'" % kin_yaml_file_name)
 
 
 def copy_file(src_path, dest_path, description, replacements=None):
-  if not os.path.exists(src_path):
-    raise Exception("Can't find %s at '%s'" % (description, src_path))
+    if not os.path.exists(src_path):
+        raise Exception("Can't find %s at '%s'" % (description, src_path))
 
-  if replacements is None:
-    replacements = dict()
+    if replacements is None:
+        replacements = dict()
 
-  with open(src_path, 'r') as f:
-    content = f.read()
+    with open(src_path, "r") as f:
+        content = f.read()
 
-  # replace templates
-  for key, value in replacements.items():
-    content = re.sub(key, value, content)
+    # replace templates
+    for key, value in replacements.items():
+        content = re.sub(key, value, content)
 
-  with open(dest_path, 'w') as f:
-    f.write(content)
-  print("Created %s at '%s'" % (description, dest_path))
+    with open(dest_path, "w") as f:
+        f.write(content)
+    print("Created %s at '%s'" % (description, dest_path))
 
 
 def main():
-  parser = create_parser()
-  args = parser.parse_args()
-
-  populate_optional(args)
-  print_args(args)
-  validate_openrave_version(args)
-  create_ikfast_package(args)
-  update_ikfast_package(args)
-  try:
-    update_moveit_package(args)
-  except Exception as e:
-    print("Failed to update MoveIt package:\n" + str(e))
-
-
-if __name__ == '__main__':
-  main()
+    parser = create_parser()
+    args = parser.parse_args()
+
+    populate_optional(args)
+    print_args(args)
+    validate_openrave_version(args)
+    create_ikfast_package(args)
+    update_ikfast_package(args)
+    try:
+        update_moveit_package(args)
+    except Exception as e:
+        print("Failed to update MoveIt package:\n" + str(e))
+
+
+if __name__ == "__main__":
+    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..d72ab98cd8 100755
--- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py
+++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py
@@ -1,6 +1,6 @@
 #! /usr/bin/env python
 
-'''
+"""
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
@@ -35,89 +35,93 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
-'''
+"""
 # Author: Dave Coleman
 # Desc:   Rounds all the numbers to <decimal places> places
 
+from __future__ import print_function
+
 from lxml import etree
 import shlex
 import sys
 import io
 
-def doRound(values,decimal_places):
+
+def doRound(values, decimal_places):
     num_vector = shlex.split(values)
     new_vector = []
 
     for num in num_vector:
-        new_num = round(float(num),decimal_places)
-        print "Old:",num,"New:",new_num
+        new_num = round(float(num), decimal_places)
+        print("Old:", num, "New:", new_num)
         new_vector.append(str(new_num))
 
     new = " ".join(new_vector)
-    #print 'Original:', values, '  Updated: ', new
+    # print('Original:', values, '  Updated: ', new)
 
     return new
 
+
 # -----------------------------------------------------------------------------
 
-if __name__ == '__main__':
+if __name__ == "__main__":
 
     # Check input arguments
     try:
         input_file = sys.argv[1]
         output_file = sys.argv[2]
         decimal_places = int(sys.argv[3])
-        assert( len(sys.argv) < 5 )   # invalid num-arguments
+        assert len(sys.argv) < 5  # invalid num-arguments
     except:
-        print '\nUsage: round_collada_numbers.py <input_dae> <output_dae> <decimal places>'
-        print 'Rounds all the numbers to <decimal places> places\n'
+        print(
+            "\nUsage: round_collada_numbers.py <input_dae> <output_dae> <decimal places>"
+        )
+        print("Rounds all the numbers to <decimal places> 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')
-    xml = f.read();
+    f = open(input_file, "r")
+    xml = f.read()
 
     # Parse XML
-    #doc = etree.fromstring(xml)
-    #print(doc.tag)
-    #doc = etree.parse(io.BytesIO(xml))
-    #element=doc.xpath('//ns:asset',namespaces={'ns','http://www.collada.org/2008/03/COLLADASchema'})
-    #print element
+    # doc = etree.fromstring(xml)
+    # print(doc.tag)
+    # doc = etree.parse(io.BytesIO(xml))
+    # element=doc.xpath('//ns:asset',namespaces={'ns','http://www.collada.org/2008/03/COLLADASchema'})
+    # print(element)
 
-    namespace = 'http://www.collada.org/2008/03/COLLADASchema'
+    namespace = "http://www.collada.org/2008/03/COLLADASchema"
     dom = etree.parse(io.BytesIO(xml))
 
     # find elements of particular name
-    elements=dom.xpath('//ns:translate',namespaces={'ns':namespace})
+    elements = dom.xpath("//ns:translate", namespaces={"ns": namespace})
     for i in range(len(elements)):
-        elements[i].text = doRound(elements[i].text,decimal_places)
+        elements[i].text = doRound(elements[i].text, decimal_places)
 
     # find elements of particular name
-    elements=dom.xpath('//ns:rotate',namespaces={'ns':namespace})
+    elements = dom.xpath("//ns:rotate", namespaces={"ns": namespace})
     for i in range(len(elements)):
-        elements[i].text = doRound(elements[i].text,decimal_places)
+        elements[i].text = doRound(elements[i].text, decimal_places)
 
     # find elements of particular name
-    elements=dom.xpath('//ns:min',namespaces={'ns':namespace})
+    elements = dom.xpath("//ns:min", namespaces={"ns": namespace})
     for i in range(len(elements)):
-        elements[i].text = doRound(elements[i].text,decimal_places)
+        elements[i].text = doRound(elements[i].text, decimal_places)
 
     # find elements of particular name
-    elements=dom.xpath('//ns:max',namespaces={'ns':namespace})
+    elements = dom.xpath("//ns:max", namespaces={"ns": namespace})
     for i in range(len(elements)):
-        elements[i].text = doRound(elements[i].text,decimal_places)
+        elements[i].text = doRound(elements[i].text, decimal_places)
 
     # find elements of particular name
-    elements=dom.xpath('//ns:float',namespaces={'ns':namespace})
+    elements = dom.xpath("//ns:float", namespaces={"ns": namespace})
     for i in range(len(elements)):
-        elements[i].text = doRound(elements[i].text,decimal_places)
+        elements[i].text = doRound(elements[i].text, decimal_places)
 
     # save changes
-    f = open(output_file,'w')
+    f = open(output_file, "w")
     f.write(etree.tostring(dom))
     f.close()
-
-
diff --git a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt
index cd3ffc0891..5f915e2e3e 100644
--- a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt
+++ b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt
@@ -7,8 +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}) 
+        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 043134b8d0..038e62d862 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
@@ -84,11 +84,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<double, 6, 1>& 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;
   }
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 f3743c467f..6b27c8977e 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
@@ -53,7 +53,7 @@
 #include <kdl/chainfksolver.hpp>
 #include <kdl/chainiksolver.hpp>
 
-// MoveIt!
+// MoveIt
 #include <moveit/kinematics_base/kinematics_base.h>
 #include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
 #include <moveit/robot_model/robot_model.h>
@@ -143,6 +143,7 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase
   typedef Eigen::Matrix<double, 6, 1> 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;
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 b947af295c..622374be31 100644
--- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp
+++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp
@@ -240,26 +240,26 @@ bool KDLKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model
       }
     }
   }
-  for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
+  for (JointMimic& mimic_joint : mimic_joints_)
   {
-    if (!mimic_joints_[i].active)
+    if (!mimic_joint.active)
     {
-      const robot_model::JointModel* joint_model =
-          joint_model_group_->getJointModel(mimic_joints_[i].joint_name)->getMimic();
-      for (std::size_t j = 0; j < mimic_joints_.size(); ++j)
+      const moveit::core::JointModel* joint_model =
+          joint_model_group_->getJointModel(mimic_joint.joint_name)->getMimic();
+      for (JointMimic& mimic_joint_recal : mimic_joints_)
       {
-        if (mimic_joints_[j].joint_name == joint_model->getName())
+        if (mimic_joint_recal.joint_name == joint_model->getName())
         {
-          mimic_joints_[i].map_index = mimic_joints_[j].map_index;
+          mimic_joint.map_index = mimic_joint_recal.map_index;
         }
       }
     }
   }
 
   // Setup the joint state groups that we need
-  state_.reset(new robot_state::RobotState(robot_model_));
+  state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
 
-  fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
+  fk_solver_ = std::make_unique<KDL::ChainFkSolverPos_recursive>(kdl_chain_);
 
   initialized_ = true;
   ROS_DEBUG_NAMED("kdl", "KDL solver initialized");
diff --git a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt
index 1dadddcd56..5867598904 100644
--- a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt
+++ b/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt
@@ -5,7 +5,7 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V
 
 target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES})
 
-install(TARGETS ${MOVEIT_LIB_NAME} 
+install(TARGETS ${MOVEIT_LIB_NAME}
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml
index 0da3673b8a..d61570684e 100644
--- a/moveit_kinematics/package.xml
+++ b/moveit_kinematics/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit_kinematics</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Package for all inverse kinematics solvers in MoveIt!</description>
 
   <author email="dave@picknik.ai">Dave Coleman</author>
diff --git a/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt
index 2cf14dd877..7cf4de66e9 100644
--- a/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt
+++ b/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt
@@ -5,7 +5,7 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V
 
 target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES})
 
-install(TARGETS ${MOVEIT_LIB_NAME} 
+install(TARGETS ${MOVEIT_LIB_NAME}
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
diff --git a/moveit_kinematics/test/test_ikfast_plugins.sh b/moveit_kinematics/test/test_ikfast_plugins.sh
index 43a363d2f4..817bc11729 100755
--- a/moveit_kinematics/test/test_ikfast_plugins.sh
+++ b/moveit_kinematics/test/test_ikfast_plugins.sh
@@ -1,38 +1,41 @@
 #!/bin/bash
 
 # Script to test ikfast plugin creation and functionality
-# This script is intended to run as a BEFORE_DOCKER_SCRIPT from Travis.
-# Particularly we assume that the travis_* utility functions are available.
 
 # We will create ikfast plugins for fanuc and panda from moveit_resources
 # using the script auto_create_ikfast_moveit_plugin.sh
 
+set -e # fail script on error
+
 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
+	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
+	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
-panda=/tmp/resources/panda_description/urdf/panda.urdf
+git clone -q --depth=1 https://github.com/ros-planning/moveit_resources /tmp/ros/src/moveit_resources
+docker run --rm -v /tmp/ros:/tmp/ros -w /tmp/ros "$DOCKER_IMAGE" bash -c "catkin build --no-status --no-summary --no-deps moveit_resources_panda_description"
 
-# Translate environment variable QUIET=[0 | 1] into actual option
-test ${QUIET:-1} -eq 0 && QUIET="" || QUIET="--quiet"
+fanuc=/tmp/ros/src/moveit_resources/fanuc_description/urdf/fanuc.urdf
+panda=/tmp/ros/src/moveit_resources/panda_description/urdf/panda.urdf
 
-travis_run --retry sudo apt-get -q install python-lxml
+export QUIET=${QUIET:=1}
 
 # Create ikfast plugins for Fanuc and Panda
-travis_run moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh \
-	$QUIET --name fanuc --pkg $PWD/fanuc_ikfast_plugin $fanuc manipulator base_link tool0
-
-travis_run moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh \
-	$QUIET --name panda --pkg $PWD/panda_ikfast_plugin $panda panda_arm panda_link0 panda_link8
+echo
+echo "Creating IKFast package for Fanuc robot"
+moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh \
+	--name fanuc --pkg $PWD/fanuc_ikfast_plugin $fanuc manipulator base_link tool0
+
+echo
+echo "Creating IKFast package for Panda robot"
+moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh \
+	--name panda --pkg $PWD/panda_ikfast_plugin $panda panda_arm panda_link0 panda_link8
 
 echo "Done."
diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp
index 31430c88f7..994cdc3242 100644
--- a/moveit_kinematics/test/test_kinematics_plugin.cpp
+++ b/moveit_kinematics/test/test_kinematics_plugin.cpp
@@ -43,7 +43,7 @@
 #include <tf2_eigen/tf2_eigen.h>
 #include <xmlrpcpp/XmlRpcValue.h>
 
-// MoveIt!
+// MoveIt
 #include <moveit/kinematics_base/kinematics_base.h>
 #include <moveit/rdf_loader/rdf_loader.h>
 #include <moveit/robot_model/robot_model.h>
diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst
index 9f2a2eee6d..7677393080 100644
--- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst
+++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst
@@ -2,6 +2,34 @@
 Changelog for package chomp_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+* Use moveit-resources@master (`#2951 <https://github.com/ros-planning/moveit/issues/2951>`_)
+
+  - Simplify launch files to use the test_environment.launch files from moveit_resources@master
+  - Provide compatibility to the Noetic-style configuration of (multiple) planning pipelines
+    Only a single pipeline can be used at a time, specified via the ~default_planning_pipeline parameter.
+* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 <https://github.com/ros-planning/moveit/issues/3009>`_)
+* Contributors: Robert Haschke
+
+1.0.8 (2021-05-23)
+------------------
+* Sanitize CHOMP initialization method parameter (`#2540 <https://github.com/ros-planning/moveit/issues/2540>`_)
+* Contributors: Cong Liu
+
+1.0.7 (2020-11-20)
+------------------
+* [feature] Start new joint_state_publisher_gui on param use_gui (`#2257 <https://github.com/ros-planning/moveit/issues/2257>`_)
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* [maint] Replace $(find xacro)/xacro -> xacro (`#2282 <https://github.com/ros-planning/moveit/issues/2282>`_)
+* Contributors: Felix von Drigalski, Robert Haschke, Yoan Mollard
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Migrate to clang-format-10
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 df9c952d62..7e946d0cd3 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
@@ -43,7 +43,7 @@
 
 namespace chomp_interface
 {
-MOVEIT_CLASS_FORWARD(CHOMPInterface);
+MOVEIT_CLASS_FORWARD(CHOMPInterface);  // Defines CHOMPInterfacePtr, ConstPtr, WeakPtr... etc
 
 class CHOMPInterface : public chomp::ChompPlanner
 {
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 8bb25b2245..3bc74ce995 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
@@ -42,7 +42,7 @@
 
 namespace chomp_interface
 {
-MOVEIT_CLASS_FORWARD(CHOMPPlanningContext);
+MOVEIT_CLASS_FORWARD(CHOMPPlanningContext);  // Defines CHOMPPlanningContextPtr, ConstPtr, WeakPtr... etc
 
 class CHOMPPlanningContext : public planning_interface::PlanningContext
 {
diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml
index bf16f03eac..e0db0dcc9f 100644
--- a/moveit_planners/chomp/chomp_interface/package.xml
+++ b/moveit_planners/chomp/chomp_interface/package.xml
@@ -2,7 +2,7 @@
   <name>moveit_planners_chomp</name>
   <description>The interface for using CHOMP within MoveIt!</description>
 
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <author email="gjones@willowgarage.com">Gil Jones</author>
 
   <maintainer email="chitt@live.in">Chittaranjan Srinivas Swaminathan</maintainer>
diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp
index 48a03b1fc1..546d516b76 100644
--- a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp
+++ b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp
@@ -69,8 +69,13 @@ void CHOMPInterface::loadParams()
   nh_.param("collision_threshold", params_.collision_threshold_, 0.07);
   // nh_.param("random_jump_amount", params_.random_jump_amount_, 1.0);
   nh_.param("use_stochastic_descent", params_.use_stochastic_descent_, true);
-  nh_.param("trajectory_initialization_method", params_.trajectory_initialization_method_,
-            std::string("quintic-spline"));
+  {
+    params_.trajectory_initialization_method_ = "quintic-spline";
+    std::string method;
+    if (nh_.getParam("trajectory_initialization_method", method) && !params_.setTrajectoryInitializationMethod(method))
+      ROS_ERROR_STREAM("Attempted to set trajectory_initialization_method to invalid value '"
+                       << method << "'. Using default '" << params_.trajectory_initialization_method_ << "' instead.");
+  }
   nh_.param("enable_failure_recovery", params_.enable_failure_recovery_, false);
   nh_.param("max_recovery_attempts", params_.max_recovery_attempts_, 5);
 }
diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp
index 7ec6538be9..7ea3e9ba65 100644
--- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp
+++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp
@@ -58,9 +58,9 @@ TEST_F(CHOMPMoveitTest, jointSpaceGoodGoal)
   move_group_.setStartState(*(move_group_.getCurrentState()));
   move_group_.setJointValueTarget(std::vector<double>({ 1.0, 1.0 }));
 
-  moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_);
+  moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_);
   EXPECT_GT(my_plan_.trajectory_.joint_trajectory.points.size(), 0u);
-  EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::SUCCESS);
+  EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS);
 }
 
 TEST_F(CHOMPMoveitTest, jointSpaceBadGoal)
@@ -69,8 +69,8 @@ TEST_F(CHOMPMoveitTest, jointSpaceBadGoal)
   // joint2 is limited to [-PI/2, PI/2]
   move_group_.setJointValueTarget(std::vector<double>({ 100.0, 2 * M_PI / 3.0 }));
 
-  moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_);
-  EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::INVALID_ROBOT_STATE);
+  moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_);
+  EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::INVALID_ROBOT_STATE);
 }
 
 TEST_F(CHOMPMoveitTest, cartesianGoal)
@@ -83,17 +83,17 @@ TEST_F(CHOMPMoveitTest, cartesianGoal)
   target_pose1.position.z = 10000.;
   move_group_.setPoseTarget(target_pose1);
 
-  moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_);
+  moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_);
   // CHOMP doesn't support Cartesian-space goals at the moment
-  EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS);
+  EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS);
 }
 
 TEST_F(CHOMPMoveitTest, noStartState)
 {
   move_group_.setJointValueTarget(std::vector<double>({ 0.2, 0.2 }));
 
-  moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_);
-  EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::SUCCESS);
+  moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_);
+  EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS);
 }
 
 TEST_F(CHOMPMoveitTest, collisionAtEndOfPath)
@@ -101,8 +101,8 @@ TEST_F(CHOMPMoveitTest, collisionAtEndOfPath)
   move_group_.setStartState(*(move_group_.getCurrentState()));
   move_group_.setJointValueTarget(std::vector<double>({ M_PI / 2.0, 0 }));
 
-  moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_);
-  EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::INVALID_MOTION_PLAN);
+  moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_);
+  EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN);
 }
 
 int main(int argc, char** argv)
diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml b/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml
index bc9af083fa..e4e92e8ed0 100644
--- a/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml
+++ b/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml
@@ -23,4 +23,3 @@ collision_clearence: 0.2
 collision_threshold: 0.07
 random_jump_amount: 1.0
 use_stochastic_descent: true
-
diff --git a/moveit_planners/chomp/chomp_interface/test/joint_limits.yaml b/moveit_planners/chomp/chomp_interface/test/joint_limits.yaml
index 071846af20..fb7bab1217 100644
--- a/moveit_planners/chomp/chomp_interface/test/joint_limits.yaml
+++ b/moveit_planners/chomp/chomp_interface/test/joint_limits.yaml
@@ -11,4 +11,4 @@ joint_limits:
     has_velocity_limits: true
     max_velocity: 6.28318530718
     has_acceleration_limits: false
-    max_acceleration: 0
\ No newline at end of file
+    max_acceleration: 0
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..56100a65ff 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 @@
 <launch>
 
-   <arg name="use_gui" default="false" />
+  <arg name="use_gui" default="false" />
 
   <!-- The name of the parameter under which the URDF is loaded -->
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param name="$(arg robot_description)" command="$(find xacro)/xacro '$(find moveit_planners_chomp)/test/rrbot.xacro'"/>
+  <param name="$(arg robot_description)" command="xacro '$(find moveit_planners_chomp)/test/rrbot.xacro'"/>
 
   <!-- The semantic description that corresponds to the URDF -->
   <param name="$(arg robot_description)_semantic" textfile="$(find moveit_planners_chomp)/test/rrbot.srdf" />
@@ -17,17 +17,18 @@
   </group>
 
   <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="use_gui" value="$(arg use_gui)"/>
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
+    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
+  </node>
+  <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
     <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
   </node>
 
   <!-- Given the published joint states, publish tf for the robot links -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
 
- <!-- move_group settings -->
-  <arg name="allow_trajectory_execution" default="true"/>
-  <arg name="fake_execution" default="true"/>
+  <!-- move_group settings -->
+  <arg name="allow_trajectory_execution" default="true" />
   <arg name="max_safe_path_cost" default="1"/>
   <arg name="jiggle_fraction" default="0.05" />
   <arg name="publish_monitored_planning_scene" default="true"/>
diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst
index f4f24acfb4..b6053babee 100644
--- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst
+++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst
@@ -2,6 +2,23 @@
 Changelog for package chomp_motion_planner
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+* Sanitize CHOMP initialization method parameter (`#2540 <https://github.com/ros-planning/moveit/issues/2540>`_)
+* Contributors: Cong Liu
+
+1.0.7 (2020-11-20)
+------------------
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Migrate to clang-format-10
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..58822b3fe9 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
@@ -54,13 +54,13 @@ class ChompCost
   virtual ~ChompCost();
 
   template <typename Derived>
-  void getDerivative(Eigen::MatrixXd::ColXpr joint_trajectory, Eigen::MatrixBase<Derived>& derivative) const;
+  void getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory, Eigen::MatrixBase<Derived>& 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 +76,8 @@ class ChompCost
 };
 
 template <typename Derived>
-void ChompCost::getDerivative(Eigen::MatrixXd::ColXpr joint_trajectory, Eigen::MatrixBase<Derived>& derivative) const
+void ChompCost::getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory,
+                              Eigen::MatrixBase<Derived>& derivative) const
 {
   derivative = (quad_cost_full_ * (2.0 * joint_trajectory));
 }
@@ -91,7 +92,7 @@ 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);
 }
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..25b3381c8a 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
@@ -57,6 +57,12 @@ class ChompParameters
    */
   void setRecoveryParams(double learning_rate, double ridge_factor, int planning_time_limit, int max_iterations);
 
+  /**
+   * sets a valid trajectory initialization method
+   * @return true if a valid method (one of VALID_INITIALIZATION_METHODS) was specified
+   */
+  bool setTrajectoryInitializationMethod(std::string method);
+
 public:
   double planning_time_limit_;  /// maximum time the optimizer can take to find a solution before terminating
   int max_iterations_;          /// maximum number of iterations that the planner can take to find a good solution while
@@ -90,7 +96,10 @@ class ChompParameters
   double collision_threshold_;  /// the collision threshold cost that needs to be mainted to avoid collisions
   bool filter_mode_;
   // double random_jump_amount_;
+
+  static const std::vector<std::string> VALID_INITIALIZATION_METHODS;
   std::string trajectory_initialization_method_;  /// trajectory initialization method to be specified
+
   bool enable_failure_recovery_;  /// if set to true, CHOMP tries to vary certain parameters to try and find a path if
                                   /// an initial path is not found with the specified chomp parameters
   int max_recovery_attempts_;     /// this the maximum recovery attempts to find a collision free path after an initial
diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml
index 2165cbc1a2..37e025c910 100644
--- a/moveit_planners/chomp/chomp_motion_planner/package.xml
+++ b/moveit_planners/chomp/chomp_motion_planner/package.xml
@@ -2,7 +2,7 @@
   <name>chomp_motion_planner</name>
   <description>chomp_motion_planner</description>
 
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <author email="gjones@willowgarage.com">Gil Jones</author>
   <author email="mail@mrinal.net">Mrinal Kalakrishnan</author>
 
diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp
index 89d700f8e4..d318bda135 100644
--- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp
+++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp
@@ -80,4 +80,18 @@ void ChompParameters::setRecoveryParams(double learning_rate, double ridge_facto
   this->planning_time_limit_ = planning_time_limit;
   this->max_iterations_ = max_iterations;
 }
+
+const std::vector<std::string> ChompParameters::VALID_INITIALIZATION_METHODS{ "quintic-spline", "linear", "cubic",
+                                                                              "fillTrajectory" };
+
+bool ChompParameters::setTrajectoryInitializationMethod(std::string method)
+{
+  if (std::find(VALID_INITIALIZATION_METHODS.cbegin(), VALID_INITIALIZATION_METHODS.cend(), method) !=
+      VALID_INITIALIZATION_METHODS.end())
+  {
+    this->trajectory_initialization_method_ = std::move(method);
+    return true;
+  }
+  return false;
+}
 }  // namespace chomp
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 3db4ea538c..3ef7b6e00e 100644
--- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp
+++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp
@@ -136,7 +136,10 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
     }
   }
   else
+  {
     ROS_ERROR_STREAM_NAMED("chomp_planner", "invalid interpolation method specified in the chomp_planner file");
+    return false;
+  }
 
   ROS_INFO_NAMED("chomp_planner", "CHOMP trajectory initialized using method: %s ",
                  (params.trajectory_initialization_method_).c_str());
diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst
index 923433cc15..57fa3d2e53 100644
--- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst
+++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst
@@ -2,6 +2,30 @@
 Changelog for package moveit_chomp_optimizer_adapter
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+* Use moveit-resources@master (`#2951 <https://github.com/ros-planning/moveit/issues/2951>`_)
+
+  - Simplify launch files to use the test_environment.launch files from moveit_resources@master
+  - Provide compatibility to the Noetic-style configuration of (multiple) planning pipelines
+    Only a single pipeline can be used at a time, specified via the ~default_planning_pipeline parameter.
+* Contributors: Robert Haschke
+
+1.0.8 (2021-05-23)
+------------------
+* Enable mesh filter (`#2448 <https://github.com/ros-planning/moveit/issues/2448>`_)
+* Sanitize CHOMP initialization method parameter (`#2540 <https://github.com/ros-planning/moveit/issues/2540>`_)
+* Contributors: Cong Liu, Jafar Abdi
+
+1.0.7 (2020-11-20)
+------------------
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Migrate to clang-format-10
diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml
index c71ea6053f..45cfdd3c9c 100644
--- a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml
+++ b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml
@@ -3,7 +3,7 @@
   <name>moveit_chomp_optimizer_adapter</name>
   <description>MoveIt planning request adapter utilizing chomp for solution optimization</description>
 
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <author email="raghavendersahdev@gmail.com">Raghavender Sahdev</author>
   <maintainer email="raghavendersahdev@gmail.com">Raghavender Sahdev</maintainer>
   <maintainer email="moveit_releasers@googlegroups.com">MoveIt! Release Team</maintainer>
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 3697230b63..9b5fe0fd3e 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
@@ -58,7 +58,8 @@ namespace chomp
 class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter
 {
 public:
-  OptimizerAdapter() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
+  OptimizerAdapter()
+    : planning_request_adapter::PlanningRequestAdapter(), nh_(planning_interface::getConfigNodeHandle())
   {
     if (!nh_.getParam("planning_time_limit", params_.planning_time_limit_))
     {
@@ -146,12 +147,20 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter
       ROS_INFO_STREAM(
           "Param use_stochastic_descent was not set. Using default value: " << params_.use_stochastic_descent_);
     }
-    if (!nh_.getParam("trajectory_initialization_method", params_.trajectory_initialization_method_))
+    // default
+    params_.trajectory_initialization_method_ = std::string("fillTrajectory");
+    std::string trajectory_initialization_method;
+    if (!nh_.getParam("trajectory_initialization_method", trajectory_initialization_method))
     {
-      params_.trajectory_initialization_method_ = std::string("fillTrajectory");
-      ROS_INFO_STREAM("Param trajectory_initialization_method was not set. Using New value as: "
+      ROS_INFO_STREAM("Param trajectory_initialization_method was not set. Using value: "
                       << params_.trajectory_initialization_method_);
     }
+    else if (!params_.setTrajectoryInitializationMethod(trajectory_initialization_method))
+    {
+      ROS_ERROR_STREAM("Param trajectory_initialization_method set to invalid value '"
+                       << trajectory_initialization_method << "'. Using '" << params_.trajectory_initialization_method_
+                       << "' instead.");
+    }
   }
 
   std::string getDescription() const override
diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst
index 1c81adc421..eaf85c1ad9 100644
--- a/moveit_planners/moveit_planners/CHANGELOG.rst
+++ b/moveit_planners/moveit_planners/CHANGELOG.rst
@@ -2,6 +2,21 @@
 Changelog for package moveit_planners
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+
 1.0.6 (2020-08-19)
 ------------------
 
diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml
index 83e74f8d62..efc27a442b 100644
--- a/moveit_planners/moveit_planners/package.xml
+++ b/moveit_planners/moveit_planners/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_planners</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Metapacakge that installs all available planners for MoveIt</description>
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="sachinc@willowgarage.com">Sachin Chitta</author>
diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst
index a1cfb44fea..1149aab6ea 100644
--- a/moveit_planners/ompl/CHANGELOG.rst
+++ b/moveit_planners/ompl/CHANGELOG.rst
@@ -2,6 +2,31 @@
 Changelog for package moveit_planners_ompl
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+* [feature] Melodic backports in ompl interface (cleanup) (`#2277 <https://github.com/ros-planning/moveit/issues/2277>`_)
+  * add consistent named logging to ompl interface
+  * add named logging to the ompl planner service script
+  * Remove dead code from ompl interface (related to subspaces and state validity cache)
+  * add some documentation to the ompl interface
+  * fix clang-tidy warnings in ompl interface
+  * fix some spelling errors in the ompl interface
+  * fix melodic specific clang-tidy warnings in ompl interface
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: Felix von Drigalski, Jeroen
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Adapt repository for splitted moveit_resources layout (`#2199 <https://github.com/ros-planning/moveit/issues/2199>`_)
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 36a70b0c49..7c00769870 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
@@ -63,7 +63,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 +142,7 @@ class OMPLDynamicReconfigureConfig
     }
   };
 
-  MOVEIT_CLASS_FORWARD(AbstractGroupDescription);
+  MOVEIT_CLASS_FORWARD(AbstractGroupDescription);  // Defines AbstractGroupDescriptionPtr, ConstPtr, WeakPtr... etc
   class AbstractGroupDescription : public dynamic_reconfigure::Group
   {
   public:
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/constraints_library.h
index ed8ddb10e8..c81112a7e5 100644
--- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/constraints_library.h
+++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/constraints_library.h
@@ -156,7 +156,7 @@ struct ConstraintApproximationConstructionResults
   double sampling_success_rate;
 };
 
-MOVEIT_CLASS_FORWARD(ConstraintsLibrary);
+MOVEIT_CLASS_FORWARD(ConstraintsLibrary);  // Defines ConstraintsLibraryPtr, ConstPtr, WeakPtr... etc
 
 class ConstraintsLibrary
 {
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 49f93dbdb0..e0ba6670f1 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
@@ -46,7 +46,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 */
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 083d91f203..9efe70a841 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
@@ -53,8 +53,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<ob::PlannerPtr(const ompl::base::SpaceInformationPtr& si, const std::string& name,
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 c3e05e4832..d80d344782 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
@@ -43,7 +43,7 @@
 
 namespace ompl_interface
 {
-MOVEIT_CLASS_FORWARD(ModelBasedStateSpaceFactory);
+MOVEIT_CLASS_FORWARD(ModelBasedStateSpaceFactory);  // Defines ModelBasedStateSpaceFactoryPtr, ConstPtr, WeakPtr... etc
 
 class ModelBasedStateSpaceFactory
 {
diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml
index e1610eada2..3274053fcb 100644
--- a/moveit_planners/ompl/package.xml
+++ b/moveit_planners/ompl/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_planners_ompl</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>MoveIt! interface to OMPL</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
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..44e3339ccd
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst
@@ -0,0 +1,27 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package pilz_industrial_motion_planner
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+* Use moveit-resources@master (`#2951 <https://github.com/ros-planning/moveit/issues/2951>`_)
+
+  - Simplify launch files to use the test_environment.launch files from moveit_resources@master
+  - Provide compatibility to the Noetic-style configuration of (multiple) planning pipelines
+    Only a single pipeline can be used at a time, specified via the ~default_planning_pipeline parameter.
+  - Rename launch argument execution_type -> fake_execution_type
+* Contributors: Michael Görner, Robert Haschke
+
+1.0.8 (2021-05-23)
+------------------
+* Fix velocity limit error (`#2610 <https://github.com/ros-planning/moveit/issues/2610>`_)
+* Add missing dependency on joint_limits_interface (`#2487 <https://github.com/ros-planning/moveit/issues/2487>`_)
+* Use kinematics solver timeout if not specified otherwise (`#2489 <https://github.com/ros-planning/moveit/issues/2489>`_)
+* Add pilz_industrial_motion_planner to moveit_planners (`#2507 <https://github.com/ros-planning/moveit/issues/2507>`_)
+* Contributors: Christian Henkel, Christian Landgraf, Immanuel Martini, Michael Görner, Robert Haschke, Tyler Weaver
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..85523e9d4a
--- /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
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..6238ec8747
--- /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 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <help_text>// 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 &gt; New..." to create new element types
+//////////////////////////////////////////////////////////////////////////////////////////////
+
+
+// This text will be stored with each diagram;  use it for notes.</help_text>
+  <zoom_level>7</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>819</x>
+      <y>308</y>
+      <w>147</w>
+      <h>56</h>
+    </coordinates>
+    <panel_attributes>MoveGroupCapability
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>917</x>
+      <y>420</y>
+      <w>168</w>
+      <h>63</h>
+    </coordinates>
+    <panel_attributes>MoveGroupSequenceAction
+--
+
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>742</x>
+      <y>420</y>
+      <w>140</w>
+      <h>63</h>
+    </coordinates>
+    <panel_attributes>MoveGroupSequenceService
+--
+
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>833</x>
+      <y>357</y>
+      <w>21</w>
+      <h>77</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;90.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>938</x>
+      <y>357</y>
+      <w>21</w>
+      <h>77</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;90.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLNote</id>
+    <coordinates>
+      <x>980</x>
+      <y>308</y>
+      <w>98</w>
+      <h>21</h>
+    </coordinates>
+    <panel_attributes>Plugin interface</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>959</x>
+      <y>308</y>
+      <w>35</w>
+      <h>21</h>
+    </coordinates>
+    <panel_attributes>lt=..</panel_attributes>
+    <additional_attributes>30.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>763</x>
+      <y>532</y>
+      <w>168</w>
+      <h>63</h>
+    </coordinates>
+    <panel_attributes>CommandListManager
+--
+
+
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1155</x>
+      <y>525</y>
+      <w>126</w>
+      <h>35</h>
+    </coordinates>
+    <panel_attributes>plan_execution::PlanExecution
+--
++planAndExecute()</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1204</x>
+      <y>469</y>
+      <w>21</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;80.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1155</x>
+      <y>427</y>
+      <w>112</w>
+      <h>49</h>
+    </coordinates>
+    <panel_attributes>MoveGroupContext
+--
++ plan_execution_
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1078</x>
+      <y>448</y>
+      <w>91</w>
+      <h>21</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;110.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>749</x>
+      <y>644</y>
+      <w>182</w>
+      <h>63</h>
+    </coordinates>
+    <panel_attributes>TrajectoryBlenderTransitionWindow
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>924</x>
+      <y>476</y>
+      <w>21</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;80.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>784</x>
+      <y>476</y>
+      <w>21</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;80.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>329</x>
+      <y>553</y>
+      <w>203</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>/planning_interface::PlannerManager/
+CommandPlanner</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>392</x>
+      <y>616</y>
+      <w>21</w>
+      <h>63</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;70.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>434</x>
+      <y>616</y>
+      <w>21</w>
+      <h>63</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;70.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>168</x>
+      <y>665</y>
+      <w>126</w>
+      <h>49</h>
+    </coordinates>
+    <panel_attributes>Plugin
+PTP TrajectoryGenerator
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>546</x>
+      <y>665</y>
+      <w>133</w>
+      <h>49</h>
+    </coordinates>
+    <panel_attributes>Plugin
+CIRC TrajectoryGenerator
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>364</x>
+      <y>665</y>
+      <w>126</w>
+      <h>49</h>
+    </coordinates>
+    <panel_attributes>Plugin
+LIN TrajectoryGenerator
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>210</x>
+      <y>574</y>
+      <w>133</w>
+      <h>105</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;130.0;10.0;10.0;170.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>224</x>
+      <y>588</y>
+      <w>119</w>
+      <h>91</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>150.0;10.0;10.0;10.0;10.0;110.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>525</x>
+      <y>588</y>
+      <w>126</w>
+      <h>91</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>160.0;110.0;160.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>161</x>
+      <y>539</y>
+      <w>532</w>
+      <h>182</h>
+    </coordinates>
+    <panel_attributes>MoveIt Planning Pipeline
+halign=left
+fg=blue</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Text</id>
+    <coordinates>
+      <x>252</x>
+      <y>637</y>
+      <w>203</w>
+      <h>21</h>
+    </coordinates>
+    <panel_attributes>planning_interface::MotionPlanRequest</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Text</id>
+    <coordinates>
+      <x>441</x>
+      <y>637</y>
+      <w>210</w>
+      <h>21</h>
+    </coordinates>
+    <panel_attributes>planning_pipeline::MotionPlanResponse</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>525</x>
+      <y>574</y>
+      <w>140</w>
+      <h>105</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;180.0;10.0;180.0;130.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>686</x>
+      <y>546</y>
+      <w>91</w>
+      <h>28</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-
+&lt;&lt;uses&gt;&gt;</panel_attributes>
+    <additional_attributes>10.0;20.0;110.0;20.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>826</x>
+      <y>588</y>
+      <w>42</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-
+&lt;&lt;uses&gt;&gt;</panel_attributes>
+    <additional_attributes>10.0;80.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>147</x>
+      <y>287</y>
+      <w>959</w>
+      <h>448</h>
+    </coordinates>
+    <panel_attributes>Motion planning
+layer=-1000
+halign=left</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1127</x>
+      <y>287</y>
+      <w>168</w>
+      <h>448</h>
+    </coordinates>
+    <panel_attributes>Motion execution
+layer=-1000
+halign=left</panel_attributes>
+    <additional_attributes/>
+  </element>
+</diagram>
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 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>7</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>287</x>
+      <y>273</y>
+      <w>567</w>
+      <h>133</h>
+    </coordinates>
+    <panel_attributes>PlanningContextLoader
+--
+...
+--
++getAlgorithm(): std::string
++setModel(model: moveit::core::RobotModelConstPtr&amp;): bool
++setLimits(joint_limits: pilz_industrial_motion_planner::JointLimitsContainer&amp;): bool
++loadContext(planning_context: planning_interface::PlanningContextPtr&amp;, name: std::string&amp;, group: std::string&amp;): bool
+#loadContext&lt;T&gt;(planning_context: planning_interface::PlanningContextPtr&amp;, name: std::string&amp;, group: std::string&amp;): bool
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>0</x>
+      <y>525</y>
+      <w>259</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>symbol=component
+PlanningContextLoaderPTP
+--
+--
++loadContext(planning_context, name, group)
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>448</x>
+      <y>525</y>
+      <w>259</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>symbol=component
+PlanningContextLoaderLIN
+--
+--
++loadContext(planning_context, name, group)
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>119</x>
+      <y>350</y>
+      <w>182</w>
+      <h>189</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>240.0;10.0;10.0;10.0;10.0;250.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>567</x>
+      <y>399</y>
+      <w>21</w>
+      <h>140</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;180.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>35</x>
+      <y>756</y>
+      <w>133</w>
+      <h>35</h>
+    </coordinates>
+    <panel_attributes>PlanningContextPTP
+--
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>511</x>
+      <y>749</y>
+      <w>126</w>
+      <h>35</h>
+    </coordinates>
+    <panel_attributes>PlanningContextLIN
+--
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1260</x>
+      <y>616</y>
+      <w>119</w>
+      <h>126</h>
+    </coordinates>
+    <panel_attributes>template=T
+PlanningContextBase
+--
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Text</id>
+    <coordinates>
+      <x>21</x>
+      <y>609</y>
+      <w>175</w>
+      <h>14</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;bind&gt;&gt;&lt;T-&gt;TrajectoryGeneratorPTP&gt;</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>133</x>
+      <y>644</y>
+      <w>1141</w>
+      <h>126</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>1610.0;10.0;10.0;10.0;10.0;160.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>602</x>
+      <y>672</y>
+      <w>672</w>
+      <h>91</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>940.0;10.0;10.0;10.0;10.0;110.0</additional_attributes>
+  </element>
+  <element>
+    <id>Text</id>
+    <coordinates>
+      <x>490</x>
+      <y>609</y>
+      <w>175</w>
+      <h>14</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;bind&gt;&gt;&lt;T-&gt;TrajectoryGeneratorLIN&gt;</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>511</x>
+      <y>1001</y>
+      <w>126</w>
+      <h>35</h>
+    </coordinates>
+    <panel_attributes>TrajectoryGeneratorLIN
+--
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>98</x>
+      <y>588</y>
+      <w>21</w>
+      <h>182</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;..</panel_attributes>
+    <additional_attributes>10.0;240.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>35</x>
+      <y>1001</y>
+      <w>126</w>
+      <h>35</h>
+    </coordinates>
+    <panel_attributes>TrajectoryGeneratorPTP
+--
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>1176</x>
+      <y>861</y>
+      <w>308</w>
+      <h>98</h>
+    </coordinates>
+    <panel_attributes>TrajectoryGenerator
+--
+--
+TrajectoryGenerator(RobotModelConstPtr, LimitsContainer)
+generate(request, response, sampling_time)
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>567</x>
+      <y>588</y>
+      <w>21</w>
+      <h>175</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;..</panel_attributes>
+    <additional_attributes>10.0;230.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>567</x>
+      <y>777</y>
+      <w>21</w>
+      <h>238</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-
+m1=1</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;320.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>98</x>
+      <y>784</y>
+      <w>21</w>
+      <h>231</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-
+m1=1</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;310.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>602</x>
+      <y>903</y>
+      <w>588</w>
+      <h>112</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>820.0;10.0;10.0;10.0;10.0;140.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>126</x>
+      <y>875</y>
+      <w>1064</w>
+      <h>140</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>1500.0;10.0;10.0;10.0;10.0;180.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLNote</id>
+    <coordinates>
+      <x>0</x>
+      <y>441</y>
+      <w>98</w>
+      <h>49</h>
+    </coordinates>
+    <panel_attributes>Plugin
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>42</x>
+      <y>483</y>
+      <w>21</w>
+      <h>56</h>
+    </coordinates>
+    <panel_attributes>lt=..</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;60.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1589</x>
+      <y>525</y>
+      <w>147</w>
+      <h>112</h>
+    </coordinates>
+    <panel_attributes>JointLimitsContainer
+--
+--
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1792</x>
+      <y>525</y>
+      <w>147</w>
+      <h>112</h>
+    </coordinates>
+    <panel_attributes>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
+--
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1673</x>
+      <y>399</y>
+      <w>196</w>
+      <h>91</h>
+    </coordinates>
+    <panel_attributes>LimitsContainer
+--
+--
++hasCartesianLimit(): bool
++getCartesianLimit(): pilz_industrial_motion_planner::CartesianLimit
++hasJointLimits(): bool
++getJointLimits(): pilz_industrial_motion_planner::JointLimitsContainer
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1764</x>
+      <y>483</y>
+      <w>126</w>
+      <h>56</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-
+m1=1</panel_attributes>
+    <additional_attributes>10.0;10.0;160.0;60.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1659</x>
+      <y>483</y>
+      <w>70</w>
+      <h>56</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-
+m1=1</panel_attributes>
+    <additional_attributes>80.0;10.0;10.0;60.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>168</x>
+      <y>1232</y>
+      <w>126</w>
+      <h>42</h>
+    </coordinates>
+    <panel_attributes>/KDL::VelocityProfile/
+--
+--
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>28</x>
+      <y>1302</y>
+      <w>147</w>
+      <h>63</h>
+    </coordinates>
+    <panel_attributes>VelocityProfileATrap
+--
++ SetProfile
++ SetProfileDuration
++ SetProfileAllDurations
+--
+--
+
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>126</x>
+      <y>1246</y>
+      <w>56</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>60.0;10.0;10.0;10.0;10.0;80.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>91</x>
+      <y>1029</y>
+      <w>21</w>
+      <h>287</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-
+m1=n</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;390.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>525</x>
+      <y>1316</y>
+      <w>91</w>
+      <h>42</h>
+    </coordinates>
+    <panel_attributes>Package::KDL
+Path_Line
+--
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>833</x>
+      <y>1316</y>
+      <w>84</w>
+      <h>42</h>
+    </coordinates>
+    <panel_attributes>Package::KDL
+Path_Circle
+--
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>980</x>
+      <y>1316</y>
+      <w>105</w>
+      <h>42</h>
+    </coordinates>
+    <panel_attributes>Package::KDL
+Path_Circle_Wrapper
+--
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLNote</id>
+    <coordinates>
+      <x>1393</x>
+      <y>1176</y>
+      <w>98</w>
+      <h>49</h>
+    </coordinates>
+    <panel_attributes>Class to manage the Cartesian path
+bg=white</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1330</x>
+      <y>1190</y>
+      <w>77</w>
+      <h>21</h>
+    </coordinates>
+    <panel_attributes>lt=..</panel_attributes>
+    <additional_attributes>10.0;10.0;90.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>560</x>
+      <y>1029</y>
+      <w>21</w>
+      <h>301</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;410.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>882</x>
+      <y>1001</y>
+      <w>168</w>
+      <h>35</h>
+    </coordinates>
+    <panel_attributes>TrajectoryGeneratorCIRC
+--
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1022</x>
+      <y>1029</y>
+      <w>21</w>
+      <h>301</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;410.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>581</x>
+      <y>1197</y>
+      <w>693</w>
+      <h>133</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>970.0;10.0;10.0;10.0;10.0;170.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>889</x>
+      <y>1232</y>
+      <w>385</w>
+      <h>98</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>530.0;10.0;10.0;10.0;10.0;120.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1050</x>
+      <y>1267</y>
+      <w>224</w>
+      <h>63</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>300.0;10.0;10.0;10.0;10.0;70.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>896</x>
+      <y>742</y>
+      <w>126</w>
+      <h>35</h>
+    </coordinates>
+    <panel_attributes>PlanningContextCIRC
+--
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>952</x>
+      <y>770</y>
+      <w>21</w>
+      <h>245</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-
+m1=1</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;330.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>987</x>
+      <y>700</y>
+      <w>287</w>
+      <h>56</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>390.0;10.0;10.0;10.0;10.0;60.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>826</x>
+      <y>525</y>
+      <w>259</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>symbol=component
+PlanningContextLoaderCIRC
+--
+--
++loadContext(planning_context, name, group)
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>952</x>
+      <y>588</y>
+      <w>21</w>
+      <h>168</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;..</panel_attributes>
+    <additional_attributes>10.0;220.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Text</id>
+    <coordinates>
+      <x>875</x>
+      <y>609</y>
+      <w>175</w>
+      <h>14</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;bind&gt;&gt;&lt;T-&gt;TrajectoryGeneratorLIN&gt;</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>987</x>
+      <y>931</y>
+      <w>203</w>
+      <h>84</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>270.0;10.0;10.0;10.0;10.0;100.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>847</x>
+      <y>350</y>
+      <w>119</w>
+      <h>189</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;150.0;10.0;150.0;250.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>91</x>
+      <y>469</y>
+      <w>434</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=..</panel_attributes>
+    <additional_attributes>10.0;10.0;600.0;10.0;600.0;80.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>91</x>
+      <y>448</y>
+      <w>812</w>
+      <h>91</h>
+    </coordinates>
+    <panel_attributes>lt=..</panel_attributes>
+    <additional_attributes>10.0;10.0;1140.0;10.0;1140.0;110.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>287</x>
+      <y>126</y>
+      <w>567</w>
+      <h>63</h>
+    </coordinates>
+    <panel_attributes>CommandPlanner
+--
+--
++getPlanningContext(const planning_scene::PlanningSceneConstPtr&amp; planning_scene,
+const planning_interface::MotionPlanRequest&amp; req,
+moveit_msgs::MoveItErrorCodes&amp; error_code):planning_interface::PlanningContextPtr
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>567</x>
+      <y>182</y>
+      <w>21</w>
+      <h>105</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-
+m1=n</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;130.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>476</x>
+      <y>0</y>
+      <w>203</w>
+      <h>49</h>
+    </coordinates>
+    <panel_attributes>PlanningInterface::PlannerManager
+--
+--
+
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>567</x>
+      <y>42</y>
+      <w>21</w>
+      <h>98</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;120.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1211</x>
+      <y>518</y>
+      <w>203</w>
+      <h>49</h>
+    </coordinates>
+    <panel_attributes>PlanningInterface::PlanningContext
+--
+--
+
+--</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1309</x>
+      <y>560</y>
+      <w>21</w>
+      <h>77</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;90.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>910</x>
+      <y>1330</y>
+      <w>84</w>
+      <h>21</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-</panel_attributes>
+    <additional_attributes>100.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>308</x>
+      <y>1302</y>
+      <w>147</w>
+      <h>63</h>
+    </coordinates>
+    <panel_attributes>Package::KDL
+VelocityProfile_Trap
+--
++ SetProfile
++ SetProfileDuration
+--
+--
+
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>420</x>
+      <y>1008</y>
+      <w>476</w>
+      <h>308</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-</panel_attributes>
+    <additional_attributes>660.0;10.0;410.0;10.0;410.0;140.0;10.0;140.0;10.0;420.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>770</x>
+      <y>1141</y>
+      <w>126</w>
+      <h>42</h>
+    </coordinates>
+    <panel_attributes>Package::KDL
+Trajectory_Segment
+--
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>889</x>
+      <y>1029</y>
+      <w>63</w>
+      <h>147</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-</panel_attributes>
+    <additional_attributes>70.0;10.0;70.0;190.0;10.0;190.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>287</x>
+      <y>1246</y>
+      <w>63</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;70.0;10.0;70.0;80.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>784</x>
+      <y>1064</y>
+      <w>98</w>
+      <h>35</h>
+    </coordinates>
+    <panel_attributes>/KDL::Trajectory/
+--
+--
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>826</x>
+      <y>1092</y>
+      <w>21</w>
+      <h>63</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;70.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1260</x>
+      <y>1183</y>
+      <w>77</w>
+      <h>112</h>
+    </coordinates>
+    <panel_attributes>/KDL::Path/
+--
+--
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>588</x>
+      <y>1029</y>
+      <w>196</w>
+      <h>147</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;190.0;260.0;190.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>357</x>
+      <y>1008</y>
+      <w>168</w>
+      <h>308</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-</panel_attributes>
+    <additional_attributes>220.0;10.0;10.0;10.0;10.0;420.0</additional_attributes>
+  </element>
+</diagram>
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\`gnWKA<d)%!D/sHMQPJC(mPa$\k
+#Vj"@VNFFUA8)uL#YVoe@7n<Y["R?H2F5FJpQ:J!*fXcl1jYagR5=G\iJUf8go5RKcC@Df1ZrRl*epF[
+NulID%eK%JJ,J?6?NThl+&];O\\,T.l"kh348$@GV=/eEH"j6Z^41!:Wir#Ii,I0I]j>W"Fg]ukHtS*Q
+MgT5QlW?uD#_u/G`2SPP'fF5e9\c\P^,kYJ5eE:[j5oCaDKi4Qp6Lp"]ggq8kK)a$%!"=BP>O^;fEK%$
+HC-?[8AqK`3#+<q/52RVb/%%%E2V0a578Mp*U\$HFd\&/E&_U/E\(<sX+oUs%KYDRi=*[l,#1SFWMjNC
+@H+@e-F1(g<^u"E]-Bh,_pp&@>G0X[LSK%YosU04)0km3'K:;6&/&t4,#1R$1CFk/S[,-u]:ccG7c<6.
+f_iq/3#p2"I1O<YW3Y!iO[;UWWJEMK29d[GS9;t6]66Kl=?:K`s0Dlt+GZ_;;[s&5X#397rPQ\(;E<5P
+%`LaY:.<+#W+fjPiiH(5%,;,7mh*'Z%P&ga<&D(aTm5c!j[U@g<\lp9/0s4S7%B.'Ael`=3b@p!r*+W0
+\nEnZI'qEP,B.Sj:Cn.Y.[khh6ZL?_-Fe8'8AqK`3#+<q/52RV]#aSk(@^`b^rtc`:PD3,MAF7`':G/Q
+4%i4nmQ3GE+"ehu<(*76RV!<^c8o5f@kIeC]=1LW$$^=55?:=nWFu<Lh3Xf.^3g0q9IqIK<(*76RV!<^
+c8o5BgGd!L\HEn#&<`-2;piJ9\kKcJg9&%ehu*Z+4p[!)'bR@<VO<-pZVK[cFX-+@NruPSJ#aMS+,l;;
+;\AbB?b^CFoL+`f((tU:,#1R$1CFk/S^*k]B-AR+!Zjp(`2SR,hV-]A1X/Gfr,2ECmt=*!MMP3Y!Bg5B
+UITNSs8D]4IC!(D39tZMN4K-SHCJ@@'(AFY,#1Q8\UTO>RSS-d\kFY#i`&=9MC/#J^](X"[jrsZD[e+%
+*;BDD@H+B;<i`n:4DanlX9k5D7hQGS.UoJcDenL;s-UiZB+kZ03#*%'U.JY*I_C]L28ekoZS6CMC$Y,u
+&bOrODeJrBcqV8)+VCUrp9<.ii2TS$r-Dpcg+S]AF#Y@=7?b<1q=:L`VtU/hAms]#e$^*7%TmcYG^6$]
+O9>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<Hhgohpf>*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=<YH=)Qj*FmRbcQX>2egj`cK-I+Y82#D_;n>osu*_Ao@WJD7@+]Sd(RYeQ/mC)7Ub@,BM,#1Rj
+\p3c)ace+0H0:ahcSoe!4Ep5Kj,ZERqtBDshes8Wc8oglp;bd"/ccKTPtgd<cp9uB:.:u2*E"a,O5)_n
+E!q\cdgU1T,#1Rj.V%qD9:(YZ4'(22q<=fLrquB6CVJ[W9Up7ZKUpEjhKoAhjC\KP5tJqnL_Sl9)S'j6
+`(_+cSAsND$n=<*p%@tg5Q(#?Dnaor43bX\=f5E&q7#+\HN`ib`k7UD!Zjp(`2SP,H\U&-3'HsnKf2[Z
+4GP0@<7Eb20QGZX:c/<@;lA?I+O/F_i/r[\S;KkBc]EI"*klK]?=475`2T\LAms]#dfZP^"?c5^_d$IK
+*_Ao@WJD7PKiAQGA`/"+R?k*RZa4Xt_)NDHVkU&VKo*-2pM]!X/,L69gN=b)+<_>p,#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_<LHn;(31Q*(]#EZ]q^U#h%S+I:W+fjPioYBSem8D!
+q(,p?J4&HV,S$#q89)]oNSHKVr1j?4Vq]5MLc?,,bq)QD4K[)tVt:MF8bOt?qO)c#Gm]CB;%Y7S'%5.H
+l-YQapCl0PE&]%:785QG'/p0^FNd4^\e3Tj8@geeM5aDc.Adq[s8?VlT>NGU*0,hR;:Eb+`3j]Xm^rZb
+p=!1t!?[brL;Zb_&Lh<9Ut+#?:1oWEfZ*.n2A6hQE[kgm=dZ^BXj]H%<AMWAi5='IOI/`l,/jtE<8G-[
+Ag4['H</9e7C8A<RV!<^DYD[rFXjZpoC`(+^KV48E[0ec2u=aQqc#t@R1!`TRlPB$F%uO3ZGXmd6joum
+BkCf5Ap0`*3d94d"El$R.@SqOBFtuA@Wk+B>oi`o^VMk7SrM4*@<O/q4]KofZS6AT[8$rZ!KVX3YY2d<
+Br44kV,Tj?=V#5u`p'2+[ViS?8%V\YlP7eO.VbDTR(-:$Vu(oYYY2d<CDK72m8QK2G(`iOq6oaq<PRF*
+g95O:[A+pXRu3YH(+*p&lZE"V=ESi@oISpDEPNqs7AOTu88E!Vh/9]N,.;Lfn?o;@As8+@Ael`]OOn2l
+diZ(3oY*e&=+FMk!M;=dOeRgY<DQ2bR>>^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]<NDE-PfV]o$_9h6Kf:@Hh0ea<2VE^Sud,,Rn"9f!.jQa0B0>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</ci:;#CPN1
+7#JqmC$3M#EVH`.p8`"b`N=qY.VbDTR#(#?E2dNSLEO,1bha?/M,P!8KN)fYY(*a0NRRQ"7%B.'AenuV
+jfP_t"ME@a47nOnmFTV,F20.u7ALJmV>?$mc!.i?dkt)mS;NbI(1s1^h?\F7oC(u0lEjnUD;p(AS;Ki\
+Ud8Q9/oTsE:1qekTPU,>9h!""W+fjPC/jfnW.tr8?NTXiqh<Kk%f!uAL_N/oWP#uiCH#",SM<)/GX+^9
+WMjNC@>7ajc.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.-<Ar2Uh2hoVmC%UVc?-TMOJ,mSpRm:
+n*!oabeN6kCZHf"OfVJZ#`1MUO[;SD<8DVkjo\\CZuK8<`2T\LB&U;-rR9FOiuW\(jpmJWmpHCQa(1a(
+6:f%oL_N15.OQEeD4I3p#8=.o3#+<q/3Gk756#iq7fYc@J%'<H'mr6O:.ZTSi`&;Q+lsQUEOsT_FQW?Y
+'Ik;%785QG[o`Ug8)s=f2)tUGZ;*t7,iVck$6`t![;gNeC!!_hANgh@VkU&VKj/*Y-QbL!Qg&TQ\b#84
+l]`)M,#1S&&5j_oCn)R@KDP"`D4FW=E@P^l=a7qd:UGUjkH"PFr:e$?EOsE>^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%<Foopt#)cU6#@C=hr:`&]2uOGHgI.>0McWf>J3PAe]/6YCZ([#0Z<;:Eb+qc)t;g$9tdqgI0q
+s"hDA*I`cGVkak:E@N/dOCh%kjfW41h5kLa<Y9=$C78:MaP]8FrS?4Rmr>kkb;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%_VD<iV<JT;c3mX3#.QB8@*3X`p(=gZfPn>S5p5A
+.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:^<cTbe.rR73h
+q\O;YYNPWR0L(qG'D=-3cbe3aSZ<]6<VNqBZ1e7";c3mX3#.QB88V<hntY?MVh_p)>"-B/BMqVgn\hc[
+526,)gMai4VVR]s]_e=THhVCNJ!J-\s(ccMric<tgG3/Ss4Z20>jYW7ZYB4Xf52n+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"M<aZ:`%OGHgI.>0M_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'<dn>cFDYUNf<6t$cC[<cm?bUN&/&t4,))-@ccXC&3R8[h7f.[*
+Nr6Z@VlsC;Pk`"l,2sBKU=l&jLMgg+?*<R%[&nd?pn[EXq_2*thQ7j6>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"<CE@7RV!<^c8m8*OGHgI.>5Wf
+gDd%PS^."cNG`U'*dMB>'bR@<VO<-pZVF(8,iVck$6_."H8'?`7(dm,[#0Z<;:Eb+`*'"6&/&t4,))*q
+WLAXdjBFDSpGYjJLc?,,bq)PYF\2ua&Z6gN#`0I2,$qD-35+u6j5oB0YH1FpQ-su=785QG'5*l!6:f%o
+L_N0b5=TO\mb`?es,J)P\f^T/mPk[93#4Br/52RV8dZ^RV]o"EM>\CI3'H9k`(Nam[<J-qE?'n*Ael`=
+3[KKALf4m7KZ'\_)lH3M_Yo=9H$OJ\*N*<ca.fH!Ko*-28KfA=<(%':EH'H\M/+GI)lM@Ja.fH!Ko*-2
+8KfA=<(%':&_64Gna+r2agYk1[W.bL3#4Br/52RV8dZ^RV]o"EM7B[FHG-G3[s$g(o($hI`bEG3/8CVV
+QoODr'LF!=7ALJ-EB>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:@EWinbn<s`p(%r=O]160QGYm.>1*ZMF\m9iY!d,<=^FZR>>^MSl:Y<
+9f!.jB:Y2P8AqK`'LEur^S5He/sC5PARaeX@o:_!_SfTTU_"/Y.O'S.Lhj#b1(g^mDK\)6'62@<l+,_S
+e24KcERlQfKLD>f80K8<)R$)_j7LdJ;RbCdZZ3S<*/g7t(+.<f-&%G:;\:I37,/qc1hPe?n:QR3-c_sY
+R>$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>f80K8<B"Xl;`p)Gb
+\3CS[3'@@U]V7Do(+.<f-&%G:;\:I36sMf'g3s6)s1uXKLLM4#Vl(OM[@(0b39ueC66]/nU_"/Y/ZWQ%
+D<jN-rBtJ^oV$&'74i$c-I+XuKoh*h&LiB$>Uq>)/eciSS6iOAq@L'?U/iuV'5*l!6:f%oL_N0"Y/M-@
+M0O/cjMpH##89o1<A&q?=4@jp@H+@e;@&+='p1gR`6CY<]/ECp4d``Q`'#';RktWjbr2m^ERlQfKLD>f
+80K8<k,4pY)lH1U'62XDl,LqC7C1,f9Up5t$6^"Z,#S]'\9Y-Rf,K#ina<+'`p%U:<mg7g$PR0DP!V^X
+WJD6T,&U^;>$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<G\fZ*0t/Q>/;@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)<jh<p_kdjf+(=dV27PS?G.:.:ri&ofDTgOkL7USd"*Q\hX=.]5j,9f!.j
+B:Y2P8AqK`'LF!=7Jk^&ot&ssrC";cQ\e_l1p\lUegW&$7%B-*+XH&H1oW5BQ^N'Q6sOgpSZO7U0+o!W
+bq)PYF\2ua&Z6gN#`1MU*DDlWbJ;l3h)N*Y0H6oie24KcERlQfKLD>f80K8<<"q0,j7N:3DO$''!?Oq9
+U8F?4M6frK+XH&H6jb(LVkOm;2bjlJS%-r#SDT=T.VbDTQoODr'LF!=7ALJ-E@OT1h`*b]#S15gT#Zi>
+.QW)M++<eF:@GstrjZl)$PR0DP!V^XWJD6T,#1Rdkk2Bm7b:>Po<iRA7?dT6?iO<LgQe%h[g&'MNr+:3
+0QGYm.>1*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=b<md]aJ,JgM-VdpWEok<#)cs4L/\aGsqXg+]IQ?in
+O%uY.mCiohSNhUQg+S#&+=,rG6jb(LVkU0coubE\\,4.lUI0ZQhiT-RcCR0\m?b@jMd4Z$A+09jqUYhL
+04)DBrKC#@l$q#lT7'`EfU#ojWFcp;ACQ>Gp6.#n8P$e!L_N0":.8+4lZKhq0`uE22GU<O`mq+k#Hue:
+\T;EE[se;RhnOF+ij_4k/52RV8dZ^RV]o"EM)Z6pJmPXi]Y$@E,-H[[=l)-?U)LLYh7>dX@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"02rEutSRu<iXA`"&M)Z646:f%oW%G$/
+dj&J`r^8l^NRK@lRTZ";l]`)M,#1S&&/&t4C7/CHlRHkAHu"Jr6f0l*U8F?4M6frK+XH&H6jb(LVkR3A
+'6+n74.Ws>p^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[;UWWFu<lgc.Ih@,fF&2GU<OBkCf5XA`"&M)Z646:f%oBTZ:6
+VjDQiR/#W.]YBHT.VbDTQoODr'LF!=7ALJ-E@R\V`*6;oa%aMLfJp"-VO<-pZVF(8,iVck$6^"Z,46rp
+E>a!_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"^B<ZmUQhLf4m7
+KLD>fL`<F/B]J`@`tQ'Y4?9`%EK9ag&/&t4,)),7;\=#]/*^Ig*\e$5?7'jEl2<=E'Ba%)$6^"Z,#S]'
+3#*$,=&H012=litM#3Nqm@CCk(+.>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<qgYWJD6T,#1SFAQ@60&l\a*h!K8$D(%:M'Ba%)$6^"Z,#S]'3#*$l?;VStQ^=&^fNUQS0?t5gc,.CZ
+g64io`2SP,66]/n`&1WDg=f_SbhhS)%qD(&U"CniS>/4rKLD>f80K8<<((!?'ifUYLPFg9:FMg`U"Cni
+S>/4rKLD>f80K8<<(,N8<AaK%igPW:cehQ8(+.>SSeOL8:.:ri&ofBFD@.,<Gs_%WTt#JGq9G9jP0#7K
+S>/4rKLD>f80K8<<(,Lu/)'/_r89TSCMR]aH`7$X@H&9o8KfA=<(%':&Z6i4Atl$5L>no)1::mG2@C0W
+'C1nV',#:7,)),7;\=#U<DbUfGIo()iK90Nm`kY>C9oU6ZaEPB/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@<QT*^38mIq9%6g=7=tWpW4eTP](gGq<)Pm
+&@4'1egW&$7%B-*+XH&H7%B-(>$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;=+:%l47<T,o8+,.?@;:^EIX3UQoODr'LF!=7ALJ-E@N/-7J2CI
+i>X^nPi%d`Gjp&TSilhc$SRJ9'95hdg[2]^4.STcIbHH+Gp"Bl<(%':&Z6i4(:g/S<aB'>s*?pDZgb[V
+D]%kA5I2PdOGHgI.>1*ZMMP3YeZ2aV0?`4_F(b""%aDj4I.2\MdMtI:PS?G.:.:ri&ofBF:.;X0h7>dX
+@q0$<h[[oJeXa#&I.PV^2cO-%]URI`ZS6CM.4Kp<S;Ki\,iVckS;QS@c,.H1F6B-Am^`=84]@2)@%EHI
+8AqK`'LF!=7Du1(hX4PAg9k_X/$HY[g:_k-55t?;1M=uANlJ]!Ko*-28KfA=<(%':&Z6i4<)df1/6lGj
+Eom)dj43=N03l#+qtBEG5J\mES97F2+VCUr;@&+='bR@<KA"o0punK6lDNQt=dV27PS?G.:.:ri&ofBF
+:.7l5$PR0DP!V^XWJD6T,#1SFWMjNC@H+@e;@&+='p1gR`2SPP'fF5e9Up5t$6^"Z,#S]'3#*%'U8F?4
+M6frK+XH&H6jb(LV]t,"e24KcERlQfKLD>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_<e$NHcD0>=(]g)Pmci<mWs',J2g962GX5;IBr:8$F^CXT5dn`3aI;8/6Ru***a=2>9='g16q!mB7[
+ls?,1Z]<mg96<u\T?rZ^]*Q6*]nPHWi4D2PSH7H(Ibr!pY9hN4S$S#?NmQRmFntsVb^BLUGHQ.]_C90d
+sa`mdRu7,e:1j&_%2!&,%`%.DV[?rlI>Lmf*B(SPadNrY/i&9\oQ(mr[+gT]mB/(]h^ifUms>*s8;K;I
+SmJrV+[/t;Z6R_IpX]%ieoJ$h<q&!5$HP+ZsOVd<K9W3\7?B;!6SG1M.^C2jmLs:J,A!Xb2MRFace?n0
+WG*On,G!`rT6&R.IV%2Dr1E[GJF%9P>=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/Q<bPTmi:H^Ql`AlskT>jmlPWR^1hgSf/!oJ7_NW*#%?
+ZPtbl`\&e`E,L?gqJ46\%hn/(LMR;s2r@$7]GV75QCNn?PP/8n(t`"rpY\-qq9<<;91<4V!d*Q>7Nj3q
+q^_<rBb22M\e%4@o<98^\ZKa8m6l9F>2.p?[r%,@upTJ(V_Bu0K@$2j_"P4<TKB+h0\V(=8i9YH0:a70
+:k.Cil)^@SCU>B%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^g9<l#\))G3$niDL>d,\<J:IUnJ,b:RpkNlL]!:Y[rpAU'+$X*2iQm#Qo<_jCXmu;Gs
+1Z)=n),"QD)/(Gr!n.:o:'[4.7^36r6PD5iiAeZL8Gfcg`4,>=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<O$$>>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@<e`VY0fhm$aHhQ-*+
++<e/^\m1HdsbT0`pP:q.6!G/WlQW56`s\)Er>_&C-1F/$PtpSJr./9k*mU)[e^7<&S>G]XMf2Fe>_^uo
+]233hRn,Tg96>+\T?rZ$ST/3m+ATBIAHo8Va!oQ7F<G,GIKWpAg0SW3TY/(`l?"g++7Zk!\alIh7NFGV
+k8G.`>@!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,?hoG8<Lr^-aDb,OP#TBcA@nb$5A=<B>OWe;(N#3ajLkLEb@^8BI&2TF\6>2(lYTuV)L8867
+7]aPH1l$HiU^X<CDQtT/kC0hbm<)cBkNpn_^9:i2m@<?lrGd:3SSQi[kP8B7g"FD4!@>+\[<\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\<I.0H#]]^/o>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<eg&M44^3`5jE>`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(TrP<Sip<$%!S[[h5(]DjbH:>8>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$rR<?*@X1GdHZVuq?]QnSWmeShXfUs^T*')I!f9`utA`<8S^NT7Mo2?YH:RX*h
+]Xu_HfGK+Yl0-Ni1M=uAGNghCGRioCnbUP%4/]-*^t,Vl'6g9(S:9_SY3Pm9PKn-Z]=SeKaa5mSG'.qm
+r8h`?jj,.!(LBhUY2>hBT)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"%:3<RaB$R`Z!*Ss@+hhsj#5`q
+rT:7@hg^Y[Pkeg3aK9fs!p+jZDWj#l2;[eTQp8$m]j5j$It]^sd;R<F]\gT9T0HB"e2fEC1jj\[Y-+qO
+[S_532eVbEcKC8enA>cp5Q;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<ughGuJ0<CO:DY$SeFj4De[;l<<WZ=:e1nE797U4*Un3](d3]Le;6
+l&k;`YE/^o;#4G#>/M46B'As-M\kj:\$'>W-)[.oHM"/=l6#<U$H%@0W2H"GHa[g[4F-LL"Uh()B-AuW
+D9Al-nA,@gY58"kN>jg2a``Q6]oY:[D1'=05[BEMm?LGdF\@*SgBdc?XE]B`B3k'=:/b-aN0Q$Qp^O61
+m>"?#/3.n?f\s`'<QCUiF2Hi(`r"SuB9/YMp"*R>m-O*&kg74H61cOU.<E2fPsMP>g9jTR:u>IO;r?S`
+927:DB2PI>7uc_uQBi:<<l`>YkB`kWL'EnX9f<q27AR#b<ho*.FL>;4QF:hqb*()B5-\sP`Qk&<X%2He
+NkQ<HZpu3d[,ff8hjiBsX`Sq1lfBLIoV+q<_@3UEp[9cRqGiA%,n,CAPq,q1LD<:MlZHTC5f)m@Yp9+#
+b.2&tKi6CdW1%C)e&!u@mG5B5H@Q2c/P:F+@f<1*Wg8aV9/l,bDV_mhGkOu^[Z.JXkk-*f&1jnJr*UY-
+IWK,i\"Xtt4Y5Lc%;KfTSE/t.ERh'hKil+KUfBc[r4bhO^#<CROlq&@E)q2W%'dsWr-kF&-hR0-CZ0'[
+XJrff,=g7%b,X`g^2E!LnD_bXlGU,f("7uBhSW4%7XSL%c9W*,2eU)Kr8>DDj6,Lt*gWFk]eLEpK4gg)
+2XIA>^L<j-*<o<MgC#[I&1s^=>W6)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#<LFq#0Q_IO#jO_n_<WG9C!?[5'6#88/mG8F&i\IC<qjAp@\)m%h,A(;8;(;0[)]s
+BLpZ%'0lLKfrs\>]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-<!Cu9eVF^>FnOP#F4!<DRO_9KWEP;dc(Ui)
+'.6O;nA#,NoijQ'CcmpN?^g776c1b#kn%%-.%J3_Uo^TqH$t+S?4+&moB4GT[uj<IHht[h`Vo(11M4kX
+&eYF*$1s^n>3K&bWfELb]4(<nc&7T$j8H!3n=-*?YZ\;E`=G@8qUIf0_Vs*9>%!`c[:I%ke''-ofs524
+UcY$6RP\pI`&<RPnJ+St<SaKHUZlN[/6NVa<]BR8knDg;r)cU:CH[+&C&6(Web:$1i*5as#B:YnZl<A_
+Ei%3<.u3Q\mW/=$c]i-U_n?C&SH$TCE(kY:/7hg[VQ3ERHUSV&DVi%:TqNY.1%^sAg?EfJdm1Qcj$q'(
+coA2gB:Y3`5E(&47_e!=Y$JZV_12)[b+*1)ep2fVf;$ZoV4c_9dI0'HdF"X$ebK$iq:iW3qmL@rqYU.[
+I3edRN.-m4qTToV,")!l1\sOpkVm-crXf0-VXjIEqXp$HMZDZVd'"8^KcU,<TjQI"Y8@%h*W:;X?eEc0
+(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLr3DrV#mU
+lpjNK>$AR?!TQpjC[9a_NSTKp;XA&oKc0i8TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]u
+dB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X
+?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosg
+HD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h
+*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=
+qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"
+Y8@%h*W:;X?eEc0(]&hk7K)rPo^#(;>$>+B<rq>VCMW7k$B<l""&eJur.Q$eHD+]udB=A_KcU,<TjQI"
+Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/E
+f[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<
+TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+
+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_
+KcU,<TjQGPeF,a84k/3>HLTk/0jC1e6GKb*W8G1RW9)L+:QO2_<AXK*h*(L:ZK2->Q]6H&QV"q(4[KD,
+s*hAa(<AKNrPG_3IbWT1R[]d:kr8./QCFAc\JY:^mN157rpMN]/aJdLoR=o-"S<'`qsTT?Q8LL[hXB9-
+%h4HR#LfEQpmrf:qLr4p^].;a)HAZ'C4t,64'KBFh[7iu5V3[E^T\[3cYnc.F#fsL]!],0]'1#S\UON-
+Soa2o@&(J$(7dt)[JaI^'5aPB?e@AfIT"5+3^SkPSL]LTjit&4fAlc7S:4m*Y!2lU??E#^NYNXhLGS8)
+iO%G,VI.u8X+#uGMuIp./`T"!MGC>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/Jp<A'nm^$Oq\B:.)M(]&h+rHb/Ef[6l=qLosgHD+]udB=A_
+KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udBAmdB*89Pkgp2H*d6m=?J+hQ@k[Di
+QA6K-OVbGgm+@6/YiT<6?K?el%m0J/05PpdYnq[obJT@Ra9u2nG3hY(f^c7V0CluGLE1`((UcHmfSdDH
+j^H29jFo/r48'jOCjFZ5Zkpsj0[>X5MZWqZzzz!,C<NbEm_UmC/7&%1WkspYC$RY7DtSP'bU'd*SU^Q[
+\l&7pU(:X/^C2g9mfTVNWB'QFqG?qXon4p/oV@KF+R;??GPdYpZ/k1Aj/MU.(!Srq^E6eZ2`a]tN.(g:
+Sa/rTpl:F;Om"?-0dY]=V:Vm+f/CO$EX=jd0=X>?`$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`j<Tio];EHlK[Q)Iia)uL>Y#IGf
++%YW``fSY?SK=jE+pKH)+!t79?WsdFDaINl<(rJ,XihU<Fplr'15Zo68DK++9sFB3imO^?^XGXS?(ck2
+te8)`C)cqtp9`!1f-qXBd\6q>PGgo3[`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%<!E(FIEg,mcj]:5VS/OGK)EihRt]?'mV;PU$Bg3%6$X5pB']Lm+RgFCF11;j%\;ihVAU"Go)e
+,#4#9dtNXb:.[d6Lm'$G&1$82Lc?j+9c#/%3[G(u,#4"d6BI"e,3#CH2dMe1l]`XtM)Zq.#sfb1MMRIj
+giQuaXAb8n,iYVU,Qrr`.3_NEEmo`!ZVF(YOGK)EOq8nuUkguZb"W[!c8m6X5pB']84&DoP9T"^-oYG!
+39ueS"Go)e+pgOZ:5XjkU>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_-<Xf[dl'
+ril!LH:HGDX-qaX,8k!M0kSPk_T!F&nkDuH[n/I;VUl7^.rG1Y47B1P*e2E==a^/#?s$/Y(ek7qa^?kF
+9Lj`0SDfqr(&%V/2oZ\EH8E.:0ufT.Z^WT3n`s>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<n,Bo`'OtOC,/%"'2hCRc4<9u\[V`6THM2r@=pmcA^G/JHuM0"Y<
+Oq=EHB!YW;'U).;-)ecq<>>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%&#2:8UASqlNL(mLUkguZb"W[!c8m6X6(1_QX'6:8X_+j'
+[QV*t%Q$VtC3!1bmkAW3>W076O1*f'qti028-^%YH:<^IPG6iS@H+@eOuVtSeSV9<Un&a#2_j[cgKdm*
+]5@9BGNgI%ZP:r:PFIRp)i\II@G?o7eSOt<8,LlWFBX1&Ed\"3TdaoZB0kN"8J,r?Paa:!S97F:`ttP^
+PC*V+X!6*uBS1AQk;r(9F$0'K_jD6a'%WWJL^WYia,m%7Jt1ilSa1#q7Sthq`%_COI?Pl7UaS0-QoODr
+,`X!<D>@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;<GECJRQ@Wpt=N2-t=c
+q$H4^OGK)EihRt]?'mV;PU$`!88\0.3#6XNo3G7C2;d4Lj:puaZe'HQWQ)ip,.r0)6Mn#^VaQW&P9T"^
+-oYG!39ueSp)Bh(bhp^4X30m?(N.FbFKS!\QZUf+GKJ%U7bUL8=4MQEMc='@?T^L:Ft?2&8J,r?Paa:!
+S97F:]+7fmJ'6a4]2u^Mpip.I\EXqb8%6l+d<NRI^HDdqEg33F,Y5jF&TL^5Rk@C)F\3:Y#u]N-bMV8s
+4E#8(npAkJa83j";:K356nC>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<mrVJ+<Rjg]Rr3XF!Ui^reQeBJe?02(hd
+SWb]t`(:P\;O!Y'9Up5t'!$U`p>;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<e
+^PB81nA<GljQ:<gh;#JX_,5VVopsY--hl6-r\aX=Pk!ug850jsHcq2O>`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/%-<d!$7/6]8J+fQ
+]^Xn6g/g4ZNpZ%`/tB\H>O[lX4\=]PUaD('*R70H.AAeR7%=r'<QuTnh\Bh?7jiE;SE8X[Vd/U\,3L)Z
+A\&R9Vhfp1&ogf;PV1`T>rYWV[,<B7H]lH482c9^OEl^3KcgsT77?CWmF-c1;cX9tEj[9O,(`e/6??A#
+,?.'gaIaRe-UgGJG2_+ML`+=^$!fM4-+;_\=*p)Y-qF_aREfZ3N^lAa,%lKqS.lPmgP*9)L1?tdkFgTG
+4QU<R&nM6g#'$6W8eK_h,iYVU-sa?2\o>DVDo#ATp*<LEODGKkVSj12+[I;IMMT]7gSYGRAJd"Db13(/
+$4W.-iiEi_,'"o!`(:P<\o@OgiU./S"])\_P=3eaZ9b";U/OHD"Go)e,"`YhT.\oqiO.Sm=*p)Y4\.Je
+F&71?aFkZJ-UgGf_X5g+].qeN4QU<R'#P!<%H;gG,=V6"OGK)EU3,Afl,%9VFpjm^[3pgI"N^jfNs:dl
+qB\\N8J+h2E;Ob9p5G[K:V#2C`5$t8K=D^"9+cRa,iYVU-sa?2\o8V.dt4K&m7Cc36*`f3^#>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,,02<p?BF+e%M@LmGME^i$h`SOr-QJC_4'm$n7nQJjc+C
+&Z#lE:D[?^R.#jfc./C2`Y0U:`9eF<MJ/7t&LU"8l,#.sIItpP.p)#N7i,Kt(;--;4\:S^82c9.19)&9
+*^LuFMMT]7""mljFS+eEf3a$@rnDPRk*i[8EUN)$g69G\K4KTo\Ik]p2@'q8;FHa%]W-l+0-3?,]^!&=
+E"^FiK#bEej(_@&is_([ij?^n,*ETbihVAY?$LW6)]MR9p!hs<L[VqN?<oP9*RRTkg69IBK4KToPqf1m
+KER,k.3_A#?Ja55=-"@`pAFO.=2/FsL!P2-)una>FR4.?^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<B']M'\6f,YBujP2F\3jtL3[SH2^_$>+\9OC&Q*0,!*si@Kc_1K
+06fM..>\l\lId:t6Ae<#OGK)EU<M6j]'sSF>HiBn("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<QK4"uoJWs/7;3[GAJ6\Gp..4HGI5pB']Lb`IaT`!9>"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
+U<M71aRAb9_1f-h8]99.N@#U_j^)>kLa3+[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]98CVm<Jd;FD.L7%=r'A]F.'4/]Qhfng:+PXLH>3@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%<cATUR;1.3]'a,#4#O11PN9e^LQ&o3^)5YVfSKM+[;`q'f@^Quqhn@/(uR5pB']Lb`Ia@/89P"nOV&
+ktit^Jp6#M][D5XTC:\+_GRZ[)R87b,Qrr`.3conh19-4Y85>'XOH-2Wm]DSr4:qe[@1').4X,YnO`V*
+@JD)S5pB']Lb`IaT\8S[JsjeC*Kmj<r7U.Ra&4`"c!OqO[M0ZE8OAMpP9QaFfH"(9HF7,rYYGFu>/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`(:P<VPBD=o?9"G%NRTLbPh1mJ*kst]'oXhoP24Da>tnSI1bmfWCtW@/c5@o
+$TEtQT-PRN`(:P<VPE7dqWb+]"[7;,m+0b=huE]Mj2[3Fo[#sJJ)SuC4b)3S$YDiYBqaZ(iEfQs'TLkQ
+gVbt_f^MHFJjc+C&S4",h@G))K*NHB@/IE_r8Sm"CAc1$T?R6A[ncB"q^,K#/$_oC<)\Hs=m],*/[M'>
+]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?Gqc<adY$YB^/a`;I&
+,L<.*,(*<*VG;*:fQ.qRnYG;d3&3Ti7h*-,k*o?eXr?is(QTOro%7H5c20$kp]'%Eo6iiq<,ME]C&Xb)
+q1%;s`jp#:o7%:p0pt_,Kc0pdZ"NOW#=n.UI/j3;]e8^*'NMp9nWApDalNBRg5"-es8L>WK3UR%<DR8V
+j,SQN\%c6t>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/<fl41LIg\R0[@>`M@nF@;W5[7qGac8m8on4C<I/\(YP(>_Qmk2EgW
+HE^Y78AoU<S.rA*(FIEg--G^KNV]-=\'&En.<<2q@(@8>M!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`(<h\FCF11;ka\m@i!i0hNglnl<c`V%d,I6+Ak2K
+&TL^u2I2\0l]`g8k;]U;*a/)FQD/X>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-&K<f['3tQ`4bKM"]+N7eThId;B=&C/kdId
+,N>M42&@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,TaT<Yl]`Z\#a-.)
+qj#8td<e$\chHnSkj^4ARnMp2(FBaC&TL^UXGr4!ldr>t]N'i3@Cfm<P/Pd7dYPN%D%$?kq3m)*IAKnu
+Z_.\iM!9^T.9]mG0"dFjAF_:!h2)dmqL/\Y_.l7sJ*V`DX(1T-_(Ul7:T*)nhF26,e+6gcP,Ds&pX<?C
+W-SmD*bY0=hB-1>alNC!rL`P(NiT?bJ(jW!*5;H#d.s"U^%um/%u5b)NeeWqMAEYVNK&pu1KNq\$i`Xl
+Y=Vu-TkKO('=T6f2oo1`^F:XXLXem*pWU?*M3s<ZPG/t.c^p]`Z!C^DTD_\oR*Jg3,`sEE)KA(Q>B!>M
+nuEH>?7ANr-a:JP_B`SO,#4#1>e5%Aobj6gS$RUE$e$!ZPU'-2a_*:r1jm(+bZaMcc4lOK.8,-V-.@Q>
+`,nD;'Wk%FVDHdP8Ycl[cX9pWq7Gp=Uo<?H=n6&UQ7biTVBb)rBP"b/SZtp]U;\8.-=3YK(-K!KMG!mC
+??Nbn\hJu.NeimnQ0H/-`]>E&eq;JVmM/W^dMUcjq:lZ!rbboY<qug]_B`SO,#4$LV@+&MSWZB26!C_q
+%CL!\FK`>0774/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,)[Qq<?Cbu`S:D!b?"K[*dlTQ.4Vb9ht6'ZZQX7-/9Oj4
+@1p]A8WCSY;;62$iiH(Aa=Be3PiBE'nW$q[6Alp^I9J6ll>S1Q%f$G>;\pdC]UmV$P8(E!(FIEg-&G<J
+Zs"p80n-%&S>ghN!%ZOh.2q3hLBg_4=1H/AH2_SSO=j3NR6ITI'Iddd/khdX8eSZ>.6a0gDT@*ijb<GY
+#64'XW(D'4R%e%KL^.%;=8qG_Wo]l]P=Y+;C6Uu@g855kB:Y2R8O7*6a"$mLE!-o#gZg4+@N@)cs7V+)
+?nD1aq&SWh(FG9-T%(iH'X^XO-I+XuM,JuU;D4&shi`jn18,E4fAV\7B<aT`r,-X7s/.\\Qok;neV-Sj
+e0d+o,"6@tKo*-28R_rkg3n<hg[07Xj`cJnMX!E0/)t)A?6/J);p:^.]=&Z`/kdHVGZ(;T;UdU1QoODr
+,_d24i/<((^;QBlV+FOP)oT_FiT;^Om>Kbhlp4^&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
+39<X@%q[&25*mMRUkguZC7?hLS97F:Jj`kq.;I9<F/jne2jZ/_h4\=`NeilCihWMi\hJ-T.7jdXEW)]3
+W(+El*H&A!SC;t"ONS5U;FI#h[7qGac8m6X5pEJX'U@.'S(?P02hEj\:J8[gLc?h-:5[c&$PR0DP(Xqi
+f7/[H2r\/5@ndt-5A=ARZYB0UocGI%-Ukt52FX0"X6@F=NHUkUQ66d+98Z>dGGuU*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>kCGmJCGOD<GgV;$GgBDicB&'G39e'^b]Pj)4(t`/S;udPaB8Y(.3<[7rce)Xl@AG8/o!b3sKW
+T&)Fig%Rh.Nuf@4!eNi6UklN8UhJV33Hdb+->j7XPR[s3EQ+_eKtEQcG3$_/kKAA/q:kmQJTq,e7b*<Q
+#ne,>,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);FK<U;RGr-kYt>1)]K_4/mS2Hj,Y"*
+V_1Zu^t'r_&L?A3.'7bZ]c[0aBaGDl;FK<%;Uis>dHg,aO$<F(_1Mt'giCFUG>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&(FH<KF\4h#M)Zr!>7Ru6#6]lC.3bqM']ngpnc&FS+$"B&Tna\hq47[]RS5'S_Uk><EMDoW
+^^pbaEDk;;alNC!rL`O=iDa0frj;,!R"-:k-Ukt=R%ikK?u>;Kps[0uWk5O@EHT9]5CN:^GE3e.HJa/F
+Xfnq[GMsmV)JV*c.kDr:`(<fb](p#u*XNI!;FFc0.I-KWiU#7(ZTGV%SbJCN?9*E.nB$kAl\k7,?'l=`
+>GpOROGK*pcIBLSGQ@J2$PIGW6sNkDlM/*W9:%9E5%AYTF8GRYVb^A'pPC8hB^u\]Ko%<qSWYQj,#4"V
+QGY5ig)rr)8J,s=UaVk-Nh/7PVQ.,3+0/@:(FH;LF\:d!M)ZsLhB;-g+;#ti`"eE/HsS[Oq=<XbLC`9f
+kFJf(h0\V(2t*s6_Sd1o:<B+N&LU#\(uE:b#nh5U6sT^?b3I$@kK]XQo?EmLJ,Jh^Imk/#0)p&d&HmqM
+.I'c2'2ai6&LU#cjd`&mmYd+PKo*uuSPg"b,#4$l24-EU8An(3ES73;TDP@8FCF17?)NoUOGK*ph@3s[
+&gZJ4Lq#QG1mfA3hUI6N+jY:'E<M+X.E]I,DO_@cihVC?geotZbPMd\0QFY*;[OgA`(<fiMV]Hk,iYVU
+\?&S-oMp;\?'r.'eg]\n,iYUJ.lDrXjuWj#7%=r'QVWKnFC4Yk?'r-LegX$$,iYUJ2hg*b-V20I&TKh,
+W6i_46b4sk3NQ-`5`gecU-'07B:U.W-UkuHStb5IhTO?$M<fS\1(8b&S.oP8XITM\,#4#OlSh7"]e=\t
+/khe94\5nlEDi%3;RRDFM)Zq.Y-/P)p!_(i?'mTQH]epcihRuP7D,=[+;#ti`:]:f;LW;IKJ#rgX$'#[
+&ogfKKS;Jt^!Bl67%=r'f6rOrE*r5g?'mTQH]epcihWLq3H#H:&ogf;>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\?j91<!.3^B9MVK"R6Ph)peXe#hM)Zq&YV%kj"c52f+tfH2pI3]_Ko'lY-OE'_:5T7ZRgigm
+,#4#OMknFBd7riA`4=TcKHLjS7:X]\#-f9_-UktUS0I\r2l$fpkA!d*Jjc+CNCU#k\g0hb8J,sCV9")4
+c0<*`\_<7#+;#tiUXJ"S/dW/1;FK<G.<8efZ?0O7>jY*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<qYeQhKs0XD69hU2;bSF'Ba++
+&qWNA'[ak9*L=h.I,C!Iijg$@='^a>(Z!m/k2r@)LRrnaamH:%5(3:c_1Prr>P%iO/khe94\5nlEDf1l
+oc(jHpO@(t,fo)'T^""+?&RJS]p+M9NA&$@@f*W70<X$6/ki<Nm7BX+Lm'$KdU7^[iE5VP>PkZ!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<hmre`=%S97F:Jjc+COb=hh-UksFdBXg@
+nA->>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<A4fzzzzzzzzzzz^9Y;[H/ZVo.p&nGrBMd``l:K5U5qP<\F0[>&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^<FL=[=n[<8fi[lI>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<N1s#tVmfK8Q
+7e)Nfi7+ET^)D\dOqoUFm<6o@J87\E/.!k0XVlJp`HgeXl7uK*Be'P`A)7bsCET9N2!ag9<V<tHc-b9)
+rR58K%HrQ?lg9k_"^%U1AbEo]QZFRMKV<H#KTBu)F;5gV!XWRQ3LJL&&7!06mKj!""ldr1qc_(4/VnO5
+Y`V[bQISsI0[X\105Q%44f'G15m-3fcX2g5@HiEj(X'a)Z(aCuabtF1]41ViXI_.XlJ,]&3B>=5DDggs
+A`ujd9+Y`sM^A)0&X*<HL\*jPgGtstqrOqZJDgd%>(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<n,2OYch`)YP[m:B0ljUPqCZUFNjCE2leQdB4aQbaIJ;R,^:rIuA"bU.Y@%=tGJF%0hu7fq>@!U
+2]_:&peW\\tJ,$qKo)$]on*IP;&Xmt>iJ3I74'%?6)]K_404$b\il'R)RQ=,`\hJ+]?.d5h`:X!dh690
+XYIsEu<F/Fl4D@g$Jm\NJQf%f9\#[^"S%&=T(V_qOkYUr1J,]&3I6TQGn\uMd9],_kp5>eBQ;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+CDH<uS(H-<3Fna^95PX/hgV<Ccn#YH
+kR57iZB&DVhQF496\M<-`(LK:Ppi,nalK`j_1eMf/*I#`AGj<aR5P7dR/8P)LPs>3*&)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
+L8<K%H,u6c(oV"X-VRIeA4F,Aq:U$()kK`I?CTEf;&>9K<Z77t,NujiuM!hk?Y)=)LpAKdqchdM7@uic
+rnDM.MpQ>I0hu;4$K;'W<hVDr-Agof>AE7XSk2th'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<QAC>(()%@<+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'!KH73M5<J:8(9r>M>9"@Bj-kSJfsY6"Jg+S9H"ILuka)PCF?
+6GE`g(P+EmDm<F2LYAU)o>: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#<OtAb6!1urAI7/@FpQASs>LKlS0UVkdMG4(pBT>kg=<N0lpqkh@fc2
+"SrQUridnYq^9&K$U+=qV4K]RO+*cm^O#S!R=4]84o//nB[5t>?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<qHi<Wh5OlZ*<M-kbRVR*7g_k9X
+7ueRO9mZa4nPCm;+/HXTVP^5:H?qo:X!s4_D_gM"55G`#\cha(r:A7Lo?KGc^2+/0+<ebT'T)*6Z#aM!
+rJ"T`!rMZ<D`n^sBD,@!]4k4:@42(%)n(prY)bgHpsW#']k#6JX096`gU:r`s893MW[s.G_8/!Eem*@J
+<O1\g?+YR_mC2:.q5pD^O7(l<HeG]9Z9Wb`]:@qOcgKW$GPUgko(^4a)\3uP=LI&<?@2)=qsX8$3HjcA
+Q7Z=8[FAB,fc^1gqJNk*(0PJ<d\YdUh7rmYMuT5a^A4XQ&UF)l-_J%jiIW$Hb:gWs`d-U(2p+UOomZc%
+Vm<P^q<+B"U0j6CAU.g,EjEi2Cl'G0S,>: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+</m.Z(O$Cckm_A\G)l#6c6oeYs;(qdJM2@(&1%um?eD/iLqb\i/BB=qs
+rqq$JG5qR?d4\,;[H>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;<FL355Js79>d=-b0fU48"Oj]\JOMqlAAn6ns&6I)**f#*BB3M
+:W]lF<kn5%ns\u65c34Te96VmppEVEPfZ;G]#5V5h!r/^&',bOZF_8[h#b"\Q/fT+-FgUt*;AQDFE1g,
+Wo%pCR=1A!SiDk`gJ:%6H%WE7p<UL@^jYt5kP"^"%G>l84%N1LchH@M%n853IgHn`/!F)G'?sp=S<2=e
+p[$S>ns!Z5pLWJ<BBAROppEVEFR+f2&rrL:ar.88EM"LVf,N2BN)lC4F)&78n%JJ,QSN+"DsHK/^-$fC
+m*@0#g9k_>%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,!MWDu<H8'g8d3,:@Z?8l;:Woe%p!co\IqE:^?=`N]^!X1S,lNWS^526%BP>JX
+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`<c<,F>HBp0h.e@_,
+p:Ke-CMR^,hf&CEQXs^pP'6JQ7'c=Qjicc%Z<<'AQM$a43O:@LrQ9"2F(TWAV)<&>6rQoH%11)R8W[.m
+`JPE0>Vr<bq*dm-rNVKtTH%D#VrG!O=eP,b1rl]R+/F8e\^K;Cntu>gZfFP#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<WGDt`>;55I]g"Re:5F(QNm+uo_uS)HB6O+*32U&*%-SCpEI8<+hkAq&%lUL,:%
+ZJV1rR=4^cjO!OgT/j52oDPQbqQUFWN42ib)4.2MoUDo!DC"iPVZ$@6$e('D<gnO6fpE5\@%)Oc(iP7[
+l*h9(5<ncn9,gL=IPX*!p/UAOVk<</V6Sr_"o8\,fpA<?eSjR\]9+TAB'TM#$+YM$l12SdP#l=4CQg%\
+04);?rVDY=&V#A3=+?3T4D)f[^,:1C6UO4F[VV)(gqWlK)._F^B;<Yu3\_GZNSf&r+T:FMhGQQ69u?R5
+79<QWr78[!J35?cZ[2:Zg"!9Np"*B.a,ddV\)sio.?R5BngP/8aKj"8q@f@n$er[;^%f9\!WD.1hGQQ6
+9u?R5j`fgjjf[@u\-pp0%UZ5aoiF_Xel?/E]]s>2@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;<t6dF^B2KI^-2-is/_ed;Sr:D.C"\W<3#V.M)J19#1+07n6)2nXc>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.r9LpYnpPOb<q?(a9l,mHL+($
+f^dC!0=&H\LE:f)'t-6sfSci8je9_$A;)TG4n^'MCjG6L(Kk1i6]\q%$<nQtlpXt-o$Cl"ZGMo_?:0':
+U]4so!6s*-%gN(7zzz!!#8VSN1cRpYUJ(qUEa5ba2[cg#DNENN2D8F)uE#:7_7?LEGT;af$u`gpqLnmH
+qrT+0YP-Y$JYW^]!jA%gf46FY=XS*q&7UE;poWZa980cT\ZR?<3&HO3<9Cc!.U5%mKbRSt]T1X1B+,NK
+)%)0>7#+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&<B1lM[VZu@63gRPo==bd]3g'dFn[-ZU)mlmd?,YDj@;36Mh73M?[5%C[Mr^7Q#
+6?qqtJunkFEhB,G/<9ieoJHHL%\RkL?J0[c9c=Q0h#4I.E-lpu-uMqTHr'ppoRZZPZu6Q#6?bmd@*A'4
+f927bM*)0ejh9%M'*IbH+pWNkHjNZ7Z+F^]4<#H2I!<Ac=6)jDipo,0;=L7G5%^;bp1bo],T9Zn*>t@h
+ti_ZDueH7l^N;4Y$1MR68Y;I=6P7iIC:N\]5b<XL#8)_M&APK?PV;KjN<C89UTup3(ZZ[<:fdnaUV"i;
+m&2\[f7,B?j1F9j%u0z!+Z$4XkDWf~>
+
+%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`<OE[khK*@@[.8dL9DMC9VnE[i92U=S+.E[mf:ON=DW&jde?,'#/JboJ./,"`,tS;J^<,]Y@YV
+kVbt@<*ilKTO9SL_<#uV]p.2C<[bGV]n_=M4BGO+XD(X,,_0dggP4QU4eWLWJD0R,,ZW*.Vd3K-Dr%[#
+`5(/-m9jV.O(dbf%a\S.\_TXaJjt.66`<OE[khK*@@[.8dL9DMC9VnE[i92U=S+.E[mf:ON=DW&jde?,
+'#/JboJ,iS'AD?.51rm,,ZVW#`5(/aJl*q>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`<OE[khK(+/47;F3q8<
+(%$9O])iP't%W]PB0JJ+XD(XU4eWLWJI(%>-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.!&/$u<O],-:_hSa]DVVaGn(rr2\[JBgCj*n,c":l)%
+R0=II,"MOEr@:c(IOP-DcbQ4\nU!2F(a=Q(*@k<a'u-!Xp>4F&ig\hRrUsV&3`9AhV[5*IJ``[aiWg"*
+.B$-_e[(%^K,`;F,<,RHTb^_p)5[Ko)F'RR?4A?,'#.Ok32#BDf>+4h7K_Cn<kk%5CHT%ZD$=tc..,VY
+'An!0@1o73+"R?6:g0:6r$/Q4F-Ln>I]_84mMXY[P4cTfcW@<heWjEQTjmj@P6cc#"f/XS;PCeBO*8u^
+?k"Lci:)pC:5h>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/<pm*_-nV&ig\h;c.>=n<?pi#Nq<a,p?F0'Ba@'#pGF0&
+NLT6Ad_C5m9Xa,.8^qF76F]o8!FPt3>IZT;/<W0e?mj6c7('^6aDLqaQ&'h66`<OE[khK(7pP0]>-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%(&/$u<O],+m[da'.roXGFVj,hQ86u8p'(PaA*)T$nj
+A\NB7BP7[A$W5/Rd6JA@e1agJTuQ)N[Q%rE[i92U=O/>ES0]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<<AnFM&8+0X,IJ;PV3&m<!9`)<qORj+uE@N/-77B!?'6j*4
+d8h=jh*=NLWG'OW4p%Y<?@M_3<Zc7bKLD>fLf70ubeh;$Vdpqri`*9gPn>uM\18GJ]CIE/\qEJAMMP2.
+dEKJG=f5dR,iVe1E[uDZ\2",pqFFgU8AqK`3#*1Q/52RV8es`;:.6\<lk@2u3s&IiM)Z4^VkToRKo*-2
+a[/P:WMkYbT*Xkm(IC&(,iVckS;N29(+.<f,mtk.VkUcNGZuY_RS:Q^]Aqr3XZjre+sc/I7%B-lAel`=
+3[G.W&Z;?l?@M_3-81/J>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<N(!XtH$k<c9JrA)&+]0X&&Z;@tR>$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'(<Rg*3_5?.bF]1i<MMd3g;:3V)`*'!K
++XH'YVP6O5MB3`W9J^^iO08A&9h;XQMMP2.dEKJG=f5dR,iZD.U*_ONG(H`;dfgoBE[i8.77B!?'5*l!
+Koh,Vk_^9a.GA!Yf3_%'7%7!bE@NH,=dV27PUqJU>j#/[o#i\5CN017glQ1k.O%VV75G;bB:Y3>OGN5@
+MIpI4"pYCH.(ZO&@S!jF'bR@<U7$^lZVJV/8AoXU7?+I4jlt^SlF;ph6Rj9T77B!?'5*l!Koh,VTSaXN
+*Lo$rqU"H%OD%iJ,#1S7()@[_Vl-_(]=.'toCMQfc`:-N0"\#aQoODr/;-DrVc;10)HgA\h'P0/,heZD
+Lf71$OX!BmqYE,*Z"MCS\[f7?H[c"lhqtq`@qEuA*dXd\6u3Vcc8m6Z+VE?*'fC:ufScKY9)FB3qbn)H
+.]Sl+S4l<9M%_8Zjmo6@QNdWu7Rfl4meP@2:Sg4HIJ2@$kN0mR+*c^pSND$R\S&6&ERlRA#`6&LYK52/
+SYF@rU*HUm3#,H[fWeq2Gdr(ke>ZA\L7Oa#=h<eCcqp]@Wnr-^Kbr1/=0>eBFO+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(0r<EfPW4W)^f[tq>D;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]6<St]=3*md:Xq7\T0!5J**/3W`sAt]X_b*VQmD2
+7e^^7-I+Xu$R$,6g5>100%smB3g%l'E@N/-72;#MPA+\nG3i>A>eYIQikW>pFYl:3rqsjYaJDOr.Q8Gr
+m'4_1L!uX"Za7Qe_tW&<s''ETe``UL_SfTTA0E5X>#g^:7<]WI,N;ZjS;O<V+$SK3f2oMMo?bk2gtZ.G
+qhYt6d[$l3WQNI)2f9TpE+%_GNRd>c^\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(+.<f,mtk.\"4<lH<p0kX*N'3S;rD(
+&TK9MRTZ";l]`rR7,5@R<;1^d?5METS;rD(&TK9MRTZ";l]`rR7,7W<<;1^d?5MElS;rD(&TK9MRTZ";
+l]`rR6sS+SH!PNQE&o1<7Du1(BP(]4XAcD9&oiV+jE8k`_s)t:'bR@<U7$^lZVJV/8Al6E;;KhmO,o><
+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&_P<m$)1MhWam"U@'bR@<U7$^lZVJV/8Ao)@B^$Ih]e6m?3#rU/,2uR$
+1p\lUegX`-M7?e*q;A%ikB'+^MMP2.dEKJG=f5dR,i[euH%Rf[SW:cW7Du1(BP(]4XAcD9&okaF]Kqnl
+V^(2#7%B-lAel`=3[G.W&JqE[I!j]gKW/oO,#1SFWMjKB@H+@eZ4IXgCTbaDZDcl,7%B-j;c1rd_SfTT
+A0E5XelInl#d]?9&TK9E:.7T-$PR0DP(f!R>AZfOYE\,X_DiPJV]t,"<&Cp8ERlRA#`6&k<E/]b=^Doh
+D5SV!:_@[s.[jM-39ue[KLFV<.rqI;RW(;3:.<+#W+]dOiiH(a&/&t5XJkQPZALh(3f)5sE@N/-77B!?
+'5*l!Koh*p8O]eH`Nl"oLf4n*.OpliQoODr/;-Dro86d%M\U7n,#1SFWMjKB@H+@eZ4IXg9e&bfnBrgu
+NK9.(j3c7/S;rD(&TK9MRTZ";l]`rR6sQLEQ<l?#Q*P)?l%h;6&Z;@tR>$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>IF3F<ZS6CM.9Qq5\4T2(&AIVI
+q\BXXhB:%b3?8^0,2uR$Fl^W+^:q;jGO4"hB5VUid@QPcPM<N_X6_i#0Q?I8\L)N9(+.<f,mtk.QR[$t
+B]>PBLf4n*.]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<oB+:&%j"u_k:<aKP4j;%p"']p3j`tkc7a\Cb@>$l:#9$#gFQ#.egX`-
+M)FnX8`\uT7%B-j;c,;-23fN>7@1gTn*`J[i4u'Mc^["Zp)FthHMHHS-RL13I.GCdgHuK7eDtAm)ceCN
+\i:%h[!VQ8<Z_D!5Y.umf[s=&[qX54r8<2<[edMtCedL=WJJ&jY%S9NnEJE"e"2U_njmou4b*J/U=WI"
+SV<OCiip(jrgsoc#`7*3q]WhZPXV+k&TK9E:.;WV^,5IS?6fEs9/!,)T7-Fdg5U6Qjsm\kERlRA#`2PQ
+KCI;^7do6L^X^@n2fLf%:.<+#kk<%uauXh=jJ"5!TC'=4j\"5j3B:6SEU22XAml$XSW\\g=LgmC,-2uo
+&/&t4,#1QA]6<Sq]6A2dAs:P?)B9`urTN4@=0L2gicLGgQoODr/;1qK/cG%31^I0sV'<2V&ofBF:.5n]
+PET]o4a-0o\Qljnn`%@"5]nnIJ,Xg6IP5!<<4VhkZd5L3%im\f5<o29]C14?S)6n$?b_''d63tQM6ft!
+6:e2rkic]DcV@@M=U9jD'bR@<Wl'T&p\"2Z0'58Q1fXFLU_>+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'<naZ/"hN*Q86:f%oLc?+abq)PYF\3E9l</67iT?c'e;0"'6:f%oLc?+abq)PY
+F\3E9l:kt(?L<LR2k_G$&/&t4,#1Qq1CFk/SW\\gH;NE.&RP&OHs9$M+sc/I7%B-lAel`=3[G.WopWIE
+E:?%k6l&*5:.<+#W+]dOiiH(a&5l:A+!4k]LBoi4,-hamE@N/-77B!?'5*l!KogW$kic[lKCE^mObo0U
+;\="!.[jM-39ue[K[_5=V:))(3,Qc[k:.Y`+VCUri`&l6ZS6CM.9QqU(G,OOUs[T;N;'M"Z4IXg.O%VV
+75G;bB:Y3>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]<T,cs1j0f96\*pnRC5Jp"*PH?a"Da?BnEfF;-oQp[6k>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$+[,2uRDqr92<SNM1<
+d?`LeBiPQP&(c5<F59nr=."\uZVJV/8F<MHB.#S43.k7/&QoBr3#*%'U5E6gG4"+t[sGC.B.>Ja/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<Yp?EsL[Y.O#NL&Z6i4<)b]\0QGYm=U8`4Fhs?KQ;S;t#`1MU&Z;@t
+R>$L(:<>gnl_\I#PdQa5a?iG9WJEMK(!S:'S97F>6<YIRYEUn*3nilZb!SU.+XH&H7%B-lAel`=3[G.W
+Xb/7<;V"_q+q#P+V]t,"<&Cp8ERlRA#cVLnGGVC;<(%Kg,#1SFWMjKB@H+@eZ4KoRlFoQo$a,`%P`!OK
+8AqK`3#*1Q/52RV8es`C8/d0M]C3J:]Md<Y.O#6D&Z6i4<)b]\0QGYm=U:u4Fl+<(5!Iigj!oqDa?iG9
+WJEMK(!S:'S97F>6HZGIhB9IeMTA+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@<U7$^lZVJV/88D9#K9Jbp7pCCH/YF;n&/&t4
+,#1Qq1CFk/SW\\getr[:32m3!ObJmQ;\="!.[jM-39ue[KX=p*?Pf7;iO$;KLXn41+q#P+V]t,"<&Cp8
+ERlRA#\d<(^+Que4ufMJ'p433Lf4n*.OpliQoODr/;1BN94&Lk;]hOZKSmi;8AqK`3#*1Q/52RV8es`s
+M+4\,mb'N)0;9Yr&/&t4,#1S7()@[_Vl-_(]=.'toCMQfcRW)"/A%f_QoODr/;1C3\k`S*k4PGf;%Zq:
+M)Z4^VkU197um67okh3Kc^m:@F)uJJ]5clNGJ<[>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<B82#kB"(G;N>)SNJe@`'[\
+8u8<P8\938DY51"%>7\82E$jr[S$ka#9S>teVD]V<(%Kg,#1SFol<L;=npNK?er#!bG1+^HK3#)k-K$u
+JJshseGlj!#9ho@n8tnXA?DAZa?iG9WJEMK-.C>lX*p!"$ST/sC"&rQTqUV$n85Kc3EW1idGlB-qU?dW
+a]arNYD?G:7%B-j;pjunYEX2#jN0hpC9OL7ld2=ml+<!@rqK+Rgal-hF\3F_*)`Q0K3]g>[<fA-1(k4>
+.O%VV't\YVg:MQIok'uF<(`/m5G#Shqlrc<R>$L(:<B4h6X'>.E95g$Sdo(rhQ2P"KLD>fLf70ijQ'hi
+>)s[c<\B^l?G&6O0MidLc_!Ed1,<n7d\ZU/m-O)99q/S0hu:J?^&IloNUGIN9Up5t(-NLtVh;>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<OIEg1g=*mkF_aFI
+1Ed@.,2uR$1p\lUegXaDAOqee'O<2R>(()?W@KK.r<\<?'bR@<U7$^lZVJW:b<frcV.eK%.TDa77&pTM
+3#*%'U8"'0M6ft!U2*)1o%%2%MCr)a<((L.M)Z4^VkToRKo*-2aeB/0C@p(b08a'p>u2.#5Y/hmLc?+a
+bq)PYF\3Di*E&(MfScKU;b'nWq[aB[.O%VV75G;bB:Y2SkD%Js$ML4XFk4)<d8kogS;N,$;:3V)`*'!K
+d>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(:<B4N6E3-[S46fQ$gpjY:_?tu8AqK`3#*1Q/52RV8f"N<WVO`2n%/*^8Z(G>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(:<?rS+g$.k2$3LXV^&JY&ofBF:.7T-$PR0DP/U$[
+8sAVb0>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<AVgE+MIgRdfF@6m2,#1SFWFua$*')I!f=0_!4aZnHc-8-!
+fdogeFikV%`np7Un]GpeCRBKbk?h!c;rMgd(.'3?7Du0=[3d7Kds-6!`//NFI-K(UJUrB7Oc_!fRtZ^*
+:0D,iNgK459Y8i+GKhp^mI9H;?Q*oF+q#P+V]t,"Zs#P7lkgcbY229`l;X3qRp7f4O8e@(`ZZ\R33>@I
+nu.MM[79I2eJS3dAOI!Q\]on:o.S;XY^n?Y7%B-j;phW#\ofMt^(V.neLgCao4cD<j2W,;8.cA;!@#tQ
+S#5PpjQlZgXGVcBA0E5X<(*76<or,IF(t2.HhM:C`m'u3+$=ePd>t[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!@^\<)-&<j,0oc[/AdO3SZICT"<`g'U_YO.%gCHLuN/JXD%OGHgIE@P.Wl07H_
+^.-h!UkuEQa[/P:WJE*Y7%B-j;phUEY*=)"jN0hpC9OMBLfp*)l]`rR7%B.U6:f%oLc?-Oeui;jp?pa@
+]t*KB.[c!IZVJV/8AqK`/;-E]MMP3YC/%GuVXaB4Z")m9q;TAO!s8Y5U[_5o@H+@eZ4IXg.O#6D&Z6i4
+<3ujb%j16*le?C!d*U.pgg$&6c,.D%Vdpqri`&;]66]/n`2REe()@Z<>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;\<.<M)Z64Koh*h&TK9M
+R_LOr.O#6D&Z6hY&/&t4,#1Qqo+r`K7:[Pei`&;]66]/n`2T\KrkcUQLb+3IS;PBS8AqK`3#*1Q?^Nef
+ObJmQ;\<.<M)Z4^VkTp]$PTE^Z4IXg.O#6D&Z6i4<)b](@H(6S(.'3?7:[Pei`&=9MMgOL9Uq!]6:f%o
+Lb+3IS;N,$;:5;qM,P!h#`1MUObJmQ;\="!.[n++E@N.?+VCUrZ4IXg.O%VV7Iq<B:.8\:,iVck(.'3?
+7Du1(B`>leWJE*Y7%B.U6:f%oLc?+ac!Qht'p433Lf4mg#`1MU&Z;@tD#aP9J@<kj.2`*czzzzzzzzz!
+!&[[KnY6Fl03!8Y-n*kk[=#Ro;SH7=0Aq,5j/DnF*"Oup0+MXJkf;uQf!&JNSj^&GNk#d*t'*jo'b;_X
+&iMR+EXD<hgBM06Yd4C=d?Q5H,uQ.OX!BmqYE,*Z"MCSoFSC[7mr-CET>&_^@RnT)`LeI)B9_j/%!h37
+;(AeGOMD'Rp(4SFJR%#a_fe@<DH$d!s@`3SSRm1d?20CrU3hf?$ke/LJ5Xfb.I,V]4K0O(LMQpG,'iJX
+NJtK4QOd?4_@67aV-$sPfr/]QS,+i%3m)BXZe-obl7S<do(VYV3b(KZa7!7;X+2Xmn`8Ye]1+V8nXrA?
+8;np#3euNMJ$aAm,lQ^n%JI%3Z[nfpNm7%eU$M[Vc$(355T0mGO=t_b7?`)^s8K4>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<c>_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
+2<UPHnq7Q*d[#n=-I5*`l?"VHu/]RmI.a_?E9-C:9nPjnW94tBKcfNa;no%P1Qb<^:In!Q:E$FoYuk0X
+ZdL]bfuV9+*c^p+92,VQ()Aof&62be*.s^7ilrO3rGR6It#f2&n**[mG#*mldk\>FFJ]<o&\',GOOCsD
+4>O4Pqrd]f45ZjO?8BCoesKkK.L>RiIs,$$k3YKQ)qe8:H\0D07>:?Ni)7>mWZG#f@QA'GiOcB>(((+N
+0djsWH.C`g5R.jZ<Cu;?1^V3ldr><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_:1<Z`+uk-;rJX1Ve!4ZZ48bRG'nhgb\)D
+nYgEX,i1!Z#klnOIOd(<0;sk"Z^u*E*lsZjlM:c6UO%dJlOgVnmpRC[X&;i&S>J&/B+r,bEg;-B]m[3Q
+S2]`ob#SsDVVaAi4so')`Lg$,Q6>3YI45"kTk:Xh<H.BX0SZJ&%RV)Ycr**_?ERFJ1aYNQf!2^c.Ue'<
+Z_U<Xn(\sjeKr=6Z)l(zzzzzzzzzzzzz!!!<$nA>c4T0>Cm1%G25cd0n^0&>LOeZ7"ZpK[R_s8:4C/A8
+S(55OK<ZY.UegTL\Z!2-r`NfJSN1S;4Y`](X%NS:B'o&\&En-=G>q: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<AXNQ0e<bs7UBqf0/fV;rr&qf>%=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.h<F64rT[IBDLV6*8"]hZk9#*@_:,;XT/guTYQ+I&X)B3]d>ra@^>?
+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^,3<r&\,-[1;ftTqfI
+fS_g^Vbhl,mL('Xa=@/X)3,JAHum^h;X>eTrG5Ps.$jfY0ITE"_Z%mKbrjYMtGp7\1RpYUIM@U^P;9H-
+4p8UpD7IoZ<Dg0+2lp"Xo_^$/a6c^seJB`J,51A]_V^6YYAb'S@L8`OY+>sJ,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<H_-p$17[r1/&gq8G"9;8&!3<LTn:o:+WB\T4Oo04!
+Q8VgG#,Si-`Szzz!78pg+!&J*WW~>
+
+%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(<rVGmICjA[ekFR%_
+k2oi;/,S^]rcUZAeH;K%9#EekYJ:)5aT%3YNICEC2k)aE?G&s<f'i%_]?:j'YG8RFhnJ^n4En=7m;W&J
+_nFU$9tusJI]38Bl`]kK\TiP6P#e841G_](j@6o0^M)i?H<'4=P+c(Chk-"sbZ76t\!!Fgd-l:Rhb&X'
+q!>?5VPg=nqff<q&O#gACc34OK:s3NZIRXcm5:\GYE/B*q]!$\l$$NOFbG2o)6/7meIhY[A>f^P<_)A_
+;W%Ke!?G^#\(<A5Anh&?6OD4[(B"lDh^nUM:;9s(9:UI;ka+:-B$i8OqTYaAiq/^/^Vo@qMggN3D\i9a
+Vr''%,u23XoLk^'Zfm`8rFXG1EVdqS?fL^I7DHe*2iE-Adr-$#&uXV<qB\nO=nG@WI^A7)3;mI:06-j5
+U0lpPRmk*1kr4NLM:6AYr=Z$c/G^Yf^KL2%S:7g.QT4qUd4]M9c(\S)oUA:a`9B4=IYlNl(B"lDh^nUM
+:;9s(9:UI;ka/fNqJA-[%Jll$X(<r825acaO7m8[?6&1%iVf)srFXG1EVdqS?fL^I7DHe*2iE-Adr-$#
+&uXV<qB\nO=nG@WI^A7)3;mI:06-j5U0lpPRmk*1kr4NLM:6AYr=Z$c/G^Yf^KL2%S:7g.QT4qUd4]M9
+c(\S)oUA:a`9B4=IYlNl(B"lDh^nUM:;9s(9:UI;ka+:-B$i8OqTYaAiq/^/^Vo@qMggN3D\i9aVr''%
+,u23XoLk^'Zfm`8rFXG1EVdqS?fL^I7DHe*2iE-Adr-$#&uXV<qB\nO=nG@WI^A7)3;mI:06'/JgMAbe
+oLZ/cb?b6c:;'sUmEt4437't/q_)CSc(\S)oUA:a`9B4=IYlNl(B"lDh^nUM:;9s(9:UI;ka+:-B$i8O
+qTYaAiq/^/^Vo@qMggN3D\i9aVr''%,u23XoLk^'Zfm`8rFXG1EVdqS?fL^I7DHe*2iE-Adr-$#&uXV<
+qB\nO=nG@WI^A7)3;mI:06-j5U0lpPRmk*1kr4NLM:6AYr=Z$c/G^Yf^KL2%S:7g.QT4qUd4]M9c(\S)
+oUA:a`9B4=IYlNl(B"lDh^nUM:;9s(9:UI;ka+:-B$i8O4n[QG2B29[X]EVO!c(QE?6AR^lFf1(6,WRQ
+@J\6C?fL^I7DHe*2iE-Adr-$#&uXV<qB\nO=nG@WI^A7)3;mI:06-j5U0lpPRmk*1kr4NLM:6AYr=Z$c
+/G^Yf^KL2%S:7g.QT4qUd4]M9c(\S)oUA:a`9B4=IYlNl(B"lDh^nUM:;9s(9:UI;ka+:-B$i8OqTYaA
+iq/^/^Vo@qMggN3D\i9aVr''%,u23XoLk^'Zfm`8rFXG1EVdqS?fL^I7DHe*2iE-Adr-$#&uXV<qB\nO
+=nG@WI^A7)3;mI:06'/JgMAbeoLZ/cb?b6c:;'sUmEt4437't/q_)CSc(\S)oUA:a`9B4=IYlNl(B"lD
+h^nUM:;9s(9:UI;ka+:-B$i8OqTVpAf<^gZ\u4F:G+LZlPf:o]B8X"IH/_Lp(-><^.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"<UDQ2MagI_6ZK0:c*<dId'QH0!o"K7Emrk:aUJ;Qn
+]]Dr8:Wa#nZjj2R''7uiflT6fe%b'mu9B?Q%uhnNO5XJE`J4nHlR[9B\TpZJ/uhQ^S2f3a#uZ-4B`qsV
+:drB,DuBXuOIDn0%sqWXp8I.PUD5Q4d"XCS.V.dsq0mci2uF-ei(at%,XjFpgqK(aX@-75=/ie]o!pd;
+f<YgO5V5C^E&rU.;darp*)Zfgl!+"n_3kN:o)(H\/@qu-Nkmh[ptgt".7=>iV0S[[B+(LQ9Fod?AZEqo
+._E\4_Dmp854m^r0g5.c"*HM?6>([pXUDCh9O;l<BU]B'[,'l:GO/WZ-^o@>mkQL4T3n%A8]_[iA&HhQ
+5HDnl8E!tRLjDHs\j+a9KYA]pt7o?TWSG<c&ue"tA>Do&WkqJ+%8L=rl'J+:Jo/,qUb4&6+)VWaqMIO]
+Oh-RY6EB?-#QVPrsLmJP4u2fC8;iJ/M@cd1]BGs$;AXC^c`7G54mkCE^?>'I/V]iq"(0#o_J2p<b$rnQ
+N0QYs0+;>K]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$<ftkKUZWe/V8e?((iqb=)NCiLdVde&KsI_8;'AR
+/^6^9.O(f&q$Il$4*YG+e9(>3c=hN0X-r#.SFQ)1bhUDaJ,TQNzzzzzzzzzzzz!"*rDm+IhrR\?FK$J1
+n3G:m)a3&k%(]`J06JDurM!<<,PB["<n!!')n1G^ieS!@9Ho:nOp_NU1Ig%tM'<B^e'[l,rq1&j-^fe3
+=*!#4MEX&lF*Xd_4cWgT,X3-FN9qtKPP$d*,@M\e&7fWjJm]6>hY/c#6[gU;6JG*_#n4psT"n=[J.4ho
+\(g=kmO5C`[FXSi2#!'h,c7Rf:ShOKS7f</EFmHs<+l=t-ObU1e+LCTU#Pg@5BS@a.BmX>!+h7In"rr)
+^g9hj<,oB4Gdf&pi6lK[[#l`X[1IeW9d!s]<H>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@<U,g*/WJDgQ
+7%B-*6:f%oLc?+abn29t73j$%i`&;Y66]/n`2T\KN\GH&+pfD)V]q98&ofBF:.7TmKS`V>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/YR<Rs`E"hUVTer^]SuWJEMKXdE7YWc=VnKTM!g;5*OmGZjKbcA(5(
+C0o)ImsIGG:.<+#W+]dOigcnl+XH&H<.oW%?LB>YJ(O0OLQI<j&TK9MRTZ";k:&t.M)Z4rPRXr6NkhJ-
+IolM"-R0nr<(,M2Y3>o2Z2b+~>
+
+%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\ZL<F>j&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'JS<V<!;qJ:\AZ'QX!urT-!mE(Aju(5/FoBRU4h&gI:J8.cGc
+#Gh_m%Z1As_*A8l0VF)1Qe_>jlZTSZbg+GurL*J1B-%T]6+<?.AGc:PG@'_(bdZI^_iZ06QLaKQSR&pT
+Fk&F%U&)C9qY$>=]!kE1[E8DcJlU^2^2Q*,KTO9SLn'?+Z\?L]V_$cZ27FCMPR2.6s.@&T((P4jT:Mlb
+N[Q%rE[i92U=O-hEm?,?S+LRB"+/</T'3eYMeSfqNT>+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`<OE[khK(+/47DNo;t
+js-[u3ckuFEtk[2&jde?,'#/Jbn_YhS?B?c5P&+S/B'qZM)AMU.\_TXaJjucMH[??aZ687C!&>KoI<!j
+>-10kruMF\;*mh7<('JM$[D[HkDt#BLECt)l()["EmEo1RH9R;NTQ@bqV[IMqrg0X;*mh7<('JMoD8%$
+0=uLVq=<YA7s&[>o_u9Q3TkMJB%XjCbGZ:%ouFEE!VC^QX,%BT,,ZW*.[ntK)`P?R4(\=2?ji0>:5A/>
+f@X6G"PtN%5&0`-iQ\<W&jde?,'#0%f9Ijf:7aR.qto-bjIF\-RICj)X4b7_K[ac>=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\]
+<Jd`3&jde?,'#0%=#YJ5J,T&88!!)Orm<7"]^j`gT1sT\>F:q%g<QBF!PAUMGb:)Kcgluo&LE)u:.=5C
+[$^7Jm>jI-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`<OE[khK(+/47c=4qW95a>"*@7Kj<VlGU.\_TXaJjucMH[??aZ68_9RdOX'DMPND7YTn
+X<L7BM4BIEWMh[YR%/5D^si\'p<S8s3$IgAVET(VU&VKW-m9jV.O(dbenec8cKVfZlsDPn+sHJ8;dWC<
+b!2dJ8dL9DMTDcq2-ZI'-D/ai?38dYS^Ad>`'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<kX^=W+1MmL]RD`?jaJjucMH[??afWTDSTLLX"C@Pi#`0ihjP7U=
+>`Qn)Ln'?+Z\?L1qOAW'E(fr)QY+SfZ!)q\"W#>(jbTFaaJl*q>F=4(cKSd7pH)THV&<N/oZa`JF#_Cf
+_rZ]EMTDcq2-ZI':sS;V0*,kkYL$*4U<ihR,,:Nab7Q`LO],+m[P57`T@6qL!n[YDK:[k:+<7K(,%_uN
+8*!W;:.;;<_W6#f:1nhY@/IEOH5I9I1VTT_=+mF-7L$ZiLn'@DWMh[YQs>-dkq"IgD#a30ImRVSVH#JG
+YJulQ)]8XB,B>7XVkVaY@</D3VmF#CYX_(kJdXcQR4=[%,89H_,6QkE6r$23<7I?g9Q'[hFS($_$+r0$
+nB&pPOf&[s17'5+.O(dbenec8909&8m^1]_lbNasgq4@r*Q0QVS'0oLS;PCeW&7eH>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<kE\@.J
+qGWNk/KD.sKZ%i/Ln'?+Z\=fXQBqSpViF(rpuM9PO[VB/I1(lT;@to([:Te&CA&GF*k_sES5M[EjEb%X
+ZoMq7pJ5]p@R*m\:.;;<_AhpYh6B=sNTH[.6:g0`?IANkm&B^UVro3X#V%$rU=O/>%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<j5UZfIWS6:g1gYbJ+Xoi^sI>?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;/<W0'[n/A4RX7rA'QMLKLAR(J$YPMOGX=#S;PCeW&7e(cXK,e4(Se<'^(\n66`>%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(<IJ),\Nuh#@k94a%P/f7pi%nWGj.'"6
+8E<`,PGs'!]BU?e$UQ&8,'#0%fCeI]]6Sbjg9obmh7In"SiLlSd\TT8hS3,r\E`t"$Z'5ac7(&m6r$0l
+R/1rIeMhBbKLAR(jA[t>(LMR[_M&BpJ,J5W&%]/qs8Msgn)&!j0B`5@F+89ME)%i*,]YA<J?PHg&e[2>
+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`<OE[mO$
+SND&XI>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`<OE[khK
+(+/47;F3q8<7D`1^,#5O7^8&rWJI(%>-iq)3N&)Q&ig^"b1&`4lU<?ME[i92U=O-hEm?*[KLAR(4Z%R!
+pN%#VM4BIEWMh[YR%/4i#pGF0;/ZnZ&IXGcMTDcq2-ZH<F%Q`^O],-W:#*U6H5S%QO])iP't%W]PB0JJ
++XD(X)N'AU\0$W28E<`<:.;;<_W74c8dL9DMM>^?&FR@Y((rVBO],+m[P0`)PS-;,VkTlR1l#-hMM4Gn
+7H@FtROq4Y3MhCjaJikBqQYj16r$23<7I?g9[5YpKTO9SBMGRS&IXGcMTDcq2-ZH<F%Q`^O]0Y2RCn>[
+^BTm`aJjucMH[??aZ68`&/$u<Xf#fdE9e;e((rVBO],+m[P0`)PS-;,VkS;l@10PH`'PZG,'#/Jbn_Yh
+S5M[EjA`Mc->_Yp74]^).O(dbenec8c7(&m6r$/i@d5'g(r\fcV]p.2C<[M@kF>F:8E<aW;;*U2+d#\8
+3>Jr6$^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:8E<ag,/t(#MjTUL&NLT6AdafD:+7>3E[kO,PLm$9bT^;P
+aJl*q>F:q%8dQZ';\:12Qms^YjAaWt/POGMV1arOWJD0m8/<p'QQpN#C<[M@kF>F:8E<`<M>`O5c!cNt
+WEbJ)WMh[YR%/4i#pGF0&L@p?1iWn-`(]<66r$1Qftug?;b@=PS;J^L.AS[cfI1>5QQpN#C<[M@kF>F:
+8E<`<M<Q0Xgqjg],@$8MWMh[YR%/4i#pGF0&LD#uaWb=PQQpN#C<[M@kF>F:8E<`<M>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-ZH<F%Q`^O])i<Q4VK_
++m`3cf7hB[3>Jr6$^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])i<eg[DO>M(-;Cn3%b<E(fY)`MYVo6/:O4hqg[kF>F:
+8E<`<M=h9kas&a$)d(FD==t?5lcPJC-Vp>(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-<<Ml'hr]8g9o=`/jFg_GjN'@n:s6@Gk'dh%q5p1
+F&"/-,]YB/`9)e2D?sIbMqQmCW2$(9*^"[dA&Q[<cGPDJDr89Wi(9`V/UZg2V+DcI(+/47;F3q8<()QP
+%r(;O[YSS(i>#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])i<KN)I:(>bPt-<:^='t%W]PB0JJ+XD(XU4eUXN>oa(Y_Z:I:.;;<_W74c8dL9DMC9XDH5!aV
+)D>P7O],+m[P0`)PS-;,V]n_=WKF04GhkioMTDcq2-ZH<F%Q`^O])i<K[e^&/7K?V;c.>=0KJ<\&jde?
+,"`,T3S&NhX,*:'>-iq)3N&)Q&ig\(6:f9VIL$JaKo&et:.;;<_W74c8dL9DMC9XDPo.8i#<RfC6r$1Q
+ftug?;b@=PS;J^<U_Oe4KEo#C<CBSNXkCSPS5W#cM4BGO+f&E]p```N3>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#'<Y7>,[: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^<UeNdn$dV"o<CBSNXkCSPS5W#cM4BGO+XFcBpY*Q,NFEWb;c.>=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<KLB^tMIrEsSq_P9\JDSb$^5X,
+.3TsW.\_TXaN[r:iBu8@1Mn2bU=O-hEm?*[KLAR(PRnJ<UnX9cY[+mB>`2dlKul?QPRnHf'i@:gjJj\u
+Ba`\7CWa&`2-ZH<F%Q`^O])i<KLDt77C16Icb$SC>`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!<B7CKEB
+3aSug=#>$-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`<O8dL:/
+H<)l.E#73EIM-p$lWI)IHhZs!3HM&:Z_Lb"qXs11>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<SiV#Zf>%<-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>GCX8E<b"(Ob+c55l]Iq5J/]Y)PV/q=3LcU]93m
+fYVk#gf`Pidac1=EEql[+XD(XU4eWLWJI(%*^"[dA&Q[<cKP:S?jlEd;q?<a]sn"OHZ'dM2Q['t@<0XB
+-%nZ]7?e>r3>IZTd<#W%E9HT@Z`h(9gAW2-V@lP[S5M[EjA\MR+d#\83>G9_lk[<arHe4(\)2X]GB`eF
+:Hgd[k03$GI[2QZc8h_SON=DW&jde?,'#0%mb'2Ig"P.@G3ml<cPN3<J,[ij7oMU'/POGMV1arOWJD0R
+,,ZW*.U('AcHaHPIlr,_n$.Sg3N&)Q&ig\(6:g0:6r$1Qftug?;b@=PS;J^<,]Y@YVkVaY@<0XB-%nZ]
+7?e>r3>IZT;/<To3TlV>66`<O8dL9DMTDcq2-ZH<F%Q`^O])i<KLAR(jAaWt/POGMV1arOWJD0R,,ZW*
+.Vd3D-Kgms6:g0:6jY$!;\;#)[:Tf1;\:=/6r$18&/$u<O],+m5/ZG\6jY$!;\:=/6r$23<7IA_0KBkF
+6:g0:6jY$!;\;#)[H98=:.:BY&ig\(6:g0:6r$1Qq$`Hc&LE)u:.:BY&ig\h;c.<o_rOcb#pGF0&LE)u
+:.=5Ce=+];3>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])i<KLAR(jAaWtcS.?o;*mh7<(%$9O])iP't*0[9MS$@&/$u<;*mh7<('JM
+XW7EB!.`p.oh#Dgzzzzzzzzzz!!!#uAnPc>cCI%(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<<Eo\iUNT]N#Bl?iKV_DS2K(J)1rQ!C-D%:X5Lm\,XLp55X]rWDamf
+pu)Qu&jt/SK"=%3qgX"nrV*q-CEAVZTt3<S=0>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'55OK</u4h?HKcONi?'[Z^?d6<
+=?tKdZ"(iRrSRYR^]42G]/uX_:S7XLJ,\V2h^A]%>I[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!4o<rKYO2A;'>1Z%GM[VCb@^B,hI0_P4$21AhL>@hkFR%'qb?hY<SoWfjEoe]hWPJGIIY8mSuJ!#
+mJWm(Dgq;IrO4n7nt`lA?+t]3KsuqJH$70:bK[\kXiOf9EdKJ;h;.JRh;/%jP>BJo3AYtViD?.rrcr\u
+i<MUB_>E,tpC'Ts(dB[t@Xe4/hf`AV[^NWQ\$mGAGjrj+YB4.'^Adup<SoD5k2O@<HTs1=h)jo9b[%_t
+iVDZ/s3"dKZKg2`?p)&1guW-S#:ab\]92MF_m*^lp)]6=rqq$p5J^U]HgeYQ%rLta0I1IC(=pHGo&Rp+
+$dhU#]l``"]iEr6L?J*ea)AW\`mLPOpRatP:Oi5iWc&S=1?m2gmW(+q!u8Aih9O<kcGq#Vlpq2^#'P37
+]*\E2kF\sSL+mFjpPq@a1kj`H"onW'zz!!!",U\lcY7Xqr~>
+
+%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<n*el_s829"j"IQB^OGk@e`$&_e#+:9o[.sUk%V8iQBl0TH"LMp6Ef9igMK_Loi.;K&+h^T
+nRtho_W+:N'.6Ogk/"mMN1jtdCY#T_o?TY%IeS,Wn\3N(elL[jgcTK:IKp&]o_*e6q<.O2X&c?#Zgh,r
+rV,3eLCa.OoP!A]Rl6P3lSj1L2>,_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!==k1m7R9Bl<LH@(!=je5%s7Ror^P;3n[A\VEG`mET!hLE54TqT&7U\^\Ar8$H4lM=KC
+V9R909iI?0>B];]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]</uiGbHhDFFG<`7(h`ApD2t8cCR_8FTM['_3
+YRJa]p?LnULRkHIFuO_`N@M?neF1%L14qI@d,i;*7_a,WofKK%Uu*&*pjP/>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<W(b'0IEVN<5-H'[
+)@moR*.<<DCJgmB7uc_tmd=sE<p.U$ENQ1Q]j53'!97]()+2q.1hl:,XdZ'IBPADDCe.k/p?gV03W&^N
+>?b=>=e^&X#BoC^.F"i9*PE8+ZmAA2<;Jg@Zi<bl4(A`WYB#P=,59P^5;c/@UQo<eW^N*b%YDNN$Q#Pj
+9%S7N@^-cA.P=HG&U@hp6G`*F\MO(Jjdb%C.IB#TW[9d0C#M+OmFefnRR%0E/P:GV@f9mI<D,kVdYh;J
+Pq,oKCVP,aU5&Wh/$D?^L<\/uf2k-idI2kSL1"EinWSM`qTWBbpRA\6eZR?NeQ%M")_P<:8/dH7e1b*/
+L^oHm&u'_ak]!C(nb[GW\*EjMQCfip5-BTXg9djDg)W2JIPZ_Y;u3B6GJK-??0<Af@EZ@YhRd[]]$u;q
+Lp,[Q=h9XgH5p4C,g^K/N.K2ng)Hg)B^j(fO+\N2l8RKH;Q%>_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`>S<K,FZVns8D]X3^)8-3ZMlJrFQHq5(B5Hh<7*A?i.2h[OqM]?B[4S&G!Yi
+#:jX>qTWBbpT$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*hP<cnYC\W@dYNhW<Hka#2l-ee.1#B*;#lbMO
+Am;omEVr3]UL3tm&e=qXU]lM-]r(L-I[f"Y-<mrsD8)lD.-S1?$eoV_N=R.AW1'MWUYF=BG'6@TWgd+;
+p5Nofs/[Mo[%;^'C&6(]A+pqi_)M$_%cVSpd;F?j_M)/0XrN1OLU1nQ)e@`92G@*niVGK.#KObiO_"qY
+j*%he[F]M1gFZX<[(!MWcU5JC]1MjidrZ_h]QpjD!-B44q6M]qZY-&jr6atL[VOKtE:P+L/S34LN7N-&
+:=l8U>K]:Hq4lj#Cg^X9(TR#OV=W<jf=-2*p@7\CC$2;Lr6[1_KmrV`^ShtFc!U-nLC?jlAih17H:e$W
+0e`_%l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#
+^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[
+@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:
+KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2K
+o]q3[@7S(=kSJF7l\0dr8&&Bo(O!4[],u1Dh;.H]"O&oD[C\`>)3A]"a'Y4]Y8@%h*W:;X?eEc0(]&h+
+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_
+KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0
+(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]u
+dB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X
+?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosg
+HD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[\^EIagr5GIR;X0^H*rmKWW%5dpV2Ipkg]
+?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr
+8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*S
+IUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7
+l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?
+G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=
+kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@96CB:"(QApg:.*50IU7nc.g>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.]A<H""n6NkNLA>97tS7:+X;Y[]?W_NortH]],Q4RC^Y]WgL+a'Rh\/J#I^p7aGF
+ZcnF+jT#8\zzz!!&C]cCI%@q=<Xs8X#@%5Q("<cJu`GPe%YGMoE#8c^m.<7Lr`neaUSu`X7F:(LMQP3q
+`=3T>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_Ym<RX*!Bhk"='$nJ55F9?YJ0l4qsVue.]Kckl'SNLT7?jNn*h6_IWK6KQ_s`uPkVe.hn
+FjfIe_'N/+e`.Q1cD-lK[Vp5Q7T%C$jW^SXl<4Oe/)5F<,1-.dqRlA0bu=4f2Rc,I&'LaK)5MpsOH\LE
+D!?em!(W>3rN7*og2_\gp#:H0CrWp?^HGT$f9lOk>RUqs6+%eKn<3XKHsDMH]]m@q/,Iq40TkG'%hRrU
+neB2l3^]Don.gH>rTE=l44.2G<C*?[hgISpp7P-h2J(#_)U2QT[gia:_^kVl(p8G(OOUA4Z&U^\mY\Sf
+U%@1d97Vjic>8l7T@'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^s<rsnu?DuK84zzzzzzzzzzz.!BQKnGW7YR6-IiNqW5Ace]04!!&AA[r:/AhRr[Ph7Mo
+5-^2ZtcHatTmbPL!G^)4!b6%_EJ6E0_#J7U[h827e$h*KMFn`OtrVH<aprf!Wh#e]]HIVO3<ri:$?u>N
+eYeP\fIer^$[;4C^hS":/hd-$NmFr-m4?1%Mp?gW&!FNTjmFd2[>.igA.\_TXaJjt.66`<OE[mODa,_=
+ODV]X<r-A#Ie%>+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<KLAR(jAaXT0$H@Q&LE)u:.:BY&ig\h;c1;t?$LjKrrR[>]'W.$$`Jjo66`<
+O8dL9DMTDcqhom^#nscrA]QnSpJ<P>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`<O8dL9DMTDcq27kP&kEHTG,,ZV
+W#`5(/aJl*q].c8.PS-;,V]n_=M4BIEWMh\d0KJ<\&jde?,"`,tS;PCeW&;1@3TlV>66`<O8dL9DMTDc
+q27kP&kEHTG,,ZVW#`5(/aJl*q].c8.PS-;,V]n_=M4BIEWMh\d0KJ<\&jde?,"`,tS;PCeW&;1@3TlV
+>66`<O8dL9DMTDcq27kP&kEHTG,,ZVW#`5(/aJl*q].c8.PS-;,V]n_=M4BIEWMh\d0KJ<\&jde?,"`,
+tS;PCeW&;1@3TlV>66`<O8dL9DMTDcq\LUa?0kTs_kCV$=dAo4G(h:c<j$sPgbb^q3fS)9(khR+dN%sH
+mNuqM`0j?C4B,=lqUR,GK0ufS4)*cmnA%$e&d-0A(@uN.^R$/DBhjm3]a*CltoP<IUf>ik,qY+iWn$C1
+L-%nZ]7H@FtRU);NF3;&EH#),=QZ7e_ic7En+d#\83>Jr6(FJ=8'%t?kC_5@-i@`I<L_<#uV]p.2C<\A
+)c8h`>S<11#SX[["]-N^f8dL9DMTDcq27kP&kEHT0KXB.mT5?OGE^U3WO])iP't%Wi-Kgms+q!S_mYZ2
+E=tW=0&/$u<O],+mG%#F:.4+7H>/-?NcJ+f2&LE)u:.=5Ce='[1S9%;Z+[R)pmBlQsbiOWn66`<OE[kh
+K/kjPNV8NW_<Ni:NhU/kR;*mh7<('JMXkAEM3N&)Y5GU_`C+l(F=r>!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;%h<r5.F>dDr7C7@=.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`<OE[khK/e"h6Q'T-YgOJBiZkST"q>^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"K4<c$rL^kRLK)"OX63e#5iAI>s&0_\u>s(N1#XT^r6kBqN,/.h\6jF#/O-#Ie+<Elq
+Ug$A\89g8Q&D?MJ9<rL->KdstF8c-k_mZn2mPo]/Am]I6NJNCHLkpkCLkpkCLkpkCLkpkCLkpkCLkr"7
+Iu*<DZ27b!9<mSM*.*fRf^^<U>J96-Op<RWpKO:4L\'\(_3QHglpZ)+bEFpZ,.k)@8K<)>?.e]nNkNLd
+Cj?Z6?=Xk\4GdQ$Z3sm.`3C-3Ke6*[fSf+5Q7$\=7<W+_P;r7[]<LBg*.*fRf^^;J]r5MWLu`9P'/I<S
+CUq]H9UK2[QsZ#1q,NLO-!?UBopB*LL&"`0q'B12?hKdnP(i^3O81))Qs]NR?]p-=<R9@oc0+]-"qM4T
+n*f`)eRgQS<;#&kEOf>W<%>;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?<UO\IlobQ6N;&9>].#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+<Hc,Q-,+aA-P'e)$NNm6Y:\oO;VM+$XL:#?Zp0u'?a
+_TfSNr;Q<>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/,L8RGX<QF++]R>lMRU62emro.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-=<KHr%=dWRhQsZ#1q,NLO-!?UBopB*L
+L&"`0q'B12?hKdnP(i^3O81))Qs]NR?]p-=<R9@oFj0Gen[;Aq'J;n%bn;bGl8^r@%X<G=V,E.<,5"0p
+pA')fDn^Xe?hGD2hbJZMCcEF.GW^gNU/n<0$AD;)jEA.(hL,2S^Zm[oF1\%N,.&50qX\2OD8sZ/Za3Rp
+;C[t$k4eTCg@U7HQs]Nt0Y\Y#r7H&#I"$OIs3G.[[9EP%nP%-g5$k(Q?hLua^Rb&q=HgY?6e(p]%u[k4
+o;eJCoBVe,a(c]'(\k/Kf\P=qWuCU,;qR[-H20>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$<HI2*k88:kX/R72gL27AYY[U%KTD'Tu^c5jMEj;[O:D!p'SO#R)AX";I]l70jNX&1iK&
+67E&"#)Wd3KReVZb-HroH/3Z$FkrIVRG[J$o/u2`4rfGta7YrYhrf4/f9t;SIQY+*f<7ERT=<^,HdX##
+rnmPO*a8-iEsHN$QS)Q9pb#MlR#HauG[cbT'2CoR,>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<jjFN@@[lrqGgqKDl=$WB,m%#O;_'ZLmOjU8C8jM_Fq=Uc`R0gV%-n(%Rd#[
+c2?Bi#H>\]`Oq-S;@$01WgrNUe/0VQ-!baUAnf!PXhS"AVaF:`#g`EcQOosi(E0V6N^t<aNN$s.92T2p
+BU!VtE[JKSH/Ge^brQjH?'mo3N^t<aNN$s.92T2pBU!Vt>Vj+b>M2&qZ;-1J>Vp*mcH?OB+P._tN$D4+
+;Ic_slr)=M.2jL<36Nkt\#-QuaZ:FD?'mo3N^t<aNN$s(92T2pBU!Vt`B:b;7e.2]^9#fG'b+qi?$P*@
+(?RRo8#-]l``.6,,t3fUU/B?mk;(>l5Te'.D-lJbcM%4'hhB+Z<6)d6,)%-sVTP;Z7?*G(ELNrafkgP&
+f'*`.=*!m#%a8Ip0QG)E.0QV)<R,\8C5`jd;r9IT2Ls%5*J3IoeV,)HHI3t[3$O&!25\NI/k(b^U2fJ1
+7bJ!RdA#_bK/<EW\hJ,A)b;,CNN$rk92T2pBUjBe:UVE`ZtMJGi!b+Yncunu%Zf?K;@$01Wgr3Le/0VQ
+VokV$j\YD$3E`I'E>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*$XE<DKUj+TK2NU7e)H5`/Q_MJjd5Y/"N6c(T
+=+4@;`"k>U9j@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&>[<FGq1(-NKKrTWpCV\2G<G>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!.\<V!4SrO:B=n9[Akbb`T!d2YGe9I1L_Vb'`qUhC<WhRqNAi2]q_DJa7hrU55#ET4p+Dn\7!&+'%L
+:O9MA^0b=8WZ3$!TTWriSM&^pW@Df[A8bVA.AAhSe<h^5[1kJfM?`$P5Ps-EF6@^qqq&U^[B%=Ir9$9l
+rdk)5j0QDh>@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&2iTYf<k0057G@l`g-`lcb8JXujn*.75CplL]dcT_D
+WkGA$X=cTJ8(Y1nS)5TcI+!/fXqC.%ifPHUcC[<c<u6VeoY+FD$ZG\`Vs]%$YMj<ZRhUV16dg=3D)Rl#
+cW6$o05BiuDU6Wpl92qkSS`f`FQ)ZXMuIS_?bJ*.gB(RgT&-*.D]<FQF#g*>c[j-lH\QAqRr)f=:X']`
+OaHN,VuHFi*;,n6gWh^l]6E_hq]>"inI,7`Ya]@QJtAQYPm8`lS#\BhnP"Etm"/5.jXHo=Jjc+C<G;,,
+/#.G+-Ui+0r^g8EY@$2YQ/+Z!CCa^<39ugJmCER'*i-q3Qh-X8@&?/iatlq@MJ.\d&LU"I8U^DSH),+[
+5QCQ*hg"1@S_kL`hc/$XM6fr[>WZ6dEM_c8+gW9.P[gleFOZp,RNP/>8Ppc\]aA#n[;-s8Rb,fR2nOB_
+q=<XLXfX7kr:7d_'>(=8ZZKX8XA]nd<#:!GN$"cOf6r0%bU<c1EDk;]k9b[&\>DpM_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<rL,=oaE
+;'?&=7.>: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<it7N#;-4WH
+"9aRkOf-8JJl@Uj78'9]'5*jK22DWNFVjmpHh6SgnQ3Uf,)%-O2qeuA6u\mJVJ'[c@H+@eU8i$0$`NA6
+$D#_Ys,#Vm,)%-O2o6:)6u]^LVJ+(n@H+@eU'>_QXQWK&@9Aa:&N>jeq*]1!,=tanFCF11;kaJ<iN2-;
+MS;BfinGL6L_PG/gu),l'>[<qp_<Br(FIEg-#-PKcWZ@(geLgrdpjA[6BI!BE/K#eG\IAPU.XXd`*'!+
+<S=Af<lNINh?dJT5nbO'#*u"JmtIq/3\Vra92UCb@H+@eU'>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@<B-"Oi:k;Uc[lQoODrW.A8mFlDF<N^t<a`&3c'
+T*VT#6Rcc7/khdX9%9Le]"7Ad)+UJ6,iY7`d?IE0;hHDpe21Hu39ug-.U,mg?$Mcr,>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?!<egWW!EW%M4PiF`M#;V<1+\9P:
+:6A<TO+CG5RP%:(F\7[6>f@'lje0rSN/SuZ#SM-2;WPO<ULG"(V@+&MSW[JVQh)CiGVj&O$KHpVJj`ji
+MC+VGd1?4"1Q*$TZ*A<CmbJ*Y*ZXrG?'mV;PSF/n]^Rlf?1NQ,k.D_U3$O&!&XSk/0*I?%=Z!=i[C+ok
+p:#ee*I@^A(LMQp/&bF.Y6u"VSWZ;UY3+IrbW$f#+;#DcO\Z*i(<u/(^\Q.%UIT7[^:gq^[CWsGg2!n*
+nA5PD+$Q4eG[6[rl]`*YlGD<(NcJ[nD$o<Z'S8a._C>&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"bB<rqaTl(V>G!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'Y<doR;Sk@I>T@+N^t<a*9b0F/4IKK;/*TqERlQfp'K0'>Mf(Ilb`6K
+N^t<a9JPQ8_,<[k'U>O''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<aj#$3PJu=eVihWM0?'mV;PSC^uJJ@&F
+)+UJ6Up4(h'>`-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(:<P[n.0O;H0);9Up5t$=.m':S3^tYAoMbEC:.")N:D_#bFeKLm+R]FCF11;c6<d
+]^WuMhJW4]NI>1#R./aZ1QG!L8Ae:#'>`-h&TL]JRP%:(F\3!HGU?A8`q5>/V/2DQN^t<amFpB>qjh+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\)8E4E<MX[-p,>nd[
+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&X5GM6frKTaU<H
+P/Bt)*0ERLLm'%BUaY5^2r6NAq!dprkeb8@Vkf\587Uk?@lVJE`XsX&M)Zq.:5Tg1p$1)1kN]o8JTXQl
+?$NP-*+LneL_PG/S.lP_7>q!<l`Z?h3HFi[=^@C@YYIc`$U84l#?Z_#6jc4(:5XjkU'5@MD4Nk=qOMie
+\T6eh0$Fr1Vkh[nT/-0D/AjFkKcgsT7DqdF!\RflpsYl7FDYl3`\'BW9CT;EB?p._$Ojka6/(-1$ggiQ
+L4+1*(e:A5,iYVU\1smpYp0,-QX=oYontK;k^MoA@?tE^\AE-)#ALuWS.>>@$8[]5+\9OC&TL]*`Q#p;
+pUbNS=^PNcl(saGr9$:_5<p#s3NQ.Wked@<^kYsXEC:.",#4#OPG0M7Io>p![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#h<c[08#-\AihVAi.4S%.-I+Xu7G@(Ec1T3ppUO>E&Ldj>3%6%HMFu?OM6frK1"%]0g^_r\
+3$O&!&LU#c8Ps350QGYm.<OO_G@_&=,>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<a`(:P\;H0);
+9Up5tMQq9^a*%F&Sf25]O[>.cP9T"^W&.eI39ueCV1MZN*Nj7/L;c+rKcgsT7DqbpBt?!<egW%;d=nXT
+R3=>G80LOB8J,r?<15F5S97G];0:d;hB6*iEC:.",#4#OPG6KI@H+@e;A9O;<HYg+I2FTsL_PG/S.lP_
+7>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&<V9FSNH%>n5pB']L`:Dm(?iUWIe[@'<R77"qoUFmI.4rWn%S\eOX!A&
+Iei(S5>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=<ET4on++Eundc5IYiiH'V/G4/EV:B!-$87`c6jc4(:5XjkU'4f4EolN4UY?k75'?/?rVQ>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?935oe<J!8_o,+Y.0PJe.3aHg&LU#c8^Y2rSiuu)CTm.'p;W(2h)TlPi5)5042]!.&Ldj>3%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'<S.q)POGK)E;@!nT;FI#h
+9l]jke_P.L?<^Y"b&@$4.p&n?4Q_D@ITT`Uq9tu82!LJh_o,+Y.0PJe.3aHg&LU#c8^S4_+$TU9Qd*e"
+gUD*FBC*p741cU.b?t8>Xgd/UoM$+r]J'<c!#2_(4+?\W62(WAJ,fJ<YH4q2+$P)I_M&@*HoD$*!,078
+YJ7gMH/Ym-XjttPF6Ci^nA5Q?J,ONIqZQs"!2Zd!Xdq\+rm7?+gFk1O4F-MhcTglFVZ6\sTX`7kc$T3Q
+:&hCG!<<*"zzzzzzzz!0Cu;F7\hU]AJ&2?/I_1__sD2\eI;Pn=]4j@0do;HM%$rTD-#8ZtaAll`75'qS
+ti"dJ]g.IA(*(f<oP2q53U$Th5"Ihgb@Ng+>7)ff)?;\T;&fFD5;tjnU9QorDGB\"J0<&WKpt>Pmf+Is
+^D+U$WQ-pu74f$LN2<B\OJE]Q*`9iP,&<-Jc=[oD%k9gZ.M9M%b+$h!'0HB[S"%n;!S#g<2.aX:[7TLC
+YIJ4aQaqO$;:r7CV'rjk1?Yj\FF`1N[:#lIDq>>?cUWqqo$?L,\";ZgXHZ0u*P/Dn;ckqtBD07B<JiYJ
+'Y`$ST0>Xl$keMcOs6WLHPl0FR+4$G,_3\8Yl)IJqo=^[9%gOX!B/"N.n/HgeX#8Ct"Ml`P$tn)&WZV3
+arJ3n<t"d>T\6h&UVSU"q<?/N7b(2aqVpe,AWG5<F)Wgs0j<qU`K*kPUtPr+D)&6sDQ-2@^`gFQq5MSZ
+d.ESR%,a)i5X1MKXFHp"*Jp3ICJ$0$FB'IJ),HB?olAp0[jrf<Ji+55k-&*d<GVFX*isakZ^ZN>jgfl^
+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+dY2QOeY<RJ6B6La$\(o2F;Mq(-Q94;o:Hn#<i1?PkEob<(oB4Ho[7dmphS"8AKR<oW"ap.-QeOs+qm
+A'#!^?PO__+8gc^oRHPug0Ei.H72ICC.c0>IFjT-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)aC<eH#<SVb5:YB^[m``p%<2H?+=jIa1\3!caF\"gs#*;'Y3=#\,@EJmVY+q[^
+NpJo#h[o:@);bE1kb3ea<q+jN3TET!nS2OC0e,0?dn.?+\Zne1P(?\?W=/nDV8lr8JOKFmXr+Icu-nB[
+#eG^V9YliU6-4]?Hg-GOC/$?MOB_YP[00Rdk3PEQcV>p8#b]G9Pd*_og-Ep=g2*SiuC(5J^U-?ba>D>?
+cLYL'V(PKlu?7HM$Fp+?FZ1eG]:=qq`ROZaR;crquRZLCW_L*9kd6s1IfkU*Z#[h<L;qg$M@n>OHMi2`
+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<Xmlj+cT.j;s/8J2@HE^L.)aq^(;qaQB.85;1bT9:HFaZ61lP11i>
+0^\m!X(O<i3@qN"A,D41hQQ'NB$JQR^\R]=B<AS6LQ7CLdaZe-*^U\.e#-"Arr'TZq3!jfq!_^>hU^9/
+\DOW*cc(eYa.;\V^\?s>A(e/!l+<NOJ"FA*fu\aF"7l88o'k;:A+Ap=ZY/11J)@A4\':C$Vf,hJC/mB=
+n`%N^@DmL?9q&G!a,Y8bM%_8rQ06Jc:S'-V5lBhprVCs?pp'2Z:S0g^O1EDo(8!,N(9a]LOC>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$?<I4>^oATu\UeeUJfo]X]#:7[9KmZ)dWN6*h-
+]Ut"$g3?8emCsW^Pom-,hs<XP<9Q9/KZB,q([cS8fWeAQ:<fBDddb\&%2ut<=A.nrQS2^c`EI;=QS.ji
+$?"\2Fm7*/O;`n8_X$*b.kh`dgtBDiHhu7;SNG$gP+6'[(Qr4meue0lDJsHS!GpQ$0mtsj8A'!)9%iSj
+Dr/\ZF-RiLW8Hr-G#[Zaat9'83;8FXpr\jPZY-J:"T?oEiVSTF(^i;un=Dgl3b$$`If9+30-(4S4=jdL
+1tHrRl8ZSuRZA-KG^/]6]lYWe<a]/U`dZotQ-\X>Siti;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=U3<QTAjkhj.)i<%2)Jap`N3/WMDZ,8_Op/F([?+Y8s[stcl[)=[U
+Y!^ZJC[<G68G3]D9UHBXdM^fGa'@^r/%tr,*b86BINK?N8UNHDh\+%lLtOEYFVaq!WQ+?b'aNVUC1P(+
+%NhtFSj._]fq$.b^2`1&qB'/rq>0c"*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^\:44gpm<r2h$8<_XF\$UL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY
+?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUf
+kSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=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[IlYO><pe>e>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+<k+Q9"\M3X%Z&2eRH0A@!0$HG"X<JHm*]d>a$Sr;6X2p!_LQ5Bq_TYBf.WSt[TjnhJ0_<Y6
+FqqFE2i3rH%fpgN\b!DrRfV30H0A@!0$HG"X<JHm*kG+KU)HeP!Hb<Dqf2Z:zzzz^`W:2acgr;hN`Mh]
+tOC<mG!,84STk\>t+R:rm@K2%j!ih4-i`$]XG8WQBmgE(G:r#G5:^jK*De]_aD:JVsGloH1L%;OX$():
+,lk0Qtt<V\VA_7i]AqC2Q!s"^IO;(M5TPRfugIl)`MZQQ7Z=LI/a'>mG#)X(G6[gFD.N"4ET/`Dr5a+g
+_jtSSis]%^Frp+L^@FoA-r4/P)NBtDr/-3/sebLq=<XO@Df6T^[;qN"+("HIE^mBh7M]BBtnYq13`0Ap
+$:3O3,jCs;+[Ta'Z]!RGja?In*h5tHum:QGPq@`..)61Y$I5=:]C"N'E'+BV,\4r](,:8SNF*<VG3O<@
+D[[WO`#)U[goi$Q1fRnhS!Ijj$*F9)F^\TNkFm90#$>XqTtp'gU:sWCU'9`BC]9jhlcUqBB$jd4*Ks]*
+mu^A;7)3UT0=8+it<mP2QhF;9=4%)@q.!Wq9>BfNu0V2o&\'prFs`X9`kV1N'!(A4n=UHorMgA?+P,,*
+ZdIj[.#qm#[Y2j[:=<<XD$a_/mVZST*&ZeOeHG'4aZlR*hGK>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['Bu<!!$rW5MO'TjqBIW.\_TXaJjt.66`<OE[khK*@@[.8d
+L9DMC9VnE[i92U=S+.E[mf:ON=DW&jde?,'#/JboJ./,"`,tS;J^<,]Y@YVkVbt@<*ilKTO9SL_<#uV]
+p.2C<[bGV]n_=M4BGO+XD(X,,_0dggP4QU4eWLWJD0R,,ZW*.Vd3K-Dr%[#`5(/-m9jV.O(dbf%a\S.\
+_TXaJjt.66`<OE[khK*@@[.8dL9DMC9VnE[i92U=S+.E[mf:ON=DW&jde?,'#/JboJ./,"`,tS;J^<,]
+Y@YVkVbt@<*ilKTO9SL_<#uV]p.2C<[bGV]n_=M4BGO+XD(X,,_0dggP4QU4eWLWJD0R,,ZW*.Vd3K-D
+r%[#`5(/-m9jV.O(dbf%a\SVl-Grr?X-#66`<O8dL9DMTDcq2-ZH<F%Q`^O])i<KLAR(jAaWt/POGMV1
+arOWJD0R,,ZW*.Vd3D-Kgms6:g0:6jY$!;\;#)[:Tf1F&"/-,]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@kF>F:8E<`<#pGF0&NLT6AdafD:+7>3E[
+mf:ON=DWS;LLVKul?QXCPgNp.Is;<(%$9O])iP't%W]PB0K%C>Sr<q1=:?&LE)u:.=5Ce=%s_c8fI-mM
+"4sVM)u%-%nZ]7H@FtROq4Y3FthO$#C1eni3@sON=DWS;LLVKul?Q4A-Hm3D@`YL_<#uV]p.2C<[M@kF
+D)gI#%GF,`A%C`t9VN6r$23<7I?g9[5Z[Bs*10Ftt=aR!q/X-m9jV.O(dbenec8cKRA?HlI9A8@C3&nM
+m7rON=DWS;LLVKul?QCTI0bgVmCL.&IB5,>&5uS;PCeW&7eHS9)8ZSd/us6sU@e3D;pa8!FPt3>IZT;/
+<To3TkLd[/7J;QA/o[Z]rKB-%nZ]7H@FtROq4Y3F*<#s+]]s+^)`7i,M-k+d#\83>Jr6$^5X,#Oeklfk
+cIfhnnEY+j=aD8!FPt3>IZT;/<To3TkM"VV_Ppq8Vnr+aJ:!R66KX+XD(X,,_/a*Zc@T/hV$ZDO+UC$^
+5X,#AhKn0&<88\$.m:1QNbQ#`5(/aJiiQp\4F&a2_Cg>FYZIgY7&p`=&iXKul?Q6e,lVf?M]C6SaFBal
+A0i66`<OE[k8CdF$A0mdADn5QCZW1E)6:J,]B)_h["$gNhE*_W769@@6t6\;ODWj9LqKg"d\m)8]);32
+2(faJjucMH9<;n?B=.20m[=md;.Jm^I,lk'`%;9[5YH^S[=cGinL$qW;S@]<hH&ZA"_-EC:*VO])iP's
+6QKY@$JUkG-Ggq;;,LrVLU%St)=pq[cu!hNn/bXOk(/c2:U;J#BoV&4j6K`TLk>r3h.h66`<OE[hEEs8
+Mt:m9X+*cF[J*Ff2=S=.aD;NF#E<guiVJjF:C(7/&2Jq"3fL^CP_fJ&0k(>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&/$u<O],+qQ^=&N]=[t;qtKjZ$S
+Jc>R:g$T:X==rl*[=;:=o0Pre9im++J`o^i(/?+XD(X,,_/acHa_D\$lkX%MfT6Yq=2W?4Ng"dqNQ732
+a[D@p<Pd[-E'JF*O7$*)T$njA\NB76Q%sqXnjeIeiQ2bJgnDq:g_NklCB\Q*7lTj"Gt-cf9fo66`<OE[
+k8CNupTXrd(W*fA"/XDX$`ZZ72<m@Hp!&?g24!-U5@+]XBq_-m9jV.O(dbQ&J2-mHs;%+$Z@qmHWtOhM
+9b#CTcim,\>Vk%c0H<Dr=7g!2.+,!"tmh?i~>
+
+%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]NB<a`NZ4,aia18WoH4(J.:I1DrcI>ssjD>ssidVMc]kP,+#3X(7A=-:sAe
+bTo"5^>&,F3'FtD=e4`$4&=3"rRf*<F3\jPml<J%kFD.Vms>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\5M<CB=A[+bBVr&8cpKf@P4jGEXAUXf_!Xm0-^"4<@j8>s182$`ZJl5oNoLCsZMsU@%FJ
+qtF;u>*:5g]6CH\XY&2jk3(pTSkD^L5o&TJ8A,1eNfH.]3eE/</^Ri4k\l!)d,0'9hG$[;a>'+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>o<L./7;mHa'g-n)n,WiE([+$V+FGBeABY-(MaZadMI;cBH+8QEAMaA8ElTs#YE
+?=$rbHi(q,M-f?.kbo2IeCUm2J,fLRJfbgHO>n=GI#(6W]dkT`aGbbD5C]]VYu8T0K7efPrT"`e^D95o
+?hEOHlKYpj\Z)R-==dKP<-3S<T"(j+r'EKXIJXd`IJNF;fc\/N`J#e;Yd!WMZY.b9e"8jS`\BlISrY6d
+G'<T;i#jY*+K-fi^WXgD33QNsS]f4"s&lrTA8cB>Z?: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:</eogGT=T7J,`h\#b=o=T+M8s9KC,^M@68.F6tHuUSB&=K#Oh6-MPsGhiD9cjs^G"6gu0^Icr9f
+chnYV#rO5p0P$u&"oYhOo)<sdO8-J=n_uu(;aV>&(u==3O=JqZOQP\;<<De\IcS=1L'%ip.LBTl.Xt;Z
+b*7Y]VV)foA`tu&Y(FUQ_,D.pqXeKMI6A&oZ:d_ilS\HqE?EL0RB'iQa0AfQ3._'4&Gc'%Sh3-@CMTcm
+.lG[rd0L:5k;&$VNN*Qg-(+6>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'<m;8+6Moo!f@JPaMa55[ET#Y*Jk98m]
+-:V!8PtZ^>&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\X<i5/X\TS]5bk9J.I!gn$3]fECI!0[f6MgQT<2qLt2`<S<rm&g7
+as%`=l0k@;5^tAA]YCsBfR]G<#!P^ciJr<#;1H".hVKk<PgG1/'Lk[l\[AiA$;'M9Na#^l;9mtP*#>sj
+$O[Id4>?PWEH.,1Rg\Jl]=[b-L>);CS!of8-O+&0"e@.n.e<3&eV^c0UE_EF&+ak<If9+#(5p<3D*<.)
+61%KQI7T%8"U,&hl'\A-Jc8gaH%46/0kZg>d7R#'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)[a<RT<ErdVdqI22rO!b8r$=qN(le@_d*Rm?5lO#u/YKkH
+#tAZ'c7s$7?V#5JrO%1TAh<oZrOonXa2c6'"8O&8CB=A[+bBVr5^u?rTYD8h?P4"8Y6FQbKEV-e!W=;f
+iHG,>M&,`MKEV,$1O+:g>fZ8kB`!/.pQ)^,*Ok/S*S]"_rbOm-rOqOof@c:H>>&5XIb)lMf$Ib3k\ku>
+gL'pBL\eHd5s]uPTYCMj3W>lXqJ/B[7WZ;#<H&mbKk159`8BH#TnHN!L7HrMg#_>WbKJ(TL8;Gr)U9:2
+^?2QV^1M"jCMIUG,es@:"R4Tg<c(J[(cJf[\)-4q7t60,\V#J0)i"<$6$%*'Xr85u6Rp5-/hTUps6&gR
+WQpuC^?2Q>^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%0</M+CGt9Lgk<H73i"&$DKgQ
+5suEE!>2jcJjD^-^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&CO<V_<\(V'LF!=7Du1(Cpl$;c7(2qZr;_KH7C+mOGHgIE@NH-/PM[U8dZ_Y
+PHTo$Jr3)B,iVckS;PI$$PV]nP!V^o@0gdG7ALJ-E@N/-7:ifZM/uE`+f%mhKKi$sU_"/Y.O%VVA[@)I
+F&#:MUobNOgS#fh#`1MU&Z;A/fttZfVkamP((B/=;@t@27%B-j;c24a@H*eU;@$tE[dZ2SL_N0":.<+#
+W,/mrER$!^KX;UaGl(H.$6^"Z,2uR$2HuP.kEHZIg5mJj,:XmkOGHgIE@NH-/PM[U8dZ`DB*dsjMF\m9
+i`&=9MTTN='BbmK6:bl/_RlHb;@&+='bR@<ZJ!+53N&Ya;Cb+ZG>ps;+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>
+S<BUZ6aDCZok[(X&e]16,#1Q9-Vg0K)B*A)H0<JD@`,p)nRDH^K7eg>cCJabH2I=&il^H1T5XFpD.2Fo
+;c<7"8;;)dhQ2NT66]/n`2T^)iPUF[Ytdtd1`ok'=XbC0+!4j>nV%>ecC?oDI.P@;)o\)#Mi4(<F'G"k
+kEHZD(m6&4Xp6JO$pl.57Du1(0JbL`/Ck%:*je+UnDM.+QS39%ENN:=q<+CMpu@FIGOHS5Eobtn^YRn?
+n(YD=n8AC2"7:OEFFtVCWHATXqmgg**it/Q#`1MU&_G2@2<X8soQ7?'Milc*@Jfot/mZ&qiOljMokJQW
+*^>,Oc'pXrDrE;I=4iQ!rgOW?H7^D7'J;bV<CBE37%B-j;piW+DW-Ki0-3<)q"NZq?WgNWp?^JJ^qc'W
+8%+f&[P2^H.4KQm,X]'e4]N*U&JB(5,#1R\\[f7fIZ8G(Ib/rhn[j#6C:2bK^\kCj41Bp#F&#<S;^'i2
+-0_Cd.$'IG;\="!qY8B4b.;`]es]*4J!sEUrVQ?@Ig5_<Dn`c,n`"0Lg_M6^3@.,_KOb@TVfsVb<'hd(
+7Du1(iRFi*_o'C:2`LW)9$a+o0-7lJj$rX&jlPS+#7b"PH?HRgC^Up>S<BU:68F3eeKYf+2B`[1,2uSO
+0Jt^fN7P8=Z$O+_jN3Vbrr2%:St2Dm)&ZRZ=D5?miPL;gpBqd=/hSad%1RaZfk2sF^>/'<gVUlhVkiO=
+ODNlRoghN99h<4%Lc?,,NK0%MC:2&WLE7ET>Zt<G4?bbc=ZeH&4l(ASrVQ>U/m[2GIt.(Q83_FmBT%(M
+$PV]nP+iaZ15*j%$PfoOKLD>fLf70Oe^`4Gq>'.[roB>Ks#-\X>F7D5PSCeBOtBqI?DrZC+=,rG7%B.W
+Ad]:8S<BW46Sa>h5hB)Uh@L<>.O%VVA[@)IF&#<Sh'!ZQH_\2'@a>=: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,p<ku,2uR$2HuP.
+kEH\:)Nl8VQb'RKp6^qt;\="!.[r"`S9%;[D@O]'hFl[.&UR2B`2SPP'p[#Z-I+Ut79>4S#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&#<S*?j+QmF<3ld<7i#:.<+#W,/mr
+ER$!^\f`7c\F$l#,!(::Lf4n*.Opo>9Up/rMCo1V(2g`:W/+1k,iVckS;PI$$PV]nP+q4b)8_r;l4q\i
+KLD>fLf71(Cgl=n;c6RU,X](/<4X87LQI<j&TK9MRk7=(F%V<^LL^d:Le:`:k7A8G.O%VVA[@)IF&#;(
+h2i'(c;EDTkU8bA:.<+#W,/mrER$!^WZ.*Qgc;B=&KO;2`2SPP'p[#Z-I+Ut77BR!KKDdp/#(JuM)Z4^
+VkUKM_SeI4Uo7rt[W"a9BLR"]+VCUri`&l8>F7D5PS@++aUmks<NLud,UDAK7%B-<XfehPc-<qMqt?4T
+f'E",:S7X^kF]^8[_C0*-I+Ut7281qKf`!D/#!+PM)Z4^VkU'J6W'o7pAOCVq=@6P9:K3I2)TO;aH=K-
+rVc[N1G`7J>?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>?lI9baIVgSXl<WoC^XLfYYQeVkg7`8;;(2X%Gg&3$Am3,2uSonXgm4]q)?h^4603ETPH72d7BZ
+E&BT-_SeI4Uo6mV[W!Up8k!h]+VCUri`,R?T:_?jldi0qO0&@tn+5_gMoV`mVHP'(I/'ot9W=O\;H+hc
+6b($6U*;XjOGHgIE@P]_Fm@KGn8S!D/UjVGdA'39J+`<sf<7.2M:52-0QGAe.4Ppm/_o;7;%]?p8AqK`
+3#+;E9hc@tF_klq?EZS=nta%8^H%9dB4m.m]<JNdGk:(BS&LYfUSB=i/$E9mqYKN]Mi6K+Hah[T[Y9QZ
+:.`k/+W8.fYOa9=&qiZCMMP2.;_`3+[9DV")@[/d2LeZRjQ'kAaRo+Ho?-UnnO&'A26K&eo&R,icaJ)-
+[P2^H.4I;B,[7eMFacbYJrkde&TK:8[CWq15<lpUmgF$3fB^BY?<gb5jH[/-ER$!^3]nZT\.Uj8,-!t#
+`2SPP'p[#Z-I+Ut7>HY/$WfZM=%$$oM)Z4^VkUKM_SeI4V!-m$%,n5MjGKlI+VCUri`&l8>F7D5PSBZ.
+OtBoRSQlaA+sc/I7%B.WAd]:8S<BUV6Sa<FUgkpL>(f;6MMP2.fkb*VS5W=-Ce2!RKlh-"</Zio;\="!
+.[r"`S9%;[c4%f+Y*RA7Lrp;BE@N/-7:ifZM/uE`d?u02_U[)&'i?tX,#1SFWMjXA0QGAe.69..6b%.t
+YTg)<8AqK`3#._'(+.<e,t/&nA.)I6#?F5-$&LVV&Z;A/fttZfVkiO`O::(tlElGI-m92#,2uR$2HuP.
+kEH[O(6U7S7@dHk'dY-b<(*76b%;"pkF@^0UI$_h/#o?oU:Q"GS;N,$;;#boig`qFFAtf>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@<ZJ!+53N&\:KTHW$Wp>=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/U<gl=oK,iVckS;PI$$PV]nP/7t]cL$E%66]/n`2T\O[P2^H.4NscOs/UTgl=oK,iVckS;PI$$PV]n
+P/7t]cL$E%66]/n`2T\O[P2^H.4Ns`Os/Ulgl=oK,iVckS;PI$$PV]nP/7t]cL$E%66]/n`2T]clK[Zi
+gt^YnDmt#jdaHR=7ut*m`YKoH3@2Z$LL^[7+1EIm#`1MU&Z;A#i^6SDeb8t/r;#s'cMcg2)'0h_E8\O_
+f@[sY3N&[WKo'k4,)),7;\="!C7(HY.RrabjCGr1?W^Fon*_>9);4ki'GhR8`>3%r32`S)j,a].Y2jao
+$6^"Z,2uSOE8nfmTqS1BRs4;U.5pON(G@\YroM=iI=6NrK7gTdHb_WfKe5uggCYlf7%B-j;c2MkLC_@5
+SX>dX5-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><PnD3T7W>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+Ut7<p$o_S*kSL_N0":.<+#W,/mr
+ER$!^Cgl>YN^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@<ZJ!+53N&[/KObA;i-%Kq+VCUri`&l8>F7D5PSC_>aUk>km>)HC,#1SFWMjXA0QGAe.;WGPgl=oK
+,iVckS;PI$$PV]nP%&VY)8`b$4!KB?Lf4n*.Opo>9Up/r$<rlW[j+cVM)Z4^VkUKM_SeI4UtA+,L+mDQ
+*K`ZZ`2SPP'p[#Z-I+Ut`D/cUKNZXgL_N0":.<+#W7C1ulJ11MgUD)cH0*2k8o`takKcA<V(GB2F%QdD
+QoMQ";@&+='bR@<VW's?X4?qBmJQeO91qns?M=BsqsV<"IfKEGN@U&24^*g(/`*T]9Up/rMB.C]#dY\`
+,>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
+<b0iX83SKHf%c9V+XH&H7%B,umWJt&I/*3XTgNJ=SpgY<r>(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(+.<e-,kT^)8b`rgCYlf7%B-j;c24a@H*eU;V8k)\K-f!8AqK`3#._'
+(+.<e-,o_B$t4>%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@<ZJ!+53N&ZdKOb?%iH@Tr
++VCUri`&l8>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,#1R<Z\=6W:5LZX8`Y%_$6^"Z
+,2uR$2HuP.kEK7f*Pt,e:.:ri&ofBF:.8a#Ko'kGfpt!m&Jc"!.O'S.Lf4n*.Opo>9Up1H0!,*$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!B<D(`4[]h9o5$6jb(LV]t,H\Y<AGiT]Uk
+@LF!1\Fm\P^:m7,0%n*,cn93.40lKq*jefW.>1*ZMMP3YNnD36>l",oNOe!]V]o"EM)Z4^VkP3"S"&0s
+f1>el=i<=uU_"/Y.O'S.Lf4n*.b^$):Wg7kldhLp>(<!l)R_>>c7(2q7%B-*+XH&H7%B-XlI;e3_o'C:
+2uQJtp4.3V$6^"Z,#S]'3#*%'U7IaARO[Ief[s<>?@VP,8Ul5>lSMqo(+.<e-&%G:;\:I37%B-j;pd4V
+NfM\*[=*ND*#r3e>2%tF>F7D5PS?G.:.:ri&ofBF:.;e$]"0pe5JQjuDQnhOF&#:M,iVck$6^"Z,2uR$
+2HuP.kEHZI,#1S&&/&t4,#1R<Z\=6W:.ZTSi`&;Q+VCUri`&l8>F7AbO[;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,<Nk#H5(3:jT77W+B_QSA_A`HW]mfpkh+r\rHM6^YqNoc>EjHu0<obQNY3X]Cf4q^AIli/ZI.7i
+Vh9Un7H2mmBpYUJ8J,fL;77WC[/P]ukT+og64?S#L^k$Yn$S#k*C?im&,E:GTIt.LBbY5X@&XE";:Hp`
+:>s!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<GkpplG!.._^t@:AdFH-i&)7%
+NW1MDad4`94o)8I[[P5>?pda`Aj2XSZ-Vp?3ieoIm?G5Vco%8YjV-:"po?TZ$n])`jMpEV<NdU`k\*\W
+B<'[^-Q^=%'r]Omt=,`L$Cu8h>Xlh!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>,:Q<N,J,auXBE,D(kg_"A<K,fmhoPW]h7M[,oZJo&[..9r/SeCGQe^rEAm<n\mOX?9gV7m
+ses],,-ZjNHFEFl<kK9&?m=$6Ye^2[i^5Z5LCu=jIc\lD4VP[N=4FHq.MV+'2IoKSB.D0T5q<$\UMsYg
+_<6gpIj!?XrYi-AkOuq9p;l>A*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[l404<o3sGKItEN(%E8
+)FJs1(3dL;WZY.V<rJ[\Hp[@!H\o_Zdrr(*6,Q6iRUNSjNWarH>DY90^m9<MOQe^\*?!X"nT6DO]kQ:F
+']pQTnMi6tO,#he,eWCpq9d27$!<<*"zzzzzzzzzzzzzzzzzzzzzzzzzzzz!!!"LT(W*`QkGR6~>
+
+%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<PNTJ^T?^EDX!KfrQ_PDgb8](
+j<%%0/c2t`k(nIjoLhro/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EF
+Di9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>oL^Jf<tA27j0JUM0>kP",^^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\*aKYND<h*n9YW1U-)]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]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/^Dn<I;gW_.zz!!!",:7X
+Ijmakfqh=Aq`o((t9J,UONLO$8g.U#tX3ICJd='pA5?;sC:i%6N4/M8Z)nA,?6*e!+38$9DbRf@V'bag
+=edA^'BDS5MZ[Ju8/o]X[u5Q.rH]Rss(1WRFol.rZ&2r8dh*7?LHD7PPLS2iD!4T"KOJ,JiW0>IGU^]P
+*PGMRDrIEZ3!/mPpUieoIM2XeBWmOe>7m^qr-KKo+U6i(@U1KMo:pYJ!"9\).Ppu-uQ7uj*'IC_/W"6f
+[Yo4'skHKh9r@WT<EC#tq5mWnBF-XV;p<W04om8hWEkDmg]hKuFf]E;tT3QEDK?Ml2'h!^(5-gnM)No,
+?d?,+Ufh's>(\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]6<Sq]6E^r#7hk>I+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,<G^:ShE1iC9sX!!!!ZS2tem(dCff4?5/P1,1Mg\@D=ohr/d5=h+0`^nkW"(LDA&DWJA/!!&*cV,F
+:B5I[NfIn;07_G?acWF*F,mgH<Q$TE<9!!!!im`h4[`U):,do1bcp$:5kcRk3;NKm#"cGIV_+0VfM<ib
+?>Oe/)5/<B^K(2iWQ!!!"\>VX6a^S6)Bg9n2XDV_n3FmIU!4*D;CT0@[,/mC@EQTs=h3]d-4n`-B7Ntt
+Ogqtk0,rSoHai,o&#!$I'kn)'K3e[JM:W$<N3`ACa\^4-$,\Rt(WI/!'bY-+qu;mpMqEp(`=qK"Rcqne
+=t(B=F8nu5S<`T[0W_hUIZXIjQlH+"QOb<L?=qmlb-!734=^?5k(!!)>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&@`2SP<KL
+D>f84#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@<h!?_7.O#0B&Z6h9&/&t4,#1S;fj
+Wl>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+<pF/Z>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>(c<OI=6OGIL3(\6>F&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+<l66]/n`2RFpbec2rqii@ZPf!\CJSS4Q&jdd;7Du0=c-4$]NVeqcKTt&B)M)['6k
+Vg1:.<+#-o\IdEUHbg&5kh6E!e4sLa7XAS;N,$;/TuSJG!OW:;HqG](%`ZXaKR~>
+
+%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
+2SnaiGKn7fpJjfQj<XKDq/nG]!<<*"zzzzzzz!<;I7ctB*Q.XY/U:Hh\k7H_d*MPB;sq+OX"SL:$YMPB
+_'1IY/_Sp[C`MU%F3'LgDpnsQ4#3_qq<'Lh3,B8WGI4S_ZJ'V-VD.>jkkj>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;t<SZY8c(Wf5*A6md@(pH$t*hH[D7^M4&!KId*(]Q8LNT1:"I$=lm`pnaZ
+;Fp&%V04B(ZT7</@-%u'('kO%gZ7=P,qr)-D*nsatp*ia'1cd0qlgY:3+dt<:*6UO64oCM/.kO2'Gq-@
+-+3BfX`g=lDn-9Zd%GUSK_k;Up_%oSt/\[ecAX2!5hf^7tb2i@P,9\Wn)f%.j\`qK0$cd'fG%hE;bAql
+(rT:VX35P(@r3AE7n]Y*$92H'TgB4n->VG]=qeJ-?K`Pkg8i8>`S2:[)"H0JYM@^iD%h^J'M1T_UAI=5
+C)>&#l4jXWhk8*QDie'k$9^QXkFAROuqn'b],4$2P=b3XI_b9ACth<S//G'3?p7=KUGURaS45+\:7Ci!
+p`A($"(,JS*1L!rK]lKW,pRf:X!9,AB\AW\V<.sANcq5&u$!+p!#bK\:+]'`;-9[hGRkifn#_S?EPh:p
+\"IH.Lij_F,[X38]0Z:L>Ys4sUDq\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!<A4T'@6Eh'<K]!28$kgX5P^r'MkGAr(BF,B(^eb)epqi\3A!)ZlJe/!Y
+6AUU5Kcl<U7L!@]5R%P$m,FU_S'?2c1HRD]pG_9*RHMC&dUp1a/>(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'?e<AuLn'?+Z\?L]V^uB42mU^nLLHXn/aUea=IQF/jA\NB7BP7[jIB02
+0lmC<Y_?KlZS"a;0lur/NLYmIWMh[YR%/6?2"U+ln#Tnfb]Pq7\VU8M7lHj?CZJG_:.;;<_W74c%Q#mk
+%[k,EKLB^_MPb=N^BMbLE]G>A,'#/Jbn_YhS2+g@YW-"lmh$2CALa.H=#Q#\&NLSg;c.>=0KJ<\_nP)O
+Y"9c^E`#9sHliH[_<$KCMTDcq2-ZH<F1SX=H^`f:#DA2[PuM9+j)RO_7H@FtROq4Y3Sc+N?E$Cbj9dE+
+j)9d6ijIlg6r$23<7I?g9[5Y(Hi%^\)ndk`ON=i.B\!qWLY%d6&NLSg;c.>=0KJ<</^;$.Dt1E?66b$a
+U;IKDCW,=jS;PCeBF3!Kqs9I6qVZu:VS0;f9[5Y(-hD^iBVTp(+d#tp7>H8G"Dbj8Ln'@DWG#.9I.9Wa
+D_GJ3r8*+pNRQbA(+/47Nnk]PG@Ck;,:%:[X(^c62N$+<b7Q`LO]0Z%DVVbLc.a,rG9:%VS9$_t1LF%&
+72'p,ON@)kO[ZpC>`Qn)Ln'><Flgc2P:'/Os8KG\3TkKh%XSN?<DVO966b#o72GD3EB,5@,'#/:eB`XD
+o<W)Urp48^48Q(#kK_nuIt,K8+'IBgn6K=3I6RiEGM#Ogs%Qj]:e9YnYA!Valh@Y=^YV=e%B,*<-<IS0
+(4P\^kDgWKIeE!V>ISKU]6:=RmHN)t5QCZQ)-_?rrt+O"3`fLSqmCrUrE$c>&e__U<Aa<.SGI\R*+D/V
+0j6%NVkQW9\8gQsh7J$KrT'/<]!_FT)fJDsIp8;1F#k_]2qi?P1FPR62n4`Wr[:+j&!!Un(D7sg,'#0U
+rTHa9\p3c)[/\Yl-KgommcdsBZ0^@>66\A8M?`ZNq93QG+YbGr,,_/h4FHqnp?gW3Sj^0E_Nul\ReEr;
+_W769^<F*t*mk,I,:%:sY%Zr?2N!9$a<<8R.O(dbL$X7YoT^q.p>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,:]I<Fb#g;;p@dou@U"Rt.3hap-Y2q!S\DIJI5-R0(W5uA4S't%W]
+PB0JlbjuI_CH6t?#`7?h.lqn_)oX<dp5g*m'p2ElCUPoWkB(G0^C]nGmb^T&,.B`N=iA!WT9/8tMTDcq
+2-ZH<F1RHIhZ`89=%ME'XWgi%Suc-eoKbq,<7I?g9[5Yha,FZOEJM/!8EA@.,(F.3EpFSUX/."\.Vd3D
+-Kgmo1](=Ih9YL6,][[,dEJL$kki2uPr0)/Pu&&Q't%W]PB0LnR5oiMrY_r+-R<^O;:3UcR.'pgXbi#C
+M8iL_:.;;<_W769D\h!?mT6;!'"JIOB]_sKp\j#>DhCBN)&894;/<To\P58co=P41]@/cWE_'0_[#/+3
+)VMKZBL,)J<7I?g9PssYkr^U"ls=d2)]04L2RXff\CSZS6m3;-;c.>=0Y%WQVt7P(YWL1LL4hrn)d\Xq
+>\1f?U4eEFWMh[YR,!"[dlCb@G]sPirF!Z_]gH6d<:nGZ7I3&#k9M<n.O(dbenec8lZZ)OD]?BokK9l5
+8?AD?-69aQ?+f;B,]Y@YVkVaY@<1[R:8e%*M@$pNlOad.;UNr'\Y='T+d#\83>Jr6$^2P&3M$-/F`&$t
+C"+:\&1X.2ORI;M]7`RI7H@FtROq3NFQ`;#rVN-;bfYks,R-e.LGOhN+=(tW,,_0dCgh?o3N)J]"L&?j
+UToKjOJuo"(@b'b,,ZW*.Vd3D-9(F83mUG!S%qBFn;nIj'eO7aX2O]0i-HUnE[i92U=O-hEW\G%o=P41
+rAuh<%#NA/d?8*J/,Od74V/5d'p2ElCUPq-iLJAB?35@]Ds`9t3$C"_.S<G8F2P^6fh)R-S;PCeW&7eH
+>efHDSlYsd")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`<OE[khK(++7`3M$,e.O2S^U,a$;
+E[huhR6Lo)pObP\ON=DWS;LLVKur_Qdhtr,AHHp$(OU6Q)PWWq$.8L"4.),NWJI(%>-iq)c$ng<aXmoW
+\DWQLd;[?XSUeErgjVR6V]p.2C<[N+3HOQEiSNWL9J6,$<&AQ(q-kKmE[i92U=O-h(,F3/!"\S%L/Dp+
+-`<\9)#fj4+d#\83>Jr6$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`<OE[khK(>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:&/$u<D5*HU#drHC'G<D@O],+m[dYtFqiZEl5J;#2.3TsW.b\"2
+Ik59\;(Nu5.O(dbenj;r3:r.(N%NgHkEHTG,,_.E=1*j<6r$23<7I?ggm:I%l?RCX%ukX&#pGF0&\GnK
+/0o6c,,_0dCuLX^oQd$pqkNLa32_uP&ig[]C$@ib((rVBO],+m[W!Ha4O<VT`pZ+:.4'X8:.79^l5#u*
+LkS5l7H@FtROt&!p=4:FYBeWdF%Q`^O])iJ*:fqPJrn'QLn'?+Z\92QqsSRgs(@l^Vj%`*3>F98_nJ5@
+M4BIEWMh[YG_Xo/_n>1Q?=V80-%nZ]74_jCf5)Um;\;#)[:Tg8>4+n,B5HOdig`n58E<ag_D^Sh\6(>L
+,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`<O;D[bDqL3CBM9?Jk'eQ-VU`DO?m)ZYBIei(qNulPjk:pl5P4)i9^h:/la#TuLON=DW
+)Dc:eqL3CBK?Fie'eS_Ip$:GH]lrescHa_k]6DSk3HOH5V&A_dW%D9IpU3_2@1pj7-%nZ]7?lF4l&]*V
+R*OfLS;J*Tr:A6A\8YmF\*q*3I^aLhhnMDCrS)2Jebm,J_B5<QS5W#cM4BGO0c?-Y$^Tgn6r$/Yq>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-<tKaJjt.LPBol*>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`<O8sG$?&T9@i,B>8;_UY\gNu^<*jBLfnT7?Lsd>W:XKul?QPRnHf'iC\bUmWmB;>D;C
+86h1rWG"<HgY3fuo[3"?]ZV';kF>F:8E<`<M'aCjI*'M,ru?H.3?kAqenec8c7(&m6r$18BH,CdQ`ak)
+\/^W0;/<To3TlV>66`<O9!f(Bjr1[af7_<Z3>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]<?L.=\7ftug?
+;b@=PS;J^LerZbF@>hW3A$(P1;,M,;0KJ<\&jde?,"deTUmZkf,$pg;=2[A&W&7eHS9%:/+d#\8-.X37
+fo+K2?@au4Lu,,02-ZH<F%Q`^O])i<eg;Z#\4:=5&Ns#hCUPoWkEHTG,,ZVWXC"L'%q.+bgsZ%!`9O>#
+2-ZH<F%Q`^O])i<ehs8&>M(-;?sP9lPuQ8%/POGMV1arOWJD2Q*R`FhC_<";mtt3V't%W]PB0JJ+XD(X
+U;Wp^1i3^A`>(;-,$IlACUPoWkEHTG,,ZVWCg9LHE<@.]&Ik\;2-ZH<F%Q`^O])i<2Dc\;\C?766n&kY
+AdafD:+7>3E[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^<U]\kJ`('8\.O(db
+enec8c7(&m6r$18&CPA1nN9c=:.;;<_W74c8dL9DMC9XDH-D_p(9raqTVC"R@<0XB-%nZ]7?eA3r2+Aj
+NF0UR,,_0dCghA0.4'X8:.:BYdLm$,_OE]G<CBSNXkCSPS5W#cM4BGO+f(r/h85YjLfHi<7BP7[jIB.@
+#`5(/-m>Af)o%2/6r$1Qftug?;b@=PS;J^<UbWn%LfHi<7BP7[jIB.@#`5(/-m>C4Rl?4)6r$1Qftug?
+;b@=PS;J^<UfO'*_JB(d`$ut/U=O-hEm?*[KLAR(PRp_s;mu1a/RfHW;c.>=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?k<Ln'?+Z\?L]Vj%`*3>GCX8-<l$(*:RG<Y7>,
+[:Tf1F&"/-,]YB/KTHE"i#t5X7B@\R.Vd3D-Kgms6:g0:6jY"Ge>O.fMTDcq2-ZH<F%Q`^O])i<KJ[?u
+(@dJUZ&6W%W&7eHS9%:/+d#\8-%n)b_d:82,,_0dCghA0.4'X8:.:BYd[3^r_nEehPD[G\W&7eHS9%:/
++d#\8-%n*=;mpY+Mc`F/<7I?g9[5YpKTO9SL_<#mZ)<=q?_^^ljAaWt/POGMV1arOWJD0R)S>6LG]epH
+7H@FtROq4Y3MhCjaJjt.6<Xou/n,QX;c.>=0KJ<\&jde?,"`,TS]kEYm7+@<aJl*q>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(=<?[U8GgWPp:^Ghj5j0\QiKul?QPRnHf'i@:g
+)e[/7??R$`Ln'><FlUK$DViYU^\c[#jIB.@#`5(/-m>D2<O[ca6r$1/\oQoTace+8It([iS9%:/+d#\8
+-%rYi.Mf:HaJiiM5QCQ*c^m;sZ_.3_kdSj.4*U`(r>G'RA8uP)GBK>Hr=E'966`<O8dL:!JQmBFU&V]]
+aDGrdbAH5$SpTo#n`*]S?G1LIf!T;F]&0)CIJWQ]5G,37Qhq<0Su$L^8O=W!,"`,tml4tg+8X^0.O(db
+)7Hu^*_t@9j,G!:mbYXhpHG3snk$<6hg"1@Bck8/-Kgms6:g0:6jY#VJ14]Ig*0Q03>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<i_*cj!AgslOXkCSPS5W#cM4BGO+X@CgS&7]MF)rpJbn_YhS5M[EjA\MR+d)Y+72@W$E\2%N
+(+/47;F3q8<(%$9OkX6*7EjarMd!H@aZ68`&/$u<;*mh7%^'LA,5)pK4O(:42-ZH<F%Q`^O])i<KLG6i
+M@S@W?C7+.>`2dlKul?QPRnHf'i@:gjQ<K+THS+`ZcBJP2-ZH<F%Q`^O])i<KLB^/MPb2uM\t'?enec8
+c7(&m6r$18&/)NB/"/JSNT(E/>1Aum9[5YpKTO9SL_<#u[p2?;0p>k#>-iq)3N&)Q&ig\(6:g1SMC`5N
+E@kqO(+/47;F3q8<(%$9OXmEpi$%H@M)U0);/<To3TlV>66`<O8dL8Y@7=,(YXQ4m>`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;/<To3TlV>66`<O8dL:/&1X-_L5k24(d^[W
+bn_YhS5M[EjA\MR+d!^=72GE<%B#qCAIF]C:+7>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>K<clCu2Mr<X
+R2)ie/POGMV1arOWJD0R,.HMGbo4Ggn_?%8U(sndaZ68`&/$u<;*mh7<4(tQ/Sl-IUnZFV@<0XB-%nZ]
+7?e>r3>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<F%Q`^O])i<KLAR(/KDKYf4u2'W&7eHS9%:/
++d#\8-%nZ]78;%7Ku^rd<RdHh9[5YpKTO9SL_<#uVkV_0Ih@b/7uUHtROq4Y3MhCjaJjt.66`>%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]<<RdHh9[5YpKTO9SL_<#uVkQ&UlkWoR+KO^&p[6l7
+o#i]\]lACGqp(`o:VZjB[P0`)PS-;,V]n_=M4BG]6El.SrZ:Fp;UK:k*]uE-o&PcL2^[G7IF=,r4*K[h
+0>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&/$u<OdiFcBNqn;a.MNj%eabu>ISKm^[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`<O8dL9DMTDcq24n;t.\_TXaJjt.66`<OE[khKB:,3H-m9jV.\_TXaJjuc
+MH]Th-Dr%[#`5(/-m9jV.O(dbepN4oWJD0R,,ZVW#`5(/aJl*qT4)aiU4eWLWJD0R,,ZW*.Vd4@R%0$^
++XD(XU4eWLWJI(%=uucc!!%Z!pIYVizzzzzzzzzz!!!#uAp\n%cCI%(IedR4%tkPLg=kgAqW*i9qL0Qg
+K1CtDrTR+-lMFF4<]`B)J,]&3s6"n2Vu&p?O+e2#rOV^ukF_MI5T$>Ep[11mC[IFWKcF3W>OG[5^A6or
+A&jUWlfFI?mVY>fIRK.=j2[5\j^8#Kkg?0;'n-,ZhsX<Rh[bTaHf^MoSpBId^)mNRFT)(/e#-"1?B3=$
+3HP)kmCjYSU\)PFDsVmXqsf_sC_="8#7hlG4F-M;N>n("FN_oqT!$-dGId.5U&.Mi]68%(BBS^=]A7>A
+V+[0OISc!IX097k*S!CPi=B3_T7-Fd5Q0%q4o=TPh>-I8EA:MM'reetfto;6].<+)\U=5DP:'/?pu75"
+Ecg<Z$%E,rUAjJnri6Fk\%hlYpN>l[]mE!=_%0jsiJ"uf'Vj&5e(mlF#u.Q>\(iOq^l$ofQ[A`-k:^)M
+YkQoHhg`E<SDV)2^W2,J\86#!*Be`@br=P8HV8S;fi?PX8G)\U(pC^PjY,G73Ih%@/^+iVqU;(.4*PT2
+\"A-Gl)@UZ55=&g*Zl#)e6R[;S'8S'^A$K2h:FOAOiX6-S"_E7Y&n$c?@;:^I4N;h2fkEgO^%6o?[r#s
+pBc61STjeadX@:j\%M\5gH@_M`oYUC8&+rWmlpX8Zb<haDf9h7rT[J!lL@_+;EHs%5P`DsIeh/oG#ohk
+Qpj/`nt`nmr;QH*n,%YiH5A5@m&id(^\uDqIab,Nq3;F"o^LhW;l<ChqU`LMIPJBBgi>3cBbe2?/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:S0iLci<phI6T,LTNep%Sili2gGq(#@bbu[^M+EGbq4/'
+eu`04-ZIu7MOZYGijIBElg#]KFru/);4!@dJRJj,J,Jj)OQh42$aYNDs8Mau6Kc[eJ+:Johg+CU?iB7k
+h<!k@5Q7p#P@b,f04-p6=$QWA&&7o!MnU8FA$U$l\E.</Za[/Cm[So$XZugTO!d_pr97",qY9U@^)m_-
+oAn#W+8k%%s7r;e\bfT8QmM[uGJEIXW;Yd)5M7!j++3Ro^@+^JH2QY<^GpB$<-J_][C(/GhS"!ncCI.I
+g)O^R[P560>M(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;l<U56?`B*A`$kRrSp-Aj"GlOMnU5FUU+mX
+\E.</#B!tZgpPG!4o<APYO)5='>1Z)\)2YHb@^<*hI0:9gY5pGhKnLiB:aCGqaL8Q^)n8'o)8$,J+AAm
+56(P%cd^+3G5kD#2o#15hHQR6H.#u\/ml?Jj.g'gSmF-h\QRG2'tNEY\,4&"h/2?#cHaHPIsgD&RE9LW
+L\gADcal2k+9(bbmsWgU1EaMSr4F`,q<`*MgO2r2HM-RSf%,HY%pjSHV=*['='pC+pZ#^n/mc3#qt%\\
+h,3oXm_XYccT_6m?iKV_^)n/dnaq]YbKZdT<TKIXOm\h"fWequhi;s\r-9q[_*[",pY>qd[FL>ROKaFA
+=r5N,P:(Q-6nc4B_S(<jqL1hkcf<',?[M1JUIUC54cf6-rq*,&*u&3?mCQTYE\kIY'tNFDB7#"&UQ7TZ
+,QV!L4$,O["TIboch`)5N8*dVOf+8;OBZT&zzz!!"q"rrQeqi<]~>
+
+%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<n*el_s829"j"IQB^OGk@e`$&_e#+:9o[.sUk%V8iQBl0TH"LMp6Ef9igMK_Loi.;K&+h^T
+nRtho_W+:N'.6Ogk/"mMN1jtdCY#T_o?TY%IeS,Wn\3N(elL[jgcTK:IKp&]o_*e6q<.O2X&c?#Zgh,r
+rV,3eLCa.OoP!A]Rl6P3lSj1L2>,_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!==k1m7R9Bl<LH@(!=je5%s7Ror^P;3n[A\VEG`mET!hLE54TqT&7U\^\Ar8$H4lM=KC
+V9R909iI?0>B];]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]</uiGbHhDFFG<`7(h`ApD2t8cCR_8FTM['_3
+YRJa]p?LnULRkHIFuO_`N@M?neF1%L14qI@d,i;*7_a,WofKK%Uu*&*pjP/>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<W(b'0IEVN<5-H'[
+)@moR*.<<DCJgmB7uc_tmd=sE<p.U$ENQ1Q]j53'!97]()+2q.1hl:,XdZ'IBPADDCe.k/p?gV03W&^N
+>?b=>=e^&X#BoC^.F"i9*PE8+ZmAA2<;Jg@Zi<bl4(A`WYB#P=,59P^5;c/@UQo<eW^N*b%YDNN$Q#Pj
+9%S7N@^-cA.P=HG&U@hp6G`*F\MO(Jjdb%C.IB#TW[9d0C#M+OmFefnRR%0E/P:GV@f9mI<D,kVdYh;J
+Pq,oKCVP,aU5&Wh/$D?^L<\/uf2k-idI2kSL1"EinWSM`qTWBbpRA\6eZR?NeQ%M")_P<:8/dH7e1b*/
+L^oHm&u'_ak]!C(nb[GW\*EjMQCfip5-BTXg9djDg)W2JIPZ_Y;u3B6GJK-??0<Af@EZ@YhRd[]]$u;q
+Lp,[Q=h9XgH5p4C,g^K/N.K2ng)Hg)B^j(fO+\N2l8RKH;Q%>_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`>S<K,FZVns8D]X3^)8-3ZMlJrFQHq5(B5Hh<7*A?i.2h[OqM]?B[4S&G!Yi
+#:jX>qTWBbpT$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*hP<cnYC\W@dYNhW<Hka#2l-ee.1#B*;#lbMO
+Am;omEVr3]UL3tm&e=qXU]lM-]r(L-I[f"Y-<mrsD8)lD.-S1?$eoV_N=R.AW1'MWUYF=BG'6@TWgd+;
+p5Nofs/[Mo[%;^'C&6(]A+pqi_)M$_%cVSpd;F?j_M)/0XrN1OLU1nQ)e@`92G@*niVGK.#KObiO_"qY
+j*%he[F]M1gFZX<[(!MWcU5JC]1MjidrZ_h]QpjD!-B44q6M]qZY-&jr6atL[VOKtE:P+L/S34LN7N-&
+:=l8U>K]:Hq4lj#Cg^X9(TR#OV=W<jf=-2*p@7\CC$2;Lr6[1_KmrV`^ShtFc!U-nLC?jlAih17H:e$W
+0e`_%l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#
+^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[
+@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:
+KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2K
+o]q3[@7S(=kSJF7l\0dr8&&Bo(O!4[],u1Dh;.H]"O&oD[C\`>)3A]"a'Y4]Y8@%h*W:;X?eEc0(]&h+
+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_
+KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0
+(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]u
+dB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X
+?eEc0(]&h+rHb/Ef[6l=qLosgHD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[6l=qLosg
+HD+]udB=A_KcU,<TjQI"Y8@%h*W:;X?eEc0(]&h+rHb/Ef[\^EIagr5GIR;X0^H*rmKWW%5dpV2Ipkg]
+?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr
+8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*S
+IUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7
+l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?
+G4b*SIUPa]?K`2Ko]q3[@7S(=kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@7S(=
+kSJF7l\0dr8&&Bo(O#H:KmmH#^L$+?G4b*SIUPa]?K`2Ko]q3[@96CB:"(QApg:.*50IU7nc.g>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.]A<H""n6NkNLA>97tS7:+X;Y[]?W_NortH]],Q4RC^Y]WgL+a'Rh\/J#I^p7aGF
+ZcnF+jT#8\zzz!!&C]cCI%@q=<Xs8X#@%5Q("<cJu`GPe%YGMoE#8c^m.<7Lr`neaUSu`X7F:(LMQP3q
+`=3T>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_Ym<RX*!Bhk"='$nJ55F9?YJ0l4qsVue.]Kckl'SNLT7?jNn*h6_IWK6KQ_s`uPkVe.hn
+FjfIe_'N/+e`.Q1cD-lK[Vp5Q7T%C$jW^SXl<4Oe/)5F<,1-.dqRlA0bu=4f2Rc,I&'LaK)5MpsOH\LE
+D!?em!(W>3rN7*og2_\gp#:H0CrWp?^HGT$f9lOk>RUqs6+%eKn<3XKHsDMH]]m@q/,Iq40TkG'%hRrU
+neB2l3^]Don.gH>rTE=l44.2G<C*?[hgISpp7P-h2J(#_)U2QT[gia:_^kVl(p8G(OOUA4Z&U^\mY\Sf
+U%@1d97Vjic>8l7T@'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`!A<As74Xs6mEY
+Z[!VKiag/&f(&M%ju$rPq%E`.zzzzzzzzzz!!"EjE,b=.bEl-d*ZZ9gn*eT[%KHJ/;tp+ZkA/\P]mfp+
+p;YqEh<jUs9>B;sIJ`O'E*[Yu!!!"6*?6'cm-Jg%n*D[>\$suFo&@WHj5]0\+$Xr;E<#q9Se#iJ!!!!g
+UM#/Vn`.Zb7uo90^3o7tX\404_DcAf\9mu<ad]J?^$=J5e#,u)]C;K4)pY?*8dL9DMC9VnE[i92U,.5r
+qXs^XlVF*7l2G19(]OF4HhZ[okKcr\ZeLq\>Fn!)G2A!pKup<M&jde?,"`,tS;PCekY0feqsE.IT>-`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`<O8dL9DMTDcq2-61#mdBMSmfJtD8E<`<#pGF0&NLT6AeufWS5M[E
+jA\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&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`<OE[khK/kjPNV8UB,'J>OpYEH'EKc=<r(,i$6
+ek8Qp<('JMXkAEM3N&+/BLq)Cft4Ihs)m.oGN%f!:3rWL0fD?BV]p.2C<\A)c8h`>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
+<UIZshbb<)'`[iQ3QK#L-%nZ]7H@FtRU);NF%V:hONq("/AIB'GScrYMEMqV6:g0:6r$1QfnIO.Vj'_=
+>3Nr6p><t/[Kt4ME't!UO])iP't%Wi-KgmsU-GIL(W9L+0:'H?n9H[ZL>b7uO])iP't%Wi-KgmsilmM0
+ghuJn(Rh1fp,oO@;\;#)[:U3Z7]:g?Z2aC<J>IMHG7agW`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"K4<c$rL^kRLK)"OX63e#5iAI>s&0_\u>s(N1#XT^r6kBqN,/.h\6jF#/O-#Ie+<Elq
+Ug$A\89g8Q&D?MJ9<rL->KdstF8c-k_mZn2mPo]/Am]I6NJNCHLkpkCLkpkCLkpkCLkpkCLkpkCLkr"7
+Iu*<DZ27b!9<mSM*.*fRf^^<U>J96-Op<RWpKO:4L\'\(_3QHglpZ)+bEFpZ,.k)@8K<)>?.e]nNkNLd
+Cj?Z6?=Xk\4GdQ$Z3sm.`3C-3Ke6*[fSf+5Q7$\=7<W+_P;r7[]<LBg*.*fRf^^;J]r5MWLu`9P'/I<S
+CUq]H9UK2[QsZ#1q,NLO-!?UBopB*LL&"`0q'B12?hKdnP(i^3O81))Qs]NR?]p-=<R9@oc0+]-"qM4T
+n*f`)eRgQS<;#&kEOf>W<%>;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?<UO\IlobQ6N;&9>].#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+<Hc,Q-,+aA-P'e)$NNm6Y:\oO;VM+$XL:#?Zp0u'?a
+_TfSNr;Q<>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/,L8RGX<QF++]R>lMRU62emro.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-=<KHr%=dWRhQsZ#1q,NLO-!?UBopB*L
+L&"`0q'B12?hKdnP(i^3O81))Qs]NR?]p-=<R9@oFj0Gen[;Aq'J;n%bn;bGl8^r@%X<G=V,E.<,5"0p
+pA')fDn^Xe?hGD2hbJZMCcEF.GW^gNU/n<0$AD;)jEA.(hL,2S^Zm[oF1\%N,.&50qX\2OD8sZ/Za3Rp
+;C[t$k4eTCg@U7HQs]Nt0Y\Y#r7H&#I"$OIs3G.[[9EP%nP%-g5$k(Q?hLua^Rb&q=HgY?6e(p]%u[k4
+o;eJCoBVe,a(c]'(\k/Kf\P=qWuCU,;qR[-H20>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-+<q:$LkJ$H%\`-a\&P$(WH<>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,%k<FX(!)-D>6AWpu&nC<Mne)C7qm8J,r?<1>MqYrVQ1F^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(0<f97Yik&3cdUJ))8<AYNPp1QTS"uu@>r[I,>54fL3HEACRal)5>+Do>
+CG\Ij,=oc78s,+59i=r[/j-5t?4MC^>k=1l[2u.+Ao<r=:D$/(&okH[eEA4@kXeWg]'rPmBYh<$bu:(]
+HB=Se`SG2j73j$I`8sUR/ZMNC<1>MQIXUkGBt<@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`NVji<cd<@NROe$:O9/r>hj"B.!UbWo9<k[)C,^03q\pcekD0]bEX8U;4F@p=dtnM
+DnYhpZ"(hGIJ-fL\<Q7@4FI>:ODCH9XH%q__X>g.N`3kW#nh6g;IgkaP8#V7cCI$E:S)ssSTcE!/CK5b
+hWAo>Q9$->f7I@ul*;@YVhh_m,iWj@a<oC`.9b<bk2s@(pND!8`O`9`D9!q.4*0Q.d&0^rmYi%[/:'4/
+'X7jo6sQ`+\RoX,Qr=\M`O`9`J-,l4lX##e8o8'/&SiL[`8sURX0jiWdE6Q>`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.Znbp3NkN<q7W8B_O$[k=pm%7%Q&I1]e0@N&j[tZ9[5LV(b&lT0mCW2Ke5Mg$
+5od5*hbhP(i)aO=<&1Hf;3)h3QRbM-f%MB</(94HnO(.#(s6ie1S\!q6`pU-5*MjuZ)*7K"GsWn.<;iI
+-7PstZ\NT=rO4Ds3^S`=iT4"#CVd4#U/]%_;FDg_6sQl/fl,Ri7FcPSE2Lj*/Tdm$G;r'8<+\iS?$IZ-
+:N7UQ&N'h,EOetX^M/lG<S,pAq2(G*YEeVuNs01GWm`WQK"iob:g)gP&]G%hEOetXZY>U;<S,X9.<KKc
+,oPf!R(aTRpW#`%ehZhNf7Lb,-;["'9bCqC&okH[jD(ek;H-pT%n?dEN8a22Hr;Qi)0.>(lX_!"1?K9J
+O<>.%.n-n72G$bgNB`rtqc$bg0FW:%QG";$s"ilULG=c\)0:Z`8<AYNPp4sVS#!!oQ62-6_'p5t^6O+q
+FlD#tU`r:","`Fmic:m:*n)GbWiiab1]7aKH^eGc3A9#br5kN,IB-T\rX_c#3%:R-5p@p_MJOt`P+q5F
+HT5mGVrO3/=H-s:p5>bg/bh[eRGt#u+p<b4>UctpZq6.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<!)8PsX+*f_2UNU\:eB:[B5eCVjNi"*u3>+$(%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*rnH<q
+NgHZM&&USl;Da_;=^8,Q#+?i/`8sURIC;jJ<*eU@1TUQL1k]Ajjh)G.!M;b/R`k1O0%XY:bn;;]Ei$iH
+dO$ECY1gd]ob5/)%3C#ZWU7LZ:ZC:[OIIP<EOd\@BU0bQQRbLJOZ#d.Fep1?r]<04jphh!,S#8)ko$/u
+jgo9$=b9%HXq]PXg*B,;Q$YK."HSd[\c[RLI8[EP,IE83QOPcEOGM)F,5Z2GUo:j;%n@!K2:$L41-C1,
+cCkD2kb3$&2UF!@+tcrSm'CPlU?VdjB2/WuBnU^:c[+n44G+NsT`mpLTlOEscn%I,/m!qsZYbl"l;i<m
+.PQA8l#0agV_?/+[(oM_Qg:r.AdK)i\Pcu0BTj-1\)bH[qf"-S^Cu3r-$f?MS43l;]^t6rCnOp_6sQl/
+ff@b37Fa!]E81i`&>IOS4O7<)1MMEfRNpEq"3hH(ic:m:cI!sfWpX:+BDN5/HQ2s]bBYfTH(MW1B@7%f
+2G:3r#+?i/`8sURD50g'<8H>bj$k^5-Q2<ooA&FfP=?Cn=`Re+cn%I,/m!pH)f:b4eSK)>?,(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_<e*5>Z?[1O`H%4eNLGSmr
+g^%LP/^+S4Om7crFY`><C^9(5CnOp_6sQl/feD,*7EbPh07S98gpt*_ro"B;QrN+\r8fRfs2%_OFsEAS
+#Yg5_4_r`rV]A6ET3R]l1eWpdbEX9`7@U)d.JN;gT/>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%0<j!Rm-7L,k6PjX`#9S=IWOJ3I?f'Yrrqbra:S0g6?&]BCKo*.i'@lN>RuDpB[2aHUR[#Bo
+Zq6.2>A&h^^)]?'il-gGNum-UpQ*Uk4aZqkh)QRY$PR0\-`Vfa2iWq1A?P#bYa^n.8@1sGL`<Q+2f3>G
+l`75'Rbr^@l,(AAm>:+?nP*EP".<pb''s5l$5.3=,3%18U2FiRY=9<Tc>$/)BU@X<`]TQtQ6X4`<j!Rm
+-7L-VrEXYaFCF11p-amc's;&R_jP:#''s5l$5.3=,3%maB]QXTpZTK4=m_'O(7-r5iQS\L$$NXq"c5-/
+&TO(6U^6G:Ko*.i'4j2,@%;k<CH3<rF3oVG6)%(O6sP]cfp\f&_SfUEM6\bc0u@qT[2b1q$0Xg\+OMMb
+,-Ga>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*eXcgnheUX<!9[OGUq0^>Jjc*(Lq)!H0T4D6M6ko7L3:/Cm`.<=
+En(&,pFB=,BTj,BpWsVb*?:j_'5+_NLqa,8C0k9)A"7<pl?fVEQ7GM58W&=nPG7=!O_(.fR4_1'eXdua
+)'fT9AK>][\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^[X<?'mUP4[gpUKl%55F>LMUj_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<JijTd'9m[&LU"0e3c/1.9bELRl=-53:YBd+3jg>?'mVCPTlFW_YQ\A*=V<JbL*Wgp0_$0l:]J<
+q\jHP`-dJZ5uP(Y)`MZ)5(.b*F*1pB^c\HL7>%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<n8Y$Se&rR0eeNK*3&Wsc6ljs``PJu!9`csi2#,XlMDL5@P<*W$ap;D`FC[t@a/7<@7sH1Rd[>?b=\
+s&U"KD9Pq9.+![5NXFJYdJYLtcRT64I0m/pQ@4`P.2o<Y*rO%c,+F)sX\9OCD9Pq9d"N-JDHm_@_J=,S
+82W>Rr.BEW3%5cD]d:G7_Cn>.dtLpDo9:Gn6..j[BPU!X8C9KiTF:M5^.+V,NQo0V_*uq(.9`,lbhAG^
+'N$2J8k0$o<P![WB`0<,n1-CRE'24U7[t0P8u80$OeUQO@H,L0MC@&0[?&`C^>,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>p38l<Dfn_0_uV2I2\0o9:HAG@<A)W&AP(%t9Ck?682K0+MW%,>O-m[[o?_MJCUoM).pV
+U1+qi6jh%"U)D6P([cm'NArBPd_Qb!<$au@l,(B,V\Dul8=E'QGm?4WR<ad6j6UR"8Pti*r,#1XW_F9V
+c9.qZ.RDKNlGtr`WJ[VN_Mtn*&LU"1floN&D5=*g?'mVCPToC9@:c75N$FeV\^lHRB3ulYML%&-MCq?*
+l,(B,V\DuY8GYj\kls$"$0f<dZ`arg73*61G46P%W_F9Vc9.qZMEt-3.S)Cd;pTBJj[^2*>a?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`9<j!Rm-7N]N_;FdEAD*9Cg2pc_bYsC$G8F#^gGNl9YrqAEZlDqr+;#ti%7tnD+@W^&
+_Shk?'JW]+cF3=%BRm@XnKfbW'Hp#c8PspsmfcLNU=s]tilk>Y(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"
+`(<f=m2"Y$.B6/[-I+`MLb(i,qE)cd'HY-15$aHgd5XE9:5Xe6[p:i(P=nbi\hJ-d.7N;)OtL!1Z12&b
+\]C$[RO(MC8^Xd=po_%>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;OY025iH<iNk.`NB&qWNA'Wl!DG4<en
+(ClF`'J8XnPq0J]hJ0lff7f&;:FRJtOOW63.P]Wp;ac6@M)ZqfJ_S$<8C:=Qh7@aqfWcfrI.I4@IJMuk
+Zi5h79UpB#&P7Uc='hAqTjE#KPGX/e#SM,TZkU!%%jaY;H1/H@%fKIj?@DLs=arjV-I+`MLnm>bL"GH/
+/N>NH8Q/#m"Go)e>+F?`ZUdr1\hJ-d.7N;,Or_-B#=gao;3X".1e!L^PG1LU*B]r.eYW]fB:`!hp)r%^
+lT)'CY#^=aVhdeJ&oghA:i6M&PSDZF$PR0E;FL4$fr?QjHR6&c2,a,^d0N#^:5\1Q4*X%h<1>L6S9[^=
+fn6(.=/$(dO!##^7C0KE]Jq@?9c#/%4Jdl#6*bQ513Z:`>r"Qbm'EUG7L0WH;KS#T_98"9g2pc_bYsC4
++@td:=c8:&oMs@-a1d*=d/lTX:5Yo#qi+<n8Ps?90QH5(7C3XP)gO_B%d"rWhF<An:QUBrU-8BSDu8_W
+Uo61WQoO]%,2*<c/mQQi,ItlYKf2gsH@ePT'^`CN*%"+^l,(B,V\INPODO2NX>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!R42C2u<Y
+RI4"\mrgBBFJ#;Dl,(B,V\INnO>O+/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<f1M&C>..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>:F<L/khd\8eHIUBl$ic6XNJd9p$5eO%5hc&aRt8MVJ\:Y)*i=FSG4ZcT_5B
+o&#4OUb5?2GtJYh#&.mb.7$1':Q!pPmTt[l&Sm^q7>H>)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
+/X<A<Y>2iSH"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]%,"<TKj>Aab\NOWU.A@eJ,#4#OPG6cQ@H,L0MAUJt
+pAWn,S:+<PAn2%$3%6%HMJCUoM).pV6BEs'cK$;M!%FWJ5pB']Lm+ReFCF1Q;i2-^#J0E_h/g.2lb\U>
++;#ti`(<gC\hJ-d.7FNjhrOSJ<Coqq8Q%qgJjc+C&LU#Tl,(B,V\C:FIlH^'fmPfVRju9:;FD[[7%=r'
+-UkH-_Shk?'Hp!X)!V<FSf?-/OWoYo,iYVU3%:k%(FIEiV$K@J`7gYoRm.p@O;(QF+;#ti`(<gC\hJ-d
+.7FNjiG.!'I@n:9,.o-h'I$)d8J,r?<1>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`<L>(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;&V<E@'G2]][HYQHRAU`B8An(370jNG;FI#h
+eYW]fB:`"S+;#ti'Hp#c8J,r?<1>L6S9[^=Jjc+C;1p&(-UksF;/NluETS]=#SM,T6kP#=S.lP_7CDBs
+`0mM;+\9OC&Ocs<EDf2p'\0&g'5*n7KHLjS72-n9`(:P\;H0,<9UpB#&V<E@'N'N+&LU#c8Q!F^]6D$b
+if!USUC.`JSl14a&LU!]+\9OC&TL\_EW5n3A&jVSrto%/3%:R-5pB']-pj#OP9T"^BOt8K?Cd%H0$G5)
+&Ocs<EDf1:Jjc+C&LU#Jq<kl`NZUK7p?YpfQo0sLM@_&mUkg2tM)Zq.:5W)V]tOC"F3hPng1scm>g\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*`>B<Jjc+C;1p&(-UksF
+d2]d5.=0=[`l;qHqqIbCr9']"o#[doH2[EfNugGFH/kB4^7"XcN%f6L6kP#=S.nhR+;#ti`(A>r2fIRN
+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]<G+]DL=Vhg[l_M9r%;2f9VJ[!'R9;@ZE`9CB)<?+P.Mkg;nMo'9Z_
+H?'<QoGQ&DNK&pa?[hglOX%IeHMm<R-Sol.PG2pWc_!FFm+BZt6'FM-i.Ft+:@0Z(Vb`r!G4"#1B1.s_
+lbEF7DdRkRR:e?cQ?)TZbnDI"I.YfVe*hD#bu,%Ka<c%MPA>op87&uUFaJK4:Oi5ijKd`u(L@sTUhItF
+0>031WZQ`gnK^Q4DVb0>YC?/s@uj@Hn'L5dX!T@FA6mW;=i'1!?kT%=(G=LTGOL;/I[DZVa,_<d$\i6+
+rV#!0+T;<6^Rsk.2f@Dqie]%SHKqK':PraX1,HJ8Sj[8'`OOu@.>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;sV<j^uVh7FbXpu/>Z\+\[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&gWV<B%ggd.Q]qWVMjp\eE\j2[4q
+o#`IMSJ&(%lDq*8&b7/#XPiEJ^YUOtPIG[C<q+-orc&REZY.TJqs:ZX^AI=;DnIf-Nh1OY)8An7'jS7f
+iR*."o#W8/pu."%c\RnRCG@HP=i("gm#jHU`2'K\5QCZ"M%e3"s89mV`mEm8A+&0bpV[_@]_U]Z]78=a
+=i#k8IQ6$1SpTn9^%2'RB:nR<>p: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^4jk</mc1M8^;+T]f,M>d&M0Mc96Ij
+/V_5mqsCkVb^Aor#Li-(H1IaFZ'='_WpI?Op[A.]mk;EMi_ZO@F8!!YQe\\#cn=[J]<%p&Ies!<o6YuZ
+$f5%&Pq,o_Z'kS3n$&mqXf]hsQS2]0G3mlBNQM'Q4uHO7pu@E2s7"P4hgb)R[BcmiDlWa'GJDmZcb4]"
+-7dMu$Ps"0/)#$J`OOu@=U.mdP.S6[J,"A2ft"f0rUncLT0@[LHChu@h7@`EEeX#DP>ApmT\\LU"KV(a
+_Q;no^]47C;nI;Eio]Ccc'pZlmGIn[ZEO6%j,ZR"(QJ78Dgo%io>6]lUWGE<mbG@n*dd7cn%Kb;]P,=$
+(?:RIAaN-Q9C>D!f*k01*o26)in:YNrUU[Rjh:EF47oAOIKYB2?'pmO_K4_TM;<mLWMc?!ET>'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<aqIco&/3Ti9+YN,!@dCD7"Hd;H4kBqZKkPP2f])r.0V5:$-4bOt8Eob<hUIW(I
+q_i`pi5('@]UbUaEW>@.^]*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<j-@+q.XkC$>:nmBSf[eVSh]&%Me+]?5jAc%[GB@p8'J/g]
+B@"RCCu2i#Ck6ghH!.G:>V;B2+nCTDa(nG@3Ycl,gprV4'hc5V^P/7DIie(WiJ/M<S]$h0f<1tE$WYp[
+feHS(dGf5DTCK7?*2q6QZ"(h3-6Z'S4F+bp"kg4`qEsMV%Sbi!H5KB#SNAc5>3R4qEo]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!:<K`TUlP?Hn#&t#@ArOqH?snsa*mjP
+\#3#oNjJ/)^Q4>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<Wc0f6%#dpm1[-gF7r'5#O[)4/bhgU"q*j[Ui7rllKp<1eU](=#L!2^[DpacD820)Ti-
+r=5ghmPbjch`,@]^EsF4Z1&f!IU)3a@fbJsE?kF?QqH5KHPa`4jO1?8fL,#'\$rihmF@9.qN(@'6,j;T
+DU0/1Q0>[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&A5dB<rV6:\dCoEu]<]$N*P
+o]niYd*]$)rBa#h0(]MlrOU?/FS+lc^X1B3$iNC^^Fo-d*W<Rr06HjP6GLmpY7(:q8&$\K$Q]2WcnBM5
+lU&A5dB<rV6:\dCoEu]<]$N*Po]niYd*]$)rBa#h0(]MlrOU?/FS+lc^X1B3$iNC^^Fo-d*W<Rr06HjP
+6GLmpY7(:q8&$\K$Q]2WcnBM5lU&A5dB<rV6:\dCoEu]<]$N*Po]niYd*]$)rBa#h0(]NWD>goME9"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*)NdB<rV
+6:\dCoEu]<]$N*Po]niYd*]$)rBa#h0(]MlrOU?/FS+lc^X1B3$iNC^^Fo-d*W<Rr06HjP6GLmpY7(:q
+8&$\K$Q]2WcnBM5lU&A5dB<rV6:\dCoEu]<]$N*Po]niYd*]$)rBa#h0(]MlrOU?/FS+lc^X1B3$iNC^
+^Fo-d*W<Rr06HjP6GLmpY7(:q8&$\K$Q]2WcnBM5lU&A5dB<rV6:\dCoEu]<]$N*Po]niYd*]$)rBa#h
+0(]MlrOU?/FS+lc^X1B3$iNCnCVQDW,e71*4TmU3[b;2<2Zp@.Er:WMIU2.R3r3um?g-_*Kmo_j?PiQm
+Ndb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY?0E(cqK8K=kigWPI\END(]&iG
+IU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[IUL+i6KoUUfkSH;VFeN1+kgtN;Tqbs2qLsrY
+?0E(cqK8K=kigWPI\END(]&iGIU2.R3r3um?g-_*Kmo_j?PiQmNdb9t(HVD8TjR$If7e[IUL+i6Ks'!U
+4#'9H\X\!PBK0k:)kq6TAF&=O$Q8oScnBM5lU&A5dB<rV6:\dCoEu]<]$N*Po]niYd*]$)rBa#h0(]Ml
+rOU?/FS+lc^X1B3$iNC^^Fo-d*W<Rr06HjP6GLmpY7(:q8&$\K$Q]2WcnBM5lU&A5dB<rV6:\dCoEu]<
+]$N*Po]niYd*]$)rBa#h0(]MlrOU?/FS+lc^X1B3$iNC^^Fo-d*W<Rr06HjP6GLmpY7(:q8&$\K$Q]2W
+cnBM5lU&A5dB<rV6:\dCoEu]<\hNa&Z&2eRH0A@!0$HG"X<JHm*kGNV;'uQ>be/9.BK0k:)kq6TA7Obq
+@hlO.o?ab">aTg#=@Fjd4ERYK(M4^L<fmbG%iq^lKpd^V<TM$@6cUZs@HO7n'tH$id'8I5R!In1>"*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"<h+m
+;LjoH1Bn5b*=J):7PGN1_7!?s8Mtg4aWpJ,tjPgmJ"_d-SaS\<ir*'<0FMD3]]<B\Qo"#l-lPo-;D(#r
+OQL+:$C]^QSG-0p?dc"V>\Eab-GU%2Jq/iq!__g74"4";&jZ;iVE*.]u+Vpn]_r84RhTrTX<UiHhYgR1
+T/"&T72ordOL,Ih0=2Ap?bsQoB4Gl3HJf&8GDfM=U3]O9)H;KpMMb^EM%\WNM9De8)AJ-(LM/gIIuaNS
+ND&8[Fh3BMtemrTIW1sR5Jd2mbPL=L\-L>'Wn)cB?nH5\-,HWN@Wo=&hX"#fURh2IbV;J;l37!EokGn5
+M<Vjle&IPj3GPt&&%!k,:cqdQS2^+LF5ie.""`/Jq*dqB?05)=jbHs?G3r:s%\ZD7u!a<o]Mir`l%64m
+_[RLRP]k$2!Bg/ro;%[mgC*pE$3Guj%aFbNY@DRoX?7+J,fK$[;46Hc_!F_(W>F&@s#tsm-gQF>lUN:V
+5l">Sn#t#]mJe_^SduBq=<XrVb_g8lBQ@N9o4=m/%^[Jn[,d7-eD_skKeYMk`KY*!:#Zlk9J`rrqaf$&
+-)\1!!!"l:B(DnTeD9~>
+
+%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<i^@jI,+$:QN7'
+di/]VJ3.,sqq!HhFe":gHipLpDIhj*n2hFsXU4eWLWJD0R,,ZW*.N2'+pta]Z>?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+<pT#!VnGiEKup<M&jde?,"`,tS;PCeBHXq!]5QQ
+=q=<XjNt8\Rm=u2op3MTH9MS$@&/$u<;*mh7<('JM"52PfmU!/`Q'Kfg3h9%?O_<"3om2KG9MS$@&/$u
+<;*mh7<('JMK=Z9q?G#$Bp[2>]a.GPD\p<o0m-O)jY?uXImg>7W&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<`<:.7luKup<M&jde?,"`,tS;PCeW+iCg3>GCX8E<`<#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`<O8dL9DMTDc
+qBkCdoegVnu6r$18&/$u<O],-+beiGB;b@=PS;J^<,]Y@YVkU&VKumJqKJ$R,kCAZ]L_<#uV]p.2C78:
+mS9;sc:?UMAO73)bL1!*;KTO9SLn'?kAel_+ST7JRk26050q=HpL_<#uV]p.2C78:mS9;uB"s&/BqboJ
+<m>)BA,,ZW*.VbDTR%/<ADtTQ9HenMbL32SR-XJu2?a_1O#`5(/aJl+PAmo`2.6eaIQ-V[KFQOt\^Tnk
+W6:g0:6r$0F1CKDQ:-$l)2:)6o$*!r0%&[o3PRnHf'p2Ele',\iXA_`/i1:faT39f7b]gK?8dL9DMTDc
+qBkCdof'-]pIgu#F6h3sLl,@g6#pGF0&NLR`bq)Q63h7\tLYlV\*AjQU_`/=QG!lW[O])iP'b0?qn8Nf
+(I;<WabVRM\jK-g%DuoEQP_A\X7s3fke!%pt6jY$!;\;#)%;LBBIplcZeS8Ch^MDH(Fbes?kF71I=f5b
+G\F0\dS3/(#R`9M=#pGF0&NLR8C4:V\AnL2mgUG5Da.5ugcCI&Ge'm(+gV(lk$^7nla3U0kD\Fp=8P$P
+_>VdO>8!FPt3>IZTd6J0P%pQ0eNu)YGB;[iYY,n_sHAYjmS9=]&lMhVj(OV^[2C*L%-^3qR&jde?,'#.
+OaPPe1ldr=%S^-IE7n6)*Bm4Q:Z.Dpu_BdFu!a<Wh*#G9)%=]Xq#UAER#"f/XS;PCe..lN<MsdsaZ*D^
+BKZf!PL?7NZO(OF7NZ\:K0/BQ^[r44qbPq2KA6^uk&X,[TjA\NbOt4Z[i=@"o[+k!Fp[@i:a*hA#1@d0
+EE<4+YPjEb3Fo"].$(iQ(C9Aa-MNGoYLn'?W[^!(l1G^gG2n-Q:hrci&0]t:`!R9$1nOL-?%KFj0DnX)
+VnRVeKKLAR(jA[t+=0Gq)9hjHWHaj"tHt!lR*6t(MXA__r=@dr4?O.4K*Do-ojA\NB7710\>3\kmNupS
+MccKd-qDd[:7-g'MURHm2`8edZ6jY$!;\;#)%;L$.p%u5-3p6<N@=E>[)OtkTYYHWe_''\Oa-,hG&jde
+?,'#.O8>jq)R[Ug:rVESpg>(KMob=_d58iH&Q<L2N\G2/7rG*MSG.&Vf#`5(/aJihq042HkmCOK(HM"/
+f;p<++2s@s5+'GM+(h#u'%Z_(R&LE)u:.=5Ce24Kcq0*[a:+>^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(<A1;>?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\5M<CB=A[+bBVr5^u?rTYD71gYYCCna5)?PY%pVX4uC`_,bc-I/N*LQ]mVsTF1<5UnIt/
+#5tqPgA]sBHag]%4_<'44*N_I4+jYQ@:93(\\,Qp<.Y%$e6?=1J%qnZ-]#8f!r]NWrWYn!rI+4q`Urf0
+*2=t0+@3r2&a6D%oJ]$%kcYKpN?3R.)G])EIicsBT0@\Wf@Q@]T:_I:fA,0ZrVLe1nu+9!1K.UdSs,@j
+SDE#W7n6'!$!/]2/e/@$()@Z06Y0gT(:bo;G^)1`MrB&#D=*fODc6H/qXs0OVC)8cdaD#\6\DBb?1"4+
+hnFMs^5?_1"U/1;?&+2jHC9t\P>VZaH[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!<t<E
+KX!2$^F118b!Q0XClI('FIHuH6"9ECW=)[*<BYg/(^Co2-)A<A2>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\n<?p[0'A=]C'>WB+ar>m1OU4`qbb*2'ZOc^s+jjQ$Gt$kElL<*?$#
+Xf\#iSXZ+475QLBhaKVXr`TkYIrU+F"#E^]I$hF\*Vk,YrbApCX%QbITasL=N0<tGZ*Bu08a.mg4$#E!
+cM[T&(:K-ZMa^''#(J9Y0(SW*+l0rLJ:??O],kKuCF_SX+@]Y`q;K5HKnY4q`S/ei;c$;@q)M>32`Kde
+\@g_MK1@!4%^jiR7*$@+`Gj6]95t%3[35&>U>'2'/DHqbQ^=a%W3-?CO`,gs>=hDC4r6*OM[Q5_Hu4N<
+0uBBWii!G,4?mO<e?0%58!')jfl0mdlRW_O4]Q6FID("iX9Xf!JpO)h$g!rTlK[Y/gGX$7Y6E>I<rlm8
+r+,p6,8d\lZZFU4@9+S7n2^N]B'lN#Fm2+D\ULQO^=O-l5C[!?<d)r*1BIH^+A;VY.DZj\q=CXgY?V>'
+)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#._'(+.<e-&%H5MLVb+otRM_'LF!=7Du1(Cpl$;c7(2q`$_ZKKPb/G$&LVV&Z;A/
+fttZfVkak:S(MBXgVT1@8AqK`3#._'(+.<e-&%F_fB2+;'buit8AqK`3#._'(+.<e-&%F_C[3L\8O]Ed
+&ofBF:.8a#Ko'kG8KfA5D<oB]/LhruOGHgIE@NH-/PM[U8dZ`hZ@`nIU`r\BM)Z4^VkUKM_SeI4U_"/u
+^^=sW&LiB$S;N,$;;#boig`qF#`7INDQ&@:66]/n`2T\O[P2^H.4Kr"kCP4o8O]Ed&ofBF:.8a#Ko'kG
+8KjnD(&r;nMi(%K,iVckS;PI$$PV]nP!V_<.;K)GWJD6T,#1SFWMjXA0QGAe.>63+EeeU$73iHXE@N/-
+7:ifZM/uE`+f'RN5(jk),)MD;;\="!.[r"`S9%:0+n^mO<cF7[80K8<<(*76b%;"pkF@]%8:@5F+l#m+
+8VBL%MMP2.fkb*VS5W;kW?7?cKGuBbM3*Nl&TK9MRk7=(F%Qc_ot7ScE'm&>&JB(5,#1R<Z\=6W:.ZV)
+hH'=WDGh&qKLD>fLf71(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<U`r\BM)Z4^VkTL:Tr?LVHiA'Zm+K@s`Jq%un])aG
+mbQX1o$jX;RkJ5TQoO8n'LJh2NXt*'MF\p:i`&=9MOJ-hAr=ts4[!)SfB;>[#6tLS`l?TCo%.5u/Zua8
+rH`JFoB4G\*^+i/`/.2PZRnk@F%Qc_<E_<iiJ*4\+sc/I7%B.'NZUJL1,@;SnCF^G`J/g:o&\RULPLZ"
+rqbs3LGs/./?XBjil+BZfbPp[3@0B@WiJdIE'm&>&JB(5,#1R\NupS%MtPf3CTUg,Q^<V7VP^6A?@;1r
+\^RHP[m%HNokJL._SX/ZTA12/c8d2)O=I8WL2]LK,S:Z/.O%VVL&:W5(Q7tC]mK-*mbbeZ_M&B`?<jP]
+Ier<,hgb\)p?gUi%j1f5Rr:4\jEn?(1AoVJcqpY/!kgii<Ra3c4+1K80,9U98AqK`\:1TZ2-4F'B8Zps
+N84+94"r?B5!H^K]UbFWB^OA\a,[i]qf_Q_]ZL0\>B/>GZQb$`.]9>],2At<;@&+='bR@<`\t<qq=Eb[
+*BNiIDndkW00ascHt[3WnDV9Sb[F_C39QM?6E6>(*5W."O[MaYWJEMK($=Z@[eN#6`V<a3F2[P)^TZ1&
+5QCZQ7\AHO-I+UtKoi=R(8cinM)Z4^VkV2gn!%tT`f4!-[(i)2e(qW[f7!o!X095uJ,]@8TA12/c8d2)
+O=I%fLN#UL,S:Z/.O%VV9d``.*$$&:ICR\n<-n)o7</4TX&iN7/1iI@gUD*1@Uc+YC^Up>S<?30<RJ3A
+\=k+0#`1MU&Z;A#R@0IPZsL:`V-^>iIJS&)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,#1R<Z\=6W:.ZV)NiW!f
+DQ&@:66]/n`2T\O[P2^H.4Kr"EOIPX$&LVV&Z;A/fttZfVkal%BD'%ODQ&@:66]/n`2T\O[P2^H.4Kr"
+6+o"+)_*%gOGHgIE@NH-/PM[U8dZ_a9$[q)L5JMG,iVckS;PI$$PV]nP!V\[d7Zj:Td#X*M)Z4^VkUKM
+_SeI4U_$EofYXkYOGHgIE@NH-/PM[U8dZa/^)e61%2G!n8AqK`3#._'(+.<e-&%G(=mlgYL5JMG,iVck
+S;PI$$PV]nP!V^OW@*I?6U`74&ofBF:.8a#Ko'kG8Kjm8e#PjGTd#X*M)Z4^VkUKM_SeI4U_(tWZ%ZV-
+66]/n`2T\O[P2^H.4KqW4*l,W2cN0X+VCUri`&l8>F7D5PS?I<"hd'P)_*%gOGHgIE@NH-/PM[U8dZ`,
+XlkIY6U`74&ofBF:.8a#Ko'kG8Kjm-otq>YW/)]c,#1SFWMjXA0QGAe.>1YM>8XeD8AqK`3#._'(+.<e
+-&%H'djIMmTd#X*M)Z4^VkS>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^1u<W\T=[YEd2j(M_DfSG4""[9:(OZ:7XGRah(.A\hJ,jrAQ5^
+#UrW#ISmVK,*E4g3#*%'U;hAbnEnu>M^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<PS?HAlgl:T:_?9P7%B-j;c2#cP:&jj7p@'QniN+s*')IHX,#TFH*cp#::Jlm`&X`k
+&5o#MX<LOJM)Z4^VkU&VR@.Nk?Fb*Q?EZSAqK-[qYVhC^4Es\]htd*Dk(hQl?SARPS=Ehj?@MkLop9$7
+=^:n(rVMR<ZRnk@F%Qc_"W>/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#._'(+.<e-&%G(bSG(n&LiB$S;N,$;;#boig`qF#\do[iH?[pKoh*h&TK9MRk7=(F%Qc_
+m=6@mMMaqJ,iVckS;PI$$PV]nP!V^oAI,K9,#S]'3#*%'U9`Ih`&X`k&CPaT_J?-,'LF!=7Du1(Cpl$;
+c7(2qZn7%%\l;')+VCUri`&l8>F7D5PS?I<3^\-:.O'S.Lf4n*.Opo>9Up/r$6a8K+hS*rO[;UWWJEMK
+Q-C'kc8d2)OLE4GhLpbW+XH&H7%B.WAd]:8S<?30>@@eU:.:ri&ofBF:.8a#Ko'kG8Kjm*15%k`7ALJ-
+E@N/-7:ifZM/uE`+f*@#_HW]X;@&+='bR@<ZJ!+53N&YadcPB)X\]TYKLD>fLf71(Cgl=n;c3m8'8Gul
+WJD6T,#1SFWMjXA0QGAe.>5&?6[7or6jb(LV]t,"<&RsJ39QM?6<]^!]IbTWKoh*h&TK9MRk7=(F%Qc_
+XihL:();ru8AqK`3#._'(+.<e-&%H315*,07ALJ-E@N/-7:ifZM/uE`+f&*Hi>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#._'(+.<e,t1%RA-ci4kDDeU,iVckS;Q%&XK9Xjc+SJuDN!+"/hT>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?<AQU's@4:.<+#W:9?;a'u;[]6:<!bOY4EEQck]e@O8`lh/`)
+:LCbZZ+.RbAf!G)V!,:L%,iu6>=FcE8AqK`3#*1rhnNMLepI/Z[GSKDs8EBB0!'EujQ,CbT7?k!_hTg]
+iU+2bs"lPheEOS10BfHug4sXFmn6I@X,8<dOGHgIEW'->CTbt.c5$ep=N'SGG$n]cI!g>/[XJ#EBlW\'
+a,c-CqmnJnj0J\s7>AM`aVTY(dT7/\*[),n,2uSOfZk7=>$<t5o]RsCr6kMi(1qbMVkiOKO::+=oM7o?
+8k?n"Lc?+ak2tg7pc[%ir:%U:T7'aF3W#03^]2#KWtg,/p?^I%TA12/c8d1>T7g_\Q`<h2U23.?:.<+#
+W:BUDA_jk9A(<0un#"Vl1,?0':\_*ih:,0-0QGAe.Jb256b$t2:_C<)8AqK`3#+<A4F(shFL:`"7__l9
+AS#G`c'9e\p?gU9r1UWKUX*`a'BbmKU?=m!KQf`.=2aM<&ofBF:.7l51Gd!`/[fN0?@VhDl/:K-CDoE^
+n6g'KCjph^mGIL+_X%cqPS?8&OtF<SHG1SHLQI<j&TK:8R@KeKeS<@tGoe"J?<g`OhEF(c?/.1s41k3i
+fYZtbkEH[o*KhSY4*nL_G)_T>;\="!C:CLSo&RoVed>/\$PV]nP+m_!15'jG%p-\e&e]16,#1R<Z\=6W
+:.`j6+W8//WE1TZQO/L.'bR@<ZJ!+53N&\:eW=V+h:/J?Z-.egS;N,$;;#boig`qFoOLLIn1O%KMV*>D
+,#1SFWMjXA0QGAe.=*Wm6[8@u?:AshOGHgIE@NH-/PM[U8r=YtaUmlr*ik)l&JB(5,#1R<Z\=6W:.`iC
++ka^Qd:3p=.7,4R7Du1(Cpl$;c7(2g.3q9j;gPDe3_2f[WJEMKQ-C'kc8d1>c\,g7G>QUb6tZg_3#*%'
+U9`Ih`&X`kk`(Z4iQg_JMWd#C&Z6i4<)bb[QoO8n'Km"j+l%'#:_DMK8AqK`3#._'(+.<e,t/>tA.*Td
+%obqU&/&t4,#1R<Z\=6W:.\=UO::)c;"]_0&jdd;7Du1(Cpl$;c7(1l*KhT$,q<14h*X`OWJEMKQ-C'k
+c8d1>F[Ip;]16uoU8nQV3#*%'U9`Ih`&X`kW+\_VE*'#P7<BIl&Z6i4<)bb[QoO8n'NGR`&FMGVXGAJS
+,iVckS;PI$$PV]nP+mgWftuRBMA=NmKLD>fLf71(Cgl=n;c6RI,X]'L<N[OWPE:T!&TK9MRk7=(F%V=)
+Kk(I5VeT4"f+J^9.O%VVA[@)IF&#;(To]]AY#V0Qk[m.*:.<+#W,/mrER$!^WY1I<gs)is&RA.&`2SPP
+'p[#Z-I+Ut73t8UKY(J8.\ceHM)Z4^VkUKM_SeI4V!.f>%,pX+r=r0966]/n`2T\O[P2^H.4Mhm,[7eU
+FalhZJrkde&TK9MRk7=(F%V<B&'5BcKJ'HaWXoe_.O%VVA[@)IF&#<SbomtA<RK&+;'A7M:.<+#W,/mr
+ER$!^\i(g$\3dd8,-"(&`2SPP'p[#Z-I+Ut7<aJs#ZjZC<^c$TM)Z4^VkS>b.k>[^F77u!qrF$cbKIki
+04)<djH[/-ER$!^\hGBs\8o0b,-"1)`2SPP't"_d7BKVFo9ZdrcRph&Y'\iW%j/O_m^p/ZpGT3;Eo]bG
+MDt@<[P2^H.4Mh[,X])!\ud/+8k?n"Lc?,,c-4BGUNLm/\mDSdcd*+3%ZZO.XaB0=hgTqAi5*$LmhStW
+/1c2Ip"'EY*-hGYogYS,(+.<e,t1mhA-c79=&hY2$&LVV&Z;A#j2[4AN/Y,-]QNO@Z!h"RI(AOJjlPU;
+HhXs5B^afk/$AgCf3^K5bdK3Fc7(3R-R;'h]"4d`.SWIPWJEMK'g@*E==i$=f<2N!.5l*abLG%kW[el%
+(N83F]s+`f\n,GaV7?*EV+VbUEa+njkEHZd&s=DCBjFUK<@9'O<(*76$MX#H0,EleHhZ64gqS@=KaS\\
+q00#I\Pi@]LCYJEFMXR9]+"]r1c3_H]0J%5L61,EE2W.444+KPd0#%iS;N-gkDEGaF`qr:=J3VWDVqZ#
+!s@TDj-pS<-Vp<JQYlAY`N:`ia[$B*]Bm:8OeE^\njVN;l\:;\!C@u*7Du1(Z-t<&nDV9;#7mCdF6:Aq
+&"?g9$PV]nP+pt[)8\P0it]r\KLD>fLf7/ZDJj@(h]mR@k08>Op'hNCDa&gn[P/Sh;c7]q,X](#GWI%&
+OGHgIE@P`$rD$KSDVS?:9Dc>8[@<\V/H47Z(GB[fPM<sm3N&\:?q?#kcgZ$E$6^"Z,2uSOFRnM7dF$@m
+LE$1eV8Ca3<:fLZ[eQ8-@H*eU;KOB66b&J-3[09>Lf4n*.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,#1R<Z\=6W:.\<+8;;(Jn8q&*+VCUri`&l8>F7D5PSBsKA-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]:8S<BU0,V,7QnT7/++VCUri`&l8>F7D5PS?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&DK<!69Up1H.U*e*CVLW9,iVck$6^"Z,2uS/>r+8m`Oqb5
+T!$]2oCMR+K7dZDA"-_A$PV]nZE<Z/dC2INM)Z646:f%oLc?,dfC@nA7n,sO+5XSOX&ute?bY=ZH<Zar
+kHq_c@-;EC!k/Gl3#.QB8AqK`3#-;Ra,_=#D0t[([FQqQbKIjV;c?U6rKZ/>\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<uH=0C'=oqtTX%LruW:ZJj!@H*eU;@&+='p1gR`2SPP(%&ftjCGAn
+g[gcjiShbp`D%tnKo'kG8KfA=<(%':&Z6i4<4"G3.k=De4P>)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
+,#1R<gB.EU!*uAK.iA<ezzzzzzzzzz!!)fo<`W8`k2tO6He7LD6A7!E0,GA:Gk%NWGD';,\@D=tdA'Io
+$`^45Y<RC-m#OC1./qTXqa,uos':!Ck"VcB#aP&[q=D!IXhEe->Lopt9C?&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<B4>=ZBkci!gUG/NdD3PJ0>Xku^]+(F&^p@<f8G.1
+Sj.^Ro]X[9-Ved[:/Mlt1W:m0,,+]Fj!?Y=Z>N*;Ei2!Jr:n@S8F+/t[r)qAq9,*X4*[*jQK@5i]mKK[
+^3mJ69C]<oW2OqadD:/uFhN+oWYdD,_o&o:4)j?+RA$=Jeq\bSq<+G=LE8R&d)!=^'bsM1`J_skLGs/.
+/H/rH*Jo/AnY553s7oI\n(/@_I'IG*Gfe2GD,cXah!%"Or)c(3f</Cph,6AV`@Y"e;]WMP<6O:\ipGL0
+(D[ci\#c[4'GXobFK'oeMMeFcl_dON^3K0WnLS2YZX$3s9?!+@SND$VI7L6KF6Cg`cUFrHY?nnGUt,Gt
+*N\RVs81]I6\k[<hu=IABO<K#EjKm-<TGHM])26j(D#H;HhZ64gqS@=KaS\XFdM:m=0>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-<o+qmM*q?g[kpoUpt^AAhdVHLPMEmG2S,tIe`99c\eWqftVq7rV*^'
+Xn]f?B?rVef;OY?GOIHIqmZMnY;Y(:@f3D*6-f5nh\XU05QCY&#O`DXZ>.@'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)H3gK4<H\@p@8N@Hr/j@K.Y
+:VD&F`]LT6?-WDJEqS_Jeh'Nf9>r^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<Z_bzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!!(BTrX]"$r#,~>
+
+%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<PNTJ^T?^EDX!KfrQ_PDgb8](
+j<%%0/c2t`k(nIjoLhro/c$ckh\Q0Rdr1.nam$9'^Z>%AU0hs;D:(!srO1pVS:4]792<5RqB`&s(B'EF
+Di9(dl*j*rjRh3$?ZQ![d4[NY[cj'JITuG;:;8CW-)]W9rK>oL^Jf<tA27j0JUM0>kP",^^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\*aKYND<h*n9YW1U-)]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]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/^Dn<I;gW_.zz!!!",:7X
+Ijmakfqh=Aq`o((t9J,UONLO$8g.U#tX3ICJd='pA5?;sC:i%6N4/M8Z)nA,?6*e!+38$9DbRf@V'bag
+=edA^'BDS5MZ[Ju8/o]X[u5Q.rH]Rss(1WRFol.rZ&2r8dh*7?LHD7PPLS2iD!4T"KOJ,JiW0>IGU^]P
+*PGMRDrIEZ3!/mPpUieoIM2XeBWmOe>7m^qr-KKo+U6i(@U1KMo:pYJ!"9\).Ppu-uQ7uj*'IC_/W"6f
+[Yo4'skHKh9r@WT<EC#tq5mWnBF-XV;p<W04om8hWEkDmg]hKuFf]E;tT3QEDK?Ml2'h!^(5-gnM)No,
+?d?,+Ufh's>(\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:<!gUD+m/bth;#7a_6eU5R7n)&?WgmN#\^GO`gE<-%>TF=at;rcou^MM,LhcI!f'lO
+k#iN]Ymiqgd=a_^"fX&nn^X0*<Y)Cg3/<\)2o!!%Q?e7uCZR@.le]m&r%\T?q[H?tHiCZ?uDl-lQiO&r
+C4CuaI3@"'n=oDJFUigS&W04u5r4aYcVE<-%>TF;29$cBm"oae:lmdBN3n*fZ2D*A=\/M.]]P24>?mb+
+oo^U`Y0<;L/ADWSA.!!#iM;l<B%j6+OJ-RU5fAnGXU<`X\IDtn0)$eO8AS6PZ/iT@Ic/&qen!.a,[PtL
+T>gq*F`,rM*ET0.Bs>efS$Kq$<'/KP].!!&1/oB&b*HO^Gm'K<aTrAstjjf,5YHqWKNI/Nd0qYFBtQ@F
+qem-JQ&jgVR8MoPu$!.a.90fFN>m^6i^VLH+!ldjXh>BfGdO_oLBoP)%@E>Hk"]QnO<o'tb\7un_9D@R
+0ff6@,H=h)H2!WW3c);`,XTDr!M>W/<O9MB!GB_M,lldt_cNO/-1]6F/KqXP`]Z=V4M4*QSZCutW4XW/
+,W!!(/?WDp1NotuOq^Kk!4F0g-M)j'ZP]dioh!!!#'?<2L!(B=F82DDHk!!!"K)m_3u!!%M<8u4:+4h\
+A8\DDE4zz88gEe!!)pO3]7@ik!3`j.O#0B&Z6h9&/&t4,#1R<F,#NM&P37b3#.R/OGHgIE@NH-1JF<r8
+4#kcWJDgQ7%B-j;c25<@H(6S&jdd;73j$%i`&=9MTY&i';m%;&/&t4+pfD)V]t,"<&SBV3#.R/OGHgI8
+I1/4'bR@<Z8oh@WJDgQ7%B-*6:f%oLc?-7bo%j'73j$%i`&;Y66]/n`2T\Oc7j5f+pfD)V]q98&ofBF:
+.8`8L5Ah@8I1/4'p402Lf4n*.OpoD9Upu26:f%oLa7XAS;N,$;;%IJi`&;Y66]/n,U"Gm<(*76b2s*GV
+]q98&ofBF#pBnY,2uR$2J8C:'p402Lf4mW#`1MU&Z;A/3Q&e7La7XAS;Kj(8AqK`3#._')(&*I,U"Gm<
+(%?c,#1SFWMjXY0QEk_U[*=EfEd-uS;Kj(8AqK`3#._'/52T"PU(oMS;Kj(8AqK`3#._'/52T"PU(oMS
+;Kj(8AqK`3#._'/52T"PU(oMS;Kj(8AqK`3#+m+c-:\8qR-,;ZS6C%;j%\)E@N.=+VCUri`+F9q>'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
+2SnaiGKn7fpJjfQj<XKDq/nG]!<<*"zzzzzzz!<;I7ctB*Q.XY/U:Hh\k7H_d*MPB;sq+OX"SL:$YMPB
+_'1IY/_Sp[C`MU%F3'LgDpnsQ4#3_qq<'Lh3,B8WGI4S_ZJ'V-VD.>jkkj>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;t<SZY8c(Wf5*A6md@(pH$t*hH[D7^M4&!KId*(]Q8LNT1:"I$=lm`pnaZ
+;Fp&%V04B(ZT7</@-%u'('kO%gZ7=P,qr)-D*nsatp*ia'1cd0qlgY:3+dt<:*6UO64oCM/.kO2'Gq-@
+-+3BfX`g=lDn-9Zd%GUSK_k;Up_%oSt/\[ecAX2!5hf^7tb2i@P,9\Wn)f%.j\`qK0$cd'fG%hE;bAql
+(rT:VX35P(@r3AE7n]Y*$92H'TgB4n->VG]=qeJ-?K`Pkg8i8>`S2:[)"H0JYM@^iD%h^J'M1T_UAI=5
+C)>&#l4jXWhk8*QDie'k$9^QXkFAROuqn'b],4$2P=b3XI_b9ACth<S//G'3?p7=KUGURaS45+\:7Ci!
+p`A($"(,JS*1L!rK]lKW,pRf:X!9,AB\AW\V<.sANcq5&u$!+p!#bK\:+]'`;-9[hGRkifn#_S?EPh:p
+\"IH.Lij_F,[X38]0Z:L>Ys4sUDq\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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"
+LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:s
+bg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!
+!5oOe.(!!!!+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'<z!
+"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!
+!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$M
+lOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+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'<z!"/QYz!!W<=zJ:db/z5
+XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==
+!!!!Ikg'5/!!!!5oOe.(!!!!+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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/
+Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.
+-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe
+.(!!!!+qD/*O!!!!&r>>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'<z!"/QYz
+!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK
+`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"
+oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+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'<z!"/QYz!!W<=zJ:db/z5XBl(z
+TV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!I
+kg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!
+a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz
+'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!
+!+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'<z!"/QYz!!W<=
+zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!
+!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!
+!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LO
+z:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/
+!!!!5oOe.(!!!!+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'
+<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(
+=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<
+z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+qD/
+*O!!!!&r>>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'<z!"/QYz!!W<=zJ:db
+/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%
+:==!!!!Ikg'5/!!!!5oOe.(!!!!+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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK
+)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg
+8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5
+oOe.(!!!!+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'<z!"/
+QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"
+lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlO
+Yz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+qD/*O!!!
+!&r>>S8!!%P$rX8c,!!#8MIg,lQ!!&[b56Clc!!#jBT)eol!!"G2cN!qF!!!`*B)ho3!!!B&1B7CT!!!
+3$)#sX:!.]\#$ig8-!.<M*6R%&3+:*SMce1m8.>`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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:
+sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!
+!!5oOe.(!!!!+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'<z
+!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!
+!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$
+MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+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'<z!"/QYz!!W<=zJ:db/z
+5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:=
+=!!!!Ikg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>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'<z!"/Jjq9bf8zB^!bBz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!
+!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+qD/*O!!!!&r>>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'<z!"/QYz!!W<=zJ:db/z5XBl(zTV+LOz:sbg8z.-StWz'
+_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!
++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'<z!"/QYz!!W<=z
+J:db/z5XBl(zTV+LOz:sbg8z.-StWz'_H#<z$MlOYz"oT;=!!!"LK)Q/Y!!!!a6%9(=!!!"lTK`MY!!!
+!qd%:==!!!!Ikg'5/!!!!5oOe.(!!!!+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'<!!$-X#H7s[M?~>
+
+%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"-:!<E0#!.^12X6U#uzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzz!!%Q(K0o\?!!!!bs'H^.!<<*"zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzYQ1.l
+1pI~>
+
+%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],&<eU~>
+
+%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;!<E0#!.pmDX)1Bm!<<*"zzzzzzzzzzzz!,ufN'A_X~>
+
+%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<NR-o/be'kOWs_(`.LE;U>%6I_YE<e!$;K
+pMqs#cC@#rfZ=$aYCFgo+[h-d$=Qq?]QitgP:,sN'XJ1JC+/AIqYT1-.hr!OIuca9n"GM!r6P=g9)l<e
+:C.peWWH08/6$17/%kQ+>BrGd#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><L\7h\TB)e5DcH$6rfn[Jq^35
+Ad&Q42Ek)?#cfG-3G!o]ai6ejA1p(=L_Ed>WX9NA17/e-*81*`<?\HD%-H'(5*OdQG))uXrSg4FTW3Tt
+WXNJC$V**=<)j4).YB_n%DOI)\<@fP[GXGQTW3TtWXNJC$V**=<)j4).YBa!NU:8J9Pt[pWWH08/6$17
+.p+d:nY)sL=B-6_nS.@EpDD"YE2"gY<P_"%#.&o8,9%`).YLfe_&SF=SkO$;]>I25M\B*AWX9NA17/e-
+*81*`<?\HD%-GnoY`h_O9Knm=_iG6SWWH08/6$17.p+d:nY)sL=B-6_JSJdE(S=BuWco^gTW3TtWXNJC
+$V**=<)j4).YB`4p$Uo+Y=tgnG9g1]We,r)$t]YN7lEP1<=#WTK/j2Hn)Wc;9MW7#!_F-*WaMMMK3?%-
+Oc9lO'uHun@(&b9PDdkj.Rd5=W[_,Lb$\8SE+Iur<J@ZW17/MKn)U4=WjW+''r\-?Tdk[t_8;=o%oj@]
+Wle^--(E'BSQqCMeQ*ln!`]sd<+Ra6=?*VVTrLF]<>j!reFf'3a7lTo]4rrhW^'Pfmo@](3AY:pRl5,`
+oH=WT"g/XFW[`[%Q,/uc]QS<mPZ@<D;?a'MGcuGZeu]IsY-5%T:#Z[Jr34V)0UNTb?50?<&6;d8F"V3W
+l%NRo(I_BT!g5$0<C'o5G^+Jl_M,2,p0*aY0UNTb?50?<&6;d8F"9-;Pd9p%X5g-n$:tu2Wh?N+B$HcW
+Sp=pW@\V"n(sE=l0+(ZY#cfG-\M]=.V-GA>WWnSbJZ9(*Wi&U_Eb"n.dA,7\`aDj*H;U?86<I5*bYki5
+V;04a>SLjOb=JWu<<[*ejnI#G[i)qPYZ.;mGRJZZl06=e+[h-d$=QqWT99!HqXT(LZO<h6;^e1&lFPKr
+G%]C(PhgcC4l-'@rucrpC*-"^W2`Zpe`0gS)B*XGZQ)#JW0Y9dn]sYDn'OOXWX<07jV_[FN'F.D-BtM2
++fPNC'm*G_IJ)+o$PlE1:!Iu[#."B_MZQln_Q(adp8-VI.He4)MsOLS%S7D6Wh?Eho[?T;q:Wrf(sE=l
+0+(ZY#cfG-\Tp&-AID9_Qa'dQ:aB&6We.27[;4Bos6SiF$t][G(BpfgKN4;Rgou*3I:6b9(E':m3g(sh
+<C'UWUQ7TZ1\f1^1UZ#+_5tHFU-M_4Yc9IG)\DF9b%47HN<;`eW2^u?\psa/2hNJ*-Yp7KK3<ck7=hHG
+@9$kmf;5XLCj%0B)6"MfOt':._*V-+Z\!lU)]u(0KN4;R>bfB;E[LO\nKFk5<>@&aAM>W:33r(JW^9ff
+)9gX]pd/QhYJ-B$=QaE18?+pT"Ue<JbR,,GCj`1]&6;d8F!X[IV-G@KF#=4K<M6@DKrPPn"UdgY5#bMf
+X"&bt5q,Lh9N^Im&4ifd<<Wb1)9`F'Nbj*AWXr32#'6bcVK+#bF#=4K<M6@DKrPPn"UdgY5#bMfX"&`^
+;p/,MoS(FfKQXF%JYWWP<No;b_5q&'aMs"8$=QqG0\8LRUY3aX@@aGb+K5EB'eE<q0Ip6bZLOWg6<I5d
+*m?'/j\/e>C(Nq95ZIsq<>j!f(633YW2^D0eMILe;`PNB\JbU6.Ss-]6<+5rJqYEg+0$fD<Ah2#bZ$20
+f!u\%Ot':._*V-+Z\!lU)]u(0KN0n#_9>l-:fumq&4ifd<<Wb1)9`F'Nbj*AWXr4]'F$?l\WX2BeKBrQ
+JZ9%mW\TqV/0*@<:fS[?Wc8P]VK-ffSZ>%!W<-'7/6$17.p+d:nY)sL=B/iObY7,%KQXF%JYWWP<No;b
+_5q&'aMs"8$=QqoD_Nu4CDYR4)?c3Q:fY=u<<fd]Kd.NYW[`[%25\46<3B;)k@tQ!W^'Pf$WS(g$5SY<
+I&O%V<n0/61K1h=@@aGb+K5EB'eE<q0Ip6bZLOWg6<I5T+3[F?AqY<0#(`QW<)p/!Q,0P0iQ0"dKrSL-
+n\s/5==aEiYc<F>R$l+A$gV%,!_F-*Wh?CR4["5`il)9A0">7f^F+Y:Ja5`JeuBZICj>U"8$?41k18HX
+pO2Y&hqYecCMWqAYc7mgR,%sY.Q9#^F#=4K<M6@DW9W?ZLV(./k1nSEh>Qla+'6!/K3<ck73U6^0>7"@
+)p+.*rqQ6&s8@0!I<Y_jrD,TLPe1MORPc\le&/1(ggH`HM.NQ)mD:ncK0W<<W[]Es%Lru[%D7,dp@7\C
+>ISJb$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<K]mJAe\9-t5oq*:+<TA_B`p`8(
+.Q8]UF#=4K<M6@DpSn;t")\)L%1W")pFfaZm$VLXftMl]n\=\@Lr$1.oqE"5eSHphn\l<@2nInX)r'-%
+eUMmI<@n_n^9^TeXE-6),ZUM;K(l98NuiEG\@8qkjiWkcp[@"MF(\!5a,1T^Wr/rW3O:.K$?)V54ZtrR
+5D0<]=uH0m^Yf"XGGR70hmsc!I.GC6s0[9'I9g,amf->&>?Fn]B;A]15AIkuiFH$AV7*foQ):ka8+$lT
+DL"IpY_@5&#(`QW<)k?IgAG>BEo`&R$Y@kg<Q0Z>qXeKGM]9uB#'9Bc0$8E!ROAhbea+195Q0hBV)L=4
+)Wjg[rH.AVXmTQ%Gk'eN=+4=/XK'jC@HKZK=t-Oo2$bI-TW3V<WlL/BG6@Wu_1SRCG'%hBhRpDILG5)1
+\6]kYXdT^]7>horDfK.RAj7U%Dh%XgG0Ah%i2')5B)Pg0iue<rm'G#1DfN,:X_"st++*@Z)1T)9=B4_s
+Au9V7KlsO&JYWWP<Ua>flIDqQ!$=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\?*<ZMU@A`s3K_Yg1cAOApe2$Athg%j#g3\A,e.EcU`jN>jObpgSGkpYEH*/)?i6
+XMFM'&8Tg40HWB"bl.D,J,fD]^qc:DIT^id.YIPT0>;BEp`&,c`XdOfK38G#VY#!L5PicgRf@l2ATAd7
+FhIu(eoN]U<:u9I$<1":<Sb-&?Y="ap+n=e/R*D[b*=H#AhY`S]dUg&[<+*(iWf##IeL*BRl3G#a<PRA
+Y?8'hhuE\"p5f4ETE"i@J,Ru;J,<IEM])hAO^$+:?!RuT:;a/<#'0l"=,urYbnjBn,H1`E'm*HRVl)1T
+^0LA4#W#pD.r`0M`*0Rg_>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<<fd]Kd.P/PcMC3>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#BQd<f4haUC?Cq!mt4d8%bE/%X#a5QNhd"]<No;b_5q&'aMoUh
+WbsNjj<m1Vkig+E$Ll.bT=f:1;V?J%0UOZ@]M9^+gC8(5O+URlPqsk]X#a5QNhd"]<No;b_5q&'aMs"8
+cBTn]Y$\s3nR'pBeHT?XiBAOAr/GQ*08!CgPr"r2$<6\YU^C($YY^V$XC1#.`eUQ$bu_%AFW,/i2S>HI
+*6\l,&P9Yq<u%\!*D%!EWaMMMK3?%-Oc9lO'ms#`rH0%HD@_/lhTaj"?uG+d8F"1)W^'SQ8?+pT"Ue<J
+bR,,GCj`1]i4\EWYL=o6.\hR>gt6"P_.taIE3:ZeeN8pD:fY=u<<fd]Kd.NYW[`h)<e$Z0nPpGE2m$ln
+f@E$N><eB/S>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]YN7lEP1<G5sodar1m%;b00
+#BRUT//N</.T"YWOt':._*V-+Z\!lU)]u(0[(^9p2UA>uB!*nVND/FBkGeS`]d'KX<>@&aAM>W:33r(J
+W^;WEB!9IX[$HW>g"gMb&"dhqS!qJ;c;b%JH:Lj:W[_,Lb$\8SE+Iur<J?`=h7@apN%`)ICGS17Qudbf
+nr,P0'r^C<8JS2(@%f'&=i%t;N=T#(e5`Qa!2JDq>WS5]5-\A2\Z.npmmsHD<<Z7M"Uc*iq7>%Q:KP+`
+K3<ck7=hHGd]0-^lK;_7B7nn.<6fHc.T"YWOt':.5uI<]cTZChGl.4%+-^_'<k:MJ=b5m-JZ9(*Wc$U0
+]s$pj`F#V:K/+M'bbb>+pOS%88]t$m_mud4<UWJY5ZItVeRQX[\IDeT+dZUk.r`0M:fY>&ePEr#nghJi
+YNuE0%m@;pTncC3HF`U*N)YcF<P9WV!`]sL<Hmiae_/VYqu%DV$=#4k<)j3uC<:\nbrhAQ4L&-*7+L3l
+eN8pD:fTeYFlUK$bEjl3l(sa#n*B6&WUp:]1(7auXqq8A.N#Ac^rr"d#ICh;S2">-: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-p<c++*@Mjf+cT<](@doo>gT<)rEo
+p%Ie-!s8X:59duSjp`7f<;I.1ZNAa:!`^!%<3>8[.mNa[iiQE\k5@h`B;]K<W9PBp'r\.j]k[6BN*X&=
+'lUX&j$nBBH[gNpV%6ug.r`0M:fY=ue_8o:jMA=F[`Lkdpe]L&Ot':.5uDd\\of#6*@FUPE50,-ji[/U
+b82a61cd[$l2D*'"g/XFW[d(6[GLZE@kag>Wnrk;JZ9'7WbrdMp%>`%[q@qcil)^@]RKg*Q2<A)'t@Qa
+W2bq\eQJg@N,M,$?qqS>]d'KX<>BUU-RU9sL5,-Q]fYOC)9`Fm0+(ZYaJJ0<hq;*$V^jQj;asdU8?+pT
+"Ue<JbR,,GCj`1]d,M(u^':>@K/mLBoo>gT<)p/!Q,0P0iQ0#oWX:>Yg>l5Y0UPd.<UWJY5ZIsq<>j!f
+(633YW2e3rWa;QCL;BB8Wd705"fa%SW6/LKY]+.56WF;4eQQ:^>l,J&0UPd.<UWJY5ZIsq<>j!f(633Y
+W2cMklLB_m+pN,2W9PBp'r\-?Tdk[t_8;=o%oj>Y<>AFm_dH4W<P9WV!`]sd<+Ra6=?*VVTrN_%Wk,1"
+@5+U<Wnrk;JZ9%mW\TqV/0*@<:fY>5e]-I%Ug=JlPYosmOt':._*V-+Z\!lU)]u(0GcBEd)j/X._439\
+H:Lj:W[_,Lb$\8SE+Iur<J;d#WaCBUL;BB8Wd705"fa%SW6/LKY]+.56WJhpX1!7&B,JiAUT/M`<M6@D
+KrPPn"UdgY5#bNqTP?)X3Q#6")-+?pW<-'7/6$17.p+d:nY)sL]iG$F&P9YQbUK<eG9g1]We,r)$t]YN
+7lEP1<N*TGN__2B0pkm/<UWJY5ZIsq<>j!f(633YW2`Ze<=;_c_dH4W<P9WV!`]sd<+Ra6=?*VVTrLHP
+Wck7[L;BB8Wd705"fa%SW6/LKY]+.56WJgi<V(XLnr+F0.ANne,ZUM;Jq^35Ad&Q42Ek)?R'E[!-^KZ-
+bUK<eG9g1]We.hIkih4XT6sQ84q)$,Xqq8A.[3pj4-_T1k*/`n4-D'i<C'o5rVH3Yk09rU*sT%Qf8B]1
+'oH"N:ZW";3Fu1_Nhd"]<KL6EE8Z6c_[P[4E4mYr<P_\+FXlSFEoe/nQr?ISmmsHD<<Z[YCY,_3FEI0K
+p+i"V_=aNf<J<cujK)9?pEu8b(sFG(Wco^gTW3UBlLa`@W`?+.CtZ+4h`_6c0">4%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`<M6@DpSq.j;j/O$mbKsChE:N?GC-.s@5'3N]dQ^b/i>hn>UZ,dqiTH\_439\H:Lj:W[asJnf$Z'
+E4mYr[75;P&P6)))9aP)Wco^gTW/'G3EBB<$<4,r/`55_qAP<sW\Tr?7TJnJW^'Pf$WS(g$5SY<I&L4S
+A)GrU<`K?Fb,FSG8JS2(@%f'&=i%t;N=T#>C$(1YKN4;RS2.*j7oBMj.TiXl?rjLNAE-&-#;uPQikh_<
+?qqS>]d'KX<>@&aAM>W:33r(JC)J[C6kWO##'7qcl5`G2W2a7!.YCm?_NBfg<JruR&P6)))9aP)Wco^g
+TW3TtWXNJC$V**=<4sZ(cb?W`172!0<UWJY5ZIsq<>j!f(633Y'!Ed2Hg@o9172!0<UWJY5ZIsq<>j!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<Hr?osb,Lm;
+X"&`^AQ8>_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*[p4<Fmk^N2k`s<.V%V
+(90-W'eC?Xpr$,hQ^95K.'0EWk*/`n4-D'i<P_"%#.&o8,9*77WZ$.l\L=PZ%-EdOeN8pD:fY=u<<fd]
+Kd.P/d&U?>b*fK6172!0<UWJY5ZIsq<>j!f(633Y_Q@Qo.VHVub$[s^<P9WV!`]tW<C]PL:S0i<rpQr_
+A2#NQ]dSP9Fk3PIb*OX).'0EWk*/`n4-D'i<C'K)@Ue?YQX??UQ&Q_A<hIVFAd,=RL#Au^nst2S4gA6+
+<`K?Fb,FSG8JS2(TW#G=gU77n((:T[\6faU%<Zf9\]nC@rX<BBXqueBW`ji\7[\e,hIA7;NN1it<.V%V
+hQ'CG:!K4jGN[8B`EW39qqKl[Vl-FCI0bI.Ja5`JLoLJMj\-4cM\sYeUT/M`<M6@DU?^h#jP@FlMnaDp
+fhH,aF,P%O-lB-+gf2Q9Y?H[sS2.*j7oBMj.d3nDKfhCW\o6K?NfK,YhgDcQ4W2HN>V?GMX4hhDP'U96
+qB,u`b,FSG8JS2(TW:+-0T^_3hpS>oY@#%NjdpO*;^`@:6CiFKHLcLj&[GQI:cR.YFfDM1:8OrVrfOgF
+]3F-QFCmG3<oQ:fp%A">r?u7o<n1#CeN8pD:fX3jWlMr'qtBE6M2;TV0!&BmHJu_p<AlYZ0Iuo]R`JU3
+)X(rjb#k]J*R?F;N2k`s<.V%V/Z:]c1=G%9^4#nP8p=Ys.r`0MfVoe0<6j-q=pkOUb,FSG8JS2(i4\FC
+gUE4a4*L5Xc-dOm3]]09g/R<((sE=l0+&Jpg4(?1H<I1M3sKcKW9PBp'r\.j^$:Rr2r"C^qsCkN[gWcX
+]3F-:j,HYQ_I-F&H;VbsC(P;O;QnlVeqLr119,Xj;?a'M.&sFW^V6J<j4(&,makOC>ZndgGun)!X]r7a
+h4MNb_"FF`A(?rQK2EtsF-XS319,Xj;?a'M.&r;eNV2u;'n=6BMA7YE?G0P(rX<BBXqudsW^]+91LZiT
+%-EdOeN8pD:fTe_F^efhE,]c,A7X8<17/fd?5-RJ2>X;B$0ti-0W0l0hN:cgWX9NA17/e-*81+oZ>-mf
+6rL4mc;b'ib,FSG8JS2(@%f'&=i%t;NDF>i-M#)q;-<!2K/mLBoo>gT<)p/!Q,0P0iQ0#/>rB$hS-eLd
+AMC$@Wnrk;JZ9%mW\TqV/0*@<kc1L?Pquj>Wsn-T;:Rak.Ss-]6<+5rJqYEg+%=`ThM@t6@@aG-k*/`n
+4-D'i<P_"%#.&o8,9%`n<MOpdP'Vh)XtlAC7TJnJW^'Pf$WS(g$5SY<HotG(q)'"#_E(a9bUK<eG9g1]
+We,r)$t]YN7lEP;W\Tj_KQXFQQr?ISmmsHD<<Wb1)9`F'Nbj*3XNFM6;-<!2K/mLBoo>gT<)p/!Q,0P0
+iQ0$:RB#Ma&P9[Q3`d3C.ANne,ZUM;Jq^35Ad&Q42Em+#jb`*a_E(a9bUK<eG9g1]We,r)$t]YN7lER(
+<b3c4,-5jN_439\H:Lj:W[_,Lb$\8SE+J!-HW?q`mD:nc)9aP)Wco^gTW3TtWXNJC$V**=[8c7I8Epr@
+\]S;T)-+?pW<-'7/6$17.p+d:nX6d7Da`$(Yg-5QF3:HH*D%!EWaMMMK3?%-Oc</%Wk8P2@:>_-%-EdO
+eN8pD:fY=u<<fd]Kd.P/)65NXRhXGab$[s^<P9WV!`]sd<+Ra6=?*VV.BCTjp*-SZQ,/tGWd705"fa'9
+W_Q;a%LrtT]m=hPHSUs'\6]kAX7A_*^4^X3172!0<UWJY5ZItVeZlBaQX>3P]tJj>II?P?\"'MST>@VE
+oqIOJjb[Q`_E(a9bUK<eG9g1]We-s[?XI/QBBSgFa/>i5LPLWmKu/EF$Z=b0;<e&gAd,=RKgO`D;q\de
+c;b'ib,FSG8JS2(i4\F:jCEUaqjortMRSP'N4;IWp=jLeq@Ek;!nW?;fsAh<D0MAihM$*T172!0<UWJY
+5ZJ!V<C]PLNuU/84?Ql]ASPu&:#_^*G?4PO#."B_MatEb=XpLoQ,/tGWd705"fa&ZT^(CIHefr2F66/m
+4#!$:\6ci+jiT.df"&q2:)R'F,H1`E("?70SitiUdBR,tL+;qc1Rg_)n.t<=e'jNT6O6niE3cP1;O,jA
+\:<T-i9lp*,7Rs]kGnE<eCW<=pS4*]=B.^S<UWJY5ZNNDWd`RjR58J:4$-NI?9L2)<dc\!rGut>0//m2
+ZcXj8f"&pZbUK<eG9g1]We.D=#9S=IDr/.`hVRO5q&Y/kKBo?7ZVG(<Y,i2c\]S;T)-+?pW<-'76rj-]
+qqV\;Z=QZ\G@iNZE4sbt`i7bDFc!Nm?!-8>K/mLBoo>gT<)laoh7%<pgVBXM=fsdiZNA`Om:uG?(ti[Y
+W2^D0<<dNeW9PBp'r\.j^$>+Vn@X&<Q7Q20_:K,*%9<69#."B_9.fFW\M@/"_Q(ad:)R'F,H1`E'tdU2
+FQh)15O%0m='hlu+UhrdY*\Dlqp%C*.YLfe_439\H:Lj:W[_,Lb$\8SE+Eg#k(d>@<?\HD%-EdOeN8pD
+:fY=u<<fd]Kd1A-o03A1WX<07AMC$@Wnrk;JZ9%mW\TqV/0%eSC7NGQrcD_O.YLfe_439\H:Lj:W[_,L
+b$\8SE+CQi23fss<=#WTK/mLBoo>gT<)p/!Q,0P0iQ10)mo7S/<?\HD%-EdOeN8pD:fY=u<<fd]Kd3''
+`Dk-Y>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;<GU\qcD4:Eotm`O
+?qqS>]d'KX<>@&aAM>W:33kTIT?<S3X6(_0WXS$S;:Rak.Ss-]6<+5rJqYH8?+,r<RECpGX6)%9WXS$S
+;:Rak.Ss-]6<+5rJqYH8;(YQm9;ceWZDpBI<L(Cj19,Xj;?a'M=K'AM<j6OR]eD4,1XT/Z-*4*Tk*/`n
+4-D'i<P_"%#.&o8,MOJB;`PLW<J//gb$[s^<P9WV!`]sd<+Ra6=?*SqX8RDA=uMX%6!#ooXJu]kPYosm
+Ot':._*V-+Z\!lU>HchsVf,TZ<jaT^WXS$S;:Rak.Ss-]6<+5rJqYFR4+j`NQr-sc6P3@!#'7qcl5`G2
+W2a7!.YCm?_NCrh-rW,>9%C/!<L(Cj19,Xj;?a'MW2j7=I!K75V##KVbR2@nQPs4Z@:q'&N)pHf=B-6_
+.ANne,ZUM;K!ag&)`Kf>4-(CD<rHnEAd,Nm93k=BgR62%ZW2Vn<`K?Fb,FSG8JS2(TW<D#b*DD_DE!:f
+bdjs$]`e1rEZ&Xmit,6k#'7qcl5`G2W2cMh3]T42CY,_7oB*/[\WXe!(H^5ANDCuBT?8$5<FZYB<<dNe
+W9PBp'r\.*W-\@7m=boa^@TW3mM?;o]]=OTeGGK9nYr&#]kWeKWle^E)-+?pW<-'77*dQJO,r21^RQ1c
+H-,T&M<QJN140j8#cfG-3Fu1_Nhd"]<KL7`I5"`(-ktN)SU,U.>5T``$7EHs0"M"i[G@grPMI7bHb'`%
+nUkXUl`6m*XK(\E;I<d<hN),h@9!a?<P9WV!`]uXWoc1+rVj@;f@hl3,CW7q9Im0@,HRn-_jiK^H:Lj:
+W[^Q?173.4K'O`*=".4kB-Du5W\Tr?7TJnJW^'Pf#'6kf8%/,^*5PRrAb[gP&6;d8F3:HH*D%!EWk`#&
+Q/]m9qpLCiVK.F;64..$AMC$@Wnrk;JZ9%ck#cmkZ7/#7>_^Qk$=QqG0W0l0hN:cgWX90s=>bS=)q`7E
+:ZW"WKbVD(b$[s^<P9WV!`]sd<+Ra61jiiea]2HQ?K)W!<<dNeW9PBp'r\-?Tdk[tU#%"uf1qA*V&,D!
+\6e/.<`K?Fb,FSG8JS2(@%f'&=eWh[gXo7rE%5KO>VGURX"&`^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`<M6@DKrPRDAi/utHp!hP0FQK*I);o+?qqS>]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-`j<Vk*/`n4-D'i
+<P_"%#'6bU7WRaWab@UR]`2>qOG3!H5%.H&0W0l0hN:cgWX9NA-9dei(NP]a\b+!S$V**=<)j4).YB`m
+<:u9I$<1$0:c6)*gs"6g281Wp_NBfg<>EBNb$[s^<P9WV!`]sd<)$dT2u,:N"5g_'iQ0#oWX<07AMC$@
+Wnrk;JZ9%mWl?ubbN<eS$b=A$E+._H`eUQ$@9$kmVi7Y4O]6n3$5TXr5+$cCis3;6YGiU_@H#;ILT1\i
+<Fmk^N2k`s<.V%V(93*HQp&aTk+>,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
+<J@ZW172!0<UWJY5ZItVe\d.Q]+'/G^k(b7l1=)aGB\7LfW_'lrRHP2as?\gMK>E1&)qn4TW1=\<>j"0
+UT/M`<M6@DU?W+>]WcSHg>[$8r:!'4M*Lt2dn9A<G;*)?_!4L-EumQ%lU?B)$=QqG0W0l0hN:cgWX@U`
+:/"?k!PEp^ro,MnPEYF#IJ`_<qN1aQT,o3Z\?9[bC*#TqlU?B)$=QqG0W0l0hN:cgWX@U`If/`VDB>E0
+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?O6<I65L,igEoo>gT<)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<J@ZW172!0<UWJY5ZItbe_Ri](Z-.C\$su[
+>@Y^2lU?B)$=QqG0W0l0hN:cgWX?>;;c?V-c^q7&Pqrh\"g/XFW[`[%Q,/tGWd705"fa%SW6/LKY]+.5
+6WJiEW\Tr?7TJnJW^'Pf$WS(g$5SY<I&O%V<`K?Fb,FSG8JS2(@%f'&=i%t;N=T#(_Q(ad:)R'F,H1`E
+'eE<q0Ip6bZLOWg6<I5*bUK<eG9g1]We,r)$t]YN7lEP1<=#WTK/mLBoo>gT<)p/!Q,0P0iQ0#oWX<07
+AMC$@Wnrk;JZ9%mW\TqV/0*@<:fS[?WXS$S;:Rak.Ss-]6<+5rJqYEg+0$fD<L(Cj19,Xj;?a'M=K'AM
+<j6ORj$Qh#Yc9IG;asdU8?+pT"Ue<JbR,,GCj`1]&6;d8F3:HH*D%!EWaMMMK3?%-Oc9lO'uHun?qqS>
+]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>W2<Iq.W@9!bJ0S`Xc'NFacU^C($YY`le@u(Xq'r\,D<`K?F
+b,FSG8JS2(@%jTS`t0>m.Ss+gXJu]kPYosmOt':._*V-0O!-\d<M6?Y=B-6_.ANne,ZUM;Jq^3?*^b=R
+W^'R<Yc9IG;asdU8?+pT"Ue<^4GNW-<Iq.W@9$kmVi7Y4O]6n3$5TXFGRX,9WWH08_Q(ad:)R'F,H1`E
+'eE=ln/1.P<<]BPKN4;RS2.*j7oBMj.TiWaiA&9+WX;^*#cfG-3Fu1_Nhd"]<No<M_IJB4<>DI45ZIuf
+.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^NITH4etk<rNsNS,;PIHPIl
+#*Edp`b>e-$6m"J*t^9l\%B2A%5pK?=[FVdo8O",6M;c@c#cJ?$PBCV;erdNIes"`oZ[;4CLr93P8AMA
+Ji5*<ELSqZOYlIDq^H?tI`]mfqV_M&ACdb^bXiF/)F[F\a.qgemrI3Nf@ckV&r^(PFX%9iSeb__/_pF#
+Cd^V>#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
+P<mn<@UVmCL0dd,MgkP]@OGOF7sCl5`01RNSnZtRkbq=Ah!hX1=)3cO#-qlT`g1WSsN&"OY[mO[bmm9R
+8Cjo55UGOF8N5,k\eZ%E"kjNI,knpV;CE*ps,^PR!QOsRSTh.l&Dc<s[_0IrYRF/H:MF7nhK@q0"o5F+
+jM'6RbnikgBWY3NlX$e`bK(`siLm^_N;GiOeM[gZkUgaCPAkKaroHhHZ(pXs]0NZC,DUnA_gEl8,d[;&
+]]+eLrH]'NFcBUJ[Cfu6'qkYU+H,SHS)D/t+b\PE9jPET(N>j: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)QMj<jpGXE`1:.p7f<1\7#I1R%Rc2QBJUrBX+-GLd1U"u:=S<I-iPUFDDd9ek+Xa9+:I"
+e7T1FCP^:5\?3Q06nrppmA?G:g_qXs04^ZEMYq8Hb8aF/3q@[@"(3Ag!?N>lFnRW!Fqf\KGQ3<:$P2)R
+6o@di0eoB+;=^YlZ"An:4Ff</Du>MRodhVR\7LsGNfKl6G?`MHp]\?DJ!0j.gf]691+^$JU>q7l[\Hc9
+4/a2d$!eJA2*d-Q`bhL5.Sl07GRkK\3oGF.`11l9'8M]6^)l<(?&8_MFCcb?Ri%<L-J7n:V!;lB'>s3G
+$>/*Ib`0J%O_L=qo9ldiUf21#d@G-3H$["t@>d#<S^&%]NLg_V"!$Te"caGV^ckVb]jc^@1255OIfIYa
+1pmde\1_jnD]g"NiL(G&\H^+a9k_q;YLGO_f?H[C,8r8SBAfp>']%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;<?(Fq%-#i8*2>89d5SY"KmR-ag2)R8%&"s4PAd`'K
++D)!j<Zku^@TFU`$Y!-ZAd`'K+D)!j<Zku^@TJT1r#bPj\lsuhN`ii4r-8T5Tu;o,-2`E/1CBK<'2$Is
+>f'8b0r&[o5G?#S_Bo,5&i0l]n.SF<`Fs%&CX-thhRtRL^(e?:QBmiheaKXj?>F?BM\e&;Cln-"9$G8A
+1M9JG+"N9g2<rVd9?$7@d%E\rk06Q6DJsH3BP?UM<G=UFr-6#-!N/_ccEg6^:FIq6,@^)q\*ngQ*'/1T
+kg6$.0\YaaDHErD(&LS;NcZN(/m"=bSND%M$lK]@MjT!ko6YuZW/Q4K(,>RPh`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<tSHf]>[c)B"=>sgY1@:1Q[M20?3<+3PDGfUE_Cn3AIEm
+9f"Q*o'\\!)a8Xolnc=)P)`O^Xj2!"$]MXcZX7bGp%9'7P3:b]oJa^Yo<!t]MT2$]n,Cb%k]EF70f>Lc
+I<t2!Ca?0LEH,<rFD3"kIG$.sk57<%c0FTZ1<Xb<iH4<;]JBMQSob(eV(SY`)=c)r,7r7709"S#9RtTq
+>?b<ARaJ`kn(90<`\G_eoB&cIYaG%N4QeeN`*Bs.^\?aLO#\O.0`j@"j-VN/63Ngl,d,7O:s:Y'j8&<0
+V[A>Bs3?J3j:_IKMjHZj-nRnra,V1.--r2JO6^c/X5e>DF)tSTZbO$_*EH0?V$`,@s3?J3j:_IKMjHZj
+0RNN[fH@>kEA&+'<EE8B4?<@gM3qAbe0:d"nR\f8E8a-"\RNO8eA"puBP<2iistM!P:&Ako'Dm69.o2l
+]m.;N3Td\'J*>F`=8MWpZEc7_QL2na1IHp-9s9Fk@ZHSB")"NVoB+<m1,`f^Cs6%IBBR3<EX@6aIn\(D
+>?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`qtKQoIf2<?iV2fu@urc*8#^n8r-@u#eY>TnWi2kPhj
+5p!p?p`upHb(RB@!/L<E3>;GVOR(^4#lJNujkk:NZ3kcY&=S<j#[F%A,V"U8+LDiPUH"VG3O=Dk?j!b1
+M=O%r,ioU=eh"c^d-69@AH?qtBF0TV8'qG`Y):bEjklOE^-JR9:/pO0Kro%?3kXVS7NX\nfp#0k85IRP
+cTCjsa6+/6o\=oB"Z3B4HQaNuf1!ZKJfHq/VF/r1:SU4q4u<gZCPmo(D_DgMu8nmGGZC?=_(r'=mo/fW
+eqS/L1-pEob:Rduj>tc-=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+a<ErPZ;^suAII<0c\o[Y(3atGXnV"mn'(OD>jTbKo>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<ce)''e^Jb*Y0(
+UaD'mhZml.R,kE:%5l,*kmgCGAigpqL(o2ekX!<<,Xc(csCgUD*&9AQllMoE9=\t#22Y$J[-gpuGT:*u
+/,!s98Q(!b7UdaIaMc[YicboLc^_M-.Tb#bcQW<_PHj_Bb.Pa\A-+ap=OhKsJ7.c8!jn*KBl.4K9%I7H
+e_WqmcdE]>%>RsOepR<hP8"onXR2ikfqNCMHmZEc7Kl-e!X<E5<QUC-2:f\#]_E(s&dHE(/L1VgOI\@o
+TSm^fj82YH^hTgS5Ph7Jp%daluMoshH=<inPE2n,)m%s-U`\Z7PF2Q?h+p$1)nf<=Z!]3!6;Eo`%sTD\
+EM>PCH@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<Q<%ud`4=A]>/@>TJ-HJ!5Mgi_RsO-So?Z:lddZ^mUa
+p5e!m@1%8cZHGkJSMYKs3$%1Nc5041<=RG%'_G_Hsr6?_Op1,UqSCG0s1QiR0ei6D.kr`gLOC8'ck<)b
+i<Dr/.+g8t/R.&qV)%1Tim$ST^-mOi96VD&)P!'m:\$hMltPT&-+fstP*F31ngVC$_'1`@AT*GoS-$\^
+o^l.rsqSb[sUjoT`s@0Q]3J@#@cF!?`;^DFcKgcKN@e>ZP$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_<nB!!!";KbrmZop4oKj)pTOQ!M"Fc2B,NGW`._YB/U,UV=H+rKmC><8!k7*bS
+S#'$4CIGB[1kaYbjj$8VY^!45s:?<22*rV,m2-'QI&Y@"=2L&?&6b0#UAfibd_i9gRFzzzzzz#_Lk4!!
+%NqMH-%%Z<r)+<=,HN_6-`^OiY_408k5mq:V%5/P&l',*bEn&kkjHKOaKn;O'MI''r5u\[e-F**B#:*K
+`r:<=("JC;M_#&#i/D<=*iF8mr2b;Updu-'5,0'2g^H)#T.g-'5,0]2GIZ#8sLU8#-bg<ItOVRW&]76`
+Lbi<J"MU;OE)q<4*rK$)B&eKN(Z*#6,iH$)B&eY#Hjo5t7m.&ZGn2Q!cqY-6.[<OK&F]Q!d[n']pM_<U
+GTA!i\dG+]T29_1,!U!i\er.ocptOHHPd6n3ae-('do$-1sgUagpp-.nJ4KfR4F<BDCi5cPGj8C`B'n/
+kPn5cPI`8sR75A>H4GO\-`r#puH__D1g],SDR"+nuVJ9!BgG>7.]BWS2r'[3JQUUaj36<%6$(7Rfk)I0
+`#)WY4YjeX"'g=/r\O<UGTA!i\dG+]SlU";B(^s,a5`]2GIZ#8sLU8#-bg<ItOV;OAbtE+]'(<$g1^.Y
+Tn(86%_qPsD>^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-.nJ4KfR4F<BDCi5cPGj8<q%C_1XW15GTdFY#Hjo5t7m.&ZGn2Q!cqY'P7XuS6
+>k8<=*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<or$+sUjOK&Gq;Pc&P)6%@@U_63uJjcWph;+2a`-qLSku1T=(Ddm/NMeI'i;
+FjN<$g1^.YTn(86%_qPsD>^,Z=$sm6,mo[/4rmHsB\2V5:"nj!i1bGeZcl<=*iF8mr2b;U+<qJ(>g+hj
+Kq%U;+/]'<)a+<@qfqBC"jJDVVblk09C;aiUEWVMc6DHAnaU9AkG.!i\er.ocptO9I#^bnf6Ps'FHgNJ
+T)LP:hK?K=h08daI^ZlW9.`]CUkI+sYa9cHa^E++b#cWW]q3<M)gK/mTt#;<2@kWj_CkWfbBGVP^<4o[
+dIM(;5e`0pt.<dGNi2nDM.Il06aKeV/\\oa0*seE##F\\]n+,VW:d[M#Q3YU`_V0F+"gWTLcgbQVE%VE
+^UMS8--F!8kD4c^b`1a6W$UL51P*X[6H^@,2("'hD>V?)^_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>EMrD4UlrpYG<t%!F^8)OS9[ozzzzz!!%N[pk(5#^E:53ljHigR!lfN$*u9e
+2.?1^X!Z')@+Bq`(Ta&Zqgqf]Am_cW_X%Y29pg@FWeoaql:8?-0dk9<X<6DU`1.D1Js[3\?4_0EbR7lT
+Lj;;1#r+)h3?dMg0djoP+Qe?E(W;gTo7E>C@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],&<eU~>
+
+%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<W?K<<3("zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!5
+N@_.C]9~>
+
+%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<UI864#;r6I5O*M'!+]]:t:,b&7K.>@b$;Zh(h((I%7Dj_5`0b<RYiUG333S&o',pea,t%
+YUUe_<<m,&KU$D-Ia>d*!FmFV'!+]-=`qfP;)Eh#C3CpM=HsGkWWP%#_TKkWl3LfuJlN,X<ZUG`TiQ2@
+c-E*fWtHUUF^QOa=P;WPGf%_*"KN13<UKMb,bJTgLAP_/-7)cZ&>?*+1EjWX7Q0,HdoIbsPq\l21\98S
+=0j\3_5`.o<?7G5&QUfq<Nig<k0.45JlN,0X#`%ta*fg,P7jrhLeuWQR1Ne<_]!l*YU[JhKJffjpR&5I
+gfLg<.D`$;#ZVn4#n^2rVFd@`ZRWekKJffVd>TfiY@_EFJlN*hW]DgI,-5TG'>>)r<J"3W_c1bj*gQV;
+Fs\M;Pn9Xh"#D+;b!n2_279MXYUX)&Wm8g5EF6u9U]d_2,#i\m-:Cth:pLK6<[Ck50RQl5k2EkSAnP"n
+<='8;3\V3kXDAle7KTV'X@a>/WZPn0,kojU9M2N=+[T06AjV09%3jgGBN?E]PqZrQ0/PTrpXSV(9!Bcg
+6<+8&6A1V`+RdLu<[Ck50Lf&Tju']QAnP"n<='8;3\V3kXDA#b0/dTGJlN-;<HVmpb^D?1P7jrhLeuWQ
+R1Ne<CR>"Vhl6g['#q*>#FZ(Z^7(Ta$!&??W9V/lLp42X,+^<k.`&-<Z)`2uMuh^8ZSO&HWe:XX*LI-F
+<jkKE/'BNJ$F)'hP,cgOrOB%CB2X=]#na0u0G>Y3CgFPdA@8rta/Xe!X#`&?b'c-/P7jrhLeuWQR1Ne<
+COt?i\$dd/WeAKp3]!u#^7(Ta$!&??W9V/lLp428U[R3s=d<T!6C+X#TZJtWX4`o*?r]OHWXd7+#V0,a
+$D%fE@FY]eQ&r&H(ReRNI5O*M'!+]]:t:,b&7K.N<6`V,D6$hLWtI0gF_:&]?d6ll"KN.Z<-;Rq6s/+l
+<HI]_[T0Ga<=*rO3\tujY@_EFJlN*hW]DgI,-5Tl._@WX<Ro5oQ&r\Z+)')B?Nlg]?r]OHWXd7+#V0,=
+.\HTQ:XpptQ&r\Z(X;0RY6%BiYUUe_<<m,&KU&Z)`NKubUh[lM8g(,p1V9i&B2X=]#na0u0G>Y3/?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"BHgJ<jbpDc#@%7cD1TD&gC:t?m\9Dfo1O\<.87U
+K<Qi!b,XisRGCrN1F^/?KEtQuQjF@*2O?CIa!F`F_lABIX2[.j;=:L<R?V)Z6A2?Kb_-6PRmj8;iaSpB
+@T>bf<J.pmKQ:GEcD1TD&gC:t?m\9Dfu\r:Q91pBNQ.uO>,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`.o<?7G5&QX'k0rO,._lJN'<C4B!"2:BW$hr3<9M2N=+[T06AjV09*Db=%
+\^TW>O]drGHds<Qk1Zl%a.rj=&gC:t?m\9DfeEWE10$]B#%pq&KW=.YbRpij`hWa<&gC:t?m\9Dfho`'
+HYm6BpoI'n;DnG>4d$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^>/<Oi:p\bTGKE`GM
+%D$(cK;phc<=&],(,.NLXP.bC-!6EdI+<q)Kt4b)N>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^<F6Cj#?@-PSWSP11hn!rG^u+Lo9Se221i?Lb1neXt=d9Q,
+C85G(YZ719U]d_2`i-K2^q>*mp._#7*C##NgpqLtoCM4iD;2LIs8D]j^46)bAnPc6eVMZ6Pe&e^OP/;?
+eueg`jdkPBD?$bcdfp+b[@sZoLg7@FiLb!]IoQ0n%2-3d6^a0H<Pl&A=U+$\8(XnLl[='Mk0<Ps7un]3
+0JF%/mqO+8Y%B8Sq\2uiWm,*T6L^ua4UD7pg-d86=HtR0eMRToQ$2NHZ%)fQIXgXu9F-!X2"Zg6.r<a*
+ZRR,IeL#a<8c5TJ@o@nY$/#RGoV^luT3lbuN&3414F"*f?g,:!Lj?WiH<T`)XkIkt,rD?2j*m)`&QP=>
+U]d_2Z4G182djkSc1PtbF6CiueC:a5a85ag%a9TVa%OmZj\Ems"gBJM.92ip1H2.FK:%m2<C3"&q;TAO
+#7hmB]l/2rrr$'0CcQk9D!1$fSa>.BF8P[up[@#.pD5kE'*94.9tpgH-Y1a/=I%7+WXYDb)cX3o9M2N=
++j7Y`5<kcX3HsmrpWGB4^[];_;c?WNCL"OKlDqC4h]D8'3\V5A*A7l5:E1K\eM:6ld&a6AGr4?YWf:Ca
+JlN+^eV/^2bEjkHGjo+cmrVqaKK<G^R$KhMWYWLPKP"5$b58T8<OEE49!Bcg6<+8&6A1Wk.V;lP7&jE'
+WtH=P[1P39j/U+3;M%mT&ZRlG'-aMoVjUVH)B_T$_H<)M`2`??jsF+<cD1TD&gC:t?m\9Dg%]c@UP_mU
+4B<abZtf)cfF=r$R7R]c,t+]s^eC\TA]pR/3ru>saf:$V<fR#(Y3&aobG59A&gC:t?maC8Z:.O3S[tkf
+b,U+a<t.5I2nE`t-7)cZ&>?*+1SMgR/Tk7b`&M)G@oYkEX2^ZtRpIrpP7jrhLeuWQR8Dqd2W^+%@u,*:
+PfKeW!icGeP8$hN)Ntf88r-<oJXK?db/Sg8.$k4[96mU2.%k=jf;]1V)N@;p<[C/4lG-)lAS'&VQ9-g]
+Qh"7D:e&uK1NOF66SU%+We:XX*LI/`YAEaXE[Sls;BDkOMQ%M/AhKrq*$ut6Pn9Xh"#HY1Pmr38e6F1;
+0BKGgTqn].B',8V#%:D]<='8;3\V5I>hM)\B=%rb96mU2.'.0FLZF.hNPM=mWtHUUF^QOq[^,:gQ9-g]
+Qh"7D:gVZXDu(reC@+\eYUUe_<<m,&2(RF]Bt4o;GU\\R#t7msI:b+;C;!;5YUUe_<<m,&"Y8?9Bt4o;
+GU\\R$)L\)G@iJ8C5knZYUUe_<<m,&"Y1P]@g-NXNgK<=Pu)GM)UCML=07BP$!&??W9V/lTNASqf1'Nf
+iHQf4<=&h1Xmrg$JudJAU]d_2,#i\m-D2mDc=sg&j*3#6<=*5<X_VN$]4rok9!Bcg6<+8&!k5J;SX)>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;<C3ad<]2bCWbOCb9!Bcg6<+8&6H5:5>4.5d]TgI:KXOm_AnGpbeI%\.?r]OHWXd7+N%<Z"%k-Zi
+_XUMFWtJT<[=U27$`h]L9!Bcg6<+8&6EZSjo0q_@MK$4ma/Xe.<it;fYSL99R?V)Z6A2?Kb_.Mtla4;u
+K]Amk<[G,Z.QoSsZRX+n"KN.Z<-;Rq9YRf-b:?"@?HQbX6>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-<oJXJ4Wb'FZ1@73(]b%"G'<I6L8
+G%T:k9M2N=+[T06B#7(hla/dM#ffS`X@f_^;RpCOX(H!BJlN*hW]DgIL0n)]jl&Wf?HQbXJlS/Z1,E,l
+4-L;Y8r-<oJXJ4Qb5),b,DgBPPfKd,X0+;Tb\Yhr9M2N=+[T06B#65PlX_e?(#aBdO&ZKe?.FU(&QgnP
+P7jrhLeuWQRBYhr3\?C9_XUMFWtMMfcQmEqZYFY1-!6EDTrJ2N@IVE#/WV9rnPUF/'!+.0fJBj6ZSO&H
+We:XX*LI.c=4U"sO!hg7;BDkOX?,!Y1UC)f1F^/?KEtQuQjEjqf#qN]/].$U*ifpT\u%3R2m@%?-7)cZ
+&>?*+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-<oJXN`YA\BWa((lRnnlV/be_pseHF.F1-7)cZ&>?*+
+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<O4J1_5[XF
+lCC"\VPg?<S2iD#gDlReBk=u6>]u]m#Z`6RF_h/OB<Z,o-7)cZ^fA'fj2T<e9H&X+QtnE<qh<L>G+OBn
+<[CjjC]iP9QHlORDd@)4;$g8L&60M^(?XcA2K%<[CY(0Dk$"Porjm5:3A"n,=bpHcJlO2GDe;c-<R7Cb
+\YBe=8g(.j]6A2ZeueDSpKm]U"*=/5J.dlT+bE]#<h5=:0jP64<.\*Vcnin,*^/dNSX^^f/*9DEj]RCI
+<J"3ukNT>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*+<F%@o\+\$!&@RXQmcm-fg:[YUUe_<<qY0NV%$VKU"-M<=)\P=I$+rWY`pIO?4r.
+We:XX*LMU:l>(k]<jk+k-!6F?<Pkb9SXCkaN_6N*+[T06ju&-<'r)$b>99gXX@f]HTjLf4k-;Il'!+]]
+:t:,BYjkK-&QZ>&<[Ck5?rb'oWm8iKECV<WPn9Xh"7lM&T7eGjCjE*SYU[JhKY^[e5a*q4UCsY#LeuWQ
+\XuHbZA>`jjfp;OWW]@8eu\.jk!nHm9!Bcg6<+9Q<DE5i?\F[rO&ZKe-!6Ed9N>`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.`&-<iN%:PN$5a,'!+]]:t:,BWEcXg79J76=I!8oJlN+%<HVmpL-$m$We:XX*LM1/?BlqU
+b*L)J"KN0:8f4t91\7'j;$g8L7&]Cdgm.W`)I[4#@o\+\$!&@2;:V([0R$KJ<='8;3\Yc.PA5c9/WWm<
+<[Ck50O@aDct!-#<.\*V&ZRlG'2Ln^=P<7IQ!dUD<=*/:og3`u\>'\<8r-<OjT@=2JmF%p/WWm<<[Ck5
+0M,8Wk#Jse'!+]]:poBe1N^1Q#V0+#WtML+YUX(uWm8g=\YBe=8r-<Obl]bGrf"(ejfp;OWW]@8-El\>
+ROmKZ9!Bcg6IjWsbilAB&QZ>&<[Ck5?rb))<P<(c1@D(DWtHUUD<J].^GZm:A]nR`JlN+X-"O"?be00%
+.`&-<#ZY(g0NW4YTa3/Q*ifpT9!Bcg=&inq*d:S8<='8;[V]ETk#<4,A]nR`JlN+X-"O+BbhV<?9!Bcg
+6If*Gba"_E,HP^,X@f]H_5`1$<W,1HN]S`.We:XXgG:Nbq344!PqA%s$!&?SQ&r\Z(RfD8-!6EDTk[4)
+k)*MrMm0MKYq!Si"KN/kW\'j2h,+JZ,t+]cP(C1'bATjPg%&71?r_eY#uK/1cckgU"KN.Z<,KOg>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`.o<D+@"lTOo0=P<7IQ!dUD
+<='C>D;3$th,+JZ,t+\8jM5$t'*StK[QIS:=I!8o68_bIFjUVA<.\*V&ZV-U0\8K?G@IXiH)sWB.`&-<
+b,Vm_Ap-Zj=HsGkC1.3cKP@t1@qEhn<J"3W<[C^J[3[W84"A(HKEtQc)jl7_:,Bc0QH-!.WtML+W46T(
+HP;W*.`&-<#ZXM9R,^:V&7K.n@o\+\$!&?O>W'UgPpgiUJlN*hWYRBEa-i5T0ik_F<[Ck5?r]Pd<K!K;
+h,+JZ,t+\8`kWR/>40=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<Fk4"A(H
+KEtSI4"uG\Q9-g%[QIS:=I!8o6L@drS!e%f=sU-],#iFC^`7W'm_i5\nlV/b<J"3W&ZQU)1Iju>*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^<Dqq^6e!k9i\al4#*BLQKkK_[cHYr&Ih7Im7qF#F$n:A-,drNLh
+Kjdu5<=)\P7'>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>Xa<l'tXPZ8%5Wb*-9c^[":
+mFnu)QT&j>9`OS/UIPOtbl.O>Fq&lDhXLUn_l<j?'!+\2?F>WnLB^Q.:f\kN=nU4>9>eLjA#12-G3n,$
+^YbZ<*LN<X/U'<`(fhqbQ!dTYr2PaGRH#Z3,-)?2XP3Ub^Jh%*/\>TYNraF[]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#n<ogPc&pJPdE`X8f4Pmn`.Z:+-!sXn(un0
+4?l'&DE2q/>kcE`q\sId0f:i=rQ\JOIJ*=$gB7H/:]kbVdrNLhKjdu5<=)\P1ok@9hH8F3UeP8t_PgHN
+(Dj8[\ME+-igtjXL!,f95Bu[*c0pTI0GcMsG<B1K<[Ck5?r]PRWlC[!)jdoDX@e:F29<R<d^7edZ*NJr
+.`&+fWtLk'2>GDN`2)p+Pn9X(OF_\#[[_'B'4YUs@o\+\$!&?/ZteZK[tZ4=cDlO'<?hpr_&n;j+i;Bf
+a/T9C'!+]=B[t2-T/JN?E)n8QW*9)6>i)jCM6XAJYq!Si"KN0(>;bckgt\]f=d9PlC:s^jOje=u`laUf
+WW]@8X@d/JeX/.6UQ^FD<C2A=2%Ii)\PhN+,-5U+X@f]H_5`0c<t.,F2a(kK<[C/4>1^P0nC3RtKU"-M
+<=)\P=Hu]JeS%_l'p(6pWtHUUX^tlmHWojU=P<7IQ!dUD<=*59%J<3/16P+:<C2A=eITmC:Wo:G.pF'q
+'!+^0.\W60EG"?BP(mFG6A2Ah&'U.h8sr>*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
+.`&-<nuC8)1RFFb!ilqX<39b?Nclup>(i.mO&ZKe-!6F/ACZ!=js$#?cDlO'<@\oRCOZn$]a-,m*ifpT
+9!Be=e>bC_7@'CL?</@?W(ITE/Y&+g0ik_F<[Ck5?r]P0<U4rC[c!r=Z77"a<HMVdkHs$$Z*NJr.`&+f
+WtNRX>0E!&Pb#06<[C/4aplRsSX)>l<jk+k-!6F?<C64UX.Pf<:Ke.,&gC<:#3H1-RB0&7PqA%s$!&?S
+Q-b8c(!k*@86S0W&gC;/#3Jrl6uJh,.pF'q'!+^0.\S.gX*h(L,tTV!<C2A=`5(1!Bt4meA]nR`JlN+X
+-+'I_Q1&=$8^`K!Pn9X(p4:n2\J&Iab@=P(<=)\P[&7YM1NRo+<lq@p#Z^I+b\d6-P("UVCjE*SYU[Jh
+KN;*TAnGp\d`P[W&><jYJ[s$l`E4J(b*L)J"KN0:8n_GZ$LUgHQF%Q2,t+]#CLD<h?7Nh7QH-!.WtML+
+BOOp;7GAS)XajCX&ZTF\R%9P0M6O;IYq!Si"KN0h=V^n?TQVZY=d9PllJr&Re+-*C<jk+k-!6F?<C1K%
+E\hcPPG5-NLer6bJ[s#I:sr-mPqA%s$!&?SQ-eZnd*1;-U`?EJ,#k+P@.kdbX,=-ejfp;OWW]@8>$dJ=
+DQ;ei+]]mWW&i&9m'PPDKU"-M<=)\P=I%8(Wl2Lk>FlO<<C2A=@h6dYb6(.7g%&71?r_eY$!,;71RjV:
+<C2A=isj4]5fmCG<jk+k-!6F?<J"44\S#N<'!+]]:j%_Jfumj+.Dj5na/T9C'!+^0C0/D&X@u6j.Nq./
+;$U^2j^CPZg%&71?r_eY$!-H8op/.4'!+]]:j&RbfkG_&&QZ>&<[Ck5?r_eYQc_'>S!7;\6A2?`%Er.!
+Gt8NJ[QIS:=I!8oJlS/*HtG8c_5`.o<JMZ`]e%1m,-5U+X@f]H_5[X=Xu,+D==,=rPn9YSO9;i<njjt+
+CjE*SYU[Jh"KS[^iVOlp9!Bcg6IfKKAuF`f79J76=I!8oJlN+X?.C2VZUkI(,t+]Ck.p=0QcBYGA]nR`
+JlN+X-!7RIZ"I[4'!+]]:j)9Clt%ok'4GIq@o\+\$!&?SFb'#MXA)<k.Nq./!<][Wo%5.fCjE*SYU[Jh
+"KN"j`ll1F-!6EDTk_>nfkG^E&QZ>&<[Ck5?r_eYf/2b'*1<%!<C2A=Wt*?Kb7fc;ZK8>@_5[X='!+-Q
+O_/3h'!+]]:j(<>fafL!:kY)cO&ZKe-!6F?eKGAd1RjV:<C2A=.gKZ>+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-!6GJ<F&j7
+]QEDdh7Im_r6@,Q4"Y+.nNsH0_5[X='!+-c!A>OaP[IJm<;"1d',riG=T?mB4aZVS^H^YGT6]RC:<V=9
+]F_/:YU[Jh"KRQbh[_-[c@/XbVA7/mPsA;3IJS&SPNV,lT0.C>mFnt9R@/[4%hAC6TDe-VS<gMQGTg1S
+?r_eY$!*V8W\a7XF[(.YT:J%nJZ/F^$SUl$\$,PfVk8F,k3#sXDY<mUVe(J#$c/hGWtML+Yj-nS&6:[D
+;$kpV5(<DhNQ3DBT'Y:=Ycs4,q<4T7qY`&g3s:T\JlN+X-!6F?e]IL]5a-<%^N0;,[dDt[DaSfbQ;sL2
+KaS^FG^T5OfqgWFg9g+h2rEur^GmeY4L#R$9!Be]WW]@tY29`iqTE6jqDGg)kK]oT3K;9VDqqX0bMg?o
+qu&2JrSRYArp)fGg&F5,bjX,d0KVAnY"#dFp[5UOTg2X)=I!8oJlT.ESc*$R\t<^ZGl6[,8Uf2F0X"\>
+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<J"3W<[Cj*4-L;Y8g(/%gq!$lJHNS=f=\2T"#IdR
+Kj[o4<=)\P=>`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?<J"5W1F^/?KEtQu
+QjF@*[QIS:=I!8oJlT-5.D`$;#ZVn4#n^4H4B<b1Q!dUD<O4J1_5`.o<?7G5&QZ>&<[Ck5?r_eYZSO&H
+We:XX*LI-F<jk+k-!6F?<J"5W1F^/?KEtQuQjF@*[QIS:=I!8oJlT-5.D`$;#ZVn4#n^4H4B<b1Q!dUD
+<O4J1_5`.o<?7G5&QZ>&<[Ck5?r_eYZSO&HWe:XX*LI-F<jk+k-!6F?<J"5W1F^/?KEtQuQjF@*[QIS:
+=I!8oJlT-5.D`$;#ZVn4#n^4H4B<b1Q!dUD<O4J1_5`.o<?7G5&QZ>&<[Ck5?r_eYZSO&HWe:XX*LI-F
+<jk+k-!6F?<J"5W1F^/?KEtQuQjF@*[QIS:=I!8oJlT-5.D`$;#ZVn4$+$tEF`0TtX@f]H_5[X=AnP"n
+<='8;3\V5K9!Be]WW]@8XI>\O$!&??W#B(`b%"G'<=)\P=I!:ElqC$U.Nq./#hr/sWtML+YU[Jh"KQ-I
+;M%mT&ZS=QL@SH3?r_eY$!&?S2*7!7X@e:F28V89a/T9C'!+^0.`&.<R?V)Z6A2?N[\5fa-!6F?<J"3W
+<mApc"KN.Z</n*kjfp;OWW]@8X@f^3Fs\M;Pn9X(KP`,u<=)\P=I!8oJlT-5.D`$;#ZW*c6[>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,ZI<sYVD;CR81G]h">c<"-RdJ9O^KeOBf3a$@mS.K0UIUC=#(W%HHE$bG3?9[8
+T:V[I:Cf%\?+ZD_Q\,MR<kJm<\oHb"lC%3(0PBpq)fMNB+.n&^H.l?7qsM)Lg"CK9R3%O@]j8nVp=jM`
+N9ANZ:VXYlq/X(49J^bG)c_o?C/&=@#NdRdpu-uBo>m%=T.H+BXF%Ptm*__'h5$)Q4o4]h+4Ef(bEa`o
+,W^dHnWX!;m^Z6V:I"fbmS.K*^]!P<.HAGjJ"-^n1gE3<giG-_pgV4snA5Pdm+]"QpLROWZq7`Eh5Y]U
+$F?t@0K1qWmNm)Eg?$,?77s!i>+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<t?B:FC<DUd%NfgGOBkG[F_:Ig:_jB08<6]r;#sK'RKTG
+jg;5*pu73GXug[bLd/b<dCF>]Hq9DnRTDFaf^Pe'rTTsSr9%^bIC="I[bq#F2cX`\G;Y`#aZnTk*^4t]
+b0;?%"#K#\Pq-Q=p&4TpFRM8S*BSH"^4!W]c"Xhk=0Hd;On<u*k$5?5hnEgRVW5^XVgGTBq>'Yo]$mbN
+kN/0c%<?/9h7In"Y$JY7Silho47Au/l:WeDj9nYlm!Pi$XXH\c5hGlVb3,CHrl6+1XR]_B@&>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%P<lC\+SkK:6,q]gP_
+*s$M&nH!5S%a92=4*E1JI-[oDlJ@UhV#<+:SBG6g:A:,^13=0+p?c)W6EJHUP=YT9q=Ed9>p: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/E<??@+]-mc"3)LEH5-H1^;t3j2T^@VGLg9AY0ur?kC4>ePH+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
+^<H!Am3hPqiPY'g;>!+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]r6qXh7ImCG3ukjs8MSQf</E6[VadYJ"f#Hk`?r8S2te-BR"-$Amo-,p$:4nPEQ4^*c*KE"+0_+e\o
+DE;!db?n(kT7S66j&oB4GT-QG3kXLQ>Gl,/8e_iQNY^:\Q"\3:C8L9"a&<?'H^n)(<R\[$Y\p&bO0XGD
+/Z^3rM3**IT4p[6kr$#e\5.BIR]HMI!g0\,.JnrpuAnC".+XX.ghT0@ZA+!87HrVH#EjY78b\`)N7gUE
+N@3Fk@E\TR4=Br.<JUIUB"WBPRr@-PmS*F.A[(K\"1%>fQ4qXqmY<A0F/n`.Y?L-4+nA&m9/B+(cq]S?
+4mQc8"*O"['7F6Ch&GrrUF=:t/aj_m\9q/^`Igc=/l1fh)R%@>%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<oBet`Cd5tQpn>
+h!VQmA0nEei7M2@)ND6mR2k2m-q5.pYFl`e48Gk'dtdqCCLA,A:K_:>SfQTi\9#4CO<S7/j%T0AN[e1f
+`fa>*r"J2R0F_3"<B~>
+
+%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<uZ29nkC\E26I<<$fZ^nF$qn0Z+e(+'KE]`GFT6$Rh]WUSEX6[LFFiME$zzzzzzzzzzzzz!!!#7+(
+985FC\pXe8#'b!.]%=f@Q@SQ(:-h52Umk=0L>.`l?"B(V">:2B2\gku1A.34<>;%U7lOa,f'4rr2ojp3
+'dNYH+^.^OEl"V#^Pr+F,"lHt[YEp$1*'#-;%<c3?CXp8l==8df]aGB[V0jj'DY]e"HthS$@.$8VY^!"
+'P`*M#EE^@fL;St>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`<d@s&]/M6Ljj:pbUrr0pW`p(sM8)l7-\<-;3[f"snG=
+Ti`(VP+cB(Tb8;q(*dZYb8:UU%2R#2%@?NfC:jD`>\.J8L;"k4eTC:AB8sD;&O$EUVaI]1l]<!!#8=eu
+i:oea`J,kOFtdSisEJ<e-r*es9C==CF!EDVTL8qK8jQmd>W.#GNsC_OS_Yk09B.GHIWH]mBADl-i[fmB
+0IHB`+2M?`^uQ0s.R[eq)XUU<XaL_ebCbe^`@?<%;HPkj7YXB8ioq[48oEK7eeWndCtQh::';:@K*q0E
+V:TTK)@;(jkuCM9jtbCR<<4kidib\8TJ04ErQK1Ggpf?!V;'hn46pk/7Y6]7'82a_6&[Rro`5>3rtOqs
+<auUIV(6<eIOE/m,?nh2QpKDu;%%mQ68DhnG*4['[3mSEAtr@bs(u)#sX:`>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
+jh<qS]1p[;]Q_@R"ABN;<).]=Ybi]f'9qp?b;_CC2it!!%o+h;)r8Z([l-h;,;D6[<s&-Bj8j\,r0rD/
+HBo()>s5=1#4-<E3%/5;$0LC"&p\.os/6O8V%\`17$?%6n1r-UhGJ!!#:S<j#[fIJWTpiF2&oes>XelJ
+dkJF`gD^DT?4?g&30geA*<s#d_,k7lHjEgGppOA(&6Lg"C'qXZ"r$7>hr+Rl@P7G>)EL!!'f13.Lq?(!
+*hl[WCWWfaW/a[E.Rqkd5'QWiE(>c=-/,lYC?SNNTJt'kV[$KferOc5in@Z=Ss+>Oc"E19D2D'AXH2-S
+<OhPtK_effJ2acTEW`1B7CTMO2S"XV+ju?*e.fKZ@dG)B,\cSiC^651?O8brg.6Pe.mL9#cSp)dEK+#$
+WgbWMt_]29qahEV]1eBZuer?foS#1OJi^Hsc_cCi%<uZ'-P2g3e+`N-FaI(&sRKbQ%VCpaOchh2dLgGa
+u4GYKs5M42uX9hS&XVpA4X^ZL>Q!4Z#"V`l&7kFk<XnWq,_$>MB&QpM\s']mB?dHdnW%/bAr<pU8:ZN4
+!G*>.d2B$hN,^Dr(68esYPG._Bamm%FKI?_.g_)#sX:5Gb-M_gR@php:@dB<JA=NP=^k20Jfim2nQAN!
+AA@d[sNEG=^DT0k85.A&iUb_!(%IE<h7>ld,)'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<P1NDd;H4i>"
+]A>C<<Z:nN^smU<?[#p6Wf-)+]S\D<?P9F;H4i>mtaqo<#%_j22$*C*`>u\<<Z9#+B8SCoo;QX86$gd<
+'scCj<7M,;H4i>"]A>C<UY`CJQ*@N82Z'C801Z/+B8SC<?P9F;Y?&@'hD=JKN(<@LZBUY"]A>C<<Z9#+
+B@ld<.S!`6:`teF\!QO<?P9F;H4i>"]G#R8/4Zq+bWf^=GPr\<<Z9#+B8SC<?P<1LfAq)'gbGKKQTg,;
+H4i>"]A>C<<Z:nN^smU<?[#p6Wf-)+]S\D<?P9F;H4i>mtaqo<#%_j22$*CSod#!l;^@t;H4i>"]A>C<
+UY`CJQ*@N8<q&nd+mD]l;^@t;H4i>"]A>C<UY`CJQ*@N8<q&n&Aj`-*`>u\<<Z9#+B@ld<.S!`6:dBYX
+:W779cd24+B8SC<?P<1LfAq)'gbHV_Q4<cQ;&Du<?P9F;H4i>mtaqo<#%_jQ,9U6_Nmn;;H4i>"]A>C<
+UY`CJQ*@N8<q&n&Aj`-*`>u\<<Z9#+B@ld<.S!`6:dBYX:W779cd24+B8SC<?P<1LfAq)'gbHV_Q4<cQ
+;&Du<?P9F;H4i>mtaqo<#%_jQ,9U6_Nmn;;H4i>"]A>C<UY`CJQ*@N8<q&n&Aj`-*`>u\<<Z9#+B@ld<
+.S!`6:dBYX:W779cd24+B8SC<?P<1LfAq)'gbHV_Q4<cQ;&Du<?P9F;H4i>mtaqo<#%_jQ,9U6_Nmn;;
+H4i>"]A>C<UY`CJQ*@N8<q&n&Aj`-*`>u\<<Z9#+B@ld<.S!`FK'`fqR*Z9Za[DfdZG5U"[4RHUoH\[$
+Da[eWnn>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<<Z9#+B@ld<.S!`]VkW!ldo)cFm7>NB[5eeT</a8['\n@$
+ADW6c\hk(.ZgY3W@F"0G=8GrWX@T]*>fW<nDHSZPDSFLqtA8an_(6]o0%fEXMF#<UFTEcdcl7Z^]&QBD
+D6@+86)"VmCMeC^T63'W<0%Kr#\hMG3t_uiYO7Sj;U?m<oET/OVNq_1+&mUDd'Z4cKl3o=1ohV"]A>sV
+u#]/X&Z3$2)R6+(pda.l#aKS5H1?MU"$-)&F&iEZY'/7o&dj.oP9ZX]s\H<qI>a$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<UoGP0otlCj<eHGQ3E"(/$F9f+QRU($CtT.%r
+*j%+/)RL&B:_0X<UY`CJQ*?uWSY.)_`Dc7b&W25QkfD/9LZ\<0l,?)<"ABL=1trRFM\^[q<-TKZ*/IDU
+^Eoti'#Ua-F8*qo%MgO]DqQpji`$ITeJ"dH22"T%a_'$J!YKK&r:s9_=lCc:FP40HrIkql5c!:OK*p,<
+:AVQ1Z=TO%NRTpmjSr_"[4TSVXXW2-<14I<NoHoSV0dOrakGsh@Z^gW@ER_.YV"KKJ!)URIoQ9;H4i>"
+d^^0fe<&!mtaqo<#%_jQ,9U6_Nt/fUskNT<Jj`j86$g*FjdrA`FE9JN^smU<?Wa?rrF=/]1;~>
+
+%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%<pGUl0,R,Y9q#,g#6oR:Kik,a$=;,*D,iMjPQK,a'dk80kg78L0KF,=s6<
+9Tg[u^%M'qrm%jPZ_#IfF2#uoI`^X:I)"ik^40JPF$AcgSlCRY63n`f&4-XGKFgHU+bUCn#U+j463n`f
+&4-XGKFgHU+bUCn#U+j463n`f&4-XGKFgHU+bUCn#U+j46N6-i657H)l_><U[$q)/eIjk/B#%QW"AZQH
+E)nQAdr#A[<D28WRfR0P&if8j^fe&J::hB_<\8mOD?cV29Am1H!GBpq3:1=n=g\CAgMM'L$4t?r$*1,-
+<d15;U!/ffRE`M[R=C=8im0->'rtmEo-Yh2U!@CNAmu<o<g,9PK;c,FPaN@VS^jO[`$"?H-=nM.;.ha8
+1GO\HX%A`N:*^H44[%%nPXm(:iUG%69?n<UYHNrX+W?>H?!S=^5n]*p*ZcAE.]B\BPWA1@%hIE2[\p%$
+bT`Cs<Duifrr0(X6@!kPf[uRR5#bN^NJrg(j8T&7lh,_=SXZ+<MMd;Ih-HR4lDu=Dc9"Nd7i5Xr(Y6]=
+&sCScc/icX\BM:cZg6rCfRVB]qg4<->[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/%CX<SP\N!4Bb]";`%pFUFGB\7DB4n0X\[a/9KsNTn2.cJ'4aDL*:465=RIr"RR6bb00Dq%V
+kND"ORtuss(P*:J=I-teEG_-rA]EaL/d`l>ZKC91-#+Xpj!cac&KD;2KnT!t0J1O7QhpRT`4"k6!k?NJ
+;<FA_K?Ol,_aH0m+7&4WO9UO.rV_-Y+lr',JA=N>6_]0sH$amBgP>iTCk$qa3#kC"%a97mcXb$9!EY%I
+BlHSX2BoTT8l=`@A.IMc[m'K`h@Fq('h$@4lKXe*RFH`T2e'%Z."Xn_BX?AaIq<S<:fNN3,HjAW[.i"i
+LQUeXMrjHt0^ICVP8X0S"@a#qqRCaB*W;7tbW/O5pniY-)#+!Ya'\`fSBCg,*i:bL$glEWroi_87&hBH
+]/DDhD3/3"1RS9(mHEaF<oY?&IgC`[o4gHcBDA>JnlpcY]A9W<m4:#$AC5E.*p=.\1NR/AN#%KoaI7rR
+![2u4E\a%VU(P0&`iueUK?>Ph-Ri>hp\+;Ker@"LheZibM_aJ7MOYe9./<fu(Q$A8O4SCEp%:j!d_6:b
+GW\e7$O!$Y2KE2Q4?l%HV:rBr/scqf&YbDs^hkoa#>#MY]j<@`i/h$8#W[j]iFm"B'1YW^;$Z[0F8>f0
+j?M:rI<b[K_N)ic6(Qu(9dg^m?q3^We+U]<WMq<`-2rW/O,)GB5q=`(AmrEPg!\:mT*NaL>a7b6h;.W:
+[$2FE#e!U1]b02PfFs*0EqZ+\k<tm">76$.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<W?K<<3("zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!5
+N@_.C]9~>
+
+%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$
+*+<RK&!9`+CsY2JrijtaYuRSgW^o3%IRS&$"Rl*P\aRFMC=9'41[">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:$)U<KeSmuM
+0NCGo<%Z=I%$RfJKc0!T&knQuWo8o$@&ekgWEPYq)(&Ps$T#q2-($0u<5TS&_,L^Y;RJ1l1/,+p(1rfC
+8ha7sWJ33,JuKA<UhX<bA=.0j/BfMdQ!gTp;[mB8"B-UW8CbRNa=u:^=I;tR.`#'jVAd`N$)U>9OK+#&
+OC>HFYV;j-<J%1_9Jr?&&knRP+]Sn++i@ik@:;X9Wru?IQteT*-($/*6`Lo66AET__nq@Q<sb`r0\(,3
+8ha73L/]bKKFF'HL4pN,XqFEn@&i1EP[LME%&mEt#oP'o%1=u7>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?QO<IU31EjYHX'oF+\Z5j3-(&F,,:/^,+[`7FAjV6o<l;e5E`WRE8hee7
+7S5@76AAGkb_-CgXbMNIjfT4iQ!gWNN0I_MKFFh`QjFWW>7Crrb$?<\.`#*%)(EC$#oQXK0G?38[Mfon
+Pe0OA<J%3)1K&h(&i-7t@4#NPCc*cf.!LqaWWZ60B;>Z/-"KQs_G&$)fkOWV;=5pM<!fE?c:A8=9$!*o
+Kpe$2ZIQ05UYJk$W=hl^S<4FXQ&m.h$SrsDA?0-I7_,Q&;^IfG3$T`:.Nq0Z(1omfa]?<rN,hu+VFiPm
+E(3GR<C2F>/^/kVPHBRn)<J"69U0"ciJa"/We:e[>*oU6-<goe1WiuLQn#pO_\jo><!aF?[4i7L9XYkA
+L&(T2gO1p/EE<ID8hee77S5@76AAGkb_-CgXbMNIjfT4iQ!gWNN0I_MKFFh`QjFWW>7Crrb$?<\.`#*%
+)(EC$#oQXK0G?38[MfonPe0OA<J%3)1K&h(&i-7t@4#NPCc*cf.!LqaWWZ60B;>Z/-"KQs_G&$)fkOWV
+;=5pM<!fE?c:A8=9$!*oKpe$2ZIQ05UYJk$W=hl^S<4FXQ&m.h$SrsDA?0-I7_,Q&;^IfG3$T`:.cFi8
+*?,q:)&YG`r-7-S=I9Dd07b[u.!LqaWWZ60B;>Z/-!XELRl5,P4*U,Zo9M%:Ie)RAamM0=rqtFk[V*pX
+amNtR-@!6-Z7r'/<J%1_9Jr?&&knQErD^7Ehd1Wti8EPgq"TCB#J.LXqYL%@GgEkVI_4BVrVH35Zm[k.
+p5prN"OEE"(1rfC8ha7sWJ33,JuKB[TB\#;gsXs3gV!C<F3g+Pr5OG*ma0<WH.*H:Gle@0_=Jt7Kc0!T
+&knQuWo8o$@&eloVL%t&g!YEV"pUO*N/ipa>IJG*qU%D!p\/lTG#&&S/Be7CQTu<uPe0OA<J%3)1K&h(
+&uM)^mbPKJ4l3_jBBtQKk07,BG3qfg]P>usV+[03IJ;Q=c<I_nf`f,&h.0q@GeO<d0VdE'JuKAV24TdA
+9U.<V</66a949H\\b3t&NfGu4epR,8$PtEH?$bPNNTIhX#7&H&"Uc(c4*GGG&)QLns78H^RluVrmdA?2
+8EG+h]+:SVh;'_@$hJ!qqnMM^SD(KR"B-V6C,dFaQmuN2WZd(ABP?VFJHO/tLCP=Sn,)$q%o-Dtr=3nM
+.Aj)&j`d(hmOH%P$)SWd8-u.cQtE?C,9tZe8]F7P=gDSWMA;>ge\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^mE9A77s<Wru?IQteT*-($1@@T+9d
+)r%i?h\=s#DAjN9h7In"WDf]Yp7<%3K',cL$T#q2-($0u<5TS&_,L^Y;RJ1l1/,+p(1rfC8ha7sWJ33,
+JuKA<UhX<bA=.0j/BfMdQ!gTp;[mB8"B-UW8CbRNa=u:^=I;tR.`#'jVAd`N$)U>9OK+#&OC>HFYV;j-
+<J%1_9Jr?&&knRP+]Sn++i@ik@:;X9Wru?IQteT*-($/*6`Lo66AET__nq@Q<sb`r0\(,38ha73L/]bK
+KFF'HL4pN,XqFEn@&i1EP[LME%&mEt#oP'o%1=u7>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?QO<IU31EjYHX'oF+\Z5j3-(&F,,:/^,+[`7FAjV6o<l;e5E`WRE8hee77S5@76AAGk
+b_-CgXbMNIjfT4iQ!gWNN0I_MKFFh`QjFWW>7Crrb$?<\.`#*%)(EC$#oQXK0G?38[MfonPe0OA<J%3)
+1K&h(&i-7t@4#NPCc*cf.!LqaWWZ60B;>Z/-"KQs_G&$)fkOWV;=5pM<!fE?c:A8=9$!*oKpe$2ZIQ05
+UYJk$W=hl^S<4FXQ&m.h$SrsDA?57;$Pm?9]1ObWJuOJ\;M(k\18JYn-=iK[2(Zs"GElR9"B,bBU]aUB
+A4Y7f9ZSp@C06j#mOH%P$)SWd8-u.caH3EUR?1j_eZ^a&gf8s*&kk0QOV1?QO<IU31EjYHX'oF+\Z5j3
+-(&F,,:/^,+[`7FAjV6o<l;e5E`WRE8hee77S5@76AAGkb_-CgXbMNIjfT4iQ!gWNN0I_MKFFh`QjFWW
+>7Crrb$?<\.`#*%)(EC$#oQXK0G?38[MfonPe0OA<J%3)1K&h(&i-7t@4#NPCc*cf.!LqaWWZ60B;>Z/
+-"KQs_G&$)fkOWV;=5pM<!fE?c:A8=9$!*oKpe$2ZIQ05UYJk$W=hl^S<4FXQ&m.h$SrsDA?0-I7_,Q&
+;^IfG3$T`:.Nq0Z(1omfa]?<rN,hu+VFiPmE(3GR<C2F>/^/kVPHBRn)<J"69U0"ciJa"/We:e[>*oU6
+-<goe1WiuLQn#pO_\jo><!aF?[4i7L9XYiTAsBo"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:$)U<KeSmuM0NCGo<%Z=I%$RfJKc0!T&knQuWo8o$@&ekgWEPYq
+)(&Ps$T#q2-($0u<5TS&_,L^Y;RJ1l1/,+p(1rfC8ha7sWJ33,JuKBm<6*"u7Rfk-+$]b0pTuUf4C8.m
+Qr39$_,L`<)U:k[VFlA:h6^nFgpqK=T=94DnrkFs:2snS1VrZ/OG+S6XqFEn@&i1EP[LKo,1Z)8\!N[K
+3&il]5(;G*JZc/Y^:q<1mVcM\GeO<d0VdE'JuKAV24TdA9U.l=HHRnLo_.u:Y<M^9F/;+90mOqN0G?QU
+!_k)eh;)B]0+/Qn]C!X8fk&u#*ruqM)Xc0\?Z,c:;W#FueJUpNnsqpj[aa>gJUur8USFUGQE=B#:A-:U
+AqCQ[JQ`Gf(GAk.V*)2qqn/niO/oN$/Sb<Ril0*;>qY%&h7H<ChRR]?l](Muc!.f'm:>\Vg!BAq>A%@S
+jo]rQ$aZ0MnPUE0(!FYJ5i(8`5=`A2\`Crq)]g%QKt5^qef-\33nCB81YKCar]&`ih4&PLQ7Z=lmR*pF
+GP;J^5<Z])\Z!]'"oiK7GuGY(<s+`dD#66P`QKQ7g/XLB5-[:fZel(GpA![^0Pi(B)!9q#0!5/7]0#"P
+G&$.!C7ql)c!C!T6Xh]4W6RZ.QZ$DFjC2JZ@::MX^eQR`JuJrX\'Di!E`W?c[hNF]4qu1/-%r2Mp^t8X
+gU9hdV=hk/-a59<4C8Ak8hed\XbD&JU37VQFkkM4(1rSb8g%#;\od2ZPq/>*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.JuKAV23<M0jfSfn0QUJu
+6AAGkb_-CgXcKFo9Jqoc)<J"*Z7p?)<'l==/^/kVPHBnF&kk0EKc0!T#-oq(>Z=%E<%Z=I%$RfSW=hl^
+OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23<M0jfSfn0QUJu6AAGkb_-CgXcKFo9Jqoc)<J"*Z7p?)<'l==
+/^/kVPHBnF&kk0EKc0!T#-oq(>Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23<M0
+jfSfn0QUJu6AAGkb_-CgXcKFo9Jqoc)<J"*Z7p?)<'l==/^/kVPHBnF&kk0EKc0!T#-oq(>Z=%E<%Z=I
+%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23<M0jfSfn0QUJu6AAGkb_-CgXcKFo9Jqoc)<J"*
+Z7p?)<'l==/^/kVPHBnF&kk0EKc0!T#-oq(>Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'H
+O/Wac23<M0jfSfn0QUJu6AAGkb_-CgXcKFo9Jqoc)<J"*Z7p?)<'l==/^/kVPHBnF&kk0EKc0!T#-oq(
+>Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23<M0jfSfn0QUJu6AAGkb_-CgXcKFo
+9Jqoc)<J"*Z7p?)<'l==/^/kVPHBnF&kk0EKc0!T#-oq(>Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2
+L/]bKKFF'HO/Wac23<M0jfSfn0QUJu6AAGkb_-CgXcKFo9Jqoc)<J"*Z7p?)<'l==/^/kVPHBnF&kk0E
+Kc0!T#-oq(>Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23<M0jfSfn0QUJu6AAGk
+b_-CgXcKFo9Jqoc)<J"*Z7p?)<'l==/^/kVPHBnF&kk0EKc0!T#-oq(>Z=%E<%Z=I%$RfSW=hl^OG+S6
+Xp20cF'M[2L/]bKKFF'HO/Wac23<M0jfSfn0QUJu6AAGkb_-CgXcKFo9Jqoc)<J"*Z7p?)<'l==/^/kV
+PHBnF&kk0EKc0!T#-oq(>Z=%E<%Z=I%$RfSW=hl^OG+S6Xp20cF'M[2L/]bKKFF'HO/Wac23<M0jfSfn
+0QUJu6AAGkb_-CgXcKFo9Jqoc)<J"*Z7p?)<'l==/^/kVPHBnF&kk0EKc0!T#-oq(>Z=%E<%Z=I%$RfS
+W=hl^OG+S6Xp/Vmb`>!Fm\?b>Mb;'PWeHk&@C#P\<J%1S.`K<Bb]-\d=dV3%.Nq0Z(1omfak"L3$)SW^
+_MiM:"'M!O/h/#3WYT25L<8#e<!fE?aMNmV<s)Tm3A)d)6`Lo66AEV5a&EAB)T]dSE`W?GQoQcK+[`7F
+AjV6of#L_rVAdOmN,hu%=dV3%.cFi8*?,q:)&YG`r-7-S=I9Dd0*G3"Qtd`O1Wiu4A7=W1W.^($Fjm3^
+F6Ci>IGXPVb>4]l3S?RYc_!H!_tNHM1YJ<NbQQ6.JuKAV23<M0jfSfn0QUJu_Xpd_^3t?B[Y'3bj5]/)
+%NO]tlK[^$]APj0If&MQch6VU5LZ@^38$g10G?QU#c:3JeNam^Pe.jT_T[!s$.D]CQ<S7kmHnasII=Z/
+9$r(INAj%Ic_7&+ntmiSncT_?K'*NX<!fE?aMNmV<s)Tm3A)diR]2lAHa_GXJUrC7/Yf)9jd*#Q[smtp
+rVc\cSXn4o"q*6'o7^SP"B,b?@7If.JLo"b(R6]!<-Rff?#&cPSNC%oS2YBCg9k]\HKS;/<\7Ugo^9?[
+So`boA]l_$NT_\s1Vr[:@&a?.%:im:\Z:94bT%Fa0[3RPLPJO)/[`.^`f?(>2/CcjoZdg\/hC:Sj2R)?
+]`n]HbLHb((*#DZCpFR8qr;dBGAl&$.aM&II0^QC<J%1SlL'+p=[H`H52,GBWK"M.W9e/<=3KI/qf&Rp
+U(SfKg1[^cUSB2?>\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?6S6<t`Yq)$
+[r'm.k'\/W[/LnM_hf$SG4"!@iE'QS_E?N?TK?dpApD$$9U0"cJLo"nG;YJ9RH("SVP!/%V+Xmm>hQbN
+frpZV]E"_A^V>%eWS_dZbh94/_djkB_G'HH+daaq<!-9j;=80A$T#gl,tpPR`ZHhdZ=V5Sq"Xjeqn'GN
+Rf.NXSiqLMIEg@qY?8'(USFUSqMg+4&!j*+E8eY_r[a^rRGdm-0NE_(#-oqXgsnY*Ar_dA\>W@cDVWnB
+e(?fH4j=JlGg5jLn8RE&\UF/9eT9lsC#&<_]C%_-q!\t>f"t,MHm+k,_,LM>Qr39d+:C!G73u2TWafOK
+qtBEHinMFHUSFT2p[11elI<l2e&"fJHm+k,_,LM>Qr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?
+a$h:b8CbRNa=u<DQn#pO"'M!gmV3jP1WMp1A4Y7f9ZSp@qNrbK<!-9j;=80A$T#gl-"KQs_G&$)g%q'c
+_,LM>Qr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=u<DQn#pO"'M!gmV3jP1WMp1
+A4Y7f9ZSp@qNrbK<!-9j;=80A$T#gl-"KQs_G&$)g%q'c_,LM>Qr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d
+=dW(SC*3m?a$h:b8CbRNa=u<DQn#pO"'M!gmV3jP1WMp1A4Y7f9ZSp@qNrbK<!-9j;=80A$T#gl-"KQs
+_G&$)g%q'c_,LM>Qr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=u<DQn#pO"'M!g
+mV3jP1WMp1A4Y7f9ZSp@qNrbK<!-9j;=80A$T#gl-"KQs_G&$)g%q'c_,LM>Qr39d+:C!G73uJ8e\Y#$
+0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=u<DQn#pO"'M!gmV3jP1WMp1A4Y7f9ZSp@qNrbK<!-9j;=80A
+$T#gl-"KQs_G&$)g%q'c_,LM>Qr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=u<D
+Qn#pO"'M!gmV3jP1WMp1A4Y7f9ZSp@qNrbK<!-9j;=80A$T#gl-"KQs_G&$)g%q'c_,LM>Qr39d+:C!G
+73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=u<DQn#pO"'M!gmV3jP1WMp1A4Y7f9ZSp@qNrbK
+<!-9j;=80A$T#gl-"KQs_G&$)g%q'c_,LM>Qr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b
+8CbRNa=u<DQn#pO"'M!gmV3jP1WMp1A4Y7f9ZSp@qNrbK<!-9j;=80A$T#gl-"KQs_G&$)g%q'c_,LM>
+Qr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=u<DQn#pO"'M!gmV3jP1WMp1A4Y7f
+9ZSp@qNrbK<!-9j;=80A$T#gl-"KQs_G&$)g%q'c_,LM>Qr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(S
+C*3m?a$h:b8CbRNa=u<DQn#pO"'M!gmV3jP1WMp1A4Y7f9ZSp@qNrbK<!-9j;=80A$T#gl-"KQs_G&$)
+g%q'c_,LM>Qr39d+:C!G73uJ8e\Y#$0\Q;i.`#'d=dW(SC*3m?a$h:b8CbRNa=u<DQn#pO"'M!gmV3jP
+1WMp1A4Y7f9ZSp@qNrbK<!-9j;=80A$T#gl-"KQs_G&$)g%q'c_,LM>Qr39d+: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&r9LW7n<IRqes"'
+L-+h5_nt1]ShC4F-(&G?$&,opob)N6mXCksa&(%BqWf`W'H^9\7&hV0S@J+/)]sFnZ18D5F1Z\2&58?s
+Wdu40.El,2>U^HsF"AD4$.*m`c^[":mX0)a0Xlu'$c,dKVAeZP(YW5F]Qh!>MhO0W&#oahe`dn4WHEE/
+Dq7?\[VT*Y&"u=e8hedt\)-t%g\[$J0@RZe=Rbd&T/<tG6'X,IeVgQJUSFS3>$Q)V^=^c4O*(cXeAj=E
+JuOK/\#qir*aQ/:S<qldWJ4d&o=K1</f!qCQ#N.j%LruWIOk3cl-lbQ[4o(Pk6o+*$)U<K<J%3)1K&h(
+&hYuj,=db&[29i%CON7;\W&^Y#c:3J<J%1_9Jr?&&knR@VJ9OW,L70XW=hmi@&a?.%;2I>;^GP5CZY@Y
+0NE_(&knQuWo8o$@&a>;F0.Q3#c:3J<J%1_9Jr?&&knRP+]Sn++i@jVA#]Z1-($0u<5TS&_,L^Y;RJ1l
+1/,,?VFiOBJuKAV24TdA9U1\_<=:T+_dr(CWWZ6HQn#pO_\jo><!aF?[4i7L9X[-^8ha7sW=hl^S<4FX
+Q&m.h$SrsDAF!a*"B-V6.`#*%)(EC$#oQXK0G?38[Q^Q\0NE_(&kk0QOV1?QO<IU31EjYHlXQpt;^Ie\
+_,S;i.D\si),djr'/E7i5,Lq+<J%1_9Jr?&&knRP+]Sn++i@jVA#]Z1-($0u<5TS&_,L^Y;RJ1l1/,,?
+VFiOBJuKAV24TdA9U1\_<=:T+_dr(CWWZ6HQn#pO_\jo><!aF?[4i9*]=ZgG#Sf*pJuKAVQ!gWNN0I_M
+KFFh`QjKW^0NE_(&knQuWo8o$@&ekgWFe,EqNrbK<!fEo0NE_(L,2_ZW=gt^Rc,+KA#]Z1-($0u<!fE?
+c:A8=9$!*/L1@0CW=hmi@&a?.-(&F,,:/^,+[`7INhJY68ha7sW=hmi@&i1EP[LME%&mhuLAI8e_,L`<
+8ha7sWJ33,JuKA<Umg6Zl=6gs;^Ie\_,L`<)U:k[VFnpkC5-5sO/WacQ!gTp;^IfG3$T`:.Nq0Z)31CS
+VFiOBJuKAVQ!gWNN0I_MKFFhl3/79s.`#'jVFiOBJuOJ\;M(k\18Hs8OsEW16F=?d99LPlOsEV^,Y=.E
+8Wk>jOsEV^,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<_<KFHJ,J=Kc^?SeZ)`cu/Li,C]+=1aGP:CVP3VE6Dq1jf4p&Z+
+\T6d@2r)dJUSu-/q=A6K=F$XmmXr*VD;W'U_1DiM0Y=p-G44;DmN]<K^%B`GZS1C2HgYVBTj@10I.4sa
+!P&+%gLn5YQ]n7nlG:<Qo$m6GJHP"]l0IZ)iFAh:_1MtcSg@\NF,8AI=-mPAJ,]&3bH;mLHF1;r]6s9&
+:J'8g>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#<I2'BSlAJ^\hZT&*/UL
+*_=Hf]&qQ69VachbiF[<c^m:(53'Go5kC:>9?Jhj[r.bAA#Rh4'.:p_o%7H5Q%)o'6_$8aElX1\jh[j?
+9.>36gX`[.92JCZ;l<AJYEh:DKsi[B^67H!^@RnBn%H1mS6D:Io_8*(G':=k.-bg7baJ#5qY]s2\,/F*
+[X@oAYC5eRV/*L95d#>^<id^1W6MBXqq^_`p9n!'g@!e/b@fV@F(afuoB+;%]6<S#*CH)a#q:p(kKZ#!
+$T$kQ^\r#kp8+*^CA3;/TX;?%Pb_9jNfK,i*dOR;LJrg7)`E*()rEW'qQtr:b,SIBQ7cHhpYC&>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<eg5*ZMgkQsmB&>(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:ADiT<U#[3o=^oo@sE7nh"h!;<LuM&n-d.6kA-H[XB;a%ltYQ<KT08C
+\8\.Ve_tO2q;db$.o^fB;4c."-QmWhF][f-XhSLHQf_(nqNJ\4.98J-)m<[fr8k6@hVS)"kEdPC1OL#Y
+2f<I2C$_F_*Zok5=dYt#gU?L$s-qcpgL$i,Vb]8N&68>jNR$UgK)]as@?+7"2O.Qbrb^H=mel>!b?+DD
+Za6tMl#!oc<h>/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^-<r.sI-n>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'<ZfR_$?V0guiRgp!t-FG!,g&oABTIpR<gPIe_lWbJ.,^?<qFj(`4[G
+]ThSWGKZIF7Re.F>'0/>f9Qs<em!(;\rc6]^V8L0]PmTggUD*>4gn8'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+5Tkt<cW8WqRU99q+Xe[4kXiX&3^0)bLWpYItj^#,"C_$;&ErUeRP]tL9"/ml?*r4Dl:
+BP;(b:Yf3AVtct#`fKn3fs7IMNf@7HIgf/:,Y<UcjY)*to\0\]s7)/"/@YZOVa#$'SY@h,g9p8/Rr<3C
+.jq0-GBS-N4aQb#X02<Pc[OHXrR@fFjdm:c)WmchUZ5S9qH!3f^K+*m,Y9e+rVH2`IkG!9nEHjC0(!l'
+NZFr,2f<>WHhlH]L&&tPh^nd[L%_XXhARIXJ,]9,YJ'Y`TD[atj\<G%]qmNOLA\jm+-cb0mSg:&p=a<,
+%s%?n=$QB-;hkl*T)&^30+N:&iJ5UsJ+:JoB?cmqg7KZ_OsF3[adnunc+f0B)X74phd>)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]3</UFNYJ,>Hd%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-?<ti=ruZm[S9*$#fmE^>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-#^KieucF<WkI-ZqsV;'>IRAJVQ5%?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&<X%b7;leGjs*>A"gDb(D0l,>k`EBis"pP8I/6JRW<Fap8qsA'.c2@@s
+O-`A8Ku]H[?9*)hbjlLH@96`WQ#<WaI-n>noWoT*/$?-UUn]tro&T$!WepO<N`j]<c?7*)4>J[dVtc96
+mO38CWg_=lYoMQVhle<tb`9/JG^-`,qp<]OAlX29X$t6IVYMndL@207995pUm\W^Jb/)&T:5K`f)jV@1
+bkb;;o<$Tijn>mlqF\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?p<h2'F7FQ:I/%[RDK!8bk&M&&bah-tNQuYs=0?Al^3T<^c%FT/
+=o`?[;>i"1Z*=/iUQ<FLp!,YfB`#A`lcSTuNU(--Y\#L'ZqZ9T@kJN*jU,-KDVWl"AnA%uqNtE'f+LNe
+YLZS$@HCg)DJR5c#qE8i]=QFS?G%<Qp%@R5RW?i5qQnL3/&#F?L5i[C\$oG4.SH>5's6&c\pESeU#K,7
+[i*GG<Fc$t_[gm[HFT*dG3mkTQ%la2@pjR-3afXiMPYc)WSVjkE&pGR<bbisP8Q/g'^mTJ`8"MXbaC8.
+k0,XLd^-$S3-+)98lI4^XB8+K>e%$9Wcl$\gA^#_%i4J8d0Im1+_AmBLGc0$f</C@omaQb]Cc6T!kWhe
+cq?XaFXP/-^8t9Ak2I[c4N-$cF/to<M>@18_Aa=/W;]YhjY!)4<R/t<;"K#tb@.0D^[0K"\l^bL2rKYL
+RiK'C0r&4i1HG^pULOik3X.=6@h:FW`W[)5E1S\DR*eh-(M?Dc@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#@`]9
+Ad;e*+D(jfNZ_p=@ZDRC#@`]9Ad;e*+D(jfNZ_p=@ZDRC#@`]9Aqu5_4?,=QO7QDQe^B6"Id6TP="C/7
+$(C)@b\TPg&]F$$bRqe[5g0hYlp/c,`9]b5&]F$$bRqe[5g0hYlp/c,`9]b5&]F$$bRqe[5g0hYlp/c,
+`9]b5&]F$$bRqe[5g0hYlp/c,`9]b5&]F$$bRqe[5g0hYlp/c,`9]b5&]F$$bRqe[5g0hYlp/c,`9]b5
+&]F$$bRqe[5g0hYlp/c,`9]b5&]F$$bRqe[5g0hYlp/c,`9]b5&]F$$bRqe[5g0hYlp/c,`9]b5&]F$$
+bRqe[5g0hYlp/c,`9]b5&]F$$bRqe[5g0hYlp/c,`9]b5&]F$$bRqe[5g0hYlp/c,`9]b5&]F$$bRqe[
+5g0hYlp/c,`9]b5&]F$$bRqe[0[Z9RFY8ks\`p!/nRLdqb=#j315-+&$hQd0>WV"6bZm3k_O<K*@En/K
+n81Vthe"hZEs+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">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;`<iad!LX:M8om0[iVO%UXki^0+5g0hYlp/c,`9]b5&]F$$
+bRqe[5g0hYlp/c,`9]b5&]F$$bRqe[5g0hYlp/c,`9]b5&]F$$bRqe[5g0hYlp/c,`9]b5&]F$$bRqe[
+5g0i5H9HZ^*Jg(-R!&^H21fOd?reM>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<Qi
+:9uC=VhCE;H6]gNVP'fA58rDr8:o`iWIScCR22plFX)/R-4?4$<H1_eOGJY$E7s,==gcM\[mJ\o_Z_T\
+A%\,=d'+H;n]<jun(bBM1)%T7?jN#5%T]"V7m`rV5E6RcNY'H`l.upIB)J]4(3-g=0JZrO$qBk%h06p?
+oQ]DM\,^gMcQJm4GNUrV"taE:iAA^eo9X!s]<L<qUBRUYZHPIer<Y+gnL\J,ONH]Qr8?D:9$GR'i%)eX
+\dGQ6V[.[_2J%X.^d*9Eu(Le?&)q^VAQupVC>#*'C5ZV*oh1mC1Kl<EE]0i.DOY-<[L`m^bn1I5Go-SP
+/RX\&J75^VARpk*p:(k#p*&O+s$[[m)0E*A_p->^]dI**jr@gS1RV<i`q3o#m(Wp=T<=oC'?`c>)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]6<Si4<HY./9j_QXs%E)b&,+T($/H0RPT6QTng!Mrpn5c7o->Vk09Ci`"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#*Z<pXIB&C(GIZDmjm<&7)5-O@[e+;PtER21fHdClhcFri'EHX6!#64`(zzzzzzzzzzzz!;N2>g"D
+d]RAHJY!!!"J_:>SfMi*BIREV-ZDjFo8#7hl_#s<V1E"TN95!IiTf,$"[5()!@IK0<F7G+ureC:jHkKZ
+NN)uos=rdS.\CagkbZefrc_M';(^nMS+OcbdSdaFm\O%*n#\M1M#[Fs[jbQ%VCbK//1:T_L)5(1#:U8"
+s!EcuVLn`R,iL&'T=Ha]Vm@@imTq>0dM,/!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>7<icCD-e?Ft
+B_Y--%ZF3h/&=e2uY)`MZAhJ9kKb^FsNo=K58gs#/F=>H3r?af*8M`*P-e(_?M[uK++g=t@fBJtj'S?9
+n'#GNZk!!!#BLt+WRDkj)&DXl/O90(PPS2tf8<h;Bs/m=I`ArDS*f</C`a8ksIDH:5A8&'HI;Hm@@/"]
+%Umaf;aF!9RLCYcREX#hE@?);DaSiuo4F];FDgWK'VEOV<b8k-HI]4kN_GVONo?Jj<+TqS0M:"ubD1%n
+@ZAb&.L#_a,r!+<?^rVEpe\%9/^BT3bM9I<\E?62<a\"]UkUnf:!K\k#"cL1+<2_8Bf<S>gK;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"(:P<?Wdj>ioP4&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.<fWeqpGk:(BBY`]iA48pUoXBCn".G^KmEtY$p@ot>?,(LGP$<lP-)6Rlq$$H]j2^
+S<H2Lg+*^+PAB1-Ao]>AsVaH:.0r2"T(g1C#*H[DMXD>s5$p$poj.*MYk!5F/e=0GeEZ_G(YJ=;oF)fX
+iW%D2Z&bo:H5<j9o?3RcmCDgg&>GrTQ[aTJj%X?Gj/le]=%\$*!]?2r0h6s!uY(Zn'(k8.*5[dJ8![^N
+W\H0<JDF8qHC<naY1Ya7"M\eC7/1V4DCJ#UiC;0s^;,BrS2Ym,*,g'",Z;4%@a!.G1@g;<[cME5eVqXp
+TYWsu\[Z*?$-`Fd-tD>\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)]<<Z9#+
+B8SCoo;QX86$gd<(!$?qN.]YW@F"0!i[Z2Wcpk25cP/7U`.)idYCd:86(4?'hD>U<4sMS"]A>++k7.dO
+?TJ?5cP0eW^*QkV!8t_.ZgYs#cWP4L\)`i"]A>C<<Z9#+B@ld<.S!`6:`te3M[1b.ZgY3W@F"0!i^K9U
+^Eot&43B?bTVC*<In*a86(4?'hHk:,#oacKb.,k@1MNhWX>Q%5cP0eW^*T@&ZGi2.YNmu#]iNY;H4i>"
+]A>C<<Z:nN^smU<?[#p6^U7;['[1kob:;[<#%r(JQ*@TeN4sT+B=+,;Pc&tFEDUUob:;[<#%r(JQ*@Te
+N4sT+B=+,;Pc&tKWo+<LK#!Z<In*a8C]dG<?P:q&5o>Y.tk->dt^m;86(4?'hHk:,#oacKb..Y0Iqg\B
+@DW8'hD>U<#%sS4!IZI<J:u^V(aEQY\2F?<#%r(JQ*@TeN4sT+B=+,;Pc&tKWo+<LK#!Z<In*a8C]dG<
+?P:q&5o>Y.tk->dt^m;86(4?'hHk:,#oacKb..Y0Iqg\B@DW8'hD>U<#%sS4!IZI<J:u^V(aEQY\2F?<
+#%r(JQ*@TeN4sT+B=+,;Pc&tKWo+<LK#!Z<In*a8C]dG<?P:q&5o>Y.tk->dt^m;86(4?'hHk:,#oac"
+a-iK?G64)iC!/Q$F9fk(Bm]:+B8SC<?P<1LfAq)'r7hCIJ^I][eB_!qoQ`(bR/`)'M)5T<#%r(JQ0<-;
+?]rud%rc]2HF0lDe5<Q]Hm9d.$1G1W@F"0!i^K9U^EotTLeb1[r9m$?/*#;oD\^c.EkG=mu%7H.ZgY3W
+@F"0G=8GrWX=V_5CWO?D;2Lg2fIE@643<+J,H4<@?-i7gP-1+Q@'2,]3W)%li$\^I^UbgWWK/G8#-KfW
+ZX@>r;#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%
+<in'&JlO>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*S</R&"+KW[@Xe0A]G3.W$o'OH0f'4m1CPuo$o'OH
+1"Y6![rl!&o1VA^gU?-%U5ufh0)NA<%_l>RhllZ20n05pk'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<X&~>
+
+%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;!<E0#!.pmDX)1Bm!<<*"zzzzzzzzzzzz!,ufN'A_X~>
+
+%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;.<A-l.QB^ES?-
+Z[\d0&ZR#6aO\m%OgZ/r#uee%,Rq#I-MY4<WMd5GSE0VDId[)[?g%ASME*c<Hgg:7:VJhq3Tntq$nP_t
+3/bO0,CP!L]"1L`4n?[rqsH9>'g?9Ph\,p'eE:3=qH)7'&0$c^:fW(H$<1#uW]:S])f&WK<B;@Hao'BP
+>eS`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<Q,3e$?jB.r4_:
+1J^*iiSf*>$qV24W^'PfW[^iqS;3n6b6kSZD#M7m@o(AkY*[4k<.V%V<)oTk!`dKj8BS<@:smE4LM%ss
+$@''e,"#63W[^hFJZ9&V2<Q,3e$;<j.pM2ccAaP=HamB+6W+)f.Ss,rWX;]_k92;'Z9l5T>L1dDnP7@N
+Y*[4k<.V%V<)oTk!`dKj8BS<@@(ER:c?FNC?6Va7"fa%K<Iq.W<&Nh^.FYSSC1S?2EqXIc7#%KE<)oTk
+!`]u6C<f1EVIbH<<n:f%cILS6iHg=h8JS2(5ZIuf.T"ZcLSRVe$>H\[c'*4OSoFCB]gRUN$<1#uWWH08
+W+sUF;Pn#Z/6B:Zak?"4m,<8I$qV24W^'PfW[^iqS;3n6b6f:kp+*OFc<d,^p/3al,ZUM;!`]u6W^'R*
+*g/91.sP1L;T6a<[neu1XI=2U<M6>n<>DI45ZMkHOtnU>GZ`Y)Er1Nrl`0bK;?a'M:fW(H$</nG+XtmG
+JqoVn8?SP'c:uje+K5EB'r\,t<<]Cko8o2N=i&"u<i=4jS5U;po2jI*Ot':.JZ9&V<M6?P%nW1T'g/R6
+m:pm_3L-j0hc3r+W<-'7W2`*_"fe$_OHaH^_8FD6W8S%qjk(PE<DQ/c5ZIuf.Ss,rWocDhAQi.(Yd29K
+IDT,@2Fgk,CEuoQWWH08W2`*_EupK"/Lu'J=dA?nk*W2]KVSrS<M6>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<4<K<HBEcTW/&n'r\,t<HF%n=0>d7"7WI!]A?*b2jV'?e$9#k93HCr
+bu?Pj;?a'M:fW(H$</m\83/^i(^AnY`o@\*b6hK*o;d=V51A48I:qpO`/1KZ<<]BP:fW(HjqLC6H[At#
+=t79Oe$9#k9,.eDZc0pr</VDiJZ9&V<M6>n<QutUDV_n3USD>: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^^&<Iq.W<)oTk!`dL1,W2!U#6tK0T0?QYnD(Qq@H-+VHhSN/ZH;!^,cfI3BJhp,
+bYubdi^?e>WWH08W2`*_F6OrU]m=fs:J\P$Q"aBPMoATU)k5NS;'et:"fa%K<Iq.W<&J<[o\-]g_o'A<
+08L4;jXj]FPku\!b6hK*pA4X"H;+gfAlG4u$<1#uWWH08W+sU?BjomfDf9RaQAZP42DmAZfs@U3+(8>R
+i*oRY\gfculu8[K:fW(H$<1#uW]<MrSt;P9k2mmYNeH;NNfJ:)FL8NdO,o?g]0FcM!Z+KmcJMK>HYHK'
+B[i"T<Iq.W<)oTk!`dJoP(petgU:rn9hg=CZrk(lrRfcT*;=)L<o@Gd8f3NaqYGC'gpqIeb5:T4+-t]!
+p8Ydt>TGe$'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_1TFa<CP
+q3G#\G%`.?U+/u>JZ9&V<M6?P%nW1T'g/S!:ORY>Z]<1h4S$S,0<e]u\10Qs3)MQ3W^'PfW[^iqS;3n6
+b6f:kese?%(LAk7er:R/I^1Uu<OIC%BQ0[q:fW(H$<1#uW]:S]R?eRMXK3LP:>WO&B:+kj5J_R4e'!0)
+%A2@S%CniY'30U`$<1#uWWH2FGt?)bXPdMuSHomLaX^Y]4L[HAG3,H_W]&98%8Kk+W[^hFJZ9&V2<Q,3
+e$=R\Q-J+3UIQE:X^je>o)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<M:Pm*P6e1_s%jTW/&n'rYlm6W0dm"V3,QNfmJ!Rod!^fpCd,C7]W_3),i_<)oTk!`]u6C<f1EVI]oA
+/";.$[j^.70O!*+f6OQg_]9ZPPNENY'r\,t<<]Cko8o2N=i&"u8sOaG32q:)c.,b;q+-r5<)oTk!`]u6
+C<f1EVI]oA.k<CLX2@NjXD6j,F*:>: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!`]u6C<f1EVI]oA.r2!P=,0_4UoL>j
+RNkDh$rn%@W^'PfWg`O@P\F::_ogQ6g/j&.MkfRhI(W^*<(Yo=WWH08W2`*_EupK"/Lu'JffE]V7<qoG
+<1QIT4#GqjRPh)p:fW(H$</nG+XtmGJqoVn=XL<^=,C0`SR3-o1a5_a&ZR<W<Iq.W<&Nh^.FYSSLR"u-
+L9D(gM4@VK::`MD)Brn\W2`*_"fa%KeXVDj9Zmba<jjKG^H,UGE`]Y$WU.uFXJBR(WWH08W2`*_EupK"
+/Lu'JfueSbFgt')X/['!%ZLZe.RmDAW[^hFJZ@ip,\:.[n?B3`nFPpaQ,lTu$-`s>L/'3c<<]BP:fW(H
+juao#>#t-sZ?,X2l>W$X?Sr:i4#BL.5ZIuf.Ss,rWocDhAQi.(Yd/?>^I<EKcE!he@YY.Z'r\,t<<]BP
+:t;(kUeJr=*`dGbDI4Ee&oOa^WP0Ak,ZUM;!`]u6W^'R**g/91.sP1L'$ai)XqD5F3`=E:%?`2;<Iq.W
+<)oTk3=e]LQ5)N`Cf)fk*Ng$XWqo`?%ZN9HXbo'eDK),E$<1#uWWH2FGt?)bXPdMu$hElRE0]ok2$_QX
+QP1WaU+/u>JZ9&V2<Q,3e$=R\Q-Ip]_c=Q@ebi##>u$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&V<M6?P%nW1T'g/R6I&LfJX/,+/Yf;O"5ZIuf.T"ZcLSRVe$6E5+5#aE69i?.Jf\o4uTW/&n
+'rYlm6W0dm"V3+&+0(d!,q6$k;)_6L"fa%K<Iq144="KA<q*?!nY)tn\WX6cUK3n`$<1#uWWH2FGt?)b
+XPdMuj$Qgfo-\+)7eJ`<'r\,t<<]Cko8o2N=i&"u`eUSVW^t8BNJ__M.Ss,rWX;]_k92;'Z@[jsN=T%6
+eWQ.;)Y;oU<M6>n<>DHIc9kL,AH`Sp)]u)KX$%mn1s4iH<M6>n<>DHIc9kL,AVD%'p?'VhPEV2fK8G><
+lQn:j8_KeW%o-(-W[^hFJZ@ip,\:.[W,Q=WCtZ+-d@q!b0!&<go"Nj"I.4tC4*R\80;gp;09NM5apsCm
+3)LEhW^'PfWg`O@P\F9o_Nir]@7-ED++<e/mskB7#7_a;n*bbcM'p.l)`MY6+(649\6^@hT9L?7S>[?*
+<M6>n<R#e[b-\6He[58,@ulV1Nu)^0o#i\YI.GBuj$3S>:ZVW>P)&?L1;]gr]L.;b<Iq.W<)oTk3=e]L
+Q5-EgQM9k*T=j^]\).ZSDV_m(F(dSriHg>S)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[<M6>n<>DHIc9kL,AVI.]j`fU12gHYFQHSf;
+]gO4EUbV;aaM)DJW2`*_"fe$_OHaH^TrMh;]tM+(bKS1(iAE@hrU54PDAa<03HO?,*1u<Sc'q4`*;SZZ
+:[0su>ju^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</EVhuE[7h#>(grNEuNIr&r&GgA'XLX5[p?i@"MK6N6/
++EMg#R\(T=H)d;T<)oTk!`dJoP!8K]1W`q+n%JI%R"s"[5@<jX9ZmdOe+/(o$_D\0WUP>_TE"iP7j!0O
+Z,[p_ieoJX^#s17]bWfdnF1CV#ONQC60@a)1_3A>kHAVA:fW(H$</lqT(L$^)e&U1n*_pd1,Up(DE2t2
+@(AM=[&dXs/O01&_o;0@P>=@KJ)oJ45;G"bi.DQC?+[PR"3q&kK$<=9C#riGl;';#\>j%<<M6>n<R$pZ
+4a-0oR!q\7(h)+'ANP)%./-6!Q5(lSfe[9d$kElp]s0[nbMD"1a]lLR^$kplVa!o4&jFE)%mBRGP>@LW
+d7Ikn#Vm2`;L^bb@]8Q&<>DI45ZMmf8V`%T_XndZk2_NHc->"PD_LiT(=/nZX6Xn-WO&q[l;N,o45VL_
+Cl^g[q"!OiM]_rrk<K!t\%]7ll-_fqnW-_#WDA7K;%QIMUKq/<>$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>6ihk<Dm
+8o@6"gg:AbAHgh&3K1nkrq3J0pu74ZdEs>5GiH?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%<GR('U_,CYY`.*?'oA<Jd^_<)oTk3F1(tl04BZ:X2*N(GIEkE3Uff
+eJq-q4#En95ZIuf.T"ZcLSRVe$6E5+5#aC0d]]),d%IB?<>DI45ZMkHOtnU>GZ`Yo:t6OW93LW.TW/&n
+'r\,t<HB4E1+b#$=P<IGeJq0(Wageg<>DI45ZIufRe)W*Bj&9ib/T4:k"%OoRW`?a%KD,>c;b%J.T"Zc
+LSRVe$6E5+5#aC0dRmLq2LiG[U#HNh)]\%@!`]u6C<f1EVI]oA.iTS1RNlOs;LFfE,]Tt::fW(Hjuao#
+>#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</&aNcS3OWX;]_k92;'Z@[jsN=T%6O]:;?WpJ*:
+`jr*7<<]Cko8o2N=i&"u`eUSV8?-W1<H`$miETBUWWH2FGt?)bXPdMuj$Qgf,ZU5..^,OC2pPD,W[^iq
+S;3n6b6f:k2Ek(t7oG&OWj5!e<h-2t<)oTk3=e]LQ5)N`Cj`0rNhd!s<FpqTPrMh[W2`*_EupK"/Lu'J
+g%\Co*(^mFW6,U0MK,dK:fW(Hjuao#>#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-sZLOWg3Kbjo<b7'+*Ls/;
+<)oTk3=e]LQ5)N`Cj`0rNhd#FWXZ^1K6(%DWX;]_k92;'Z@[jsN=T%6O]4VK[=NO8E3:Ze<Iq144="KA
+<q*?!nY)sCOt*8)[IJERE3:Ze<Iq144="KA<q*?!nY)sCOt*82[:+5lE3:Ze<Iq144="KA<q*?!nY)sC
+Ot(!=[/kGaE3:Ze<Iq144="KA<q*?!nY)sCOt(!F2;T>lS>uur<M6?P%nW1T'g/R6I&Le?U^A`S7^kb&
+7FgTu<Iq144="KA<q*?!nY)sCOt&Gk)UT$(7o^d#<<]Cko8o2N=i&"u`eUSV8?1UQCArEjGc[?`<)oTk
+3=e]LQ5)N`Cj`0rNhd#bX6+S\L?kkM79'MkRe)W*Bj&9ib/T4:k")M6K;preToju-3CHjNWg`O@P\F::
+_ofuSWocjPOJf,SfqFP+Wqeq\"fe$_OHaH^_8FF8LT9K-WfYE(mWW@*ZDr7N<M6?P%nW1T'g/R6I&Le?
+U^B,blM.@@**79AWX;]_k92;'Z@[jsN=T%6O]3KYWg/XiBg'ud6WJW7W^'R**g/91.sP1Lpe]K\8JRu"
+l=Pf@<)qbIWWH2FGt?)bXPdMuj$Qgf,ZTZ)We2<1&QX%)WX;]_k92;'Z@[jsN=T%6O]3LdWnBPc*-9Zs
+:fW(Hjuao#>#t-sZLOWg3Kbl1?T!b.CQ?/E(9"5u<HB4E1+b#$=P<IGeJq-q)Q7D.4#@kSC5Ssi5ZMkH
+OtnU>GZ`Yo:t6OW'cO?WIYUB;<n3tF!`dKj8BS<@i*g3W6WJ1Q<G:0farMC[>?-N[$</nG+XtmGJqoVN
+%opl:<.V',*?%mWTW/&nc$;k&[&i1oA`H-XF!ObYl8S'QBM_][WX;]_k92;'Z@[jsN=T%6O]6n3V(?e'
+:fW(H$</nG+XtmGJqoVN%opl:<.V&\eZhJoWX;^*TW1!_8XYkZ4KN@HW+q?g$<3R'ofG!jLB#+/kGeS`
+c$;k&[&i1oA`H-XF!ObYC8IqUWql7?$Z4c@&@XW#25k986W0dm"V3+&+0(bSW<-)#QK#NH+Gk/>JZ@ip
+,\:.[n?B4gTrL,9.T!NrqKFqc''H7r<R#e[b-\8.@:eId<HBEcTPD0)mrF"*c;b%JRe)W*Bj&9ib/T4:
+k")L;<AX!g&E@"qTW1!_8XYkZ4KN@HW+q?g$<4Fbf6=+"Q)C(qENUcfeXVDj9Zmba<]3-@2+LOJW[`Br
+PoUdo3*-inC<f1EVI]oA.iTS1RNnd5<>Fr%apnk*S>uur2<Q,3e$=R\Q&Q;Sc&ktVWX?(6jY\m[_mud4
+<HB4E1+b#$=P<IGeJq-qW2_OQ<H`'*MK;dKWocDhAQi.(Yd3emW]?4l:fX3`WpJ*2''H7r<R#e[b-\8.
+@:eId<HBEcTW1>$<S='C)9u4cWgXSI?+t^^ik$3qI_=I,R?eSt=;7]KNhd"]<N)`D4#H*;\Z.npF,Hbp
+Y$;88n?-'/hFRu3QFfB1Z2ti#!`dL-<.V%VF]E".7N%_s$</m\83/^i(^AnY`o@\*b6hK*$</lQ;?a'M
+fMlF;`n`UU'rYkBOgMU,\##pto(t3p.r_aA:t6OW'r\-?=d:!GGAs]7:t;'[l.2t@4$#D\CtR*V>A%U3
+q=Ee4\3JHJ!Z-ar<R$Y*JZ9'?WXZ^1K6(%DWob!egY:JL/R,Z-n(sb3Tn.cGn6p*JlfH<VbM@[<I(Ho#
+?:U9t2^qlO6A2:PNhd"]<KL6Eg=:>C[Is-F$g4(K_gTBb_loh4HtDAD<M6A&P,-V<``#f7f+JEL%Lru?
+5(1"eT6'2@Ko($JmbMqXbLQU*Qra`tM0VtK*(^mDWfT^#`nR3'\Z.npF*/fM(`5e-#s:4.3U>L%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+5FL7g<UV4?hI!iT^rL&rd\6'HDC6#(s<)mm727aeH/uhBrWob!e_dL,ogU<uV
+VSZs-p$:4jNK)2&J#6c\/f"VjRNnd5<>Bm]NFdm"YH"k#W]=A5WMuj1-Vd?/9[%iZVI]o`('U_,,H1`E
+'g,OArsfL!<V)9Ojuao#>#t-sZLOWg3Kbjg<P`h.h!WO.>rB*iC<f1EVI]oA.iTS1RNnd5<>G(%2/bQ5
+nkh8_W+sUF;Pn"/%kAS@WgY03!`^!+<&Hm92<UrRISeRbLSRVe$6E5+5#aC0;?a'MjK`O^@rk+82<V9F
+HW2lc+XtmGJqoVN%opl:<.V%Vq6'"*LIT"YH::hT*g/91.sP1Lpe]K\8JS2(&49HI4#CEFl46n>Gt?)b
+XPdMuj$Qgf,ZUM;6<$t@4OkEO<B$jkc9kL,AH`Sp)]u)K,H1`E'dBaEi8pCPXQfdcc$;k&[&i1oA`H-X
+F!ObYW^=p0ZeM/cF4KVReXVDj9Zmba<]3-@2+LOJW[]hne^SeV1domD;:V1lUeJr=*`b0_<&I0D"fa%/
+:t68(BJ`Q9'N*DNLSRVe$6E5+5#aC0;?a'M1h'J`%ZNrZW^g'1*g/91.sP1Lpe]K\8JS2(d*Pd(4#@ke
+WXMnPGt?)bXPdMuj$Qgf,ZUM;!`]ss1oRE><HB4E1+b#$=P<IGeJq-qW2`,5DqOI(TW/&nc$;k&[&i1o
+A`H-XF!ObYWWH15;>1P0W[^iqS;3n6b6f:k2Ek(t7oBMj.T!OS3d,15<Iq144="KA<q*?!nY)sCOt':.
+JZA=BIV_1dmL3m$X6)MiAQi.(Yd3emW]?4l:fW(H<N(.t`od^G.c+"6_/ZWHo8o2N=i&"u`eUSV8?+pT
+"fbTT`HO=.''H8T%nW1T'g/R6I&Le?U^C($TPEldmrF"*c;ep,OtnU>GZ`Yo:t6OW'r\,t<<KON3C+;@
+W]:S]R?eRMXDE9_C6#(s<)oTk)0T\LENYc%OHaH^_8FF8LT9K-W^'PfWnQe)44Dp+fPHRheXVDj9Zmba
+<]3-@2+LOJW[^hFiTmM/n'GHYXoRXb3=e]LQ5)N`Cj`0rNhd"]<M6A/>udrZTu#Ad*p9O`%nW1T'g/R6
+I&Le?U^C($TW5lq<GNPCR@3O%>W([f.FYSSLR!i0<R$Y*JZ9&V<LB\!D*JQ`n<5ZkW]:S]R?eRMXDE9_
+C6#(s<)oTk6D[]8UeqTH<HB4E1+b#$=P<IGeJq-qW2`*_KrV1/2HPEMeXVDj9Zmba<]3-@2+LOJW[^hF
+U%YKd]o5F5NQmuojuao#>#t-sZLOWg3Kbjg<Iq.Woi<Y.e]bAE$(S'Y4="KA<q*?!nY)sCOt':.JZ9'C
+<S=0f#Nr*I%nW1T'g/R6I&Le?U^C($TW3U>C;Z+WS?(d7,\:.[n?B4gTrL,9.Ss,rWX@I[236!BT:@oC
+c$;k&[&i1oA`H-XF!ObYWWH08;Q<pffUVO\S2jZqGt?)bXPdMuj$Qgf,ZUM;!`]u`Y$00(S=dokF4He9
+UeJr=*`b0_<&I0D"fa%K<?\]K22ofUH,>tjeXVDj9Zmba<]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
+6WJ1Q<M6>n<>D7.>4WEQek\@t<Rh<b8XYkZ4KN@HW+q?g$<1#uW^='m%4C?(LZ`?OB#d8:VI]oA.iTS1
+RNnd5<>DI4ck'4CS=HX0g:8B,VTV>)VI]oA.iTS1RNnd5<>DI4cu<!KPI'UikD$%;k92;'Z@[jsN=T%6
+O]6n3$<1#=oW#>q45UpI)ni8%LSRVe$6E5+5#aC0;?a'M:fW(Hhl-Qhk<$TQRFYCLD^cM:%jrjUZ@[js
+N=T%6O]6n3$<1#uC0C3om<n@Yccj_Cno1<Y/Lu'Jg%\Co*(^mDW^'PfW`I(!BkQSk3d<V]R?eRMXDE9_
+C6#(s<)oTk!`a#E3'8u]=2r.7b-\8.@:eId<HBEcTW/&n'rYl_G'KU$IW"omg;Aci[&i1oA`H-XF!ObY
+WWH08W+rN>n!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"]<M6>n<>FZ"WgfV*SYGF7;(l7gf[A2'3&m;<ESnKY99q,3I+uZ#
+)op">kP,HZb6g?7.8\P78JS2(5ZIuf.b%AhhYGiPKUu\2G!AaRH(p=ZNjc@'KlK%3/K[0(38?8C0JG:=
+F8GOnE/-#L[`n85R$JJ3#Z@=q3Kbjg<Iq.W<)kWQRa)1EeMs,I6sXl^WMulRhVP)%Q`aQ%Y%1^bk*]j?
+D;%ten@eSQ$[4R[rd;7P`O)_^6]_@Lc&ktVWX;^*TW3UPlDu5;HA\kKU:-Z#CJK3Cg9hYG<p\_Y5Puig
+/`4'X,CJ/$hd=$/R&e+V#7hk,pSjOG'mL!^TW1#8.Ss,rWX;^*aJN_7;m^Sn/q'.MMM2rCqXs0`DNE&a
+C&-DdA)Y<M7uek`]PQ9,*BSG/qc$84$9SG?:t6OW'r\,t<<]BPpo7M8MY9&*=88hB-.DG3cqsKkbfn3+
+1cI@3hnHeZB379gY-+qZ;5MR`cE%O"6<#g4;?a'M:fW(H$Mg]Jo_()MF8u)aiPOq/jS>\7WMuk4$&WH-
+r%O-L^$b/ib?.Tk*(^mDW^'PfW[^D=Fl"LE;h".\2?O$`a_,oI-RY,XCWWqG1,?^A$kFPp=<+!63Kbjg
+<Iq.W<)oUV?0[ibX4at0n`%O(K^%acjSrC=/Jil5n'Iaq`B+cO8cILP$)V8;/f"VjRNnd5<>DI45ZIuf
+R]AYeag<%0FED?U]Xde\Y99/</f"VjRNnd5<>DI45ZIufRSuo'Km+n)E8\OUbfm%bVqTkYP/Hn)R$JJs
+=;7]KNhd"]<M6>n<>DHI:,+=*HhZq[^@QJkG;nu<SN\YA"fe"9W<-'7W2`*_"fe$?-(UX!5'c_[giIK:
+e$9#kMZQo'O]6n3$<1#uWWH2FGt?)bXPdMuj$Qgf,ZUM;!`]u6W^'R**g/91.sP1Lpe]K\8JS2(5ZIuf
+.T"ZcLSRVe$6E5+5#aC0;?a'M:fW(H$</nG+XtmGJqoVN%opl:<.V%V<)oTk!`dKj8BS<@i*g3W6WJ1Q
+<M6>n<>DI45ZMkHOtnU>GZ`Yo:t6OW'r\,t<<]BP:t;(kUeJr=*`b0_<&I0D"fa%K<Iq.W<&Nh^.FYSS
+LR!i0<R$Y*JZ9&V<M6>n<R#e[b-\8.@:eId<HBEcTW/&n'r\,t<HB4E1+b#$=P<IGeJq-qW2`*_"fa%K
+eXVDj9Zmba<]3-@2+LOJW[^hFJZ9&V2<Q,3e$=R\Q&Q;Sc&ktVWX;^*TW/&nc$;k&[&i1oA`H-XF!ObY
+WWH08W2`*_EupK"/LtpcF_,t,&Yc9cOt':.JZ9&V<M6?P%nW1T'r\,t<HBEcTW/&n'r\,t<HB4EH8pZS
+:fW(Hk")L;<<]BP:fW(Hjuap^E")91WWH2FO]6n3$<1#uWWH2FGt@5Z(udn_"fe"9W<-'7W2`*_"fe$_
+OOV0&`.t?"<&I0D"fa%K<Iq.W<&Nh^WJ=u>%oj@CC6#(s<)oTk!`]u6C<f1E&'#5'W[^iq*(^mDW^'Pf
+W[^iqS;3m]NZgSc<M6?POt':.JZ9&V<M6?P%nSo(6[Yi45ZMkN<M6>n<>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&<pDM,:/Id759iI2'J%AnGX=rUeR"5Q3Y?-L5.Basqf3^O5it<X5jIhnCC0Q7Ta
+ej2R(+d55t8il.4LQGLDRX:HQ\PEV3Fr8fT$R[OTHlH5AB_hF<I606j\)`L+3`HQ.S\s:Irf&k5HQS)R
+4ohe+dq=<X*mhWX\bPIlPn*o;RXf\^ZSo&=]a,V1`qM=_0_[et:4I[-nGTlB?bI?<>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<rYJ0jJnWl=bp4UNbCTN@#(G:B
+WUV".-C"Yn!'FiYUj0>`LSpRWHZa9Gcf\"i6'e=OKhU&)lQU9-Xrq<g(+%X,"#L`+D>k,`$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_aEYFrq<h&%jYa+2fAQP%t>F.XfN$Yhg0T
+p_BWgc_`-J#1Ru%uGsLcLZ")a24EBLRo#>7aQL3>0,IM$;;A_a;"4=7*@l)T>a;]'1&__^/iJ1PX8FD2
+Z/poGfk.9^;k1<pd/aL<2_4Ef:If/4J:JOY?/R-o>F_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%/:pLR<i,*Qn4D@dq5B08-N'iB^jdC2.j4['R(pI']@*^g-Y[=E\0;0J/c
+@kFHh6psIpRc)>IHlIDq09hjSNpoFuOa+!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><SU"'`',N9+i&-h&'JBD
+EhgDbTbOt2Q_rGLSrsW(UGEN%5%a</@S2temXfX084JPYFb>uJ&(TrcuX&bLun9a,,WDK('Yr=U#%h0!
+(])Ag<iHsaP2hOP[IEPZ;H@!H=>1bc93*4khgN`gT3kb4;>N%)3pnf9`6PmH;=hfE6QiWAR.Z)mWoril
+,-(><sbJ19"\a\HdRl5+e^3k5$Q6bS0fXl?GnXZkq]=W@d0uUG8jj9\gZi1%cHgL&Q3B:5eT;gi)M5L)
+pmu;j3fh2%V'[G7g>Mk1GNZUbK&Pg`>QBjkf2/CcrmmOHFU)<OspP5@!H]Y<>,SK'/eb8CbMjOID<NA=
+[kbCh3A9rRujN3V0aZos>ET>'[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&<W%[_EVL##Yc'YN"$pBRF4<Nc%6(\Md/XS1/Obt.t:D-OagJdl2HMg.RR($njUP`
+6t,GCJQe1<KHW4I/<NflO)]O,iHr,l?*CkWYLi<9Sdu=:ST?oBp:8LrmcWG0-p0e=mFmj!&@>KA?9N)T
+XT+PFGjo*>;cE&\n])aOok!$+kFK+Xo(C_ECS-%KfntQ^`L8Z"SppP#rr2oenaYqsYJ:#+b>1:oe>!44
+`<Wc\\ToZS4[feh%HY4)?*!"&Jgbp<m+Gr3LPCQC#QN_7&#NnJb=\@4GOF7#Ci'%5UQ::ZIf8rk)#WgX
+].9I[i;p&LVFp:?LqRrA4obGu+0]4r[r:/a++4%`ouX[ip[4S:IsqM6pU[N1Y"5C6k2jdQQS2]0F6:\+
+g=k8,r:oems6Hl'rpe#N(EV?6T6p":3$E1)kK]VK4]MC>ES?g:B=;[3p<5>=`6l$%[e&\.:E9BVJ,I^G
+f,(pUigtMkm`",AHgeYi5+o<Vj<nE+FNI%6*fV7hhnFO/oo:%SOt[(E#*O\\$ST.H2E"T\d*/"?F'a:f
+eQA$Rd<Lq;1G<(I(l.)t*4Or&@X*.-USB(5M3B;_rq+Z8V2)'X4AMA-GAd?7G]k&eE[8AZUuPg0lpLL"
+B:m.#.qW/a!NcMO57uWp8rb3;hu=]XFD0.#9lYN=W1p6<dF#HG`lb`7rfo!8M@5K]_85n-1/8=u1,WsL
+AL/->X=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
+eO<?Jil;HqZ)'PBNYdW1jQj"ar]4'r1M4jedpb$O9C#]i(+*+'oo#/CTeM=dj(^<!='g0K9#_$,cE?FY
+a*JtBi9JjnV)t"r>oNC%mYSPq/;%GH&LXbd\#qEqP,hs&VNPFT&k?F3O8X>Q3NK0H*1=bSm+J_C\EB2c
+IYg.*XJH:8?HqO0=o(ub7+g)A<g[.6eLs,^P5"D#)i%$MLoZ,jZ`8>n@F_AXEC/GFj3:Z4(TYnb)^h7E
+`*,3.csp2tho=&pJUoe%bEa`6LHFkjmZ'Kk/R.dK=m;V0(S>6[EQo=f92!IELCF\%<W$hb3NF3*Ei`d2
+<P,N@0mQ"9g=fa,*j*2l^T"1]3>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<hNtf8lhTq5>"tLA3W[e
+TO^8Z<hNtf8lhTq5>"tLA3W[eTO^8Z<hNtf8lhTq5>"tLA3W[eTO^8Z<hNtf8lhTq5>"tLA3W[eTO^8Z
+<hNtf8lhTq5>"tLA3W[eTO^8Z<hNtf8lhTq5>"tLA3W[eTO^8Z<hNtf8lhTq5>"tLA3W[eTO^8Z<hNtf
+8lhTq5>"tLA3W[eTO^8Z<hNtf8lhTq5>"tLA3W[eTO^8Z<hNtf8lhTq5>"tLA3W[eTO^8Z<hNtf8lhTq
+5>"tLA3W[eTO^8Z<hNtf8lhTq5>"tLA3W[eTO^8Z<hNtf8lhTq5>"tLA3W[eTO^8Z<hNtf8lhTq5>"tL
+A3W[eTO^8Z<hNtf8lhTq5>"tLA3W[eTO^8Z<hNtf8lhTq5>"tLA3W[eTO^8Z<hNtf8lhTq5>"tTWT92(
+OWI2!/sf4X#k]R;cSn(KmV.91HFP=1:pM/h.o<Mn,qDft+!iF6Z6(AC:pM/h.o<Mn,qDft+!iF6Z6(AC
+:pM/h.o<Mn,qDft+!iF6Z6(AC:pM/h.o<Mn,qDft+!iF6Z6(AC:pM/h.o<Mn,qDft+!iF6Z6(AC:pM/h
+.o<Mn,qDft+!iF6Z6(AC:pM/h.o<Mn,qDft+!iF6Z6(AC:pM/h.o<Mn,qDft+!iF6Z6(AC:pM/h.o<Mn
+,qDft+!iF6Z6(AC:pM/h.o<Mn,qDft+!iF6Z6(AC:pM/h.o<Mn,qDft+!iF6Z6(AC:pM/h.o<Mn,qDft
++!iF6Z6(AC:pM/h.o<Mn,qDft+!iF6Z6(AC:pM/h.o<Mn,qDft+!iF6Z6(AC:pM/h.o<Mn,qDft+!iF6
+Z6(AC:pM/h.o<Mn,qDft+!iF6Z6(AC:pM/h.o<Mn,qDft+!iF6Z6(AC:pM/h.o<Mn,qIJEo#SmC&#LnE
+bG@@TqNK7gRib/p1dOPnglhmifoM:2.+sZE'e%arOqfCuO-15+foM:2.+sZE'e%arOqfCuO-15+foM:2
+.+sZE'e%arOqfCuO-15+foM:2.+sZE'e%arOqfCuO-15+foM:2.+sZE'e%arOqfCuO-15+foM:2.+sZE
+'e%arOqfCuO-15+foM:2.+sZE'e%arOqfCuO-15+foM:2.+sZE'e%arOqfCuO-15+foM:2.+sZE'e%ar
+OqfCuO-15+foM:2.+sZE'e%arOqfCuO-15+foM:2.+sZE'e%arOqfCuO-15+foM:2.+sZE'e%arOqfCu
+O-15+foM:2.+sZE'e%arOqfCuO-15+foM:2.+sZE'e%arOqfCuO-15+foM:2.+sZE'e%arOqfCuO-15+
+foM:2.+sZE'e%arOqfCuO-15+foM:2.+sZE'e%arOqh^^3M0rRqKDP.QVnnmp9r[IDJ3EfHSRNPB$81`
+a_]Y_==<u)f#`f/cS#.jlp?8dr?tg"P5Qe3JWfs[=b2b7-s;+ar?tg"P5Qe3JWfs[=b2b7-s;+ar?tg"
+P5Qe3JWfs[=b2b7-s;+ar?tg"P5Qe3JWfs[=b2b7-s;+ar?tg"P5XtII+gs/+A2?;c4u^H5NZ4%=nQ:M
+'pquck$3I?ojc8^QrQB5>_-&NA'PgV<BoVNJQ_YUc4u^H5NZ4EVHIa?Sm].-#ljr*zz!&-#(r:A7niSh
+&HZ*Xb!&`8$2h=&,Y?FKJ)rVJ=k'DH!?r;HBI>ISKeBk_:!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"Ahr<D)^:rTpd<t:@I/%YQ,L)nQkKh
+25SiqG/\0kV^P1=T1UT.8?k?i`;<HsUIBuq/D>UZ*&Y]/R<pYUHbKi)G]9RaVmDVVb,\ECee'/AmUihi
+HOjQ)"JH7h@Y[C%mKTqjESb_fK>R-.^,GMp8fmG#*'?G-'@f3TtE)muF$[)>&lHp5;]fs5hZI<TdT['R
+)[ldi1P&#tYTPoO_7=^CgY^OJK!ah/LZMR&D/pNNlY/"Vjh.=D__WU&d3kj@ahIXCG6Z'aV<f<;)3Uj?
+*CEejnRL(,)Zhsr4d_HUrl'l2iJ^\tV1?q&](hS+'ocEmgVh^4YZr+75B(usZ:?[_UtOsNan_oB'nFmI
+WTp$7h!U;je(_[etj"\.3uWc&PD%UXVR7Idk;1#`&==sPHgd\TSn//?VQP>&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]X<Xf\^2s0He0cHa_\QBi:j^LG0>T76XYlWf_HnEnu>mGKn1epUQ]QX>3Hrt2IKg$
+a1Rgpt/5^3/bmfqSGKJb6ST!!$,W_R+-CC=W0$\\)S$*dd.bpYPL&=/f/fR[UN,gX"KS?*f/b:<Sp+l5
+Vj,aNj\17is$mSiqGoem!(Gb)qeul)=[`!3kD*2D%_Nr#%EJPu0ETg7h</2VHB!bfLaekH1;@B@H?lQ(
+'WN[TBS7[;=O%Wm*/$\6s6PYHP.gRfNKh^nQuI/K5W[mF>`F*?,s`mHk?5_]?L)cC@Ha/sT$V:7Q!8D@
+Wj.2)QC6E+"'@4PAZN_1O<k/`2$SXo5J5`<tA>%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:'25<RZ!+dj;VBgplm85Q43^Bl.^pPpq2hXqGWiY_-KA*WQ0?Mas/#_]9>GjQ6]::+%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&GT
+;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:<Md*W+Z'T8D*Z,_r\T27[;c
+DL"aPFU\5MUg+"7qKk2l:AfgkM`-h'HWD0KqZA*qs)ulE&Cjh`mHZ!!!#?YO'a/oZW+1rjj%"?bX1VY:
+k1j`Ns4'D"J22.L-nODQNbR^8a2:s8Mt*Z_!b[g/f5bY3mpKXUM@;\[#ah=h]V0!!!!i2_d$qZfnMq\#
+LJ\(R.Ogn%\nZ2TYFi\uDYXS$D9#-faFkzzzz!<*KM4F$Afl-lYt04):.)&]DZ%fcS0/07TY4P@)U;q7
+(N55X]Qf3a#Um>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]36reFV<M6>n<>DI45ZN#.8`skmT#5i9NiEFcF[o=5W[^hFJZ9&VFm&b5jcP
+Z;m_jn^<>DItNhd"]<M6>n<>DItc@&k+TBZ7<X07Zq_OAWnrOKZJe=rrE<S`d:JZ9&V<M6>n<Sb2]b@]
+A/>mCiZ8/?K&gq9;[!`e'=<.V%V<)oTk!`e&*VWI=7,fO5t>V?'Oj,Y3$*L4OKi]WqM<>DItNhd"]<M6
+>n<>DIt:,+>iZY.T2hgE+UhnJ`$^?2^N:fW(HmRX?C<<]BP:fW(HmW;$-]QeGZcCOSf=oBBWZ_!b[q0i
+]36reFV<M6>n<>DI45ZN"_,\:.[R+q34OcHI08?+pT"fa%K<Iq1<@S"YKX:W:7bo`ss,H1`E'r\,t<<]
+D6M]jOuZ9et$2Lh[jNhd"]<M6>n<>DIt/r$/qb6ft/g&.PD4-D'i<Iq.W<)oTk],6\d/M"bYAb1\WmRX
+?C<<]BP:fW(HmUA`0[&ahVQ-CDN\g+0U<>DI45ZIuf\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&V<M6A((m6X+/&ZC<o7s@68?+pT"fa%K<Iq1<@S"YKX:W:7bo`ss,H1`E'r\
+,t<<]D6M]lg//$DY@oJld?q<.AJX$e4f/"8b&F[o=5W[^hFJZ9&VFVdEnJX9]3?F%N.T3#B2bYH)qAb1
+\WmRX?C<<]BP:fW(HmUA`P2D(uMlif7Ga)GV]LT9c5W^'PfW[^hFJZA-I836e"ZHH&bfdn/]Q-CDN\g+
+0U<>DI45ZIuf\r.`rnX!USqn:-1_9Volh/J[OO$Pg]/"8b&F[o=5W[^hFJZ9&VFVdEnJ+ZJD)ar^.ZY(
+5QPuDc5=#PQ,lAhYI<)oTk!`]u6l7RgeoCE*qN+9;<W3/3N4,qTA>hT%KW]Q@n:fW(H$<1#uWk0+27fC
+!]oV#GC908-[0jNE0TrL2;.Ss,rWX;^*TW5U@P"%SdVb<GSI0]#j&\?Bf*5e&d+0)msW<-'7W2`*_"ff
+.l+mB'"Q;c!c8:+(YNTTC@Y&+)6eK%3rW2`*_"fa%KeQiSTo/SDKVX>/c1.)"hIf?PL)r>'!mj2U;W^'
+PfW[^hFJZA.<P5YWarS[\~>
+
+%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'_<Vg9zzzzz!!&['
+TCB^prdOc,2QpN=4I]EVC7`[#4LAkL1CTaKI>.FEFe.F3YK#]>XI<en$<GEXGm182ft;o:o5%aLWd-M+
+@+Doml__B_K9VnXp\iTCBTSDKP[$MrGtP64j?+_i:i`.j.k!DRaVC&1VrS1Wc;S`e-0H_fJs,_i>X?JW
+9!4iJ4=+'*Eh8FE-ok)F'pP5dj9eN);fW$fk:*rnP4OoC6-4CEY40.OJ'a]?4hPpZ;dY)g;DN)c12TiC
+D,<!):blla12Tj.Wt'p&f+5p^nT)1!5Bag!%nAeZBT1p(]A7B"3;;=-jArB`X5Ept/=4PRMD</skOZr1
+F*i<rS[R;?T9#T'Hp4acle-)t]DQ-C\9%IVakn#:#8$~>
+
+%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<X&~>
+
+%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<c&Hul!71`?b>%Ccf
+)H$W>g]f3*qC'0(,`!b"<$Gb<E1U:3SW7]%11.313S1gfqaFW2A@-",pGjVQ9I=D8,QkZ&n,DEET7*%'
+n,Fa7o0:to_N!MIkequNqqnJ6+[T-c7oBMj.Ss,rWX;_m9XE]aDZ99ioXANp@:2!7q=<XK2^(Le2+LOJ
+W[^hFJZ9&Vm6DVcEoJ>.CX[=pC<K8H"rkn(g^+KA)]u)K,H1`E'r\,t<<]CG+:ObLkN$T/h9>+,l1Si%
+WnI)%0Xo*?b/T4:k")L;<<]BP:fW(Hm:uF7EhcL\6dj!'1,$c5NJEdY%opl:<.V%V<)oTk!`e"'p8FMV
+8D8;l:)K@FiaHEY6WJ1Q<M6>n<>DI45h2EJ1+FY8OuAks'*dUeH!&bp:t6OW'r\,t<<]BP:j!#fB$4)O
+g#):\[+2OR0JI^m_ofuSWocjP5ZIuf.Ss,rWi^A#<I.3I_mGqUo-aS[<]3-@2+LOJW[^hFJZ9&VD5u><
+?p,dkH!3s(IYA(jXDE9_C6#(s<)oTk!`]u6g/nlh<Uc*WX_Be(@.&u/Q&Q;Sc&ktVWX;^*TW/&n)scJ2
+`#b)M4"VfgbT62WZLOWg3Kbjg<Iq.W<)oT+"2,/**g1\#/LS2H8\7*dCj`0rNhd"]<M6>n<>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]6WJ1Q<M6>n<>DI45h05oc&*R*Fu(]t>_IA-*`b0_<&I0D"fa%K<Iq.W<.2m\BiP[8
+]:>0#/s4P=LR!i0<R$Y*JZ9&V<M6>n<R"m%i#uh/drN=@gm!SZN=T%6O]6n3$<1#uW^>+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^f<u(Fje8/;`(mog5@:eId<HBEcTW/&n'r\-/\>oDYcU1URBfY-X97Gi.
+A`H-XF!ObYWWH08W2`[qWi8*Tl63.s[%))0\X,F[Cj`0rNhd"]<M6>n<>>j?26BpoGXuR6/LLtN/ljko
+<&I0D"fa%K<Iq.W7B)a8Mf1[O>,bI:()BqP<t2W)1+akB+..t,Yb[2Wk")L;<<]BP:fX2;<E":KNK00.
+\o_Z4Sis\mq;?l9f%8NCh/ct3@/'!LkNA>j=Z%cYKmqX:U)Y`KU8g5:.Ss,rWX;^*-q)U-T+Pp)l.99l
+*QHgtl05=PXTo@Xm,,R-@Uc(j4eFla(@HJ9gprns_Muh>A`:"$i(ISpia>=u<R$Y*JZ9&V<M6?Yr)H[F
+nNctChgYIij2[5455jcIYJQP$q!mB'ml<0['>Dt*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?bCV<r2o5Jl1!s,nF4o):Rh=C]6@.&PqMgtDQ3:"
+Y$KchLKj1Mr8S9@#:Weo6A2:PNhd"]<M6>n<>D70g2MpBD33M<ZoYpebN6,mSPI?a1G]O_4K*\>['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&V<J[QQO&7q%0^6Q7(MiJe
+II,Ukg&PB+Sb)47D"Z7rql*MlqYL&gpO*#>pRaQndc-nD?s6=b+.2(K=?N:W3Kbjg<Iq.W<)naMWi'Y4
+G+,d9WSG7<^Vpl*2<Ak(EH1OEoB/3l1Ri)=M0r]kjBut^WocjP5ZIuf.Ss+gL8rNG1_3/6S!F^pO'HLH
+=P<IGeJq-qW2`*_"fa'5^8eM:3Ur4gb@rtVoVra4*`b0_<&I0D"fa%K<Iq.Wq6'"$<).fdc$3DeiZ+=K
+XDE9_C6#(s<)oTk!`]tU>h.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<Iq.Wq5`e-<).fdFYkZtnI3V_<]3-@2+LOJW[^hFJZ9&;X6P.5:4+>[2(^2<%*Tbn2Ek(t
+7oBMj.Ss,rWX=$ORZk4on;u/U[%-2hq-*9lZLOWg3Kbjg<Iq.W<)q"6ebhNikom%HBfX;UNs8eWYd3em
+W]?4l:fW(H$<1$H8(INrb))1T:8d2!hZL3?Q&Q;Sc&ktVWX;^*TW1<gWoe%q:4+>[4"Vg?*gf*lCj`0r
+Nhd"]<M6>n<>E/Jc.f1epdj[r>#&=00"V8t.iTS1RNnd5<>DI45ZJ!V<HC-#Gt@4K>0c)ZTZ"mn`eUSV
+8?+pT"fa%K<P`h.I/_4D6W3C%Eg"Usfu(tPLT9K-W^'PfW[^hF_/IkONlm3>UcFg.PPbm6*`b0_<&I0D
+"fa%K<Iq.W:93N#<).fdc,[2]jD85tYd3emW]?4l:fW(H$<1$p=>f8[A)SSFDPlM@:Wr-6A`H-XF!ObY
+WWH08W2edYC9F.ECreul4AF!jDLRi.@:eId<HBEcTW/&n'r\.jX(qW(jhs[;S5%0u4pGU0.iTS1RNnd5
+<>DI45ZItX<MLCr'-Bg;2_?C;(mmIfCj`0rNhd"]<M6>n<>?KRc/GLa)tK(IQd^!7I37s_ZLOWg3Kbjg
+<Iq.W<)rEcec#)smIQDrYLBfGT.bFkg%\Co*(^mDW^'PfW[bNW)UYLRpdgk*BfU/r0RHpTpe]K\8JS2(
+5ZIuf.V)mDY#.4LO>PS<EoM:Np9:jmTrL,9.Ss,rWX;^*aLGu6Y-m"J%m+O?Vrbk]_!GK@j$Qgf,ZUM;
+!`]u6WfUdlQP36b4RB#WT9^.OO/+n>Cj`0rNhd"]<M6>n<>@9i)X42jpt/GRR$JD0YL;sFZLOWg3Kbjg
+<Iq.W<)q#:C:p<^U/1XTiPX3re^jJOA6M`U%Et>o'W>c,U^C($TW/&n'f)^+or+C:P:eLA%h/umbfmH.
+><LATib2`^'VmuoF!ObYWWH08W2bqF<T!71d5tX^7un_YhnMBL_Sqt0'/04kV]Z/CRNnd5<>DI45ZNNF
+X1ESJ:e-YT>V8fI:A4GV+$P(^LsrYjT1<i-;P:$MOt':.JZ9&V<CjN4!Cl0=PB:YYD;W'UT76WNpd5Xk
+ji#-RbUH)6WocjP5ZIuf.Ss+g]rK$gPnVhlb:d2787%;#"eCsClj%,t5mgRK7oBMj.Ss,rWX:1Toloe.
+*GNsqVrcK%V:+HF\m$Z_]PH,g4&WTYY5\IKf[uSf5>a[F2+LOJW[^hFJZ9&+<6]U'8`-tPS^,.7Ed=c^
+f@+c<s4uJOmZCQeX,.ioeJq-qW2`*_"fa&nX(M=ZoCV^CFF&8Er^l%-NK*e<1+g8*dAqRQ"Y!9u*(^mD
+W^'PfW[_\_(2m/IH;;&a]KB,j;Q8/qQ"g%1=U`NEJeY.uNhd"]<M6>n<>C0e$TT[aDI0fRgBK</+.,%l
+.8\P78JS2(5ZIuf.L<*OGXmV_0eK:Nh0k5\O9/G,O]6n3$<1#uWe.6c@BLF@-DonsV#0`k:-f4d2+LOJ
+W[^hFJZ9'?WWaH/'-B;N;5WG#eUcDkb/T4:k")L;<<]BP:fY?GC9^:j`5dmATP6DXDd`:CZLOWg3Kbjg
+<Iq.W<)kWT>,6?cBu\&N>-)Eu8SQ6:Yd3emW]?4l:fW(H$<1$0XBO,3?u4k37kNN'S>>[B%1as.+0(bS
+W<-'7W2`*_"gb4fb.mMOUD@iQ:?2M`LR!i0<R$Y*JZ9&V<M6@D9iX[E3Z`IoJkjLGG$t$JZLOWg3Kbjg
+<Iq.W<)p0%>1?1q2rnC;/ag!9#kpe!.iTS1RNnd5<>DI45ZIu?WeJ1n`\nl]e3ZsSZCc5`=P<IGeJq-q
+W2`*_"fa'E>*Vt\%3B]B6;Pd]C1-b>ZLOWg3Kbjg<Iq.W<)r.].\h#/*d6&-:WnT*@.ph1nY)sCOt':.
+JZ9&V<Tq.HQWA"@_iDmHr0$`glsXJc6WJ1Q<M6>n<>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"]<M6>n
+<>@Qqb,F)"%m)8inobcNZ\Og*%opl:<.V%V<)oTk6>';D(Qp\$fMS'&)sDS9ortalTrL,9.Ss,rWX;^*
+kb'EWMeeB/*L1?LJY%?HiaHEY6WJ1Q<M6>n<>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\,t<J;Hn
+EY48UH.Yu^Q?U^+@mCPT)]u)K,H1`E'r\,t<J=IR)3qhn@7Om4hHB$F0Cl?QnY)sCOt':.JZ9&V<G5R$
+;7TmPG#Y@#[WFJ_HW\tr:t6OW'r\,t<<]BP1p+osQ+Q-d7qGkA=!n*VLR!i0<R$Y*JZ9&V<M6?Yj/Q>e
+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<VW[G(Vqfb
+@7Om4m`&"_(-Sa8I&Le?U^C($TW/&n'n3.)W,0MlNln`aV3\;Qo!#FiTrL,9.Ss,rWX;^*GZg;/fi3/W
+lGJAf='&J*[Xi/'Hd)e-:-f4d2+LOJW[^hFJZ9'$9-m&=>@$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*<bELAk
+M;9.C'r\,t<<]B@j3l'gmI-uhCMWh@mJHP):W"8X:Wm1F*Z>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<HB4E1+b"5?!/eBd/sp"94USn5ZIuf.St8#SNk.`<HB4E1+b#`>$2'lDL"3J,"#63
+W[^hFJZ:fqlLg??2<Q,3e$?h#/'G``'bA>XA4W^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[h8<U"84="KA<p4@2)491=iHU1f8JS2(
+5ZIuf^;fC2M5Y176W0dmKoT6#G:dN]LZ9[9,ZUM;!`]u6qAQ,I`)JVW+XtmG6H:V",r434iHU1f8JS2(
+5ZIufrl4A]]MpcO+XtmG68pF<^q:<ka%Kr6W^'PfW[^j<(T+BkUoHEc8XYkZ\m/ABk*A+L09IEW!`]u6
+W^,+$XFGAKWocDhAQi-uIUsCaS+CU37#%KE<)oTk!`]tRHpC\Q*g/91/+g3S%*VFj3*mdr;?a'M:fX4Q
+<AiZNMdBE!6W0dmKjIiH\+sgX_sn?WOt':.JZ9'2Xt"d-WmFa'4="KA<\RWY0#IMj&'%=R8?+pT"fa$d
+\#T"3\AbR"iuol#Z@^?D1,-Jg_sn?WOt':.JZ9%\XX\e1@mR.FGt?)bXC/KB%*WiV*&!EtW<-'7W2`\@
+WoCJi.qhp/%nW1T'eEFP\,#odLZ9[9,ZUM;!`]tm=B5A@<"Z0gGt?)bXC/TE>U&30*fcT.OApe2$<1"R
+>h)f&>[+_<H6/5)/Lq[A/-GJo*fcT.OApe2$<1"R<n4Q`fiY>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?V<O;IgD#@:3B:cJ80),&3?+P-WG0B19C7'd[>e/;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)m<Wh]%QE7(FNIFZ=u7L)TW/&n'k4<!nG?^1h]-..R\=Ri3us(!
+jmlTGD-su#n(uF<d#fPC2E!Hs:Wo=.GHmnq<jN_K];Ah*<JuO+$<1#uW^<4UZbCZ`S5hAqA('W+l1ucY
+l_ia32fJEapE)J?XS&N^0J.(9GH_cq@-//6Nhd"]<M6?YKN0%?:Q2Mmep:$$]Y$@*Vc'KnDkEJe$8_2)
+ZY/%g8!oSW;E\-fIN4ms:-n#5W^'PfW[bqfe\S2S`)kIi7FGu'Y(ZndrZZBb0eFoOIBXFG&fI3kb`PkU
+WX;^*LoRJ9;)sK7WXn5`=&rAuqtgmF4F$I?Mi3M<rX.,[63[Zf!=!.260:_<5ZIuf.`>99c)@/N<7MHL
+Uc32HBR!g@1M=uiq^Y@`$J?CMYn,'KK$9?VJZ9&V<J[QQVb=&0;q2<f8XYkZ10[mWB+/i&NgIhK<.V%V
+<)naMWj?>q,,sjaLSRVe$CS#tVk6GHo5M?1L85/U<M6?Y$rrjXb"3RQSahf+BiuV&Q&Tuqp2IZ4L85/U
+<M6?Ym]61'Q'2KRqVr_Je$;<[.i[`gm,<2G$qV24W^'R<Hrouep!e@jFid!KVIbJJ=#S'L3W#i(+fPNC
+'r\-oj/S-Sa5sh2=!!M[/Lu'J=gsI(]B5i&>Y(k[<<]BPjL)#J;)ua:<Q@3IAQi.(Yd,pmHqd\o]Msjr
+8JS2(5ZNMC<qSU*-TS<oRtme5b6f:kp(n;Mb`8^CJtr`Q<M6?Yri3OOQ'4=2FjEEQVI]oA/,Pf<#%h4B
+UHh]\W[^hF+b*M`n=B^WX6Q:3UeJr=*`htlGrW_.aBd?o,H1`E'r\-oZ`6$2jsOeAo>!^kVI]oA/,Pbt
+r:F_om],Ck8JS2(5ZNM#<R&XaSE-_c:3]=P[&i1oA[fk__!J=@cB2(WWX;^*aA(4_N64_PWof(lUeJr=
+*`f_#S.Eh=X6(9k'r\,t<J>g"c.gcFo2.AH4="KA<q*?!NTKQ7;pTC4g`i^!<>DI4OFsPnI$,i^<V+3t
+;Pn"/%kHC6Y(!0HcZRcc,,kWD'r\.j]kS$IjhuZ!*:f2UBj&9ib-mJZJ_>D#*$V?`<>DI4i4<Ye8*.[7
+eU6qLP\F::_ohBiLBDT-Woaa^'r\,t<<^uSF*lkEf2_Fa6W0dm"V3,qZb2ienkh8G8?+pT"fa'IXd[/j
+F)(VRbq0GtR?eRMXR+7'p2dBa<6Z/@.Ss,rWX;M%k'9p$gfDZU%nW1T'g/R6>B2STkGE!COApe2$<1$p
+=>f9:EoOE?c?Vt'[&i1oA_WKBJN6M+F+U1p<)oTkK"J%.^;alH];k\i+XtmGJqoWY5Y_OGS5mf9Ot':.
+JZ9'kWoiA(d>iTSNIlq]9Zmba<jjf0GXS1=WpZ!KJZ9&V<H(npQ;KY^<AbO5OtnU>GZ`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;/2f<VRu*=k`0Q5)N`D"kUA
+>V+c4>p?6L<>DI4OE&ef8*)#fW\O(+P\F::_oh\kjhRD`WrA)ZJZ9&V<QMI\Un_tI^+/%a.FYSSLR%7l
+^O\D"Wm6]*JZ9&V<QM7V*j/Z"2>1$kSL,qTb6n[c91qp4gVC1t&BVcJCg`:!W[B&lTW/&n'sah(loU+c
+p/Q2$-f*G-3.:XF?XM^Y<t630&B<jEG9.S<f_rDWq>"WHAm/5kk@!iP$pZpTh&rZu<>DI4O?qFL8*,DP
+<M9;dVbWd$)B-28qtAUlf#l--WDj3Lnac8AS+h=(lrb0Uc^d.AZVJ$8ftYb<X:Wpuio\d&;-!:?U^C($
+TW1?4<=:K1-g[l8D'gj$bC+eeQS)RDh6OW')Q):?=0B2]kF%a+n>-o\nMX(a$F(U&4pL0%W]ECC"fa%K
+<?]nm`>FjRp/QZ%PHM*mhn!rGAS%]]9;PMm;so>I(Z)grjQ,CKcqsVo^pL0SiPFM]@#hI+ejuf+<<]BP
+Ufu=7Q2Jn3YF=c<l.)h9_j#N:hRtk_FRnM7dF$@:8!!AW1,_&aq!mA[rtNqf6C\9Rf&sV(X(I6b$<1#u
+W^;#3*B`5B)YR0Ph'Kaas8Ls7i5EQ%_lo)ZN9oB4pYs/MZa7!ee'm:)iSian:S0QBf66VY<%;0CLVe\.
+X>duPh,<M#?+\r5R)=o-;qXa:XI:`(.Ss,rWX@uiD>: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+<c
+0/(Z$WDk@tgS^pG_IS#em+E0Si7co=gt^\n$Y"d'\_5.a&uK6"a:FSq:4SI.blUHOW2`*_"`;f,>QeOU
+p/Q2$-bX]j=r;&@0-]"EolV(7PEWCY_X-+1f,#e$Aq,MK=X!cs-&WkJ`EtKF,ZUM;!`]tg<Jrmf-g[l8
+D'iOk=m>DD5Bu\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<*b<Du:`"fa%K<C'NjfV<K(^+,dK=tZ-X(Di].J%jr+R?eQpFKhQc&`s-p9aaN-W2`*_"\I7G
+YKWV!p/Q/TOtnU>GZ`X`V"cc5JZ>T$U^C($TW3Tle`'jfmS3&Kjuao#>#t-sZKXqr?nPL1<01(1WX;^*
+0WrQsr!N>$3=e]LQ5)N`CoUANQjZtqO/*+^<M6@DAQ;4]?a6f'o8o2N=i&"uVG09/(U9tW8JS2(5ZIuo
+WeGg:P%N?Jc$;k&[&i1oAMYGm@2?5M&kNh:W^'PfS#V1W#f\dhX6)MiAQi.(Yd2qYr\1Lb<Fn_$5ZIuf
+.Mu,r<.4rHW]:S]R?eRMXDDKZ"*:0:C62DGWX;^*kd;n8E[0qOjuao#>#t-sZ<>TabXJ]b5Z.cc.Ss-=
+VlG]u9sCgio8o2N=i&"uAk[/pHE*DYW<-'7W2e3Ce\]'/8Mf_6juao#>#t-sZ5F<<m6FZm,ZUM;!`]uH
+YKHOtRU]\^k92;'Z@[jsc(=UD9N?-Z<M6>n<>E*Hb,F).ql2Xe8XYkZ4KPUlV?8VSc&ktVWX;^*kb]i)
+nkRh=c$;k&[&i1oA\G;eR&Oh\U^C($TW1?!<I#CD2R'BRo8o2N=i&"u3b!2_V5cP=.Ss,rWX>:sjWYQ/
+@:(nT+XtmGJqoWa`SuYs<&I0D"fa%K<M>&FN66!I?T%!i.FYSSLR!"[kCK&$k")L;<<]BPZs/em\J+>1
+juao#>#t-sZ6&Zs0N6LA8JS2(5ZNNHW\Q&JLjdgDLSRVe$6E7!W?*[*:K;H]<M6>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>j<A&l3KRMC@LSRVe$6E5+BA3+e+B?[K<.V%V<)r^q.\j9ip/Q/TOtnU>GZ`Ys
+fkFikW]?4l:fW(H$GTPO<&V_0C/1+;I=6NmIQm\uNq['f<o@BM;TjWZWocjP5ZIuf._Mk*V3S0fYF8B8
+h;$dJIJWU(?_'<)ZLuGc[&h'Zd_4cFo3KYuf%<h7W2`*_KpO/e/qRMkIFXi3l,Olq4>8HC2`DQP]XYI2
+4?OSfjqd](>#sQhkt.(iq>Z2)Zei"?YbN[E<Iq1D55X^0?+Ur=c*=I@1J_2R<;,\3F5auig+?6%;gj(B
+>?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+<EPM`fb=O=\K6qe0/(Z$WDk?_CMVl>:S0hQ^Y@])
+i.iD'f^'\g>#sQh;4spLOt':.JZ9&V<M6?Po;#fD%b'm3.p$JpXK8LV><_lDP@f;uBj$">7=hK$8?+pT
+"fa%K<Iq14'M2)j3&*BP3'02@rqu!-UX@K:iSiafIep%srL&rd3*6lnC6#(s<)oTk!`]u6C/0n5@q4QK
+WDg2@1V`hT]1Ydi/agrlXMDZH5ZMkN<M6>n<>DI45ZMm:UmmjC$O[?CX/e>=PA+0NK11?kk")L;<<]BP
+:fW(Hjuao#>#t-sZLOWg3Kbjg<Iq.W<)oTk3=e]LQ5)N`Cj`0rNhd"]<M6>n<>DHIc9kL,AH`Sp)]u)K
+,H1`E'r\,t<<]Cko8o2N=i&"u`eUSV8?+pT"fa%K<Iq144="KA<q*?!nY)sCOt':.JZ9&V<M6?P%nW1T
+'g/R6I&Le?U^C($TW/&n'rYlm6W0dm"V3+&+0(bSW<-'7W2`*_"fe$_OHaH^_8FF8LT9K-W^'PfW[^hF
+JZ@ip,\:.[n?B4gTrL,9.Ss,rWX;^*TW1!_8XYkZ4KN@HW+q?g$<1#uWWH08W+sUF;Pn"/%kAS@WgY03
+!`]u6W^'PfWg`O@P\F::_ofuSWocjP5ZIuf.Ss,rWocDhAQi.(Yd3emW]?4l:fW(H$<1#uW]:S]R?eR[
+V,Gm6&0\'B,H1`E'r\,t<<]Cko8o2N=b3R[<&I0D"fa%K<Iq.W<&Nh^WJ>>H%oj@CC6#(s<)oTk!`]u6
+C<f1E&).X;W[^iq*(^mDW^'PfW[^iqS;3m][NRh6<M6?POt':.JZ9&V<M6?P%nSp#6[Yi45ZMkN<M6>n
+<>DI45ZMkHP'Yc8peXtAWocjP5ZIuf.Ss,rWocDhornnqTrJ/oc&ktVWX;^*TW/&nc$;kF@2n2n<<]Ck
+,H1`E'r\,t<<]Cko8qJ>2T1BN$</lQ;?a'M:fW(H$</nG+fU66L_!K#W+q?g$<1#uWWH08W+sUFdbOm'
+!'hT94ZENDzzzzzzzzzzzz?;mh&()@Zd=')`bO9e!splM_7b2mZr=-o5=ZJL[fXKShRcd.CHc$D`$'WQ
+WZmC-5:"jkECf%(?rr:'[ZhhOF,]e)KJeaNK5T0CM7""mljZtI^Vh624q_$>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<Vn<n0.@85APEXt;jPe"3R?gR#Uu?=TV+[
+.i[tI\+BH%GU%(@_<.[A/J4aSed\59eKC2,4dk,5I2If/`V0R+R!G)U.g6o^*Zj.5_mer_1I`l?#5T0@
+PkADb+L4C`;CAS!/5rTdhjRe;Er><%b"LN9du_dedI2G!ECR@B\8iP0X9qXrPo@3u;i>e>/2[;C`Jg=f
+^i5X,/=e*mU>AJ=CqlaLcf$>5sa/Rd9>D<ls?F=J:!c_!G*I2ZRLq3bs-GG3uBn%Er=m+H>1AQguH3B9
+)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,XNTeudoiiBRA<cY78^FXaf5W]tmOr90o:dbKe;0eJ==^OJK7W4^.,7.m$]E9PZB-Vp=E]fEXth07
+aVLQM"<VI_8&,9qR0m_*l8@edAh4ZsfL%Grlrr9a7bKIJP?W5jo=4+-hmp$:3_>dt>?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_P<cZ`Ie;jOf[s;NYajjk>lB(Q?AYAnZu#0mc'r/>3-OX*UU%G\e^_CJ^@2[tlOCt@^]!
+P<^\cjU_F\nDQ&e0/GgDsEi=A0Nn3bcr(7R[H%ZQO1B>#$?G5M"#l?*<rDrg.<.t@B<?Np]=.[<WW)j5
+PV4Pof/FO-RD&*MJmr8SluD+!u?qW]1"<,^`hV%ok=>>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[]R<EjBj]fR*
+MlqSL/5%(@\f#7_`PJ&Dp?4*+7M'0lKk,M-&+;Earmpu74c/MCGC#j9a&IgfM/GEU9:s*STtlauW"gV8
+5frU>ae0eHT9K3UAs%N[aBmDA>Bhk`Y`_SJDq3&iu]S%ST3=*X)%nr(>Ya6T%nN/s$TIC8p__<S`q1,?
+]M*.%8=QX>L9:uO]1GX*&dJ@tp]qdB]-^T&Qjo<3ZBMb>rG$AX-(Fj&>0-9H\SpqX[1GY""gAmDunZ1c
+Jpa,V2#r*tOsfuf9n2]D[GS(H<e@UioCqE@ic[hDL-HLF;<Ztq%9@g`*JmB(<E>fX%CRL$Ohd*PUsl9"
+II'-<!@]gXZcIf4TV)'4E@]ciLDhl8m:kbHkjJ+2C4Dnl6/P:`b5.eiDhB2/X<BQJ69@:=_r]7k`Cr)d
+3/jN12%>?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/5<bVmo_%m*o?B5C\HVg#cT_p]4F$5F
+Z&*\"jLU-)dacl*CY#RCB?lIMB][C%F6Cgh*ZgpJ(,!5cZ8HFK9P4c8??tY4<NGc2l-lPWH?uH7FU_[O
+ifZqe.Gjj0k*g(MoQWGrEns*%d4\f"DY./f6UO3jr5g=YSFdSo?h23r)gC`*W@uc-Wh/M+++3Rob:gVX
+n(ta(d%*:Eo(LgV^\#rI^\dH20mBMU`_@gU[-F8\k0K[7I/3>fNGlK"L@4a1//tHnEp3WtetPL;nak'k
+?Z,\uLEAADQX4uPbl'!$EKbkEg=V:4[VTZf.pKK#eK*.U<o(9%fs5ALn%A8;NAo^mB'.=*ZYnU1T76X9
+]VqN74Tqq$p0-j_M/hq:[Had)B\Tt4`Js)ULE:c?>ghDSn%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:io<pYL8#*0kuq
+p"*P8kqDA1f3Zo@Ffp&)^WNOKj;IT&<691$*HrpWo%U(7ITiQ_o:c07'WQGg]Q@OhTXqK_9@ip-*YDTJ
+Q?sZ7G"d6tcIWE3iC7p3Faj&NfoaDp?c2.q6b;Td]8EeM@;+qtpc#%)K>sZXIL<j4WM&;ioOk7g2%Ah7
+C)SY;e`ji?rJRlRn)(=,1th<#:b<2?n3F6G=n;IY^L+[:_Tb$sI.uH@CPL>-]&K*4GJ'+3>bHjAZ'*LU
+J\K]?4i+<n5=MK!=r31o;TuYcb,K'lP&!:%CYfr$B[49UWiI1fm'2]Q^RF]C='l[Y.3X+$CY#T?s)m4I
+pD#cS-#5N74W@^Wf(>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]<dA3])EV):p%I4e:,UiC,*$!I=rl?=F$k3[17^c832BR>9JTh%bdn`I\/:$^@
+]u<TJ5Ge3;7TJu1@J,S%!gHsa&/)@eS2YAXStd6rTJb?#6`$iL'XlT+qY#h#^*kV9^36c%j[ss%a]Ze%
+Krl<DR:@uGp@S"@PYGq-?)4=?r;#rN/mVXcX"7ENW-h$?^8b3bHaL&([TK@h2;j/r([h4),=dcQ\N_Cl
+4bSK*gU?@<Y$SfQ@ClSio[-0o;u(TUFe)>\=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/[<PjCZWH'IY\aa'-;5fn;9lp/[<PjCZWH'IY\aa'-;5fn;9lp/[<PjCZWH'IY\aa'-;
+5fn;9lp/[<PjCZWH'IY\aa'-;5fn;9lp/[<PjCZWH'IY\aa'-;5fn;9lp/[<PjCZWH'IY\aa'-;5fn;9
+lp/[<PjCZWH'IY\aa'-;5fn;9lp/[<PjCZWH'IY\aa'-;5fn;9lp/[<PjCZWH'IY\aa'-;5fn;9lp/[<
+PjCZWH'IY\aa'-;5fn;9m(fOtk4*0#(P#<YSUl*TP78p#O-1M33KK=D-om?O*@Nq/OrYr2O-1M33KK=D
+-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/OrYr2O-1M33KK=D-om?O
+*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/
+OrYr2O-1M33KK=D-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/Ora=Ve3#o>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%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c
+%[<JRaUXQ*8')7*S4C[2Pak7c%[<L8h;)B2&D@[AkAe]d33%6DEsI_?*lm&eURK4XGMoF^EQfF_Wd,S*
+#EEO?a4u6@R`@.,i2GdXbT/[CGoUm\.36r1rs,ttNI26MzzzTIJ0X4knT?GNR,;\d-Y1^A[c[T7?jN@H
+4n>kB)eUDb!b(n])`@2JlX-E+)7,L]1VpIf7X?:u-b=D;TjCrCo6,FPsrclGg<iHL&0.Xt0Kch!3Fj3Q
+Oqhpu@Dci8)"h%hB0]$k04>Z!u7`<HShPKcNC,]X+qC\Q2B&kbF3GV)ZOdNulg]4Ztqc%^Q'2pu@Dr_Q
+U]MC:Qc/MqXmQ_&>hoUAk#TYLd-2rIu8/\GuR*)bRj9c9'X\?!DciDr3bGE8nfm)FiDW%NRU+6`Ro%)t
+bKOieoJ@25Jgrl`\&U=gDT8p?h^N>-c^tqKCJn?0.Ym?<c97pYD9-g4g(Z5-.+tnDM-'Mk9p*eT-YEC/
+O9f\)2WRq)@PQae*tO8]*i[7>hpEp!cntN3Z1Bi9T(ciJ/L-[s[u0LH=e'ihiKEjPl"l4]hA\]Qg^,;!
+Jm'gX<F%NcS9'f@t5YrQI(D/6fRLSpTn)[Z.8\jLXR!]">TOem!'@ie]$XT0Bqt`JRBZ2Ussh(=l<f:7
+kZpjIJAV:?uFc7])rSjPp)4bI2GH@S?A6&C\+9q=5OfG*LnPhS!+uq;fY]2%HV923]XqYcV-=Y5FSei>
+aalfV#YBf3a"jo'_HRac[sr55T0PK5,7u9.o'=d>TeB4T+<-?ZpU9<l7C*$k3ZJ=gOuM(KKdYkFR#q$]
+MY%e_3KZ/8r8uB)->P%QNoXS*7Y:rqY_6Co,%MK1s/_!!'f6r<!\M&_-~>
+
+%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'SL<N7jW`9Vc8pP1W?gqjU]Y
+'?M4Ka\9[fPI9!.qei%;^=Tp)qqc([Q[R00o-)0Blb;e&f:KI@pUP\>q]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]<TH[C*dmB5QoQV[X3hP]ItOceV]@679^q8S
+C5G4:nnbB7rMG(aAGN^[.V#7">3=CE/tl*Q5p"TSNfZ5blF,\)@`*'\o-m[A6k_V;P<D2=amIF;i_AmH
+?J-f'85a,J5i=tVBYSMOo=S%U&kc-:FaqYL&l4tH[6!0X@[VoNDg++O3.ia'!tTV15nX&m]7JUrB`]"7
+[2hkm9Jpd6g6jhAICR[P-6:o"!8SNE2.>O;-3p[4$$Bts<2jic&b[C*Cg,pd_"qk9seaEFe_fbW>_`d%
+AhihC3-Veu=gJ,_\%0C%cbL`bDdD/H\u*]eCVMGaES!;ImXfW`33`<Qnp`&HPfJ)<0<oG_YP;;!O)W3!
+)5FQnt5^$b%!K*S0"im[JAQS7/=iGI>X[C!<23d&Rl@Cp5pDm]P62fEMcG5KqAC'1\e82Sdbl^$qpMM_
+c<\ZDmVNf[#6h*R7SX&m>;qsE.I2W<h$M*97`9q1K/2fIRNP%F[o!;KU)&&,!CX,CoOZY,>9m-<kn[f$
+!fY8Pr"LPCQ7XBB$Ps8)%7[j=3i]:Vlh3"bO"/J='R]79j0]6X&4d:aVkJSj1JZtPT6)1S"1`0D2-9o)
+T;5)m!-2)X'Q`l?#4rp=S@`i<%>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<k,9rJ=Q1g<ICY/:SpZLBN!!!"<)B'N1L2:X@4*0PCrQrs>&\iO3FOVo4buhI1gY:K:g);PTLo+
+amb/19jLi_1H[r:0lDAJ8U32Xs<p-AAt_'u9n;cZq2-YDNqSI^<n+08lQo?bk2FKtlH!!$Nh=*e^)[W-
+NiqXp%YTrkK#[Q3sSn`0/!2YYqFP:'/;S+4hCfXuL9)o&/=D(7.k09:TJS6JM2gtWn:oB+;eq=Ed9Pa#
+^*Qf#`"WBP=]KPbq*O7DSiZQ+Ii>@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<Iq1,O]6n3$<1#uWWH26$R'3ikE%f*<M6@9,ZUM;!`]u6W^'SQ6:`$33@#,8'r^C<8JS2(5ZIuf
+.T"[-&<_ce%`q(<"fcl=;?a'M:fW(H$<6[NKVTc]6h7$NWS'Ih"JVo?,ZUM;!`]u6W^'SQ6:f8+XO,VP
+38:?m5ZIE:U^C($TW/&n'r^D'#YCGM'gd.WPD?0pO]6n3$<1#uWWH26$R$tJZ9eiCD":EQ4-D'i<Iq.W
+<)oTkG9J5C>#kpfR8"a&H:Lj:W[^hFJZ9&Voe(on9Zmbd4L<-s<UWJY5ZIuf.Ss,rWnrGkP\F8DKP*2g
+W9PBp'r\,t<<]BP:t</hOtnU>K^d24%op:fW<-'7W2`*_"fckb6;j[lKb0j/jIrSt,ZUM;!`]u6W^'SQ
+6:f8+X:W6)Rlj[CNhd"]<M6>n<>DIt>mO#iQ5)`2As8"MhN:cgWX;^*TW/&n]aGu4e$;:GcYW/KeN8pD
+:fW(H$<1#uWcoU3AQi+?iL*C]Wd705"fa%K<Iq.W<:qksUeJr=+[!*Q6WNSGW^'PfW[^hFJZ@R\OHaH^
++bXJdH)U!K8JS2(5ZIuf.T"YW?.'6*5,?Zlf<8OW\L5k3!\Y;mWd705"fa%K<Iq.W<:s"No]!iJbEaaW
+Bj=A,c+q+[Nh)T('mQu3:t7Xj.Ss,rWX;^*TW33nUn+'I2?7*pEb0?7oZ<:EjlPU3[C&$gB3J?o`M,Ut
+mmsHD<<]BP:fW(HhC4Weo#i\5p?[WR?GCsFk2GLLT4coNBj$!'MZQnl,H1`E'r\,t<<]CKk<8(AGOKAE
+]mTWr[,q8`pU92Xhg`uGOQmp:(e^\hrql(A^F/V)j2I`X<UWJY5ZIuf.Ss,rW]o*LRS#a?q<(P;Wr"_b
+p[6kDfsn)`/&$#bb*@_m+3sSAL@;l!bKKeXcc6@-iPhNV<UWJY5ZIuf.Ss,rWnuC,]QitE))_D3`pVWm
+A(.a.bjg[n=0Ff,i'=`+r9(]\gi%[>:iLBiJtFA\V,E<J6IjW:Z[E/e6ri\HW^'PfW[^hFJZ<&SO\5R2
+\ss_T[/>suYSj&*(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'i<Iq.W<)oUV9MKhAg%T0kFmIVl
+HhTugVu-\.(BjsNbhhl!;4sq5,ZUM;!`]u6W^,)PR;>k%\: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^,+<P\h0Vs("%8n,2\6_kT__EmMkQVsj1d@.oHoMZQnl,H1`E'r\,t<<]CONLWp\#YG'rc.CXH
+qH5ttJ#*^Jg#Sj,G9g1]WWH08W2`,50/h>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)RHYmO<EIaZUq'.(Q8:
+"XWo-]V+>WOr'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<W?K<<3("zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
+zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz!*
+f_]+#Su~>
+
+%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<nk@9?1m7S&Z9
+a;CU/."Me2&IG9iLdd,U,m8-X#_s)+YBfQ,SjjBQq7cI4Ep`hb0UeB^_u"ZVi[8G6Siiu;>?b;Hs4Y><
+rUncB5D9/A24R5E<[Cj*dbVN2rp.=JHmk4l2:AX559A<M5,n%.\<[H!:S4fd.29k'iClkJ.`&-I>g"D=
+C*_DOdC`Tf<Xi\>JlN+XNT=7iGuc0#QAanM6Ek2aP7jrhY\GH81<Hj@agGA1D\dq)_jQ`s<J"3q\X#gY
+eOZk)U3X'VX;]B["KN0:)TfB\nuP<$/Jl[$Kj`GM-7)cZ@+@fNAWg[`P&%UAhCJf1L,19pWW]@lqW]T_
+1<6^>agGA1D\dq)_jQ`s<J"3q\X#gYeOZk)U3X'VX;]B["KN0:)TfB\nuP<$/Jl[$Kj`GM-7)cZ@+@fN
+AWg[`P&%UAhCJf1L,19pWW]@lE\3J;X-$Z17It+7=>cU?$!&?S23MaCjB7K'=tZ7&$c/k%9M2N=_5`W&
+b9YDK,hI(a]NGM@%;/Rj<=)]cjB<mU<[5/@Mrr8NY\H.]'!+^0C*_Cdag2o-[9PP,(4oQ(R?V)ZJlO&+
+Q:[[u8?V'KH*\'`)U>,]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%<c/2%oqur*Fh(YAIZTi3_(7^V@PKVbeM:L,19p
+WW]@,\od"(lWUIfl'02o)X)WS>P"M:p$XnEeQS"3pZdSW=u*/tcD1TD'!*"ncRk>CX-%H!gbSD<rt";;
+6Ek2aP7jrhY\GH81<Hj@agGA1D\dq)_jQ`s<J"3q\X#gYeOZk)U3X'VX;]B["KN0:)TfB\nuP<$/Jl[$
+Kj`GM-7)cZ@+@fNAWg[`P&%UAhCJf1L,19pWW]@lE\3J;X-*1br/t=."E,'-<Pt5h?r_eY,Is+3T%-B7
+je@%,&?HeqU]d_2X;]3g9_,8SE\/*PRtY8#E$cl5Q!i-5Y%<c/23MacB\k9CX+iDZ_5[X=7WL)E4KFTL
+b=2#7+]pUl8I8ED=>c7WRH.M1jB=1)2[dI%iClkJ.`&-I>g"D=C*_DOdC`Tf<Xi\>JlN+XNT=7iGuc0#
+QAanM6Ek2aP7jrhY\GH81<Hj@agGA1D\dq)_jQ`s<J"3q\X#gYeOZk)U3X'VX;]B["KN0:)TfB\nuP<$
+/Jl[$Kj`GM-7)cZ@+@fNAWg[`P&%UAhCJf1L,19pWW]@lE\3J;X-$Z17It+7=>cU?$!&?S23MaCjB7K'
+=tZ7&$c/k%9M2N=_5`W&b9YDK,hI(a]NGM@%;/Rj<=)]cjB<mU<[5/@Mrr8NY\H.]'!+^0C*_Cdag2o-
+[9PP,(4oQ(R?V)ZJlO&'Y2u;U]]s-.)UE.Df5f9O=U8%;B2X=]$!*&H=seMq,Is+uk065sC,hCiX@f^3
+,hFfnZPprn>Vp@7JlT^VP[ILC<?o&?8Y4I'Y2uC'ZOj2VcD1TD'!*"n[7p.m7WL'sc'sDpeSli]=I!:E
+8?QNeB/Pif\S0hN"KR85.D`"eX%%+]P<>n.>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,'-<Pt5h?r_eY
+,Is+3T%-Bt?d#?BSa,G&BRB$[JHLrS4^.5)GO3u0p/@0=PH2C%CRO,*6Ek2aP7jrhY\GH81<Hj@Bq9&8
+d\TTNerG`L/MAbQ:J^`WoF)6@5PH`O5(&aSo(_b8?aNA->jZ`Y/JGd"#Z_CI;M%mT<fQ0D-@&W:X`H)_
+fsBl"_1TuK04);Y<pneUGFs<,naZ/A95Dc3'jSobD\dq)_jQ`s<J"3q\X#gYeO]"/b=bK$C"&Y<J%k`I
+>-4)sF7.hoBA[rEA>H9,lfFapCRO,*6Ek2aP7jrhY\GH81<Hj@Bg$8-g9k^1dpBS*V=g,.[r4KSm+AX_
+I/*3i^:q>[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#<Pt5h?r_eY"'ms.Uq&bin'?G`p!Y@L
+B/S'7[^P=$rVEY*)QsQPbN%XCe'ldj([p.iPQ&<E:7PEr/[+k@H=^ks9t`<_L,19pWW]@lnoMu"cYB9K
+Hu=4EC?joq''!-,GOHRk)HuN0?-<@h(4oQ(R?V)ZJlO&+Q:[[u8F'F%NPEqdiPL9`YA*r^Igu#@o2i,%
+?1A@YdTTuEgW_m$/Mk%B#Z_CI;M%mT<fQ0D-@&W:Xn+Wqp%@tu3cq=ribI@:Y46WN,9nGXf?DOc[^W`<
+4F(tThq>D2n7EhG24R5E<[Cj*P&$KH=cN&G5Fpm%fsAlFbI;BFlKRPINp"O+al24;k1;'n%1RH'FuI'N
+@qK="c0X<DhDKcOT50kQbsJXL30OuV9!E%Uf.tIS)JT.7q0I.)[/#oh#6tL'"*eWu/A5Y)@\WC%HMrsN
+8(u_P)fN9*hnFI3m]H$6\s<ZZ/-N#.1F^/?"KP"4/X&;tOk.:5PBsokoB+:T5#L*7X4;+C\FK@aitoG+
+CRO,*6Ek2aP7jrhY\GH81<Hj@l'9gT_6lu+Ag(86S4O#f-!3$fCRO6dNT=7qRPtVjWo6WDYU[JhO^-'T
+cY=`WEm_O&#Z_CI;M%mT<fQ0D-@&W:\X&09bsJXL30OuV9!E%Uf.tIS)TfBl1iF.]<Pt5h?r_eY,Is+3
+T%-B7je@%,&?HeqU]d_2X;]3g9_,8SE\/*PRtY8#E$cl5Q!i-5Y%<c/23MacB\k9CX+iDZ_5[X=7WL)E
+4KFTLb=2#7+]pUl8I8ED=>c7WRH.M1jB=1)2[dI%iClkJ.`&-I>g"D=C*_DOdC`Tf<Xi\>JlN+XNT=7i
+Guc0#QAanM6Ek2aP7jrhY\GH81<Hj@agGA1D\dq)_jQ`s<J"3q\X#gYeOZk)U3X'VX;]B["KN0:)TfB\
+nuP<$/Jl[$Kj`GM-7)cZ@+@fNAWg[`P&%UAhCJf1L,19pWW]@lE\3J;X-$Z17It+7=>cU?$!&?S23MaC
+jB7K'=tZ7&$c/k%9M2N=_5`W&b9YDK,hI(a]NGM@%;/Rj<=)]cjB<mU<[5/@Mrr8NY\H.]'!+^0C*_Cd
+ag2o-[9PP,(4oQ(R?V)ZJlO&+Q:[[u8?V'KH*\'`)U>,]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
+8cs<SCi"BBH$XeJVl+0PG^,4WG:aK@=U8%;B2X=]$!*&H=seMq,@PA5c^m9)%j#Q$Gk'e4["n,,]t<"Q
+]9Wc[/-N#.1F^/?"KP"4/X&;tOTl#G[CilRPh,=!oB2UHWRYbJ2<W8U>ODIM=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@hcD<VnB7MWE"pWV&IoqXEI]LQB6kY$G&B
+M_C%l+OY<q=12P@Ya-7.lITBiX_edT!s>I/:At.i<Bl1XI/VW#ec13[_hSaE`BA\FTDtne*INDX!.k-q
+n`.sK!#mgR27;;E`;'B4lIGnQTO,HK5I:jkiJ0YS,<u3Roj@8kd)3$j[H^e<=I".+7hrk?_=$1Yf3iJZ
+\f95q4+.nt>ZgNbf"^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?<J"5gAWbKM%*7GN(;mP=
+nXpMr9!Be]WW]AWb9Ep$)3Dh&/V\$Yj$=tnQ!dUD<=)\8QUOg(1*DK*>7B+=`e-mf.`&+fWtMKP/rQO.
+AO%#4[MZ/YN!GVU<J"3W<[Cj*>S]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<Cd?CDi5QPq@`+)<4"/=I!8oJlN+X%@9L]f.nJ,.pEG61<+r=YU[Jh"KN0:)D6rD
+Y%9j6=1/sKAW-kZ?r_eY$!&?S2-^ng?-@aLY&#gtb8:a>_5[X='!+^0C:GgX\s<A">hNSrQ8'F[JlN+X
+-!6F?eSnY:FY*^$\?a(m/RpoA"KN0:9!Be]Wo:6Sl<4C&EFk'e>/ke`$!&?SQ!dUD<PrC1e?o_+ilW%S
+[>aXK'!+^0.`&+fX+neAWG<B5`K`$0CDl.u-!6F?<J"3W<XkBa;V*ZHM^r!?f.t?u9!Be]WW]@8X;W^L
+Uon8o(0@p]Y%<OsQ!dUD<=)\P=>a>!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[;-Z<bnA.+N:EPRX@f]H
+_5[X=L3r`lHhQedGjo,4`JYNcVFc10GnYCLHp%AU>4ZtBf.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\+<X+[%kIRf6Z(@q4cBmdBKJ%j%hK]0@;ZYJ9d7gF<C3##8s)dK_4$5D=-*rRP2t[J\A=m])mS
+;*:c[WtML+YU[JhiDq4@XOgXX2W%uWCO(;TI!U+Mq;Rr,GLHNc`/,.CY@Tr-*:'sR=pFP=b8:a>_5[X=
+'!+^0C:NFR*?,r5rRC8P_SXgPeB<=f`j9jWhE<\P[-B<VQ3#hnO^-'H%k>Dq"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="c0X<DhDKcOT50kQbnA.+N:EPRX@f]H_5[X=L0Ia-dF$A(
+:VIki*,H,C`n*4H9M@9L9Wp[$j@FWEHfH^PmFujRhS$=G?e7'KRjF55)<4"/=I!8oJlN+X%Cb2iom6T!
+c'rKsUA3V\[G$TZ2]b24r8QV`[1)P:M<%j[GYe]I-!6F?<J"5gAWbKM%*7GN(;mP=nXpMr9!Be]WW]AW
+b9Ep$)3Dh&/V\$Yj$=tnQ!dUD<=)\8QUOg(1*DK*>7B+=`e-mf.`&+fWtMKP/rQO.AO%#4[MZ/YN!GVU
+<J"3W<[Cj*>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<Cd?CDi5QPq@`+)<4"/=I!8oJlN+X%@9L]f.nJ,.pEG61<+r=YU[Jh"KN0:)D6rDY%9j6=1/sKAW-kZ
+?r_eY$!&?S2-^ng?-@aLY&#gtb8:a>_5[X='!+^0C:GgX\s<A">hNSrQ8'F[JlN+X-!6F?eSnY:FY*^$
+\?a(m/RpoA"KN0:9!Be]Wo:6Sl<4C&EFk'e>/ke`$!&?SQ!dUD<PrC1e?o_+ilW%S[>aXK'!+^0.`&+f
+X+neAWG<B5`K`$0CDl.u-!6F?<J"3W<XkBa;V*ZHM^r!?f.t?u9!Be]WW]@8X;W^LUon8o(0@p]Y%<Os
+Q!dUD<=)\P=>a>!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_\GeS6<R,MTYDgUh[5CUkqO/<O2\(_8+4
+WW]@8X@f^33$RVbr;=j243sEWPQ&D+>SV1C8?QN_#F/[sJlN+X-!3#EoXX)&`Pj3#B<uBa.c)_=>g!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<J"3W<[Cj*
+S85Z7hCOdob8:a>_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?<J"5gAWbKM%*7GN(;mP=nXpMr9!Be]WW]AWb9Ep$)3Dh&/V\$Y
+j$=tnQ!dUD<=)\8QUOg(1*DK*>7B+=`e-mf.`&+fWtMKP/rQO.AO%#4[MZ/YN!GVU<J"3W<[Cj*>S]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<Cd?CDi5QPq@`+
+)<4"/=I!8oJlN+X%@9L]f.nJ,.pEG61<+r=YU[Jh"KN0:)D6rDY%9j6=1/sKAW-kZ?r_eY$!&?S2-^ng
+)^FFEYH!$m7EOQ3(;mP=nXpMr9!Be]WW]AWb9Ep$)3Dh&/V\$Yj$=tnQ!dUD<=)\8QUOg(1*DK*>7B+=
+`e-mf.`&+fWtMKP/rQO.AO%#4[MZ/YN!GVU<J"3W<[Cj*goDa7IJ``ngg&KqE%M1)=X(DB0cj;G<=)\P
+=I!:EE3G@FG':<pX)!*1I#YVgZ"$;Y:JFSZY56_PAUl^4,hFfkK1`?t_5[X='!*!h]h)jKApScLde[6Z
+/O+XX0YP<mj2\oJO^>T`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^:sRCpU<B;hqrp-m@Vj/q>TpCj5hAc
+j\WZm-WlXA<J"3W<Xnd18RJunH[C+KDr3`>3]\uM0fCqU?L7+JIdl:3WkW'*qo=JQQ[=,W^]/XaU!cH[
+<Bl0==d<ApJlN+X%C[NqA%gH.[J:U(?-DK!VPg>]0Aa6\-b]J8'.6h-qg\P<J,ej#6@FMNm^KT(rZ"OW
+$k/[""86CT-!6F?<J"3W<XnfYUgmnEXl'%n@:>l&='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\s<ZZ/-N#.1F^/?"KN0:9!E%i/nqFg]5clNj2[3fpm/'oJi=(RWo6WDYU[Jh"KN0:)D6f@Y%9p0
+=114l%;/Rj<=)\P=I!:EE&f1d/JE07ZK8?+30OuV9!Be]WW]AWb9<j#R?5=O/WWoLR?V)ZJlN+X-!3$0
+%S9sLKH..2H)qq0U]d_2X@f]H_5cI78EsA*jD\\RA(>NF-!6F?<J"3W<XkB_;V*ZILFZRMX+iDZ_5[X=
+'!+^0C:G7H\s<X_>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#<Pt5h?r_eY$!&?S2-^V_
+?-<AO'!*"sU]d_2X@f]H_5cI78=IG;.$%@7<Xi\>JlN+X-!6F?eSmMoc/C[Ia/T9CL,19pWW]@8X@f^3
+\/^[on/TS\<J"5gR?V)ZJlN+X-!3$0%S:_d%csrF_5cHf9!Be]WW]@8X;W^HUcCPmnlV/beSli]=I!8o
+JlN+X%@9H1*1fNCX@f^330OuV9!Be]WW]AWb9:R0c_3I%-!3$p8I8ED=I!8oJlT_MO>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
+aiX1i<BlTIV5iF#o1dc]6<_S'o(%"^IJ<-<hkkn%qRK.tCAd>ri5(&GT"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<Y`D!E-oJY`okO?d
+)`MZ15;^hsY51_T,J@GY\%c]=`5@?@;p=:BO+5P$]s?qd[C&_'9f8q'/h[.m_[l"&C1h1Ws"SU?;T:A=
+.fIJS,%&!-Z0J6qL&L\JIn^A:s8;K\VghlNDnPV[gMaj?aj4DASim;h*c/QX/nfQ'GOODEjJPkB>-0\<
+16VX$gq.d6CH4dTRQdl:Fm;rt2<eCmC7B+8$D5k7<j1+:3]]J(puCLP:7aS9,5Y_W6[\q4pM"j\%M$(9
+6_sINl$dgta*R3fp\OdaRkNX1GAlZiSUSRBd/Ek9;psS'rqBDWOf^4K,>B9&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\+9RDVr0VFEBA<Yr+EtQ<m77@TN95isUrL>WSg9
+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.HZ<l(TMp.-$]IgpX>a/l(()A<h#MFl0n(:+"V?m?uGmj=MGK$j&ure
+`>;mWq(XigkYr$N2`K@FhOZoaYPO)l@=S'i++Ea<?_hnG'2O&Kd752t*U+Z[qsM(rT3>rPn(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<WPGeGg.t-csa^5HW4MPbob0\L33ie7D)f2%phVMSRJ\q\?rqYaA:T3nO8,@`gI/pQkhnHGu
+>/.,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<<pJjN5)^pDuA;W:=Ja
+gg!!*4aO?UQS3R_U'iWeE956-dsI]WNuljNQWGiSpu:Od+NKNbn*]V84aZn(dk8E5E[>"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$uZJE<ig]9())sdT4&?pJIj6P'WM!^A=jY<]p5(c^m;]=rhmG
+BC4`?5Mj%uZ_$f9m^]7cWY]#i;9RRo?*!(%5jbON*SH<S<L;KK9-%Mb<Oo+m]jA'61KFA\r?CXc]C3Jg
+dF"XCp"Igj]-U+9k\bV4h"ehu5E)0o:O_;[-d&"0PiK'm/t4e<?L`p%At&8T[^rrWhgScoli'oKKN5\_
+o#nVjEO-b%RE;,_F"UVA[172qPH/u%T&8:?]<,23X/`!U,Sp7Qq@pi('.<(I)BKs&=["Z!Ug_d,ibDW%
+RVB"&G!l")G,8QD6`q%O2_JEYl>q'km%Q%6mf[d;e[2klFm.0qh<Lu;Sf7Rr`M.]3<iK`6dS[Sa&c[;6
+U5,E%H+aY8\QL6E=JU-tjjaSWGX0T(EAH'Q7jG6bp[5;qWjbSnZs9eph,X*shg=h*`<KnqNOMl!j\9Lf
+o[?UiElEnNjUTcjq42`dd5IVHrd"1]YIj3`9qmNiFIY"NlVLsW_,`d8^%GUBG]EBjX%(9sXJog\Vn;,>
+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@ti7N<q0o<$AAPL:c4JA,W(LD@FKaCCQ_qITK/'Cq]
+RHPf0"uSH0OuI73UfGqe`&0g%H,W8Yb-GWiMUKsZS:Z*6HM"pF+mBR:eQ6)mT-k>BeQ\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.t3ibP<ol@$E7aYT?J
+lgO;GrdnXs<B:>aRA$=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%[<JRaUXQ*
+8')7*S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaUXQ*8')7*
+S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2
+Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c%f@'J4T:5k3T.*.bs6[sP78p#
+O-1M33KK=D-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/OrYr2O-1M3
+3KK=D-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/OrYr2O-1M33KK=D
+-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@=j.V8?g/Qc&k#?dnbCr@X9a
+H.FN!>\<,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!s<t=ZL7/KB(&(PWLWtR:6#'U-5]f+a(,9EcD+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`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%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaUXQ*
+8')7*S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaUXQ*8')7*
+S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaUXQ*8')7*S4C[2Pak7c%[<JRaU\4`ZTS5Eff@uo
+R*@mE*0Ug[3KK=D-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/OrYr2O-1M33KK=D-om?O*@Nq/OrYr2
+O-1M33KK=D-om?O*@Nq/OrYr2O-1M33Nl>[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\Gj<d-%J&05Qkk2Mp"-ctlT.'fk0K['s/oY>G'J8AffRa<If/`VcHa]uHhLDlI-n>nY.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"mT2sZ7aOu7a<ea;,#QWpJ8/U?Za\!j\%D.rtKr>O+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_aNZC2<a@D71C#UbQ\sedN'=QGl=-m
+"a'[G[2LE=n!pmZ39)*L-X>I8(kP>=Bqm%%m.@nSGg):9%+A7XcE0>Hl=lDb=5dpdQHGid)5`ODe7=$6
+HRe!aZ$H`('%fTP?]='d>jXk&$oD7,3)p5:T<TOB>[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*$MGVIH8d<D@!=,-.X\2>K^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$<E]-8753ZNL).>$;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$<b&MnZ[IGMKrEt9[ABrSPRmI%(;>4X<%\6`"eJW&
+9'bg"/r3Z,;Be)%]\mn5=I'H&ei3bgJ8lg'JD/cYkO.VNmEF8,+Y<p/i<g"BNoDheiF/fs`Iack2O:C#
+'\p[-`0mC1t"+0`>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)?<LFqm-JgUb2mZ9I.GDMnoC^km0uT3/i,
+CDB%K1mp2$^D!<<+miiZOsig+#_ZZ!MHL7MUT>RZ[uAn\$g[Ti@1Nd($Q7CNU_BRoUTXSTM.#9=g*=0G
+r<o'+F(45KaW>>?s1/N.K]C9">84*^7fm<u%>-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&<dXrGiUD>n_9K=:i3aiR*n@(-djJ*"h<ik6U"Yf=W"KY<YLXf[S1L(=7aJ5l@(M_D
+0lSN_IJ-H(=i)mmHSolW?Ckg6#$2\J>_@:@!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@$7r<F1;Y5?<^VQZg39]q6SC2X"&KEnc&
+LanKS&7!)Qfs;c?\MDl\j3EPGKDgMaiD'c,n3'n02YeEt0bE!LHE.GIVEQ4o:.Aik.mRlblai3d@PAnP
+c4@ck`MM3t`!j2MQ*UU.9_Z7kt*o"KPL@WoG%>IMXqFECIsf"E>9h@'AaJ4_hFlr[9O\/&N@,LE$R*d>
+'(+L7*]$>2^nKDXA.b^b'7m^Y3dO\><Ar7KY(YSn[tDE5i6ic[@I5OXnW7.*]P>?etQm$<k=.kUnkj)0
+dUe?2G]%!0JKYehB-2\5rh!3,YPacfs^=&W&X5(JWsMJLa[p[6k?g7:?Oda!l5NWoSd<`\MNl@LYk`Xh
+;"m'KF_qtn:CE*[m"@)/<B*tom@pu6Jnj,ZF9O35AlqVd2Ogo?<@KsPR<iLcVMhpgW&L[FPGf<,:DgAG
+/Z>JkA0StdmL7Fs8dcN=.I+B:%"\n(ca>&L;?9he>?i:?!Qp@lI[&$>\3]0:T8m,[6'Pt?1QiLh74&'3
+r*G,b@,G@G#jI!j]ilHR<O+1-dWEg#t3mF5mQ>G?N-baGG^F6CiZ+'u432Cg`Qrr)_3&[E-`=/!Mj2*X
+<D(Bo$D]C3J?qJDSQ"G.YZgQq5dM>OCg];bKJ?[_TIR1C;<!!)*80nbI7Bf[q)1fP2N+Rm7bJib`o@r4
+.%8b0:lX8j>&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><dk$[R>ArPR(g=f_@Blo`S,BI
+pI0k11`zzzzzzzn=k.J!!!"D&QGb^=(K)!7]Hfkl5c!:OK&A].ZgY3WHsnc8CK8Jllf@4;Y?&@'hD>U<
+#%r(JQ0<U6_nF`6hZVA!i^K9U^Eot5cP0eW^*T@V'48DbTV=(<In,_,>n\C<?P9F;H4i>n'Vb])Wm,EU
+oH\[h@Z^gW@F"0!i[Z2Wcr051_"NI*`>u\<UY`CJQ*@T<In*a8Cb=/7%/ijA.PXhOK*LmW^*QkUoH\[$
+Dcrm)Bqnn*3t?!'hHk:,#oac"]A>C<<Z:n/e*r?@1LsXWX>Qe*K_h5WWK.LOK&A]HC'@WdkHRD+]S\Do
+o;QX86(4?'hD>U<4tplUhQ2`p2kG"<I+ui7&o)UW<0%KOK&A].ZgZSBo&"^X=ub!>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\C<?P9F;H4i>n'VarD-8"kO)4NOWcpk25cP0eW^*QkV!;5(,,HkAUu@H\*`DZk8/4ZqJ
+Q*@T<In,_91,M^AVDs'ZIqUFh@Z^gW@F"0!i[Z2Wcr05Pd[ZTL.=.K8C]dG<?P9F;H4i>"]G"G%?]))(
+"2TmAe1ke8#-KfW^*QkUoH\[hPDAng9O$`*nfm'<UY`CJQ*@T<In*a8Cb=/6qUZa8Ac[A4f%?`OXcHm!
+i[Z2WWK/GpO&,&rql/6r:,QG3RF',Gd/ZYWcpk25cP0eW^*QkV!?cW7/H,pUZU_$Rl>9*N7ksqs6K].k
+K]VrK5&L9]N>k=eN4sT+B8SC<?P9F;Y@3J8/E$W>$G45ho<%bjlL%mE;oe2s+K%E;WDK7$G9IT8C]dG<
+?P9F;H4i>"]G"GH))Au)hWcqDnc&0LECs^hj3XR)\>r/,A>7-]N>k=eN4sT+B8SC<?P9F;Y;YfUjrC>k
+<&.F1HIR3J&Jg-/(Mnhc,o-6>?9\$U-Pa8&ZGi2.ZgZ[[r+@;!moN<amY'c]q=FJ3YFWU2`<SnqWk>e6
+>_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&h<O:*p`-S\C26bTA\3%rAgVO`$u=&Bh;+oFOLD7+5B[3.o
+o;QX86(4?I>YCCGf=aS-V[fglRiAH#D4Uh*^+7SC`F?3.WpBNAHcBk$Dcr-OXcHm!ic#a>?9aolBAukI
+-8eGmdBLNXn&GiVja'CVPg?$$GU1no]X[)#7di<Y*QXbg9Lc5('RV/N^smU<In+tU+Inc>kQ3G>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<Cm7[,k!A`M,Vth@Z^gW
+@F"0:UF[ZSuY'&Q8=L:MZTl(8#-KfW^*RJd?^&BY51Ojc88iY$Dcr-OXcHm!ic#a>?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<VpU(@+@H1-Wme&jp>)UZ[%.&qeVj3e\3cXl4Z(OjM!Y;l4Z(Oj?=k;.(N-q#tjVaEe9JO
+PanE-4s_R)\PL<R9(#VuJs-"qS=nq,,pt+O:WWo8mSe.XP5CK6T]A9_-RcN$aU(T-;f[JQqimUiWT;/S
+*65>\K`<jJn;"onRD-cD9(#VuJs-"qS=nq,,pt+O:WWo8mSe-mW8nb"pJ#\=OqWmf@nL>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 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+   xmlns:dc="http://purl.org/dc/elements/1.1/"
+   xmlns:cc="http://creativecommons.org/ns#"
+   xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+   xmlns:svg="http://www.w3.org/2000/svg"
+   xmlns="http://www.w3.org/2000/svg"
+   xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+   xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+   width="210mm"
+   height="297mm"
+   id="svg2"
+   version="1.1"
+   inkscape:version="0.48.5 r10040"
+   sodipodi:docname="ptp.svg"
+   inkscape:export-filename="C:\Users\khu\My Documents\figures\ptp.png"
+   inkscape:export-xdpi="119.83647"
+   inkscape:export-ydpi="119.83647">
+  <defs
+     id="defs4">
+    <marker
+       inkscape:stockid="Arrow1Lstart"
+       orient="auto"
+       refY="0.0"
+       refX="0.0"
+       id="Arrow1Lstart"
+       style="overflow:visible">
+      <path
+         id="path3945"
+         d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+         style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt"
+         transform="scale(0.8) translate(12.5,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1Lend"
+       orient="auto"
+       refY="0.0"
+       refX="0.0"
+       id="Arrow1Lend"
+       style="overflow:visible;">
+      <path
+         id="path3948"
+         d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+         style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;"
+         transform="scale(0.8) rotate(180) translate(12.5,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1Lend"
+       orient="auto"
+       refY="0"
+       refX="0"
+       id="Arrow1Lend-9"
+       style="overflow:visible">
+      <path
+         inkscape:connector-curvature="0"
+         id="path3948-4"
+         d="M 0,0 5,-5 -12.5,0 5,5 0,0 z"
+         style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+         transform="matrix(-0.8,0,0,-0.8,-10,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1Lstart"
+       orient="auto"
+       refY="0"
+       refX="0"
+       id="Arrow1Lstart-8"
+       style="overflow:visible">
+      <path
+         inkscape:connector-curvature="0"
+         id="path3945-8"
+         d="M 0,0 5,-5 -12.5,0 5,5 0,0 z"
+         style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+         transform="matrix(0.8,0,0,0.8,10,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1Lend"
+       orient="auto"
+       refY="0"
+       refX="0"
+       id="Arrow1Lend-3"
+       style="overflow:visible">
+      <path
+         inkscape:connector-curvature="0"
+         id="path3948-1"
+         d="M 0,0 5,-5 -12.5,0 5,5 0,0 z"
+         style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+         transform="matrix(-0.8,0,0,-0.8,-10,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1Lstart"
+       orient="auto"
+       refY="0"
+       refX="0"
+       id="Arrow1Lstart-2"
+       style="overflow:visible">
+      <path
+         inkscape:connector-curvature="0"
+         id="path3945-3"
+         d="M 0,0 5,-5 -12.5,0 5,5 0,0 z"
+         style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+         transform="matrix(0.8,0,0,0.8,10,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1Lend"
+       orient="auto"
+       refY="0"
+       refX="0"
+       id="Arrow1Lend-8"
+       style="overflow:visible">
+      <path
+         inkscape:connector-curvature="0"
+         id="path3948-9"
+         d="M 0,0 5,-5 -12.5,0 5,5 0,0 z"
+         style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+         transform="matrix(-0.8,0,0,-0.8,-10,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1Lstart"
+       orient="auto"
+       refY="0"
+       refX="0"
+       id="Arrow1Lstart-3"
+       style="overflow:visible">
+      <path
+         inkscape:connector-curvature="0"
+         id="path3945-4"
+         d="M 0,0 5,-5 -12.5,0 5,5 0,0 z"
+         style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+         transform="matrix(0.8,0,0,0.8,10,0)" />
+    </marker>
+  </defs>
+  <sodipodi:namedview
+     id="base"
+     pagecolor="#ffffff"
+     bordercolor="#666666"
+     borderopacity="1.0"
+     inkscape:pageopacity="0.0"
+     inkscape:pageshadow="2"
+     inkscape:zoom="1.33"
+     inkscape:cx="523.11689"
+     inkscape:cy="719.13004"
+     inkscape:document-units="px"
+     inkscape:current-layer="layer1"
+     showgrid="true"
+     inkscape:snap-grids="true"
+     inkscape:snap-to-guides="false"
+     showguides="true"
+     inkscape:window-width="1858"
+     inkscape:window-height="1058"
+     inkscape:window-x="54"
+     inkscape:window-y="-8"
+     inkscape:window-maximized="1">
+    <inkscape:grid
+       type="xygrid"
+       id="grid2987" />
+  </sodipodi:namedview>
+  <metadata
+     id="metadata7">
+    <rdf:RDF>
+      <cc:Work
+         rdf:about="">
+        <dc:format>image/svg+xml</dc:format>
+        <dc:type
+           rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+        <dc:title></dc:title>
+      </cc:Work>
+    </rdf:RDF>
+  </metadata>
+  <g
+     inkscape:label="Ebene 1"
+     inkscape:groupmode="layer"
+     id="layer1">
+    <path
+       style="fill:none;stroke:#000000;stroke-width:0.88465172px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-start:url(#Arrow1Lstart)"
+       d="m 210,82.362183 0,179.999997"
+       id="path2989"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:0.93189114px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-start:none;marker-end:url(#Arrow1Lend)"
+       d="m 210,202.36218 330,0"
+       id="path2991"
+       inkscape:connector-curvature="0" />
+    <text
+       xml:space="preserve"
+       style="font-size:40px;font-style:normal;font-weight:normal;line-height:113.99999857%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans"
+       x="540"
+       y="222.36218"
+       id="text4571"
+       sodipodi:linespacing="114%"><tspan
+         sodipodi:role="line"
+         id="tspan4573"
+         x="540"
+         y="222.36218"
+         style="font-size:20px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:113.99999857%;writing-mode:lr-tb;text-anchor:start;font-family:Sans;-inkscape-font-specification:Sans">t</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:20px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans"
+       x="190"
+       y="102.36218"
+       id="text4575"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan4579"
+         x="190"
+         y="102.36218">v</tspan></text>
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:8,8;stroke-dashoffset:0"
+       d="m 320,92.362183 0,169.999997"
+       id="path4587"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:8,8;stroke-dashoffset:0"
+       d="m 420,92.362183 0,169.999997"
+       id="path4589"
+       inkscape:connector-curvature="0" />
+    <text
+       xml:space="preserve"
+       style="font-size:20px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="190"
+       y="222.36218"
+       id="text4591"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan4593"
+         x="190"
+         y="222.36218">0</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ff0000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="326"
+       y="152.36218"
+       id="text4595"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan4597"
+         x="326"
+         y="152.36218"
+         style="font-size:12px;fill:#ff0000;fill-opacity:1">joint 1 (lead axis)</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000aff;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="350"
+       y="182.36218"
+       id="text4595-1"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan4597-7"
+         x="350"
+         y="182.36218"
+         style="font-size:12px;fill:#000aff;fill-opacity:1">joint 2</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="350"
+       y="242.36218"
+       id="text4595-4"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan4597-0"
+         x="350"
+         y="242.36218"
+         style="font-size:12px">joint 3</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="230"
+       y="112.36218"
+       id="text4629"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan4631"
+         x="230"
+         y="112.36218">acceleration</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="346"
+       y="112.36218"
+       id="text4633"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan4635"
+         x="346"
+         y="112.36218">constant</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="440"
+       y="112.36218"
+       id="text4637"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan4639"
+         x="440"
+         y="112.36218">deceleration</tspan></text>
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:8,8;stroke-dashoffset:0"
+       d="m 210,142.36218 110,0"
+       id="path4641"
+       inkscape:connector-curvature="0" />
+    <text
+       xml:space="preserve"
+       style="font-size:40px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="170"
+       y="142.36218"
+       id="text4643"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan4645"
+         x="170"
+         y="142.36218"
+         style="font-size:12px">max v</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:40px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="220"
+       y="172.36218"
+       id="text4647"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan4649"
+         x="220"
+         y="172.36218"
+         style="font-size:12px">max acc</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="460"
+       y="172.36218"
+       id="text4651"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan4653"
+         x="460"
+         y="172.36218">max dec</tspan></text>
+    <path
+       style="fill:none;stroke:#ff0000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 210,202.36218 110,-60 100,0 60,60"
+       id="path4785"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#0c00ff;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 210,202.36218 110,-30 100,0 60,30"
+       id="path4787"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 210,202.36218 110,30 100,0 60,-30"
+       id="path4789"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:0.88465172px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-start:url(#Arrow1Lstart)"
+       d="m 210,276.36218 0,179.99999"
+       id="path2989-3"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-start:none;marker-end:url(#Arrow1Lend)"
+       d="m 210,396.36217 380,0"
+       id="path2991-4"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <text
+       xml:space="preserve"
+       style="font-size:40px;font-style:normal;font-weight:normal;line-height:113.99999857%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans"
+       x="580"
+       y="422.36218"
+       id="text4571-1"
+       sodipodi:linespacing="114%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4573-1"
+         x="580"
+         y="422.36218"
+         style="font-size:20px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:113.99999857%;writing-mode:lr-tb;text-anchor:start;font-family:Sans;-inkscape-font-specification:Sans">t</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:20px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans"
+       x="190"
+       y="296.36218"
+       id="text4575-3"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4579-8"
+         x="190"
+         y="296.36218">v</tspan></text>
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:8, 8;stroke-dashoffset:0"
+       d="m 380,286.36218 0,169.99999"
+       id="path4587-7"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:8, 8;stroke-dashoffset:0"
+       d="m 480,286.36218 0,169.99999"
+       id="path4589-4"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <text
+       xml:space="preserve"
+       style="font-size:20px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="190"
+       y="416.36218"
+       id="text4591-2"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4593-7"
+         x="190"
+         y="416.36218">0</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ff0000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="386"
+       y="346.36218"
+       id="text4595-7"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4597-9"
+         x="386"
+         y="346.36218"
+         style="font-size:12px;fill:#ff0000;fill-opacity:1">joint 1 (lead axis)</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000aff;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="410"
+       y="376.36218"
+       id="text4595-1-3"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4597-7-1"
+         x="410"
+         y="376.36218"
+         style="font-size:12px;fill:#000aff;fill-opacity:1">joint 2</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="410"
+       y="436.36218"
+       id="text4595-4-9"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4597-0-8"
+         x="410"
+         y="436.36218"
+         style="font-size:12px">joint 3</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="290"
+       y="306.36218"
+       id="text4629-6"
+       sodipodi:linespacing="125%"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4631-5"
+         x="290"
+         y="306.36218">acceleration</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="406"
+       y="306.36218"
+       id="text4633-0"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4635-2"
+         x="406"
+         y="306.36218">constant</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="500"
+       y="306.36218"
+       id="text4637-8"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4639-6"
+         x="500"
+         y="306.36218">deceleration</tspan></text>
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1.24316311;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:9.94530497, 9.94530497;stroke-dashoffset:0"
+       d="m 210,336.36218 170,0"
+       id="path4641-0"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <text
+       xml:space="preserve"
+       style="font-size:40px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="170"
+       y="336.36218"
+       id="text4643-2"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4645-4"
+         x="170"
+         y="336.36218"
+         style="font-size:12px">max v</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:40px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="280"
+       y="366.36218"
+       id="text4647-8"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4649-6"
+         x="280"
+         y="366.36218"
+         style="font-size:12px">max acc</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="520"
+       y="366.36218"
+       id="text4651-5"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4653-0"
+         x="520"
+         y="366.36218">max dec</tspan></text>
+    <path
+       style="fill:none;stroke:#ff0000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 270,396.36217 110,-59.99999 100,0 60,59.99999"
+       id="path4785-9"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <path
+       style="fill:none;stroke:#0c00ff;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 270,396.36217 110,-29.99999 100,0 60,29.99999"
+       id="path4787-0"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 270,396.36217 110,30 100,0 60,-30"
+       id="path4789-0"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <path
+       style="fill:none;stroke:#0000ff;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 210,336.36218 60,60"
+       id="path4960"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <path
+       style="fill:none;stroke:#ff0000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 210,416.36218 60,-20"
+       id="path4962"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 210,436.36218 60,-40"
+       id="path4964"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <text
+       xml:space="preserve"
+       style="font-size:40px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="230"
+       y="356.36218"
+       id="text4966"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4968"
+         x="230"
+         y="356.36218"
+         style="font-size:12px;fill:#0000fe;fill-opacity:1">lead axis</tspan></text>
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:8, 8;stroke-dashoffset:0"
+       d="m 270,286.36218 0,169.99999"
+       id="path4587-7-6"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\text4629-6-1.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="222"
+       y="306.36218"
+       id="text4629-6-1"
+       sodipodi:linespacing="125%"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4631-5-3"
+         x="222"
+         y="306.36218">brake</tspan></text>
+    <path
+       style="fill:none;stroke:#000000;stroke-width:0.88465172px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-start:url(#Arrow1Lstart)"
+       d="m 210,470.36218 0,179.99999"
+       id="path2989-4"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:0.82717019px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-start:none;marker-end:url(#Arrow1Lend)"
+       d="m 210,592.36218 260,0"
+       id="path2991-6"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <text
+       xml:space="preserve"
+       style="font-size:40px;font-style:normal;font-weight:normal;line-height:113.99999857%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans"
+       x="470"
+       y="612.36218"
+       id="text4571-0"
+       sodipodi:linespacing="114%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4573-6"
+         x="470"
+         y="612.36218"
+         style="font-size:20px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:113.99999857%;writing-mode:lr-tb;text-anchor:start;font-family:Sans;-inkscape-font-specification:Sans">t</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:20px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans"
+       x="190"
+       y="492.36218"
+       id="text4575-6"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4579-1"
+         x="190"
+         y="492.36218">v</tspan></text>
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:8, 8;stroke-dashoffset:0"
+       d="m 250,470.36219 0,169.99999"
+       id="path4587-8"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:8, 8;stroke-dashoffset:0"
+       d="m 350,470.36219 0,169.99999"
+       id="path4589-49"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <text
+       xml:space="preserve"
+       style="font-size:20px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="188"
+       y="606.36218"
+       id="text4591-6"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4593-3"
+         x="188"
+         y="606.36218">0</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="220"
+       y="502.36218"
+       id="text4629-5"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4631-9"
+         x="220"
+         y="502.36218">acc</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="276"
+       y="502.36218"
+       id="text4633-8"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4635-4"
+         x="276"
+         y="502.36218">constant</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="370"
+       y="502.36218"
+       id="text4637-0"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4639-7"
+         x="370"
+         y="502.36218">deceleration</tspan></text>
+    <path
+       style="fill:none;stroke:#000000;stroke-width:0.60302269;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:4.82418151, 4.82418151;stroke-dashoffset:0"
+       d="m 210,532.36219 40,0"
+       id="path4641-6"
+       inkscape:connector-curvature="0"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <text
+       xml:space="preserve"
+       style="font-size:40px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="168"
+       y="536.36218"
+       id="text4643-3"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4645-6"
+         x="168"
+         y="536.36218"
+         style="font-size:12px">max v</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="212"
+       y="558.36218"
+       id="text4647-1"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4649-5"
+         x="212"
+         y="558.36218"
+         style="font-size:10px">max acc</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="390"
+       y="562.36218"
+       id="text4651-4"
+       sodipodi:linespacing="125%"
+       inkscape:export-filename="C:\Users\khu\My Documents\figures\path5132.png"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180"><tspan
+         sodipodi:role="line"
+         id="tspan4653-2"
+         x="390"
+         y="562.36218">max dec</tspan></text>
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 210,552.36218 40,-20 100,0 60,60"
+       id="path5132"
+       inkscape:connector-curvature="0"
+       inkscape:export-xdpi="180"
+       inkscape:export-ydpi="180" />
+    <text
+       xml:space="preserve"
+       style="font-size:20px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="140"
+       y="202.36218"
+       id="text5134"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan5136"
+         x="140"
+         y="202.36218">(1)</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:20px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="140"
+       y="392.36218"
+       id="text5138"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan5140"
+         x="140"
+         y="392.36218">(2)</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:20px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Sans"
+       x="140"
+       y="582.36218"
+       id="text5142"
+       sodipodi:linespacing="125%"><tspan
+         sodipodi:role="line"
+         id="tspan5144"
+         x="140"
+         y="582.36218">(3)</tspan></text>
+  </g>
+</svg>
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 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>5</zoom_level>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>520</x>
+      <y>155</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;MotionPlanResponse&gt;&gt;
+Item_1 (Group_1)
+bg=cyan
+transparency=0
+group=8</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>520</x>
+      <y>190</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;MotionPlanResponse&gt;&gt;
+Item_2 (Group_1)
+bg=cyan
+transparency=0
+
+group=8</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>520</x>
+      <y>230</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>bg=yellow
+&lt;&lt;MotionPlanResponse&gt;&gt;
+Item_3 (Group_2)
+transparency=0
+group=8</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>520</x>
+      <y>270</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;MotionPlanResponse&gt;&gt;
+Item_4 (Group_2)
+bg=yellow
+transparency=0
+group=8</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>520</x>
+      <y>310</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;MotionPlanResponse&gt;&gt;
+Item_5 (Group_1)
+bg=cyan
+transparency=0
+group=8</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>520</x>
+      <y>365</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;MotionPlanResponse&gt;&gt;
+Item_N (Group_2)
+bg=yellow
+transparency=0
+group=8</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>510</x>
+      <y>125</y>
+      <w>125</w>
+      <h>275</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt; Vector&lt;MotionPlanResponse&gt; &gt;&gt;
+MySequence
+layer=-50
+bg=light_gray
+transparency=0
+
+group=8</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>855</x>
+      <y>155</y>
+      <w>110</w>
+      <h>65</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;RobotTrajectory&gt;&gt;
+Item_1 + Item_2  (Group_1)
+bg=cyan
+transparency=0
+group=2</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>855</x>
+      <y>230</y>
+      <w>110</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>bg=yellow
+transparency=0
+&lt;&lt;RobotTrajectory&gt;&gt;
+Item_3 + Item_4 (Group_2)
+group=2</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>855</x>
+      <y>310</y>
+      <w>110</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;RobotTrajectory&gt;&gt;
+Item_5  (Group_1)
+bg=cyan
+transparency=0
+group=2</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>855</x>
+      <y>365</y>
+      <w>110</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;RobotTrajectory&gt;&gt;
+Item_N  (Group_2)
+bg=yellow
+transparency=0
+group=2</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>850</x>
+      <y>125</y>
+      <w>125</w>
+      <h>275</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt; Vector&lt;RobotTrajectoryPtr&gt; &gt;&gt;
+MySequence
+layer=-50
+bg=light_gray
+transparency=0
+group=2</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>1240</x>
+      <y>205</y>
+      <w>110</w>
+      <h>65</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;RobotTrajectory&gt;&gt;
+Item_1 + Item_2  (Group_1)
+bg=cyan
+transparency=0
+group=3</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>1240</x>
+      <y>280</y>
+      <w>110</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>bg=yellow
+&lt;&lt;RobotTrajectory&gt;&gt;
+Item_3 + Item_4 (Group_2)
+group=3
+transparency=0</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>1240</x>
+      <y>360</y>
+      <w>110</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;RobotTrajectory&gt;&gt;
+Item_5  (Group_1)
+bg=cyan
+transparency=0
+group=3</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>1240</x>
+      <y>415</y>
+      <w>110</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;RobotTrajectory&gt;&gt;
+Item_N  (Group_2)
+bg=yellow
+group=3
+transparency=0</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1230</x>
+      <y>90</y>
+      <w>130</w>
+      <h>365</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;ExecutableMotionPlan&gt;&gt;
+MySequence
+layer=-100
+group=3</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1235</x>
+      <y>180</y>
+      <w>120</w>
+      <h>270</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt; Vector&lt;&lt;ExecutableTrajectory&gt;&gt; &gt;&gt;
+plan_components_
+bg=light_gray
+transparency=0
+layer=-50
+group=3</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1235</x>
+      <y>135</y>
+      <w>120</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>planning_scene_monoitor_
+
+group=3</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1235</x>
+      <y>155</y>
+      <w>120</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>planning_scene_
+
+group=3</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1235</x>
+      <y>115</y>
+      <w>120</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>error_code_
+
+group=3</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>1595</x>
+      <y>215</y>
+      <w>110</w>
+      <h>65</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;RobotTrajectory&gt;&gt;
+Item_1 + Item_2  (Group_1)
+bg=cyan
+transparency=0
+group=9</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>1595</x>
+      <y>290</y>
+      <w>110</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>bg=yellow
+&lt;&lt;RobotTrajectory&gt;&gt;
+Item_3 + Item_4 (Group_2)
+transparency=0
+group=9</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>1595</x>
+      <y>370</y>
+      <w>110</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;RobotTrajectory&gt;&gt;
+Item_5  (Group_1)
+bg=cyan
+transparency=0
+group=9</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>1595</x>
+      <y>425</y>
+      <w>110</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;RobotTrajectory&gt;&gt;
+Item_N  (Group_2)
+bg=yellow
+transparency=0
+group=9</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1590</x>
+      <y>190</y>
+      <w>120</w>
+      <h>270</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt; Vector&lt;&lt;:RobotTrajectory&gt;&gt; &gt;&gt;
+planning_trajectory
+bg=light_gray
+transparency=0
+layer=-50
+group=9</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1590</x>
+      <y>135</y>
+      <w>120</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>planning_time
+
+group=9</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1590</x>
+      <y>155</y>
+      <w>120</w>
+      <h>25</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt; Vector&lt;RobotState&gt; &gt;&gt;
+trajectory_start
+
+group=9</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1590</x>
+      <y>115</y>
+      <w>120</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>error_code
+
+group=9</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1585</x>
+      <y>80</y>
+      <w>130</w>
+      <h>385</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;MoveGroupSequenceResult&gt;&gt;
+MySequence
+layer=-100
+transparency=0
+group=9</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>1125</x>
+      <y>230</y>
+      <w>85</w>
+      <h>25</h>
+    </coordinates>
+    <panel_attributes>halign=left
+valign=top
+*Build*
+ExecutionMotionPlan</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLSpecialState</id>
+    <coordinates>
+      <x>0</x>
+      <y>235</y>
+      <w>20</w>
+      <h>20</h>
+    </coordinates>
+    <panel_attributes>type=initial</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>15</x>
+      <y>240</y>
+      <w>70</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>120.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>245</x>
+      <y>225</y>
+      <w>130</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>halign=left
+valign=top
+*Validate:*
+- blend radii all positive
+- last blend radius zero
+- only first request has start state
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>195</x>
+      <y>240</y>
+      <w>60</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>100.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>370</x>
+      <y>240</y>
+      <w>30</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>40.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>390</x>
+      <y>230</y>
+      <w>105</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>halign=left
+valign=top
+*Calculate*
+individual sequence items</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>490</x>
+      <y>240</y>
+      <w>30</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>40.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>770</x>
+      <y>230</y>
+      <w>60</w>
+      <h>25</h>
+    </coordinates>
+    <panel_attributes>halign=left
+valign=top
+*Blend*
+*Trajectories*</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>630</x>
+      <y>240</y>
+      <w>25</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>30.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>970</x>
+      <y>240</y>
+      <w>55</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>90.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1205</x>
+      <y>240</y>
+      <w>35</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>50.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>1385</x>
+      <y>235</y>
+      <w>60</w>
+      <h>25</h>
+    </coordinates>
+    <panel_attributes>halign=left
+valign=top
+*Execute*
+*Trajectories*</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1355</x>
+      <y>240</y>
+      <w>40</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>60.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1440</x>
+      <y>245</y>
+      <w>50</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>80.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>1480</x>
+      <y>235</y>
+      <w>95</w>
+      <h>25</h>
+    </coordinates>
+    <panel_attributes>halign=left
+valign=top
+*Generate:*
+MoveGroupSequenceResult</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1570</x>
+      <y>245</y>
+      <w>25</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>30.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>645</x>
+      <y>230</y>
+      <w>110</w>
+      <h>25</h>
+    </coordinates>
+    <panel_attributes>halign=left
+valign=top
+*Validate*
+Blend radii do not overlap</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>825</x>
+      <y>240</y>
+      <w>35</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>50.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>750</x>
+      <y>240</y>
+      <w>30</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>40.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLSpecialState</id>
+    <coordinates>
+      <x>1015</x>
+      <y>225</y>
+      <w>50</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>type=decision</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Text</id>
+    <coordinates>
+      <x>1060</x>
+      <y>230</y>
+      <w>35</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>Action</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Text</id>
+    <coordinates>
+      <x>1045</x>
+      <y>265</y>
+      <w>40</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>Service</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1060</x>
+      <y>240</y>
+      <w>75</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>130.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLState</id>
+    <coordinates>
+      <x>1100</x>
+      <y>565</y>
+      <w>120</w>
+      <h>25</h>
+    </coordinates>
+    <panel_attributes>halign=left
+valign=top
+*Build*
+GetMotionSequence::Response</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1035</x>
+      <y>260</y>
+      <w>75</w>
+      <h>325</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>130.0;630.0;10.0;630.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1215</x>
+      <y>570</y>
+      <w>380</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>740.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLSpecialState</id>
+    <coordinates>
+      <x>1785</x>
+      <y>310</y>
+      <w>20</w>
+      <h>20</h>
+    </coordinates>
+    <panel_attributes>type=final</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1710</x>
+      <y>245</y>
+      <w>95</w>
+      <h>75</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>170.0;130.0;170.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1710</x>
+      <y>325</y>
+      <w>95</w>
+      <h>260</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>170.0;10.0;170.0;500.0;10.0;500.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>65</x>
+      <y>80</y>
+      <w>145</w>
+      <h>310</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt; MotionSequenceRequest &gt;&gt;
+MySequence
+layer=-500
+
+group=7</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>90</x>
+      <y>140</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;MotionSequenceItem&gt;&gt;
+Item_1 (Group_1)
+bg=cyan
+transparency=0
+group=7</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>90</x>
+      <y>175</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;MotionSequenceItem&gt;&gt;
+Item_2 (Group_1)
+bg=cyan
+transparency=0
+group=7</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>90</x>
+      <y>215</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>bg=yellow
+&lt;&lt;MotionSequenceItem&gt;&gt;
+Item_3 (Group_2)
+bg=yellow
+transparency=0
+group=7</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>90</x>
+      <y>255</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;MotionSequenceItem&gt;&gt;
+Item_4 (Group_2)
+bg=yellow
+transparency=0
+group=7</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>90</x>
+      <y>295</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;MotionSequenceItem&gt;&gt;
+Item_5 (Group_1)
+bg=cyan
+transparency=0
+group=7</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>90</x>
+      <y>350</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;MotionSequenceItem&gt;&gt;
+Item_N (Group_2)
+bg=yellow
+transparency=0
+group=7</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>75</x>
+      <y>110</y>
+      <w>125</w>
+      <h>275</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt; Vector&lt;MotionSequenceItem&gt; &gt;&gt;
+MySequence
+layer=-100
+bg=light_gray
+transparency=0
+
+group=7</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>240</x>
+      <y>65</y>
+      <w>765</w>
+      <h>460</h>
+    </coordinates>
+    <panel_attributes>symbol=component
+halign=left
+valign=top
+layer=-1000
+CommandListManager</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>760</x>
+      <y>100</y>
+      <w>230</w>
+      <h>335</h>
+    </coordinates>
+    <panel_attributes>symbol=component
+symbol=component
+halign=left
+valign=top
+layer=-100
+PlanComponentsBuilder</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>45</x>
+      <y>45</y>
+      <w>1710</w>
+      <h>830</h>
+    </coordinates>
+    <panel_attributes>symbol=component
+halign=left
+valign=top
+layer=-1000000
+MoveGroupSequenceAction/Service</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>1365</x>
+      <y>180</y>
+      <w>100</w>
+      <h>125</h>
+    </coordinates>
+    <panel_attributes>symbol=component
+layer=-100
+MoveGroupContext</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>1375</x>
+      <y>210</y>
+      <w>80</w>
+      <h>75</h>
+    </coordinates>
+    <panel_attributes>symbol=component
+layer=-50
+PlanExecution</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>1595</x>
+      <y>615</y>
+      <w>110</w>
+      <h>65</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;RobotTrajectory&gt;&gt;
+Item_1 + Item_2  (Group_1)
+bg=cyan
+transparency=0
+group=10</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>1595</x>
+      <y>690</y>
+      <w>110</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>bg=yellow
+&lt;&lt;RobotTrajectory&gt;&gt;
+Item_3 + Item_4 (Group_2)
+transparency=0
+group=10</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>1595</x>
+      <y>770</y>
+      <w>110</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;RobotTrajectory&gt;&gt;
+Item_5  (Group_1)
+bg=cyan
+transparency=0
+group=10</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLDeployment</id>
+    <coordinates>
+      <x>1595</x>
+      <y>825</y>
+      <w>110</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;RobotTrajectory&gt;&gt;
+Item_N  (Group_2)
+bg=yellow
+transparency=0
+group=10</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1590</x>
+      <y>590</y>
+      <w>120</w>
+      <h>270</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt; Vector&lt;&lt;:RobotTrajectory&gt;&gt; &gt;&gt;
+planning_trajectory
+bg=light_gray
+transparency=0
+layer=-50
+group=10</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1590</x>
+      <y>535</y>
+      <w>120</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>planning_time
+
+group=10</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1590</x>
+      <y>555</y>
+      <w>120</w>
+      <h>25</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt; Vector&lt;RobotState&gt; &gt;&gt;
+trajectory_start
+
+group=10</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1590</x>
+      <y>515</y>
+      <w>120</w>
+      <h>15</h>
+    </coordinates>
+    <panel_attributes>error_code
+
+group=10</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1585</x>
+      <y>480</y>
+      <w>130</w>
+      <h>385</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;GetMotionSequenceResponse&gt;&gt;
+MySequence
+layer=-100
+transparency=0
+group=10</panel_attributes>
+    <additional_attributes/>
+  </element>
+</diagram>
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 <string>
+
+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 <string>
+#include <vector>
+
+#include "cartesian_trajectory_point.h"
+
+namespace pilz_industrial_motion_planner
+{
+struct CartesianTrajectory
+{
+  std::string group_name;
+  std::string link_name;
+  std::vector<CartesianTrajectoryPoint> 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 <ros/duration.h>
+
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Twist.h>
+
+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 <string>
+
+#include <boost/optional.hpp>
+
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/planning_pipeline/planning_pipeline.h>
+#include <moveit_msgs/MotionPlanResponse.h>
+
+#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<robot_trajectory::RobotTrajectoryPtr>;
+
+// 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<planning_interface::MotionPlanResponse>;
+  using RobotState_OptRef = boost::optional<const robot_state::RobotState&>;
+  using RadiiCont = std::vector<double>;
+  using GroupNamesCont = std::vector<std::string>;
+
+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 <ros/ros.h>
+
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/planning_interface/planning_response.h>
+#include <moveit/robot_model/joint_model.h>
+
+#include <map>
+
+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_<position|velocity|acceleration|deceleration>_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<const moveit::core::JointModel*>& 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 <map>
+#include <vector>
+#include <string>
+
+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<std::string>& 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<std::string, JointLimit>::const_iterator begin() const;
+
+  /**
+   * @brief ConstIterator to the underlying data structure
+   * @return
+   */
+  std::map<std::string, JointLimit>::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<std::string>& joint_names,
+                            const std::vector<double>& 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<std::string, JointLimit> 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 <joint_limits_interface/joint_limits.h>
+#include <map>
+#include <string>
+
+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<std::string, JointLimit> 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 <joint_limits_interface/joint_limits_rosparam.h>
+
+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 <math.h>
+
+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 <memory>
+
+#include <actionlib/server/simple_action_server.h>
+#include <moveit/move_group/move_group_capability.h>
+
+#include <moveit_msgs/MoveGroupSequenceAction.h>
+
+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<plan_execution::ExecutableTrajectory>;
+
+  using StartStateMsg = moveit_msgs::MotionSequenceResponse::_sequence_start_type;
+  using StartStatesMsg = std::vector<moveit_msgs::MotionSequenceResponse::_sequence_start_type>;
+  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<actionlib::SimpleActionServer<moveit_msgs::MoveGroupSequenceAction>> move_action_server_;
+  moveit_msgs::MoveGroupSequenceFeedback move_feedback_;
+
+  move_group::MoveGroupState move_state_{ move_group::IDLE };
+  std::unique_ptr<pilz_industrial_motion_planner::CommandListManager> 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 <moveit/move_group/move_group_capability.h>
+
+#include <moveit_msgs/GetMotionSequence.h>
+
+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<CommandListManager> 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 <kdl/path.hpp>
+#include <kdl/path_circle.hpp>
+#include <kdl/rotational_interpolation_sa.hpp>
+#include <kdl/utilities/error.h>
+
+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<KDL::Path> 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<KDL::Path> 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..443c8099fb
--- /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 <ros/ros.h>
+
+#include "pilz_industrial_motion_planner/joint_limits_extension.h"
+#include "pilz_industrial_motion_planner/planning_context_loader.h"
+
+#include <moveit/macros/class_forward.h>
+#include <moveit/planning_interface/planning_interface.h>
+
+#include <pluginlib/class_loader.h>
+
+// Boost includes
+#include <boost/scoped_ptr.hpp>
+
+namespace pilz_industrial_motion_planner
+{
+/**
+ * @brief MoveIt Plugin for Planning with Standard 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<std::string>& 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<pluginlib::ClassLoader<PlanningContextLoader>> planner_context_loader;
+
+  /// Mapping from command to loader
+  std::map<std::string, pilz_industrial_motion_planner::PlanningContextLoaderPtr> 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 <memory>
+#include <string>
+
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_trajectory/robot_trajectory.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"
+#include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h"
+
+namespace pilz_industrial_motion_planner
+{
+using TipFrameFunc_t = std::function<const std::string&(const std::string&)>;
+
+// 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<pilz_industrial_motion_planner::TrajectoryBlender> 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<robot_trajectory::RobotTrajectoryPtr> 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<pilz_industrial_motion_planner::TrajectoryBlender> 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<robot_trajectory::RobotTrajectoryPtr> 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<pilz_industrial_motion_planner::TrajectoryBlender> 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 <ros/ros.h>
+
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/planning_interface/planning_response.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_state/conversions.h>
+
+#include <atomic>
+#include <thread>
+
+namespace pilz_industrial_motion_planner
+{
+/**
+ * @brief PlanningContext for obtaining trajectories
+ */
+template <typename GeneratorT>
+class PlanningContextBase : public planning_interface::PlanningContext
+{
+public:
+  PlanningContextBase<GeneratorT>(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 <typename GeneratorT>
+bool pilz_industrial_motion_planner::PlanningContextBase<GeneratorT>::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 <typename GeneratorT>
+bool pilz_industrial_motion_planner::PlanningContextBase<GeneratorT>::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 <typename GeneratorT>
+bool pilz_industrial_motion_planner::PlanningContextBase<GeneratorT>::terminate()
+{
+  ROS_DEBUG_STREAM("Terminate called");
+  terminated_ = true;
+  return true;
+}
+
+template <typename GeneratorT>
+void pilz_industrial_motion_planner::PlanningContextBase<GeneratorT>::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 <ros/ros.h>
+
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/planning_interface/planning_response.h>
+
+#include <atomic>
+#include <thread>
+
+#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<TrajectoryGeneratorCIRC>
+{
+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<TrajectoryGeneratorCIRC>(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 <ros/ros.h>
+
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/planning_interface/planning_response.h>
+
+#include <atomic>
+#include <thread>
+
+#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<TrajectoryGeneratorLIN>
+{
+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<TrajectoryGeneratorLIN>(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 <memory>
+#include <vector>
+
+#include <moveit/planning_interface/planning_interface.h>
+
+#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 <typename T>
+  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<PlanningContextLoader> PlanningContextLoaderPtr;
+typedef boost::shared_ptr<const PlanningContextLoader> PlanningContextLoaderConstPtr;
+
+template <typename T>
+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 <moveit/planning_interface/planning_interface.h>
+
+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<PlanningContextLoaderCIRC> PlanningContextLoaderCIRCPtr;
+typedef boost::shared_ptr<const PlanningContextLoaderCIRC> 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 <moveit/planning_interface/planning_interface.h>
+
+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<PlanningContextLoaderLIN> PlanningContextLoaderLINPtr;
+typedef boost::shared_ptr<const PlanningContextLoaderLIN> 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 <moveit/planning_interface/planning_interface.h>
+
+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<PlanningContextLoaderPTP> PlanningContextLoaderPTPPtr;
+typedef boost::shared_ptr<const PlanningContextLoaderPTP> 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 <ros/ros.h>
+
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/planning_interface/planning_response.h>
+
+#include <atomic>
+#include <thread>
+
+#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<TrajectoryGeneratorPTP>
+{
+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<TrajectoryGeneratorPTP>(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 <stdexcept>
+
+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 <cassert>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#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 <class JointModelGroup>
+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 <class JointModelGroup>
+static const std::string& getSolverTipFrame(const JointModelGroup* group)
+{
+  if (!hasSolver(group))
+  {
+    throw NoSolverException("No solver for group " + group->getName());
+  }
+
+  const std::vector<std::string>& 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 <string>
+
+#include <moveit/robot_trajectory/robot_trajectory.h>
+
+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 <string>
+
+#include <moveit/robot_trajectory/robot_trajectory.h>
+
+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 <moveit/robot_model/robot_model.h>
+
+#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<TrajectoryBlender> 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:<br>
+   *    align the end of the first trajectory with second_interse_index
+   *    <pre>
+   *    first traj:  |-------------|--------!--------------|
+   *    second traj:                        |--------------|-------------------|
+   *    blend phase:               |-----------------------|
+   *    </pre>
+   *  - else:<br>
+   *    align the first_interse_index with the beginning of the second
+   * trajectory
+   *    <pre>
+   *    first traj:  |-------------|-----------------------|
+   *    second traj: |-----------------------!----------|-------------------|
+   *    blend phase:               |----------------------------------|
+   *    </pre>
+   *
+   * @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 <Eigen/Geometry>
+#include <eigen_conversions/eigen_kdl.h>
+#include <eigen_conversions/eigen_msg.h>
+#include <kdl/trajectory.hpp>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <tf2/transform_datatypes.h>
+#include <trajectory_msgs/MultiDOFJointTrajectory.h>
+
+#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<std::string, double>& seed, std::map<std::string, double>& 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<std::string, double>& seed, std::map<std::string, double>& 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<std::string, double>& joint_state, Eigen::Isometry3d& pose);
+
+bool computeLinkFK(const robot_model::RobotModelConstPtr& robot_model, const std::string& link_name,
+                   const std::vector<std::string>& joint_names, const std::vector<double>& 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<std::string, double>& position_last,
+                             const std::map<std::string, double>& velocity_last,
+                             const std::map<std::string, double>& 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<std::string, double>& 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<std::string, double>& initial_joint_position,
+                             const std::map<std::string, double>& 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 <stdexcept>
+#include <string>
+
+#include <moveit_msgs/MoveItErrorCodes.h>
+
+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 <moveit_msgs::MoveItErrorCodes::_val_type ERROR_CODE = moveit_msgs::MoveItErrorCodes::FAILURE>
+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 <moveit_msgs::MoveItErrorCodes::_val_type ERROR_CODE>
+inline TemplatedMoveItErrorCodeException<ERROR_CODE>::TemplatedMoveItErrorCodeException(const std::string& msg)
+  : MoveItErrorCodeException(msg)
+{
+}
+
+template <moveit_msgs::MoveItErrorCodes::_val_type ERROR_CODE>
+inline TemplatedMoveItErrorCodeException<ERROR_CODE>::TemplatedMoveItErrorCodeException(
+    const std::string& msg, const moveit_msgs::MoveItErrorCodes::_val_type& error_code)
+  : MoveItErrorCodeException(msg), error_code_(error_code)
+{
+}
+
+template <moveit_msgs::MoveItErrorCodes::_val_type ERROR_CODE>
+inline const moveit_msgs::MoveItErrorCodes::_val_type&
+TemplatedMoveItErrorCodeException<ERROR_CODE>::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<ERROR_CODE>                                    \
+  {                                                                                                                    \
+  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 <sstream>
+#include <string>
+
+#include <Eigen/Geometry>
+#include <kdl/frames.hpp>
+#include <kdl/trajectory.hpp>
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/robot_model/robot_model.h>
+
+#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<std::string, double> start_joint_position;
+    std::map<std::string, double> goal_joint_position;
+    std::pair<std::string, Eigen::Vector3d> 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<KDL::VelocityProfile> cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor,
+                                                                     const double& max_acceleration_scaling_factor,
+                                                                     const std::unique_ptr<KDL::Path>& 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<std::string>& expected_joint_names, const std::string& group_name) const;
+
+  void checkJointGoalConstraint(const moveit_msgs::Constraints& constraint,
+                                const std::vector<std::string>& 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 <eigen3/Eigen/Eigen>
+#include <kdl/path.hpp>
+#include <kdl/velocityprofile.hpp>
+
+#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<KDL::Path> 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 <eigen3/Eigen/Eigen>
+#include <kdl/rotational_interpolation_sa.hpp>
+
+#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<KDL::Path> 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<std::string, double>& start_pos, const std::map<std::string, double>& 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<std::string, JointLimit> 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 <iostream>
+
+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..1c44de0024
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/package.xml
@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>pilz_industrial_motion_planner</name>
+  <version>1.0.11</version>
+  <description>MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof.</description>
+
+  <maintainer email="a.gutenkunst@pilz.de">Alexander Gutenkunst</maintainer>
+  <maintainer email="c.henkel@pilz.de">Christian Henkel</maintainer>
+  <maintainer email="i.martini@pilz.de">Immanuel Martini</maintainer>
+  <maintainer email="j.schleicher@pilz.de">Joachim Schleicher</maintainer>
+  <maintainer email="h.slusarek@pilz.de">Hagen Slusarek</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://moveit.ros.org</url>
+  <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
+  <url type="repository">https://github.com/ros-planning/moveit</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>eigen_conversions</depend>
+  <depend>joint_limits_interface</depend>
+  <depend>kdl_conversions</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>moveit_msgs</depend>
+  <depend>moveit_core</depend>
+  <depend>moveit_ros_planning</depend> <!-- RobotModelLoader -->
+  <depend>moveit_ros_move_group</depend> <!-- move_group capability -->
+  <depend condition="$ROS_DISTRO != noetic">orocos_kdl</depend>
+  <depend condition="$ROS_DISTRO == noetic">liborocos-kdl-dev</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_kdl</depend>
+  <depend>tf2_ros</depend>
+
+  <!-- Test dependencies -->
+  <test_depend>rostest</test_depend>
+  <test_depend>rosunit</test_depend>
+  <test_depend>cmake_modules</test_depend>
+  <test_depend>moveit_resources_panda_moveit_config</test_depend>
+  <test_depend>pilz_industrial_motion_planner_testutils</test_depend>
+  <test_depend>moveit_resources_prbt_moveit_config</test_depend>
+  <test_depend>moveit_resources_prbt_support</test_depend>
+  <test_depend>moveit_resources_prbt_pg70_support</test_depend>
+  <test_depend>code_coverage</test_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <moveit_core plugin="${prefix}/plugins/pilz_industrial_motion_planner_plugin_description.xml"/>
+    <moveit_ros_move_group plugin="${prefix}/plugins/sequence_capability_plugin_description.xml"/>
+    <pilz_industrial_motion_planner plugin="${prefix}/plugins/planning_context_plugin_description.xml"/>
+  </export>
+</package>
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 @@
+<library path="lib/libpilz_industrial_motion_planner">
+  <class type="pilz_industrial_motion_planner::CommandPlanner" base_class_type="planning_interface::PlannerManager">
+    <description>Pilz Industrial Motion Planner</description>
+  </class>
+</library>
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 @@
+<library path="lib/libplanning_context_loader_ptp">
+  <class type="pilz_industrial_motion_planner::PlanningContextLoaderPTP"
+         base_class_type="pilz_industrial_motion_planner::PlanningContextLoader">
+    <description>Loader for PTP Context</description>
+  </class>
+</library>
+<library path="lib/libplanning_context_loader_lin">
+  <class type="pilz_industrial_motion_planner::PlanningContextLoaderLIN"
+         base_class_type="pilz_industrial_motion_planner::PlanningContextLoader">
+    <description>Loader for LIN Context</description>
+  </class>
+</library>
+<library path="lib/libplanning_context_loader_circ">
+  <class type="pilz_industrial_motion_planner::PlanningContextLoaderCIRC"
+         base_class_type="pilz_industrial_motion_planner::PlanningContextLoader">
+    <description>Loader for CIRC Context</description>
+  </class>
+</library>
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 @@
+<library path="lib/libsequence_capability">
+  <class name="pilz_industrial_motion_planner/MoveGroupSequenceAction"
+         type="pilz_industrial_motion_planner::MoveGroupSequenceAction"
+         base_class_type="move_group::MoveGroupCapability">
+    <description>Motion Planner a sequence of trajectories</description>
+  </class>
+
+  <class name="pilz_industrial_motion_planner/MoveGroupSequenceService"
+         type="pilz_industrial_motion_planner::MoveGroupSequenceService"
+         base_class_type="move_group::MoveGroupCapability">
+    <description>Plan a sequence of trajectory via ROS serivce</description>
+  </class>
+</library>
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 <cassert>
+#include <functional>
+#include <sstream>
+
+#include <moveit/planning_pipeline/planning_pipeline.h>
+#include <moveit/robot_state/conversions.h>
+#include <ros/ros.h>
+
+#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<pilz_industrial_motion_planner::TrajectoryBlender>(
+      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 <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+
+#include <vector>
+
+pilz_industrial_motion_planner::JointLimitsContainer
+pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(
+    const ros::NodeHandle& nh, const std::vector<const moveit::core::JointModel*>& 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 <stdexcept>
+
+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<std::string, JointLimit>(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<std::string>& 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<std::string, JointLimit>::const_iterator JointLimitsContainer::begin() const
+{
+  return container_.begin();
+}
+
+std::map<std::string, JointLimit>::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<std::string>& joint_names,
+                                                const std::vector<double>& 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 <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+
+#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..d0ff22bf3f
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp
@@ -0,0 +1,293 @@
+/*********************************************************************
+ * 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 <time.h>
+
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/plan_execution/plan_execution.h>
+#include <moveit/plan_execution/plan_with_sensing.h>
+#include <moveit/planning_pipeline/planning_pipeline.h>
+#include <moveit/robot_state/conversions.h>
+#include <moveit/trajectory_processing/trajectory_tools.h>
+
+#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<moveit_msgs::MoveGroupSequenceAction>(
+      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 =
+      planning_scene::PlanningScene::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 =
+      (planning_scene::PlanningScene::isEmpty(goal->planning_options.planning_scene_diff)) ?
+          static_cast<const planning_scene::PlanningSceneConstPtr&>(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/class_list_macros.h>
+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/class_list_macros.h>
+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<KDL::Path> 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<KDL::Path>(
+        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<KDL::Path> 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<KDL::Path>(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 <boost/scoped_ptr.hpp>
+
+#include <pluginlib/class_list_macros.h>
+
+#include <pluginlib/class_loader.h>
+
+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<PlanningContextLoader>(
+      "pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader"));
+
+  // List available plugins
+  const std::vector<std::string>& 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<std::string>& 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<request>\n" << req << "\n</request>");
+
+  // 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 <cassert>
+
+#include "pilz_industrial_motion_planner/tip_frame_getter.h"
+
+namespace pilz_industrial_motion_planner
+{
+std::vector<robot_trajectory::RobotTrajectoryPtr> PlanComponentsBuilder::build() const
+{
+  std::vector<robot_trajectory::RobotTrajectoryPtr> 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 <ros/ros.h>
+
+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 <pluginlib/class_list_macros.h>
+
+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 <pluginlib/class_list_macros.h>
+
+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 <pluginlib/class_list_macros.h>
+
+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 <algorithm>
+#include <math.h>
+#include <tf2/convert.h>
+#include <tf2_eigen/tf2_eigen.h>
+
+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<std::string, double> 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<robot_trajectory::RobotTrajectory>(
+      new robot_trajectory::RobotTrajectory(req.first_trajectory->getRobotModel(), req.first_trajectory->getGroup()));
+  res.blend_trajectory = std::shared_ptr<robot_trajectory::RobotTrajectory>(
+      new robot_trajectory::RobotTrajectory(req.first_trajectory->getRobotModel(), req.first_trajectory->getGroup()));
+  res.second_trajectory = std::shared_ptr<robot_trajectory::RobotTrajectory>(
+      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 <moveit/planning_scene/planning_scene.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/convert.h>
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+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<std::string, double>& seed,
+                                                   std::map<std::string, double>& 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<std::string, double>& seed,
+                                                   std::map<std::string, double>& solution, bool check_self_collision,
+                                                   const double timeout)
+{
+  Eigen::Isometry3d pose_eigen;
+  tf2::convert<geometry_msgs::Pose, Eigen::Isometry3d>(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<std::string, double>& 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<std::string, double>& position_last, const std::map<std::string, double>& velocity_last,
+    const std::map<std::string, double>& 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<std::string, double>& 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<double> 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<std::string, double> 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<double>::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<std::string, double>& initial_joint_position,
+    const std::map<std::string, double>& 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<std::string, double> ik_solution_last = initial_joint_position;
+  std::map<std::string, double> 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<std::string, double> 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<geometry_msgs::Quaternion, tf2::Quaternion>(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 <cassert>
+
+#include <eigen_conversions/eigen_kdl.h>
+#include <eigen_conversions/eigen_msg.h>
+#include <kdl/velocityprofile_trap.hpp>
+#include <moveit/robot_state/conversions.h>
+
+#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<std::string>& 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<std::string>& 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<KDL::VelocityProfile>
+TrajectoryGenerator::cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor,
+                                                  const double& max_acceleration_scaling_factor,
+                                                  const std::unique_ptr<KDL::Path>& path) const
+{
+  std::unique_ptr<KDL::VelocityProfile> 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<double>::epsilon())  // avoid division by zero
+  {
+    vp_trans->SetProfile(0, path->PathLength());
+  }
+  else
+  {
+    vp_trans->SetProfile(0, std::numeric_limits<double>::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 <cassert>
+#include <sstream>
+
+#include <eigen_conversions/eigen_kdl.h>
+#include <eigen_conversions/eigen_msg.h>
+#include <kdl/rotational_interpolation_sa.hpp>
+#include <kdl/trajectory_segment.hpp>
+#include <kdl/utilities/error.h>
+#include <kdl/utilities/utility.h>
+#include <kdl_conversions/kdl_msg.h>
+#include <moveit/robot_state/conversions.h>
+#include <ros/ros.h>
+#include <tf2/convert.h>
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+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<geometry_msgs::Pose, Eigen::Isometry3d>(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<std::string, double> 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<KDL::Path> cart_path(setPathCIRC(plan_info));
+  std::unique_ptr<KDL::VelocityProfile> 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<KDL::Path> 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 <cassert>
+#include <ros/ros.h>
+#include <sstream>
+#include <time.h>
+
+#include <eigen_conversions/eigen_kdl.h>
+#include <eigen_conversions/eigen_msg.h>
+
+#include <moveit/robot_state/conversions.h>
+
+#include <kdl/path_line.hpp>
+#include <kdl/trajectory_segment.hpp>
+#include <kdl/utilities/error.h>
+
+#include <tf2/convert.h>
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+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<geometry_msgs::Pose, Eigen::Isometry3d>(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<std::string, double> 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<KDL::Path> path(setPathLIN(plan_info.start_pose, plan_info.goal_pose));
+
+  // create velocity profile
+  std::unique_ptr<KDL::VelocityProfile> 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<KDL::Path> 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<KDL::Path>(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..cfb1887952
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp
@@ -0,0 +1,269 @@
+/*********************************************************************
+ * 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 <iostream>
+#include <sstream>
+
+#include <tf2/convert.h>
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+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())
+  {
+    const auto& active_joints = jmg->getActiveJointModelNames();
+
+    // no active joints
+    if (active_joints.empty())
+    {
+      continue;
+    }
+
+    JointLimit most_strict_limit = joint_limits_.getCommonLimit(active_joints);
+
+    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<std::string, JointLimit>(jmg->getName(), most_strict_limit));
+  }
+
+  ROS_INFO("Initialized Point-to-Point Trajectory Generator.");
+}
+
+void TrajectoryGeneratorPTP::planPTP(const std::map<std::string, double>& start_pos,
+                                     const std::map<std::string, double>& 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<std::string, VelocityProfileATrap> 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<double> 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<geometry_msgs::Pose, Eigen::Isometry3d>(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..867c7f6587
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md
@@ -0,0 +1,62 @@
+<!--
+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.
+-->
+
+# 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 demo.launch 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..be2dce5489
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md
@@ -0,0 +1,54 @@
+<!--
+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.
+-->
+
+# 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 demo.launch 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 <memory>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include <functional>
+#include <ros/ros.h>
+#include <ros/time.h>
+
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit_msgs/DisplayTrajectory.h>
+#include <moveit_msgs/MotionPlanResponse.h>
+
+#include <tf2_eigen/tf2_eigen.h>
+
+#include <pilz_industrial_motion_planner_testutils/gripper.h>
+#include <pilz_industrial_motion_planner_testutils/lin.h>
+#include <pilz_industrial_motion_planner_testutils/sequence.h>
+#include <pilz_industrial_motion_planner_testutils/xml_testdata_loader.h>
+
+#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<pilz_industrial_motion_planner::CommandListManager> manager_;
+  planning_scene::PlanningScenePtr scene_;
+  planning_pipeline::PlanningPipelinePtr pipeline_;
+
+  std::unique_ptr<pilz_industrial_motion_planner_testutils::TestdataLoader> 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<pilz_industrial_motion_planner::CommandListManager>(ph_, robot_model_);
+  scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
+  pipeline_ = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, ph_);
+}
+
+/**
+ * @brief Checks that each derived MoveItErrorCodeException contains the correct
+ * error code.
+ *
+ */
+TEST_F(IntegrationTestCommandListManager, TestExceptionErrorCodeMapping)
+{
+  std::shared_ptr<NegativeBlendRadiusException> nbr_ex{ new NegativeBlendRadiusException("") };
+  EXPECT_EQ(nbr_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
+
+  std::shared_ptr<LastBlendRadiusNotZeroException> lbrnz_ex{ new LastBlendRadiusNotZeroException("") };
+  EXPECT_EQ(lbrnz_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
+
+  std::shared_ptr<StartStateSetException> sss_ex{ new StartStateSetException("") };
+  EXPECT_EQ(sss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
+
+  std::shared_ptr<OverlappingBlendRadiiException> obr_ex{ new OverlappingBlendRadiiException("") };
+  EXPECT_EQ(obr_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
+
+  std::shared_ptr<PlanningPipelineException> 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<moveit_msgs::DisplayTrajectory>("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<LinCart>(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<LinCart>(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<PtpJointCart>(0) };
+  const CircInterimCart& circ{ seq.getCmd<CircInterimCart>(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<Gripper>(0));
+  ASSERT_TRUE(seq.cmdIsOfType<Gripper>(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<Gripper>(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<PtpJointCart>(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 @@
+<!--
+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.
+-->
+
+<launch>
+  <!-- Load the context with the pg70 -->
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/planning_context.launch" >
+    <arg name="gripper" value="pg70" />
+  </include>
+
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <param name="/use_gui" value="false"/>
+    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
+  </node>
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+  <arg name="display_plots" default="false" />
+  <node if="$(arg display_plots)"
+        name="plot_trajectory_msg"
+        pkg="pilz_industrial_motion_planner"
+        type="plot_display_trajectory_msg.py"
+        args="/my_planned_path"  />
+
+  <include ns="integrationtest_command_list_manager" file="$(find moveit_resources_prbt_moveit_config)/launch/planning_pipeline.launch.xml">
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
+  </include>
+
+  <!-- run test -->
+  <!--test pkg="pilz_industrial_motion_planner" test-name="integrationtest_command_list_manager" type="integrationtest_command_list_manager"
+  time-limit="885.0" launch-prefix="xterm -e gdb - -args" -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_command_list_manager" type="integrationtest_command_list_manager"
+  time-limit="885.0" >
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/testdata_sequence.xml" />
+  </test>
+  <!--launch-prefix="xterm -e gdb - -args"-->
+
+</launch>
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 <gtest/gtest.h>
+
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <eigen_conversions/eigen_msg.h>
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/planning_interface/planning_request.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_state/conversions.h>
+#include <moveit_msgs/Constraints.h>
+#include <moveit_msgs/GetMotionPlan.h>
+#include <moveit_msgs/JointConstraint.h>
+#include <pilz_industrial_motion_planner_testutils/command_types_typedef.h>
+#include <pilz_industrial_motion_planner_testutils/xml_testdata_loader.h>
+#include <ros/ros.h>
+#include <tf2/convert.h>
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#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<pilz_industrial_motion_planner_testutils::TestdataLoader> 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<moveit_msgs::GetMotionPlan>(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<moveit_msgs::GetMotionPlan>(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<geometry_msgs::Pose, Eigen::Isometry3d>(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<moveit_msgs::GetMotionPlan>(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<moveit_msgs::GetMotionPlan>(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<moveit_msgs::GetMotionPlan>(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<geometry_msgs::Pose, Eigen::Isometry3d>(start_pose, start_pose_iso3d);
+    tf2::convert<geometry_msgs::Pose, Eigen::Isometry3d>(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<moveit_msgs::GetMotionPlan>(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<geometry_msgs::Pose, Eigen::Isometry3d>(start_pose, start_pose_iso3d);
+    tf2::convert<geometry_msgs::Pose, Eigen::Isometry3d>(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..6d26187e4d
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test
@@ -0,0 +1,50 @@
+<!--
+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.
+-->
+
+<launch>
+
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/test_environment.launch">
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_command_planning" type="integrationtest_command_planning">
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/testdata_sequence.xml" />
+    <param name="planning_group" value="manipulator" />
+    <param name="target_link" value="prbt_flange" />
+    <param name="pose_norm_tolerance" value="1.0e-5" />
+    <param name="orientation_norm_tolerance" value="1.0e-3" />
+  </test>
+
+</launch>
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..44f7d54b3d
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test
@@ -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.
+-->
+
+<launch>
+
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch">
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_command_planning_frankaemika_panda" type="integrationtest_command_planning">
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml" />
+    <param name="planning_group" value="panda_arm" />
+    <param name="target_link" value="panda_link8" />
+    <param name="pose_norm_tolerance" value="1.0e-5" />
+    <param name="orientation_norm_tolerance" value="1.0e-5" />
+    <param name="joint_prefix" value="panda_joint" />
+  </test>
+
+</launch>
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..27212a2b92
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test
@@ -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.
+-->
+
+<launch>
+
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/test_environment.launch">
+    <arg name="gripper" value="pg70"/>
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_command_planning_with_gripper" type="integrationtest_command_planning">
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/testdata_with_gripper.xml" />
+    <param name="planning_group" value="manipulator" />
+    <param name="target_link" value="prbt_tcp" />
+    <param name="pose_norm_tolerance" value="1.0e-5" />
+    <param name="orientation_norm_tolerance" value="1.0e-5" />
+  </test>
+
+</launch>
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 <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include <moveit/robot_model/joint_model_group.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <ros/ros.h>
+
+#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 @@
+<!--
+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.
+-->
+
+<launch>
+
+  <!-- load robot models -->
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/planning_context.launch" >
+    <arg name="gripper" value="pg70" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_get_solver_tip_frame"
+        type="integrationtest_get_solver_tip_frame" time-limit="120.0" >
+  </test>
+  <!--launch-prefix="xterm -e gdb - -args"-->
+
+</launch>
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 <memory>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include <ros/ros.h>
+
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_trajectory/robot_trajectory.h>
+
+#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<NoBlenderSetException> nbs_ex{ new NoBlenderSetException("") };
+  EXPECT_EQ(nbs_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE);
+
+  std::shared_ptr<NoTipFrameFunctionSetException> ntffse_ex{ new NoTipFrameFunctionSetException("") };
+  EXPECT_EQ(ntffse_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE);
+
+  std::shared_ptr<NoRobotModelSetException> nrms_ex{ new NoRobotModelSetException("") };
+  EXPECT_EQ(nrms_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE);
+
+  std::shared_ptr<BlendingFailedException> 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 @@
+<!--
+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.
+-->
+
+<launch>
+  <!-- Load the context with the pg70 -->
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/planning_context.launch" >
+    <arg name="gripper" value="pg70" />
+  </include>
+
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <param name="/use_gui" value="false"/>
+    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
+  </node>
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+  <arg name="display_plots" default="false" />
+  <node if="$(arg display_plots)"
+        name="plot_trajectory_msg"
+        pkg="pilz_industrial_motion_planner"
+        type="plot_display_trajectory_msg.py"
+        args="/my_planned_path"  />
+
+  <include ns="integrationtest_plan_components_builder" file="$(find moveit_resources_prbt_moveit_config)/launch/planning_pipeline.launch.xml">
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_plan_components_builder" type="integrationtest_plan_components_builder">
+    <param name="planning_group" value="manipulator"/>
+  </test>
+  <!--launch-prefix="xterm -e gdb - -args"-->
+
+</launch>
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 <chrono>
+#include <condition_variable>
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <thread>
+
+#include <actionlib/client/simple_action_client.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_state/conversions.h>
+#include <moveit_msgs/Constraints.h>
+#include <moveit_msgs/GetMotionPlan.h>
+#include <moveit_msgs/JointConstraint.h>
+#include <ros/ros.h>
+
+#include <pilz_industrial_motion_planner_testutils/async_test.h>
+
+#include <pilz_industrial_motion_planner_testutils/checks.h>
+#include <pilz_industrial_motion_planner_testutils/sequence.h>
+#include <pilz_industrial_motion_planner_testutils/xml_testdata_loader.h>
+
+#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<moveit_msgs::MoveGroupSequenceAction> ac_{ ph_, SEQUENCE_ACTION_NAME, true };
+  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> 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<std::string>(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<moveit::planning_interface::MoveGroupInterface>(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<CircInterimCart>(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..5fb2934c69
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action.test
@@ -0,0 +1,47 @@
+<!--
+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.
+-->
+
+<launch>
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/test_environment.launch">
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
+    <arg name="fake_execution_type" value="last point" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_sequence_action" type="integrationtest_sequence_action" time-limit="300.0">
+    <param name="joint_position_tolerance" value="0.01" />
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/testdata_sequence.xml" />
+  </test>
+
+</launch>
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..75c4afc29f
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_frankaemika_panda.test
@@ -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.
+-->
+
+<launch>
+
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch">
+    <arg name="fake_execution_type" value="last point" />
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_sequence_action_frankaemika_panda"
+        type="integrationtest_sequence_action" time-limit="300.0">
+    <param name="joint_prefix" value="panda_joint" />
+    <param name="joint_position_tolerance" value="0.01" />
+    <param name="group_name" value="panda_arm" />
+    <param name="testdata_file_name"
+           value="$(find pilz_industrial_motion_planner)/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml" />
+  </test>
+
+</launch>
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..2c36fb699a
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_capability_with_gripper.test
@@ -0,0 +1,50 @@
+<!--
+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.
+-->
+
+<launch>
+
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/test_environment.launch">
+    <arg name="gripper" value="pg70" />
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
+    <arg name="fake_execution_type" value="last point" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_sequence_action_with_gripper"
+        type="integrationtest_sequence_action" time-limit="300.0">
+    <param name="joint_position_tolerance" value="0.01" />
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/testdata_sequence.xml" />
+  </test>
+
+</launch>
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 <chrono>
+#include <condition_variable>
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <thread>
+
+#include <actionlib/client/simple_action_client.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_state/conversions.h>
+#include <moveit_msgs/Constraints.h>
+#include <moveit_msgs/GetMotionPlan.h>
+#include <moveit_msgs/JointConstraint.h>
+#include <ros/ros.h>
+#include <sensor_msgs/JointState.h>
+
+#include <pilz_industrial_motion_planner_testutils/async_test.h>
+
+#include <pilz_industrial_motion_planner_testutils/checks.h>
+#include <pilz_industrial_motion_planner_testutils/sequence.h>
+#include <pilz_industrial_motion_planner_testutils/xml_testdata_loader.h>
+
+#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<moveit_msgs::MoveGroupSequenceAction> ac_{ ph_, SEQUENCE_ACTION_NAME, true };
+  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> 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<std::string>(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<moveit::planning_interface::MoveGroupInterface>(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..11f7c5572a
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_action_preemption.test
@@ -0,0 +1,48 @@
+<!--
+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.
+-->
+
+<launch>
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/test_environment.launch">
+    <arg name="gripper" value="pg70" />
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
+    <arg name="fake_execution_type" value="interpolate" /> <!-- we need to actually execute the trajectory to test preemption -->
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_sequence_action_preemption" type="integrationtest_sequence_action_preemption" time-limit="20.0">
+    <param name="joint_position_tolerance" value="0.01" />
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/testdata_sequence.xml" />
+  </test>
+
+</launch>
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 <functional>
+#include <gtest/gtest.h>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_state/conversions.h>
+#include <moveit_msgs/Constraints.h>
+#include <moveit_msgs/GetMotionPlan.h>
+#include <moveit_msgs/JointConstraint.h>
+#include <moveit_msgs/MotionPlanResponse.h>
+#include <ros/ros.h>
+
+#include <pilz_industrial_motion_planner_testutils/sequence.h>
+#include <pilz_industrial_motion_planner_testutils/xml_testdata_loader.h>
+
+#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<moveit_msgs::GetMotionSequence>(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<PtpJointCart>(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<CircInterimCart>(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..049cb5a253
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test
@@ -0,0 +1,45 @@
+<!--
+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.
+-->
+
+<launch>
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/test_environment.launch">
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_sequence_service_capability" type="integrationtest_sequence_service_capability" time-limit="300.0" > <!-- launch-prefix="xterm -e gdb -args"-->
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/testdata_sequence.xml" />
+  </test>
+
+</launch>
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..604d6ace07
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test
@@ -0,0 +1,47 @@
+<!--
+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.
+-->
+
+<launch>
+
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch">
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_sequence_service_capability_frankaemika_panda"
+        type="integrationtest_sequence_service_capability" time-limit="300.0">
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/frankaemika_panda/test_data/testdata_sequence.xml" />
+  </test>
+
+</launch>
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..0c516453ac
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test
@@ -0,0 +1,47 @@
+<!--
+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.
+-->
+
+<launch>
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/test_environment.launch">
+    <arg name="gripper" value="pg70"/>
+    <arg name="pipeline" value="pilz_industrial_motion_planner" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="integrationtest_sequence_service_capability_with_gripper"
+        type="integrationtest_sequence_service_capability" time-limit="300.0">
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/testdata_sequence.xml" />
+  </test>
+
+</launch>
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/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 @@
+<!--
+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.
+-->
+
+<testdata>
+
+  <poses>
+
+    <pos name="ZeroPose">
+      <joints group_name="panda_arm">0.0 -0.785 0.0 -2.356 0.0 1.571 0.785</joints>
+    </pos>
+
+    <pos name="P1">
+      <joints group_name="panda_arm">-0.022096 -0.753529 0.007612 -1.716165 0.0063464 0.962660 0.771123</joints>
+      <xyzQuat group_name="panda_arm" link_name="panda_link8">0.2 0 0.8 0.924138 -0.382057 0.0 0.0
+        <seed><joints group_name="panda_arm">-0.022096 -0.753529 0.007612 -1.716165 0.0063464 0.962660 0.771123</joints></seed>
+      </xyzQuat>
+    </pos>
+
+    <pos name="P2">
+      <joints group_name="panda_arm">-0.061035 -0.301221 0.410282 -1.665777 0.120602 1.389535 1.138547</joints>
+      <xyzQuat group_name="panda_arm" link_name="panda_link8">0.4 0.2 0.7 0.924138 -0.382057 0.0 0.0
+        <seed><joints group_name="panda_arm">-0.061035 -0.301221 0.410282 -1.665777 0.120602 1.389535 1.138547</joints></seed>
+      </xyzQuat>
+    </pos>
+
+    <pos name="P3">
+      <joints group_name="panda_arm">0.0875228 -0.540765 0.770139 -1.990062 0.366596 1.582282 1.560837</joints>
+      <xyzQuat group_name="panda_arm" link_name="panda_link8">0.2 0.4 0.6 0.924138 -0.382057 0.0 0.0
+        <seed><joints group_name="panda_arm">0.0875228 -0.540765 0.770139 -1.990062 0.366596 1.582282 1.560837</joints></seed>
+      </xyzQuat>
+    </pos>
+
+    <pos name="P4">
+      <joints group_name="panda_arm">0.164988 -0.805223 0.304894 -2.020563 0.230321 1.247100 1.238191</joints>
+      <xyzQuat group_name="panda_arm" link_name="panda_link8">0.2 0.2 0.7 0.924138 -0.382057 0.0 0.0
+        <seed><joints group_name="panda_arm">0.164988 -0.805223 0.304894 -2.020563 0.230321 1.247100 1.238191</joints></seed>
+      </xyzQuat>
+    </pos>
+
+  </poses>
+
+  <ptps>
+    <ptp name="Ptp1">
+      <planningGroup>panda_arm</planningGroup>
+      <targetLink>panda_link8</targetLink>
+      <startPos>ZeroPose</startPos>
+      <endPos>P1</endPos>
+      <vel>1.0</vel>
+      <acc>0.2</acc>
+    </ptp>
+
+    <ptp name="Ptp2">
+      <planningGroup>panda_arm</planningGroup>
+      <targetLink>panda_link8</targetLink>
+      <startPos>P3</startPos>
+      <endPos>P2</endPos>
+      <vel>1.0</vel>
+      <acc>0.2</acc>
+    </ptp>
+
+    <ptp name="Ptp3">
+      <planningGroup>panda_arm</planningGroup>
+      <targetLink>panda_link8</targetLink>
+      <startPos>P2</startPos>
+      <endPos>P3</endPos>
+      <vel>1.0</vel>
+      <acc>0.2</acc>
+    </ptp>
+  </ptps>
+
+  <lins>
+    <lin name="lin3">
+        <planningGroup>panda_arm</planningGroup>
+        <targetLink>panda_link8</targetLink>
+        <startPos>P2</startPos>
+        <endPos>P3</endPos>
+        <vel>0.1</vel>
+        <acc>0.05</acc>
+    </lin>
+
+    <lin name="lin2">
+        <planningGroup>panda_arm</planningGroup>
+        <targetLink>panda_link8</targetLink>
+        <startPos>P1</startPos>
+        <endPos>P2</endPos>
+        <vel>0.1</vel>
+        <acc>0.05</acc>
+    </lin>
+
+    <lin name="lin4">
+        <planningGroup>panda_arm</planningGroup>
+        <targetLink>panda_link8</targetLink>
+        <startPos>P2</startPos>
+        <endPos>P4</endPos>
+        <vel>0.1</vel>
+        <acc>0.05</acc>
+    </lin>
+  </lins>
+
+  <circs>
+    <circ name="circ3_interim">
+      <planningGroup>panda_arm</planningGroup>
+      <targetLink>panda_link8</targetLink>
+      <startPos>P1</startPos>
+      <intermediatePos>P2</intermediatePos>
+      <endPos>P3</endPos>
+      <vel>0.1</vel>
+      <acc>0.1</acc>
+    </circ>
+
+    <circ name="circ1_center_2">
+      <planningGroup>panda_arm</planningGroup>
+      <targetLink>panda_link8</targetLink>
+      <startPos>P3</startPos>
+      <centerPos>P2</centerPos>
+      <endPos>P1</endPos>
+      <vel>0.1</vel>
+      <acc>0.1</acc>
+    </circ>
+  </circs>
+
+  <sequences>
+    <blend name="ComplexSequence">
+      <sequenceCmd name="Ptp1"            type="ptp_joint_cart"     blend_radius="0.1" />
+      <sequenceCmd name="circ3_interim"   type="circ_interim_cart"  blend_radius="0.1" />
+      <sequenceCmd name="Ptp2"            type="ptp_cart_cart"      blend_radius="0.1" />
+      <sequenceCmd name="lin3"            type="lin_cart"           blend_radius="0.1" />
+      <sequenceCmd name="circ1_center_2"  type="circ_center_cart"   blend_radius="0.1" />
+      <sequenceCmd name="lin2"            type="lin_cart"           blend_radius="0.1" />
+      <sequenceCmd name="Ptp3"            type="ptp_cart_cart"      blend_radius="0.0" />
+    </blend>
+    <blend name="SimpleSequence">
+      <sequenceCmd name="lin2" type="lin_cart" blend_radius="0.149" />
+      <sequenceCmd name="lin3" type="lin_cart" blend_radius="0" />
+    </blend>
+    <blend name="NoIntersectionTraj2">
+      <sequenceCmd name="lin2" type="lin_cart" blend_radius="0.21" />
+      <sequenceCmd name="lin4" type="lin_cart" blend_radius="0" />
+    </blend>
+  </sequences>
+
+</testdata>
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..49ac94b65d
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch
@@ -0,0 +1,43 @@
+<launch>
+
+  <!-- Defining a gripper will load all parameters with the defined gripper -->
+  <arg name="gripper" default=""/>
+
+  <!-- URDF and SRDF without gripper -->
+  <!-- The name of the parameter under which the URDF is loaded -->
+  <arg if="$(eval not gripper)" name="robot_description" default="robot_description"/>
+  <arg unless="$(eval not gripper)" name="robot_description" default="robot_description_$(arg gripper)"/>
+
+  <!-- Load universal robot description format (URDF) -->
+  <param name="$(arg robot_description)"
+         command="$(find xacro)/xacro --inorder $(find moveit_resources_prbt_support)/urdf/prbt.xacro gripper:=$(arg gripper)"/>
+
+  <!-- The semantic description that corresponds to the URDF -->
+  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro --inorder
+         $(find moveit_resources_prbt_moveit_config)/config/prbt.srdf.xacro gripper:=$(arg gripper)" />
+
+
+  <!-- Load updated joint limits (override information from URDF) -->
+  <group ns="$(arg robot_description)_planning">
+    <rosparam if="$(eval not gripper)" command="load" file="$(find moveit_resources_prbt_moveit_config)/config/joint_limits.yaml"/>
+    <rosparam unless="$(eval not gripper)" command="load"
+              file="$(eval find('moveit_resources_prbt_'+ gripper + '_support') + '/config/joint_limits.yaml')" />
+
+    <!-- Load cartesian limits -->
+    <rosparam command="load" file="$(find moveit_resources_prbt_moveit_config)/config/cartesian_limits.yaml"/>
+  </group>
+
+  <!-- Load limits again in default namespace (which may not be configurable for planners) -->
+  <group ns="robot_description_planning">
+    <rosparam if="$(eval not gripper)" command="load" file="$(find moveit_resources_prbt_moveit_config)/config/joint_limits.yaml"/>
+    <rosparam unless="$(eval not gripper)" command="load"
+              file="$(eval find('moveit_resources_prbt_'+ gripper + '_support') + '/config/joint_limits.yaml')" />
+    <rosparam command="load" file="$(find moveit_resources_prbt_moveit_config)/config/cartesian_limits.yaml"/>
+  </group>
+
+  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
+  <group ns="$(arg robot_description)_kinematics">
+    <rosparam command="load" file="$(find moveit_resources_prbt_moveit_config)/config/kinematics.yaml"/>
+  </group>
+
+</launch>
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 @@
+<!--
+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.
+-->
+
+<testdata>
+
+  <poses>
+
+    <pos name="ZeroPose">
+        <group name="manipulator">
+            <joints>0.0 0.0 0.0 0.0 0.0 0.0</joints>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="PTPPose1">
+        <group name="manipulator">
+            <joints>0.5 0.0 0.0 0.0 0.0 0.0</joints>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="PTPJointValid">
+        <group name="manipulator">
+            <joints>0.0 0.785398 0.785398 0.0 1.570796 0.0</joints>
+        </group>
+    </pos>
+
+    <pos name="PTPPose">
+      <group name="manipulator">
+          <xyzQuat link_name="prbt_tcp">0.45 -0.1 0.62 0.0 0.0 0.0 1.0</xyzQuat>
+      </group>
+    </pos>
+
+    <pos name="LINPose1">
+        <group name="manipulator">
+            <joints>0.0 -0.52 1.13 0.0 -1.57 0.0</joints>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="LINPose2">
+        <group name="manipulator">
+            <joints>0.0 -0.63 1.4 0.0 1.19 0.0</joints>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="LINPose3">
+        <group name="manipulator">
+            <joints>0.463648 -0.418738 1.03505 0.0 -1.45379 -0.149488</joints>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="LINPose4">
+        <group name="manipulator">
+            <joints>0.0 0.278259 2.11585 0.0 -1.83759 0.785398</joints>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="LINPose1Opposite">
+        <group name="manipulator">
+            <joints>0.0 1.52001 1.13 0.0 -1.57 0.0</joints>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCPose1">
+        <group name="manipulator">
+            <!-- (0.4, 0.1, 0.6 0 0 0) tcp without gripper -->
+            <joints>0.244979 0.333691 -1.48422 0.0 1.81791 -0.244979</joints>
+            <xyzQuat link_name="prbt_tcp">0.4 0.1 0.6 0. 0. 0. 1.0</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCPose2">
+        <group name="manipulator">
+            <!-- (0.1, 0.4, 0.6 0 0 0) tcp without gripper -->
+            <joints>1.32582 0.333691 -1.48422 0.0 1.81791 -1.32582</joints>
+            <xyzQuat link_name="prbt_tcp">0.1 0.4 0.7 0 0 0.923916 -0.382596</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCAuxPose1">
+        <group name="manipulator">
+            <!-- (0.1, 0.1, 0.6) only position is needed for aux point -->
+            <xyzQuat link_name="prbt_tcp">0.1 0.1 0.6 0 0 0 1.0</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCPose3">
+        <group name="manipulator">
+            <!-- (-0.2, 0.1, 0.6 0 0 0) tcp without gripper -->
+            <joints>-0.463648 -1.64103 -2.06227 0 0.421241 0.463648</joints>
+            <xyzQuat link_name="prbt_tcp">-0.2 0.1 0.6 0 0 0 1.0</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCAuxPose2">
+        <group name="manipulator">
+            <!-- wrong center point -->
+            <xyzQuat>0 0.4 0.5 0 0 0 1</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCPose4">
+        <group name="manipulator">
+            <joints>0.0 -0.0107869 -1.72665 0.0 1.71586 0.0</joints>
+            <xyzQuat link_name="prbt_tcp">0.3 0.0 0.65 0.0 0.0 0.0 1.0</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCPose4_delta1">
+        <group name="manipulator">
+            <xyzQuat link_name="prbt_tcp">0.300000001 0.0 0.65 0.0 0.0 0.0 1.0</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCPose4_delta2">
+        <group name="manipulator">
+            <joints>0.0 -0.010787 -1.72666 0.0 1.71586 0.0</joints>
+            <xyzQuat link_name="prbt_tcp">0.300000001 0.0 0.650000001 0.0 0.0 0.0 1.0</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCPose5">
+        <group name="manipulator">
+            <joints>0.0 -1.56309 -1.72665 0.0 0.163561 0.0</joints>
+            <xyzQuat link_name="prbt_tcp">-0.3 0.0 0.65 0.0 0.0 0.0 1.0</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCPose6">
+        <group name="manipulator">
+            <joints>2.61799 -0.0107869 -1.72665 0.0 1.71586 -1.0472</joints>
+            <xyzQuat link_name="prbt_tcp">-0.25980762113533159 0.15 0.65 0 0 0.7071067811865475 0.7071067811865475</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCAuxPose3">
+        <group name="manipulator">
+            <xyzQuat>0.0 0.0 0.65 0.0 0.0 0.0 1.0</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCAuxPose4">
+        <group name="manipulator">
+            <xyzQuat>0.0 0.3 0.65 0.0 0.0 0.0 1.0</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCPose7">
+        <group name="manipulator">
+            <!-- (0.4, 0.1, 0.5 0 0 0) tcp without gripper -->
+            <joints>0.244979 0.333691 -1.48422 0.0 1.81791 -0.244979</joints>
+            <xyzQuat link_name="prbt_tcp">0.4 0.1 0.7 0. 0. 0.923916 -0.382596</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCPose8">
+        <group name="manipulator">
+            <!-- (0.1, 0.4, 0.5 0 0 0) tcp without gripper -->
+            <joints>1.32582 0.333691 -1.48422 0.0 1.81791 -1.32582</joints>
+            <xyzQuat link_name="prbt_tcp">0.3 0.2 0.7 0 0 0.923916 -0.382596</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+    <pos name="CIRCPose9">
+        <group name="manipulator">
+            <!-- (-0.2, 0.1, 0.5 0 0 0) tcp without gripper -->
+            <joints>-0.463648 -1.64103 -2.06227 0 0.421241 0.463648</joints>
+            <xyzQuat link_name="prbt_tcp">0.2 0.1 0.7 0 0 0.923916 -0.382596</xyzQuat>
+        </group>
+        <group name="gripper">
+            <joints>0</joints>
+        </group>
+    </pos>
+
+  </poses>
+
+    <circs>
+      <circ name="ValidCIRCCmd1">
+        <!-- valid circ cmd-->
+        <planningGroup>manipulator</planningGroup>
+        <targetLink>prbt_tcp</targetLink>
+        <startPos>CIRCPose1</startPos>
+        <centerPos>CIRCAuxPose1</centerPos>
+        <endPos>CIRCPose6</endPos>
+        <vel>0.05</vel>
+        <acc>0.05</acc>
+      </circ>
+
+    <circ name="ValidCIRCCmd2">
+      <!-- valid circ cmd-->
+      <planningGroup>manipulator</planningGroup>
+      <targetLink>prbt_tcp</targetLink>
+      <startPos>CIRCPose4</startPos>
+      <centerPos>CIRCAuxPose3</centerPos>
+      <endPos>CIRCPose6</endPos>
+      <vel>0.1</vel>
+      <acc>0.1</acc>
+    </circ>
+
+    <circ name="ValidCIRCCmd3">
+      <!-- valid circ cmd-->
+      <planningGroup>manipulator</planningGroup>
+      <targetLink>prbt_tcp</targetLink>
+      <startPos>CIRCPose4</startPos>
+      <intermediatePos>CIRCAuxPose4</intermediatePos>
+      <endPos>CIRCPose6</endPos>
+      <vel>0.1</vel>
+      <acc>0.1</acc>
+    </circ>
+
+    <circ name="CIRCCmdAllPointsTheSame">
+      <!-- valid circ cmd-->
+      <planningGroup>manipulator</planningGroup>
+      <targetLink>prbt_tcp</targetLink>
+      <startPos>CIRCPose4</startPos>
+      <centerPos>CIRCPose4_delta1</centerPos>
+      <endPos>CIRCPose4_delta2</endPos>
+      <vel>0.1</vel>
+      <acc>0.1</acc>
+    </circ>
+
+    <circ name="CIRCCmdToFast">
+      <!-- valid circ cmd-->
+      <planningGroup>manipulator</planningGroup>
+      <targetLink>prbt_tcp</targetLink>
+      <startPos>CIRCPose4</startPos>
+      <intermediatePos>CIRCAuxPose4</intermediatePos>
+      <endPos>CIRCPose6</endPos>
+      <vel>1.0</vel>
+      <acc>1.0</acc>
+    </circ>
+
+      <circ name="CIRCCmd2">
+          <planningGroup>manipulator</planningGroup>
+          <targetLink>prbt_tcp</targetLink>
+          <startPos>CIRCPose4</startPos>
+          <centerPos>CIRCAuxPose3</centerPos>
+          <endPos>CIRCPose5</endPos>
+          <vel>0.1</vel>
+          <acc>0.1</acc>
+       </circ>
+
+        <circ name="CIRCCmd3">
+          <!-- circ cmd with wrong center point-->
+          <planningGroup>manipulator</planningGroup>
+          <targetLink>prbt_tcp</targetLink>
+          <startPos>CIRCPose1</startPos>
+          <intermediatePos>CIRCAuxPose2</intermediatePos>
+          <centerPos>CIRCAuxPose2</centerPos>
+          <endPos>CIRCPose2</endPos>
+          <vel>0.05</vel>
+          <acc>0.05</acc>
+        </circ>
+
+        <circ name="CIRCCmd4">
+          <!-- circ cmd with colinear center point due to interim point-->
+          <planningGroup>manipulator</planningGroup>
+          <targetLink>prbt_tcp</targetLink>
+          <startPos>CIRCPose7</startPos>
+          <intermediatePos>CIRCPose8</intermediatePos>
+          <endPos>CIRCPose9</endPos>
+          <vel>0.1</vel>
+          <acc>0.1</acc>
+        </circ>
+
+
+        <circ name="CIRCCmd5">
+            <planningGroup>manipulator</planningGroup>
+            <targetLink>prbt_tcp</targetLink>
+            <startPos>CIRCPose4</startPos>
+            <intermediatePos>CIRCAuxPose3</intermediatePos>
+            <endPos>CIRCPose5</endPos>
+            <vel>0.1</vel>
+            <acc>0.1</acc>
+         </circ>
+
+        <circ name="CIRCCmd6">
+            <planningGroup>manipulator</planningGroup>
+            <targetLink>prbt_tcp</targetLink>
+            <startPos>CIRCPose4</startPos>
+            <intermediatePos>CIRCAuxPose4</intermediatePos>
+            <endPos>CIRCPose5</endPos>
+            <vel>0.01</vel>
+            <acc>0.01</acc>
+         </circ>
+
+
+    </circs>
+
+
+    <lins>
+      <lin name="LINCmd1">
+        <planningGroup>manipulator</planningGroup>
+        <targetLink>prbt_tcp</targetLink>
+        <startPos>LINPose1</startPos>
+        <endPos>LINPose2</endPos>
+        <vel>0.4</vel>
+        <acc>0.3</acc>
+      </lin>
+
+      <lin name="LINCmd2">
+        <planningGroup>manipulator</planningGroup>
+        <targetLink>prbt_tcp</targetLink>
+        <startPos>LINPose3</startPos>
+        <endPos>LINPose4</endPos>
+        <vel>0.2</vel>
+        <acc>0.2</acc>
+      </lin>
+
+      <lin name="LINCmdLimitViolation">
+        <planningGroup>manipulator</planningGroup>
+        <targetLink>prbt_tcp</targetLink>
+        <startPos>LINPose1</startPos>
+        <endPos>LINPose1Opposite</endPos>
+        <vel>1.0</vel>
+        <acc>1.0</acc>
+      </lin>
+
+      <lin name="LINStartEqualsGoal">
+        <planningGroup>manipulator</planningGroup>
+        <targetLink>prbt_tcp</targetLink>
+        <startPos>LINPose1</startPos>
+        <endPos>LINPose1</endPos>
+        <vel>0.4</vel>
+        <acc>0.4</acc>
+      </lin>
+
+    </lins>
+
+
+    <ptps>
+      <ptp name="FirstPtp">
+        <planningGroup>manipulator</planningGroup>
+        <targetLink>prbt_tcp</targetLink>
+        <startPos>ZeroPose</startPos>
+        <endPos>PTPJointValid</endPos>
+        <vel>1.0</vel>
+        <acc>1.0</acc>
+      </ptp>
+    </ptps>
+
+ </testdata>
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 @@
+<!--
+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.
+-->
+
+<testdata>
+
+  <poses>
+
+    <pos name="ZeroPose">
+      <joints group_name="manipulator">0.0, 0.0, -0.4353981633974483, 0.0, -1.5700000000000003, 0.0</joints>
+    </pos>
+
+    <pos name="P1">
+      <joints group_name="manipulator">0, -0.016763700542892668, -1.673499069949556, 0, -1.4848572841831293, 2.3561944901923377</joints>
+      <xyzQuat group_name="manipulator" link_name="prbt_tcp">0.3 0.0 0.5 0.923879532509 0.38268343237 0.0 0.0</xyzQuat>
+    </pos>
+
+    <pos name="P2">
+      <joints group_name="manipulator">0.380506 0.731299 -0.960008 0.0 -1.450284 1.951302</joints>
+      <xyzQuat group_name="manipulator" link_name="prbt_tcp">0.5 0.2 0.4 0.707106 0.707106 0.0 0.0</xyzQuat>
+    </pos>
+
+    <pos name="P3">
+      <joints group_name="manipulator">0.927295 0.708913 -1.343015 0.0 -1.089664 2.498091</joints>
+      <xyzQuat group_name="manipulator" link_name="prbt_tcp">0.3 0.4 0.3 0.707106 0.707106 0.0 0.0
+        <!--Seed needed because there seem to be two possible joint solutions-->
+        <seed><joints group_name="manipulator">0.927295 0.708913 -1.343015 0.0 -1.089664 2.498091</joints></seed>
+      </xyzQuat>
+    </pos>
+
+    <pos name="P4">
+      <joints group_name="manipulator">0.588002 0.221990 -1.742660 0.0 -1.176941 2.158798</joints>
+      <xyzQuat group_name="manipulator" link_name="prbt_tcp">0.3 0.2 0.4 0.707106 0.707106 0.0 0.0</xyzQuat>
+    </pos>
+
+    <pos name="gripper_closed">
+      <joints group_name="gripper">0.0</joints>
+    </pos>
+
+    <pos name="gripper_open">
+      <joints group_name="gripper">0.03</joints>
+    </pos>
+
+  </poses>
+
+  <ptps>
+    <ptp name="Ptp1">
+      <planningGroup>manipulator</planningGroup>
+      <targetLink>prbt_tcp</targetLink>
+      <startPos>ZeroPose</startPos>
+      <endPos>P1</endPos>
+      <vel>0.1</vel>
+      <acc>0.1</acc>
+    </ptp>
+
+    <ptp name="Ptp2">
+      <planningGroup>manipulator</planningGroup>
+      <targetLink>prbt_tcp</targetLink>
+      <startPos>P3</startPos>
+      <endPos>P2</endPos>
+      <vel>1.0</vel>
+      <acc>0.4</acc>
+    </ptp>
+
+    <ptp name="Ptp3">
+      <planningGroup>manipulator</planningGroup>
+      <targetLink>prbt_tcp</targetLink>
+      <startPos>P2</startPos>
+      <endPos>P3</endPos>
+      <vel>1.0</vel>
+      <acc>0.4</acc>
+    </ptp>
+
+    <ptp name="Ptp2_1">
+      <planningGroup>manipulator</planningGroup>
+      <targetLink>prbt_tcp</targetLink>
+      <startPos>P2</startPos>
+      <endPos>P1</endPos>
+      <vel>1.0</vel>
+      <acc>0.4</acc>
+    </ptp>
+  </ptps>
+
+  <lins>
+    <lin name="lin3">
+        <planningGroup>manipulator</planningGroup>
+        <targetLink>prbt_tcp</targetLink>
+        <startPos>P2</startPos>
+        <endPos>P3</endPos>
+        <vel>0.1</vel>
+        <acc>0.2</acc>
+    </lin>
+
+    <lin name="lin2">
+        <planningGroup>manipulator</planningGroup>
+        <targetLink>prbt_tcp</targetLink>
+        <startPos>P1</startPos>
+        <endPos>P2</endPos>
+        <vel>0.1</vel>
+        <acc>0.2</acc>
+    </lin>
+
+    <lin name="lin4">
+        <planningGroup>manipulator</planningGroup>
+        <targetLink>prbt_tcp</targetLink>
+        <startPos>P2</startPos>
+        <endPos>P4</endPos>
+        <vel>0.1</vel>
+        <acc>0.2</acc>
+    </lin>
+  </lins>
+
+  <circs>
+    <circ name="circ3_interim">
+      <planningGroup>manipulator</planningGroup>
+      <targetLink>prbt_tcp</targetLink>
+      <startPos>P1</startPos>
+      <intermediatePos>P2</intermediatePos>
+      <endPos>P3</endPos>
+      <vel>0.1</vel>
+      <acc>0.2</acc>
+    </circ>
+
+    <circ name="circ1_center_2">
+      <planningGroup>manipulator</planningGroup>
+      <targetLink>prbt_tcp</targetLink>
+      <startPos>P3</startPos>
+      <centerPos>P2</centerPos>
+      <endPos>P1</endPos>
+      <vel>0.1</vel>
+      <acc>0.2</acc>
+    </circ>
+  </circs>
+
+  <grippers>
+    <gripper name="open_gripper">
+      <planningGroup>gripper</planningGroup>
+      <startPos>gripper_closed</startPos>
+      <endPos>gripper_open</endPos>
+    </gripper>
+    <gripper name="close_gripper">
+      <planningGroup>gripper</planningGroup>
+      <startPos>gripper_open</startPos>
+      <endPos>gripper_closed</endPos>
+    </gripper>
+  </grippers>
+
+  <sequences>
+    <blend name="ComplexSequence">
+      <sequenceCmd name="Ptp1"            type="ptp_joint_cart"     blend_radius="0.1" />
+      <sequenceCmd name="circ3_interim"   type="circ_interim_cart"  blend_radius="0.1" />
+      <sequenceCmd name="Ptp2"            type="ptp_cart_cart"      blend_radius="0.1" />
+      <sequenceCmd name="lin3"            type="lin_cart"           blend_radius="0.1" />
+      <sequenceCmd name="circ1_center_2"  type="circ_center_cart"   blend_radius="0.1" />
+      <sequenceCmd name="lin2"            type="lin_cart"           blend_radius="0.1" />
+      <sequenceCmd name="Ptp3"            type="ptp_cart_cart"      blend_radius="0.0" />
+    </blend>
+    <blend name="ComplexSequenceWithGripper">
+      <sequenceCmd name="open_gripper" type="gripper" blend_radius="0.0" />
+      <sequenceCmd name="Ptp1"            type="ptp_joint_cart"     blend_radius="0.1" />
+      <sequenceCmd name="circ3_interim"   type="circ_interim_cart"  blend_radius="0.1" />
+      <sequenceCmd name="close_gripper" type="gripper" blend_radius="0.0" />
+      <sequenceCmd name="Ptp2"            type="ptp_cart_cart"      blend_radius="0.1" />
+      <sequenceCmd name="lin3"            type="lin_cart"           blend_radius="0.1" />
+      <sequenceCmd name="circ1_center_2"  type="circ_center_cart"   blend_radius="0.1" />
+      <sequenceCmd name="open_gripper" type="gripper" blend_radius="0.0" />
+      <sequenceCmd name="lin2"            type="lin_cart"           blend_radius="0.1" />
+      <sequenceCmd name="Ptp3"            type="ptp_cart_cart"      blend_radius="0.0" />
+      <sequenceCmd name="close_gripper" type="gripper" blend_radius="0.0" />
+    </blend>
+    <blend name="SimpleSequence">
+      <sequenceCmd name="lin2" type="lin_cart" blend_radius="0.149" />
+      <sequenceCmd name="lin3" type="lin_cart" blend_radius="0" />
+    </blend>
+    <blend name="NoIntersectionTraj2">
+      <sequenceCmd name="lin2" type="lin_cart" blend_radius="0.21" />
+      <sequenceCmd name="lin4" type="lin_cart" blend_radius="0" />
+    </blend>
+    <blend name="PtpPtpSequence">
+      <sequenceCmd name="Ptp2" type="ptp_cart_cart" blend_radius="0.1" />
+      <sequenceCmd name="Ptp2_1" type="ptp_cart_cart" blend_radius="0" />
+    </blend>
+    <blend name="LinPtpSequence">
+      <sequenceCmd name="lin2" type="lin_cart" blend_radius="0.1" />
+      <sequenceCmd name="Ptp3" type="ptp_cart_cart" blend_radius="0" />
+    </blend>
+    <blend name="PtpLinSequence">
+      <sequenceCmd name="Ptp2" type="ptp_cart_cart" blend_radius="0.1" />
+      <sequenceCmd name="lin4" type="lin_cart" blend_radius="0" />
+    </blend>
+    <blend name="PureGripperSequence">
+      <sequenceCmd name="open_gripper" type="gripper" blend_radius="0.0" />
+      <sequenceCmd name="close_gripper" type="gripper" blend_radius="0.0" />
+    </blend>
+  </sequences>
+
+</testdata>
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 @@
+<!--
+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.
+-->
+
+<testdata>
+
+  <poses>
+    <pos name="ZeroPose">
+      <joints group_name="manipulator">0.0 0.0 0.0 0.0 0.0 0.0</joints>
+    </pos>
+
+    <pos name="P1">
+      <joints group_name="manipulator">0.0 0.005583 -1.323096 0.0 -1.812912 -2.356194</joints>
+      <xyzQuat group_name="manipulator" link_name="prbt_tcp">0.3 0.0 0.5 0.707106 0.707106 0.0 0.0</xyzQuat>
+    </pos>
+
+    <pos name="P2">
+      <joints group_name="manipulator">0.380506 0.752267 -0.595009 0.0 -1.794315 -1.975688</joints>
+      <xyzQuat group_name="manipulator" link_name="prbt_tcp">0.5 0.2 0.4 0.707106 0.707106 0.0 0.0</xyzQuat>
+    </pos>
+
+    <pos name="P3">
+      <joints group_name="manipulator">0.927295 0.607232 -1.173008 0.0 -1.361351 -1.428899</joints>
+      <xyzQuat group_name="manipulator" link_name="prbt_tcp">0.3 0.4 0.3 0.707106 0.707106 0.0 0.0
+      </xyzQuat>
+    </pos>
+
+  </poses>
+
+  <ptps>
+    <ptp name="Ptp1">
+      <planningGroup>manipulator</planningGroup>
+      <targetLink>prbt_tcp</targetLink>
+      <startPos>ZeroPose</startPos>
+      <endPos>P1</endPos>
+      <vel>1.0</vel>
+      <acc>0.4</acc>
+    </ptp>
+
+  </ptps>
+
+  <lins>
+    <lin name="lin2">
+        <planningGroup>manipulator</planningGroup>
+        <targetLink>prbt_tcp</targetLink>
+        <startPos>P1</startPos>
+        <endPos>P2</endPos>
+        <vel>0.1</vel>
+        <acc>0.2</acc>
+    </lin>
+  </lins>
+
+  <circs>
+    <circ name="circ1_center_2">
+      <planningGroup>manipulator</planningGroup>
+      <targetLink>prbt_tcp</targetLink>
+      <startPos>P3</startPos>
+      <centerPos>P2</centerPos>
+      <endPos>P1</endPos>
+      <vel>0.1</vel>
+      <acc>0.2</acc>
+    </circ>
+  </circs>
+
+</testdata>
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..d88f58dc42
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/testpoints.py
@@ -0,0 +1,228 @@
+#!/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 <eigen_conversions/eigen_msg.h>
+#include <gtest/gtest.h>
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/robot_state/conversions.h>
+#include <tf2/convert.h>
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#include "test_utils.h"
+
+pilz_industrial_motion_planner::JointLimitsContainer
+testutils::createFakeLimits(const std::vector<std::string>& 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<std::string, double> 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<geometry_msgs::Pose, Eigen::Isometry3d>(goal_pose_msg, goal_pose_expect);
+  return true;
+}
+
+bool testutils::isGoalReached(const trajectory_msgs::JointTrajectory& trajectory,
+                              const std::vector<moveit_msgs::JointConstraint>& 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<std::string, double> 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<std::string, double> 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<std::string, double>& 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<double>& joint_values, geometry_msgs::Pose& pose,
+                          const std::string& joint_prefix)
+{
+  {
+    std::map<std::string, double> 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<Eigen::Isometry3d, geometry_msgs::Pose>(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<testutils::BlendTestData>& 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<pilz_industrial_motion_planner::TrajectoryGenerator>& 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<moveit_msgs::DisplayTrajectory>("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<double> 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<double> 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<double> accelerations;
+  std::vector<Eigen::AngleAxisd> 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 <gtest/gtest.h>
+#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 <boost/core/demangle.hpp>
+#include <math.h>
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit_msgs/Constraints.h>
+#include <moveit_msgs/MoveGroupAction.h>
+#include <ros/console.h>
+#include <sensor_msgs/JointState.h>
+#include <string>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/convert.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <utility>
+
+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<std::string>& 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<double>& pos, const std::vector<double>& 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<double>& pos,
+                                                  const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX)
+{
+  return generateJointState(pos, std::vector<double>(), joint_prefix);
+}
+
+inline moveit_msgs::Constraints generateJointConstraint(const std::vector<double>& 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<moveit_msgs::JointConstraint>& 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<double>& 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<std::string, double>& 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<double> start_position;
+  std::vector<double> mid_position;
+  std::vector<double> 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<BlendTestData>& 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<pilz_industrial_motion_planner::TrajectoryGenerator>& 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<double> 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<double>& 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 <gtest/gtest.h>
+
+#include <ros/ros.h>
+
+#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 @@
+<!--
+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.
+-->
+
+<launch>
+
+<include file="$(find moveit_resources_prbt_moveit_config)/launch/planning_context.launch">
+    <arg name="load_robot_description" value="true"/>
+</include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner"
+        test-name="unittest_cartesian_limits_aggregator"
+        type="unittest_cartesian_limits_aggregator">
+    <rosparam command="load"
+    file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_only_vel.yaml"
+    ns="vel_only"/>
+
+    <rosparam command="load"
+    file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/unittest_cartesian_limits_aggregator/test_cartesian_limit_all.yaml"
+    ns="all"/>
+  </test>
+</launch>
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 <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <moveit/robot_model/joint_model_group.h>
+
+#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<std::string>&());
+};
+
+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<NoSolverException> nse_ex{ new NoSolverException("") };
+    EXPECT_EQ(nse_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE);
+  }
+
+  {
+    std::shared_ptr<MoreThanOneTipFrameException> 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<std::string> 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 <gtest/gtest.h>
+
+#include <moveit_msgs/MoveItErrorCodes.h>
+
+#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 @@
+<!--
+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.
+-->
+
+<launch>
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="unittest_joint_limit" type="unittest_joint_limit">
+      <rosparam command="load" file="$(find pilz_industrial_motion_planner)/test/unittest_joint_limit_config.yaml" />
+  </test>
+</launch>
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 <gtest/gtest.h>
+
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+
+#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<std::string, pilz_industrial_motion_planner::JointLimit> 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<std::string, pilz_industrial_motion_planner::JointLimit> 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<std::string, pilz_industrial_motion_planner::JointLimit> 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 @@
+<!--
+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.
+-->
+
+<launch>
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/planning_context.launch">
+      <arg name="load_robot_description" value="true"/>
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="unittest_joint_limits_aggregator" type="unittest_joint_limits_aggregator">
+    <rosparam command="load"
+    file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_valid_1.yaml"
+    ns="valid_1"/>
+
+    <rosparam command="load"
+    file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_min.yaml"
+    ns="violate_position_min"/>
+
+    <rosparam command="load"
+    file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_position_max.yaml"
+    ns="violate_position_max"/>
+
+    <rosparam command="load"
+    file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/unittest_joint_limits_aggregator_testdata/test_joint_limits_violate_velocity.yaml"
+    ns="violate_velocity"/>
+  </test>
+</launch>
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 <gtest/gtest.h>
+
+#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<std::string> joint_names{ "joint1", "joint2" };
+  std::vector<double> 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<double> 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 <gtest/gtest.h>
+
+#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 <gtest/gtest.h>
+
+#include <iostream>
+
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+
+#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<std::string>
+{
+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<planning_interface::PlannerManager>(
+          "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<pluginlib::ClassLoader<planning_interface::PlannerManager>> 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<std::string> 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<std::string> 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<std::string> 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<std::string> 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 @@
+<!--
+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.
+-->
+
+<launch>
+  <!-- Parametrized test running with and without gripper! -->
+
+  <!-- Load the context with and without the pg70 -->
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch" />
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch">
+    <arg name="gripper" value="pg70" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="unittest_pilz_industrial_motion_planner" type="unittest_pilz_industrial_motion_planner">
+      <param name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner"/>
+  </test>
+</launch>
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 <memory>
+
+#include <gtest/gtest.h>
+
+#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<PlanningException> p_ex{ new PlanningException("") };
+  std::shared_ptr<ContextLoaderRegistrationException> 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 <boost/core/demangle.hpp>
+#include <gtest/gtest.h>
+
+#include <moveit_msgs/MoveItErrorCodes.h>
+
+#include <moveit/planning_interface/planning_interface.h>
+
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+
+#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 <typename T, int N>
+class ValueTypeContainer
+{
+public:
+  typedef T Type_;
+  static const int VALUE = N;
+};
+template <typename T, int N>
+const int ValueTypeContainer<T, N>::VALUE;
+
+typedef ValueTypeContainer<pilz_industrial_motion_planner::PlanningContextPTP, 0> PTP_NO_GRIPPER;
+typedef ValueTypeContainer<pilz_industrial_motion_planner::PlanningContextPTP, 1> PTP_WITH_GRIPPER;
+typedef ValueTypeContainer<pilz_industrial_motion_planner::PlanningContextLIN, 0> LIN_NO_GRIPPER;
+typedef ValueTypeContainer<pilz_industrial_motion_planner::PlanningContextLIN, 1> LIN_WITH_GRIPPER;
+typedef ValueTypeContainer<pilz_industrial_motion_planner::PlanningContextCIRC, 0> CIRC_NO_GRIPPER;
+typedef ValueTypeContainer<pilz_industrial_motion_planner::PlanningContextCIRC, 1> CIRC_WITH_GRIPPER;
+
+typedef ::testing::Types<PTP_NO_GRIPPER, PTP_WITH_GRIPPER, LIN_NO_GRIPPER, LIN_WITH_GRIPPER, CIRC_NO_GRIPPER,
+                         CIRC_WITH_GRIPPER>
+    PlanningContextTestTypes;
+
+/**
+ * type parameterized test fixture
+ */
+template <typename T>
+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<typename T::Type_>(
+        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_interface::PlanningContext> 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 @@
+<!--
+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.
+-->
+
+<launch>
+  <!-- load test parameters-->
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch" />
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch">
+    <arg name="gripper" value="pg70" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="unittest_planning_context" type="unittest_planning_context" >
+    <param name="planning_group" value="manipulator" />
+    <param name="target_link" value="prbt_tcp" />
+  </test>
+
+</launch>
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 <gtest/gtest.h>
+
+// Boost includes
+#include <boost/scoped_ptr.hpp>
+
+#include <pluginlib/class_loader.h>
+
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+
+#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<std::vector<std::string>>
+{
+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::PlanningContextLoader>(
+              "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<pluginlib::ClassLoader<pilz_industrial_motion_planner::PlanningContextLoader>>
+      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<std::string>{ "pilz_industrial_motion_planner::PlanningContextLoaderPTP", "PTP",
+                                                PARAM_MODEL_NO_GRIPPER_NAME },  // Test for PTP
+                      std::vector<std::string>{ "pilz_industrial_motion_planner::PlanningContextLoaderPTP", "PTP",
+                                                PARAM_MODEL_WITH_GRIPPER_NAME },  // Test for PTP
+                      std::vector<std::string>{ "pilz_industrial_motion_planner::PlanningContextLoaderLIN", "LIN",
+                                                PARAM_MODEL_NO_GRIPPER_NAME },  // Test for LIN
+                      std::vector<std::string>{ "pilz_industrial_motion_planner::PlanningContextLoaderLIN", "LIN",
+                                                PARAM_MODEL_WITH_GRIPPER_NAME },  // Test for LIN
+                      std::vector<std::string>{ "pilz_industrial_motion_planner::PlanningContextLoaderCIRC", "CIRC",
+                                                PARAM_MODEL_NO_GRIPPER_NAME },  // Test for CIRC
+                      std::vector<std::string>{ "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 @@
+<!--
+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.
+-->
+
+<launch>
+  <!-- Parametrized test running with and without gripper! -->
+
+  <!-- Load the context with and without the pg70 -->
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch" />
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch">
+    <arg name="gripper" value="pg70" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="unittest_planning_context_loaders"
+        type="unittest_planning_context_loaders"/>
+
+</launch>
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 <memory>
+
+#include <gtest/gtest.h>
+
+#include <eigen_conversions/eigen_msg.h>
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_state/conversions.h>
+#include <moveit_msgs/DisplayTrajectory.h>
+#include <tf2/convert.h>
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#include <pilz_industrial_motion_planner_testutils/command_types_typedef.h>
+#include <pilz_industrial_motion_planner_testutils/sequence.h>
+#include <pilz_industrial_motion_planner_testutils/xml_testdata_loader.h>
+
+#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<std::string>
+{
+protected:
+  /**
+   * @brief Create test scenario for trajectory blender
+   *
+   */
+  void SetUp() override;
+
+  /**
+   * @brief Generate lin trajectories for blend sequences
+   */
+  std::vector<planning_interface::MotionPlanResponse> 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<TrajectoryGenerator> lin_generator_;
+  std::unique_ptr<TrajectoryBlenderTransitionWindow> 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<planning_interface::MotionPlanResponse>
+TrajectoryBlenderTransitionWindowTest::generateLinTrajs(const Sequence& seq, size_t num_cmds)
+{
+  std::vector<planning_interface::MotionPlanResponse> responses(num_cmds);
+
+  for (size_t index = 0; index < num_cmds; ++index)
+  {
+    planning_interface::MotionPlanRequest req{ seq.getCmd<LinCart>(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<planning_interface::MotionPlanResponse> 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<planning_interface::MotionPlanResponse> 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<planning_interface::MotionPlanResponse> 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<planning_interface::MotionPlanResponse> 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<planning_interface::MotionPlanResponse> responses(num_cmds);
+
+  for (size_t index = 0; index < num_cmds; ++index)
+  {
+    planning_interface::MotionPlanRequest req{ seq.getCmd<LinCart>(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<planning_interface::MotionPlanResponse> 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<planning_interface::MotionPlanResponse> 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<planning_interface::MotionPlanResponse> 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<planning_interface::MotionPlanResponse> 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<planning_interface::MotionPlanResponse> 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<planning_interface::MotionPlanResponse> 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<LinCart>(1).setGoalConfiguration(seq.getCmd<LinCart>(0).getStartConfiguration());
+
+  std::vector<planning_interface::MotionPlanResponse> 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<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 2) };
+
+  // prepare looping over trajectories
+  std::vector<robot_trajectory::RobotTrajectoryPtr> 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::Isometry3d, geometry_msgs::Pose>(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<std::string, double> 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_trajectory::RobotTrajectory>(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 @@
+<!--
+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.
+-->
+
+<launch>
+  <!-- Parametrized test running with and without gripper! -->
+
+  <!-- Load the context with and without the pg70 -->
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch" />
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch">
+    <arg name="gripper" value="pg70" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner"  time-limit="120.0" test-name="unittest_trajectory_blender_transition_window" type="unittest_trajectory_blender_transition_window">
+    <param name="planning_group" value="manipulator" />
+    <param name="target_link" value="prbt_tcp" />
+    <param name="cartesian_velocity_tolerance" value="1.0e-6" />
+    <param name="cartesian_angular_velocity_tolerance" value="1.0e-6" />
+    <param name="joint_velocity_tolerance" value="1.0e-6" />
+    <param name="joint_acceleration_tolerance" value="1.0e-2" />
+    <param name="sampling_time" value="0.01" />
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/testdata_sequence.xml" />
+    <rosparam command="load" file="$(find moveit_resources_prbt_moveit_config)/config/joint_limits.yaml" />
+  </test>
+
+  <arg name="display_plots" default="false" />
+  <node if="$(arg display_plots)"
+        name="plot_trajectory_msg"
+        pkg="pilz_industrial_motion_planner"
+        type="plot_display_trajectory_msg.py"
+        args="/my_planned_path"
+  />
+
+</launch>
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 <gtest/gtest.h>
+
+#include <map>
+#include <math.h>
+#include <string>
+#include <vector>
+
+#include <Eigen/Geometry>
+#include <eigen_conversions/eigen_msg.h>
+#include <kdl/frames.hpp>
+#include <kdl/path_roundedcomposite.hpp>
+#include <kdl/rotational_interpolation_sa.hpp>
+#include <kdl/trajectory.hpp>
+#include <kdl/trajectory_segment.hpp>
+#include <kdl/velocityprofile_trap.hpp>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_model/joint_model_group.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit_msgs/RobotState.h>
+#include <moveit_msgs/RobotTrajectory.h>
+#include <tf2/convert.h>
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#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<std::string>
+{
+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<std::string> joint_names_;
+  std::map<std::string, double> 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<std::string, double> 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<Eigen::Isometry3d, geometry_msgs::Pose>(rstate.getFrameTransform(ik_fast_link_), pose_expect);
+
+    // prepare inverse kinematics
+    std::vector<geometry_msgs::Pose> ik_poses;
+    ik_poses.push_back(pose_expect);
+    std::vector<double> 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<std::vector<double>> 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<std::string, double> 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<std::string, double> 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<std::string, double> 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<std::string, double> 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<std::string, double> ik_seed;
+
+  // compute the ik
+  std::map<std::string, double> 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<std::string, double> ik_seed;
+
+  // compute the ik
+  std::map<std::string, double> 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<std::string, double> ik_seed;
+
+  // compute the ik
+  std::map<std::string, double> 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<double> ik_seed_states = { -0.553, 0.956, 1.758, 0.146, -1.059, 1.247 };
+  auto joint_names = jmg->getActiveJointModelNames();
+
+  std::map<std::string, double> 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<geometry_msgs::Pose, Eigen::Isometry3d>(pose, pose_expect);
+
+  // compute the ik without self collision check and expect the resulting pose
+  // to be in self collission.
+  std::map<std::string, double> 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<double> ik_state;
+  std::transform(ik_actual1.begin(), ik_actual1.end(), std::back_inserter(ik_state),
+                 boost::bind(&std::map<std::string, double>::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<std::string, double> 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<double> ik_state2;
+  std::transform(ik_actual2.begin(), ik_actual2.end(), std::back_inserter(ik_state2),
+                 boost::bind(&std::map<std::string, double>::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<std::string, double> ik_seed;
+  for (const auto& joint_name : jmg->getActiveJointModelNames())
+  {
+    ik_seed[joint_name] = 0;
+  }
+
+  // create goal
+  std::vector<double> 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<std::string, double> 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<std::string, double> 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<std::string, double> position_last{ { test_joint_name, 2.0 } };
+  std::map<std::string, double> position_current{ { test_joint_name, 10.0 } };
+  std::map<std::string, double> 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<std::string, double> position_last{ { test_joint_name, 2.0 } };
+  std::map<std::string, double> 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<std::string, double> 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<std::string, double> position_last{ { test_joint_name, 20.0 } };
+  std::map<std::string, double> 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<std::string, double> 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<std::string, double> 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<std::string, double> 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_trajectory::RobotTrajectory>(robot_model_, planning_group_);
+  robot_trajectory::RobotTrajectoryPtr second_trajectory =
+      std::make_shared<robot_trajectory::RobotTrajectory>(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_trajectory::RobotTrajectory>(robot_model_, planning_group_);
+  robot_trajectory::RobotTrajectoryPtr second_trajectory =
+      std::make_shared<robot_trajectory::RobotTrajectory>(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_trajectory::RobotTrajectory>(robot_model_, planning_group_);
+  robot_trajectory::RobotTrajectoryPtr second_trajectory =
+      std::make_shared<robot_trajectory::RobotTrajectory>(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 @@
+<!--
+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.
+-->
+
+<launch>
+  <!-- Parametrized test running with and without gripper! -->
+
+  <!-- Load the context with and without the pg70 -->
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch" />
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch">
+    <arg name="gripper" value="pg70" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="unittest_trajectory_functions" type="unittest_trajectory_functions" >
+    <param name="planning_group" value="manipulator" />
+    <param name="group_tip_link" value="prbt_flange" />
+    <param name="tcp_link" value="prbt_tcp" />
+    <param name="ik_fast_link" value="prbt_flange" />
+    <param name="random_test_number" value="1000" />
+  </test>
+
+</launch>
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 <memory>
+
+#include <gtest/gtest.h>
+
+#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<TrajectoryGeneratorInvalidLimitsException> tgil_ex{ new TrajectoryGeneratorInvalidLimitsException(
+        "") };
+    EXPECT_EQ(tgil_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE);
+  }
+
+  {
+    std::shared_ptr<VelocityScalingIncorrect> vsi_ex{ new VelocityScalingIncorrect("") };
+    EXPECT_EQ(vsi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
+  }
+
+  {
+    std::shared_ptr<AccelerationScalingIncorrect> asi_ex{ new AccelerationScalingIncorrect("") };
+    EXPECT_EQ(asi_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
+  }
+
+  {
+    std::shared_ptr<UnknownPlanningGroup> upg_ex{ new UnknownPlanningGroup("") };
+    EXPECT_EQ(upg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME);
+  }
+
+  {
+    std::shared_ptr<NoJointNamesInStartState> njniss_ex{ new NoJointNamesInStartState("") };
+    EXPECT_EQ(njniss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
+  }
+
+  {
+    std::shared_ptr<SizeMismatchInStartState> smiss_ex{ new SizeMismatchInStartState("") };
+    EXPECT_EQ(smiss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
+  }
+
+  {
+    std::shared_ptr<JointsOfStartStateOutOfRange> jofssoor_ex{ new JointsOfStartStateOutOfRange("") };
+    EXPECT_EQ(jofssoor_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
+  }
+
+  {
+    std::shared_ptr<NonZeroVelocityInStartState> nzviss_ex{ new NonZeroVelocityInStartState("") };
+    EXPECT_EQ(nzviss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
+  }
+
+  {
+    std::shared_ptr<NotExactlyOneGoalConstraintGiven> neogcg_ex{ new NotExactlyOneGoalConstraintGiven("") };
+    EXPECT_EQ(neogcg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+  }
+
+  {
+    std::shared_ptr<OnlyOneGoalTypeAllowed> oogta_ex{ new OnlyOneGoalTypeAllowed("") };
+    EXPECT_EQ(oogta_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+  }
+
+  {
+    std::shared_ptr<StartStateGoalStateMismatch> ssgsm_ex{ new StartStateGoalStateMismatch("") };
+    EXPECT_EQ(ssgsm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+  }
+
+  {
+    std::shared_ptr<JointConstraintDoesNotBelongToGroup> jcdnbtg_ex{ new JointConstraintDoesNotBelongToGroup("") };
+    EXPECT_EQ(jcdnbtg_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+  }
+
+  {
+    std::shared_ptr<JointsOfGoalOutOfRange> jogoor_ex{ new JointsOfGoalOutOfRange("") };
+    EXPECT_EQ(jogoor_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+  }
+
+  {
+    std::shared_ptr<PositionConstraintNameMissing> pcnm_ex{ new PositionConstraintNameMissing("") };
+    EXPECT_EQ(pcnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+  }
+
+  {
+    std::shared_ptr<OrientationConstraintNameMissing> ocnm_ex{ new OrientationConstraintNameMissing("") };
+    EXPECT_EQ(ocnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+  }
+
+  {
+    std::shared_ptr<PositionOrientationConstraintNameMismatch> pocnm_ex{ new PositionOrientationConstraintNameMismatch(
+        "") };
+    EXPECT_EQ(pocnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+  }
+
+  {
+    std::shared_ptr<NoIKSolverAvailable> nisa_ex{ new NoIKSolverAvailable("") };
+    EXPECT_EQ(nisa_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION);
+  }
+
+  {
+    std::shared_ptr<NoPrimitivePoseGiven> 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 <memory>
+
+#include <gtest/gtest.h>
+
+#include <eigen_conversions/eigen_msg.h>
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_state/conversions.h>
+#include <tf2/convert.h>
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#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<std::string>
+{
+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<TrajectoryGeneratorCIRC> circ_;
+  // test data provider
+  std::unique_ptr<pilz_industrial_motion_planner_testutils::TestdataLoader> 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<geometry_msgs::Point, Eigen::Vector3d>(
+        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<geometry_msgs::Point, Eigen::Vector3d>(
+        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<CircleNoPlane> cnp_ex{ new CircleNoPlane("") };
+    EXPECT_EQ(cnp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
+  }
+
+  {
+    std::shared_ptr<CircleToSmall> cts_ex{ new CircleToSmall("") };
+    EXPECT_EQ(cts_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
+  }
+
+  {
+    std::shared_ptr<CenterPointDifferentRadius> cpdr_ex{ new CenterPointDifferentRadius("") };
+    EXPECT_EQ(cpdr_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
+  }
+
+  {
+    std::shared_ptr<CircTrajectoryConversionFailure> ctcf_ex{ new CircTrajectoryConversionFailure("") };
+    EXPECT_EQ(ctcf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
+  }
+
+  {
+    std::shared_ptr<UnknownPathConstraintName> upcn_ex{ new UnknownPathConstraintName("") };
+    EXPECT_EQ(upcn_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
+  }
+
+  {
+    std::shared_ptr<NoPositionConstraints> npc_ex{ new NoPositionConstraints("") };
+    EXPECT_EQ(npc_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
+  }
+
+  {
+    std::shared_ptr<NoPrimitivePose> npp_ex{ new NoPrimitivePose("") };
+    EXPECT_EQ(npp_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
+  }
+
+  {
+    std::shared_ptr<UnknownLinkNameOfAuxiliaryPoint> ulnoap_ex{ new UnknownLinkNameOfAuxiliaryPoint("") };
+    EXPECT_EQ(ulnoap_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME);
+  }
+
+  {
+    std::shared_ptr<NumberOfConstraintsMismatch> nocm_ex{ new NumberOfConstraintsMismatch("") };
+    EXPECT_EQ(nocm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+  }
+
+  {
+    std::shared_ptr<CircJointMissingInStartState> cjmiss_ex{ new CircJointMissingInStartState("") };
+    EXPECT_EQ(cjmiss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
+  }
+
+  {
+    std::shared_ptr<CircInverseForGoalIncalculable> 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<double>(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 @@
+<!--
+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.
+-->
+
+<launch>
+  <!-- launch-prefix="xterm -e gdb - -args" time-limit="1000" -->
+
+  <!-- Parametrized test running with and without gripper! -->
+
+  <!-- Load the context with and without the pg70 -->
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch" />
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch">
+    <arg name="gripper" value="pg70" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="unittest_trajectory_generator_circ"
+  type="unittest_trajectory_generator_circ" time-limit="120.0">
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/testdata_sequence.xml" />
+    <param name="planning_group" value="manipulator" />
+    <param name="target_link" value="prbt_tcp" />
+    <param name="cartesian_position_tolerance" value="1.0e-3" />
+    <param name="angular_acc_tolerance" value="1.0e-3" />
+    <param name="rot_axis_norm_tolerance" value="1.0e-6" />
+    <param name="acceleration_tolerance" value="1.0e-3" />
+    <param name="other_tolerance" value="1.0e-6" />
+    <rosparam command="load" file="$(find moveit_resources_prbt_moveit_config)/config/joint_limits.yaml" />
+  </test>
+
+
+</launch>
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 <boost/core/demangle.hpp>
+#include <gtest/gtest.h>
+
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_state/conversions.h>
+#include <moveit/robot_state/robot_state.h>
+#include <moveit_msgs/MoveItErrorCodes.h>
+
+#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 <typename T, int N>
+class ValueTypeContainer
+{
+public:
+  typedef T Type_;
+  static const int VALUE = N;
+};
+template <typename T, int N>
+const int ValueTypeContainer<T, N>::VALUE;
+
+typedef ValueTypeContainer<pilz_industrial_motion_planner::TrajectoryGeneratorPTP, 0> PTP_NO_GRIPPER;
+typedef ValueTypeContainer<pilz_industrial_motion_planner::TrajectoryGeneratorPTP, 1> PTP_WITH_GRIPPER;
+typedef ValueTypeContainer<pilz_industrial_motion_planner::TrajectoryGeneratorLIN, 0> LIN_NO_GRIPPER;
+typedef ValueTypeContainer<pilz_industrial_motion_planner::TrajectoryGeneratorLIN, 1> LIN_WITH_GRIPPER;
+typedef ValueTypeContainer<pilz_industrial_motion_planner::TrajectoryGeneratorCIRC, 0> CIRC_NO_GRIPPER;
+typedef ValueTypeContainer<pilz_industrial_motion_planner::TrajectoryGeneratorCIRC, 1> CIRC_WITH_GRIPPER;
+
+typedef ::testing::Types<PTP_NO_GRIPPER, PTP_WITH_GRIPPER, LIN_NO_GRIPPER, LIN_WITH_GRIPPER, CIRC_NO_GRIPPER,
+                         CIRC_WITH_GRIPPER>
+    TrajectoryGeneratorCommonTestTypes;
+
+typedef ::testing::Types<PTP_NO_GRIPPER, LIN_NO_GRIPPER, CIRC_NO_GRIPPER> TrajectoryGeneratorCommonTestTypesNoGripper;
+
+typedef ::testing::Types<PTP_WITH_GRIPPER, LIN_WITH_GRIPPER, CIRC_WITH_GRIPPER>
+    TrajectoryGeneratorCommonTestTypesWithGripper;
+
+/**
+ * type parameterized test fixture
+ */
+template <typename T>
+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<typename T::Type_>(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<double>(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<pilz_industrial_motion_planner::TrajectoryGenerator> 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 <typename T>
+class TrajectoryGeneratorCommonTestNoGripper : public TrajectoryGeneratorCommonTest<T>
+{
+};
+TYPED_TEST_SUITE(TrajectoryGeneratorCommonTestNoGripper, TrajectoryGeneratorCommonTestTypesNoGripper);
+
+template <typename T>
+class TrajectoryGeneratorCommonTestWithGripper : public TrajectoryGeneratorCommonTest<T>
+{
+};
+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 @@
+<!--
+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.
+-->
+
+<launch>
+  <!-- Parametrized test running with and without gripper! -->
+
+  <!-- Load the context with and without the pg70 -->
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch" />
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch">
+    <arg name="gripper" value="pg70" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="unittest_trajectory_generator_common"
+        type="unittest_trajectory_generator_common" time-limit="180.0">
+    <param name="planning_group" value="manipulator" />
+    <param name="target_link" value="prbt_flange" />
+    <param name="random_trial_number" value="100" />
+    <param name="joint_position_tolerance" value="1.0e-3" />
+    <param name="joint_velocity_tolerance" value="0.3" />
+    <param name="pose_norm_tolerance" value="1.0e-6" />
+    <param name="rot_axis_norm_tolerance" value="1.0e-6" />
+    <param name="other_tolerance" value="1.0e-6" />
+    <param name="velocity_scaling_factor" value="0.1" />
+  </test>
+
+
+</launch>
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 <memory>
+
+#include <gtest/gtest.h>
+
+#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 <eigen_conversions/eigen_msg.h>
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_state/conversions.h>
+#include <moveit/robot_state/robot_state.h>
+
+#include <ros/console.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 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<std::string>
+{
+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<TrajectoryGenerator> lin_;
+  // test data provider
+  std::unique_ptr<pilz_industrial_motion_planner_testutils::TestdataLoader> 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<LinTrajectoryConversionFailure> ltcf_ex{ new LinTrajectoryConversionFailure("") };
+    EXPECT_EQ(ltcf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE);
+  }
+
+  {
+    std::shared_ptr<JointNumberMismatch> jnm_ex{ new JointNumberMismatch("") };
+    EXPECT_EQ(jnm_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+  }
+
+  {
+    std::shared_ptr<LinJointMissingInStartState> ljmiss_ex{ new LinJointMissingInStartState("") };
+    EXPECT_EQ(ljmiss_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
+  }
+
+  {
+    std::shared_ptr<LinInverseForGoalIncalculable> 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<double>(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 @@
+<!--
+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.
+-->
+
+<launch>
+  <!--launch-prefix="xterm -e gdb - -args" time-limit="1000" -->
+
+  <!-- Parametrized test running with and without gripper! -->
+
+  <!-- Load the context with and without the pg70 -->
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch" />
+  <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch">
+    <arg name="gripper" value="pg70" />
+  </include>
+
+  <!-- run test -->
+  <test pkg="pilz_industrial_motion_planner" test-name="unittest_trajectory_generator_lin"
+  type="unittest_trajectory_generator_lin">
+    <param name="testdata_file_name" value="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/test_data/testdata_sequence.xml" />
+    <param name="planning_group" value="manipulator" />
+    <param name="target_link_hand_computed_data" value="prbt_flange" />
+    <param name="random_trial_number" value="100" />
+    <param name="joint_position_tolerance" value="1.0e-3" />
+    <param name="joint_velocity_tolerance" value="0.3" />
+    <param name="pose_norm_tolerance" value="1.0e-6" />
+    <param name="rot_axis_norm_tolerance" value="1.0e-6" />
+    <param name="other_tolerance" value="1.0e-5" />
+    <param name="velocity_scaling_factor" value="0.1" />
+    <rosparam command="load" file="$(find moveit_resources_prbt_moveit_config)/config/joint_limits.yaml" />
+  </test>
+
+</launch>
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 <memory>
+
+#include <gtest/gtest.h>
+
+#include "pilz_industrial_motion_planner/joint_limits_aggregator.h"
+#include "pilz_industrial_motion_planner/trajectory_generator_ptp.h"
+#include "test_utils.h"
+
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <pluginlib/class_loader.h>
+
+// 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<std::string>
+{
+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<TrajectoryGenerator> 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<std::string> 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<PtpVelocityProfileSyncFailed> pvpsf_ex{ new PtpVelocityProfileSyncFailed("") };
+    EXPECT_EQ(pvpsf_ex->getErrorCode(), moveit_msgs::MoveItErrorCodes::FAILURE);
+  }
+
+  {
+    std::shared_ptr<PtpNoIkSolutionForGoalPose> 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<TrajectoryGeneratorPTP> 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<TrajectoryGeneratorPTP> 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<double> tolerance_pose(3, 0.01);
+  std::vector<double> 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<double> tolerance_pose(3, 0.01);
+  std::vector<double> 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<double> tolerance_pose(3, 0.01);
+  std::vector<double> 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<double>(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 @@
+<!--
+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.
+-->
+
+<launch>
+    <!-- Parametrized test running with and without gripper! -->
+
+    <!-- Load the context with and without the pg70 -->
+    <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch" />
+    <include file="$(find pilz_industrial_motion_planner)/test/test_robots/prbt/launch/test_context.launch">
+      <arg name="gripper" value="pg70" />
+    </include>
+
+    <!-- run test -->
+    <test pkg="pilz_industrial_motion_planner" test-name="unittest_trajectory_generator_ptp"
+    type="unittest_trajectory_generator_ptp">
+      <param name="planning_group" value="manipulator" />
+      <param name="target_link" value="prbt_tcp" />
+      <param name="joint_position_tolerance" value="1.0e-8" />
+      <param name="joint_velocity_tolerance" value="1.0e-8" />
+      <param name="joint_acceleration_tolerance" value="1.0e-8" />
+      <param name="pose_norm_tolerance" value="1.0e-6" />
+      <rosparam command="load" file="$(find moveit_resources_prbt_moveit_config)/config/joint_limits.yaml" />
+    </test>
+
+</launch>
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 <gtest/gtest.h>
+
+#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<double>::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<pilz_industrial_motion_planner::VelocityProfileATrap*>(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..041f9ad3a5
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst
@@ -0,0 +1,17 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package pilz_industrial_motion_planner_testutils
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+* Add pilz_industrial_motion_planner to moveit_planners (`#2507 <https://github.com/ros-planning/moveit/issues/2507>`_)
+* Contributors: Christian Henkel, Immanuel Martini, Robert Haschke, Tyler Weaver
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..236af5e988
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_circ_auxiliary.uxf
@@ -0,0 +1,133 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>40</x>
+      <y>100</y>
+      <w>380</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>CartesianPathConstraintsBuilder
+--
++ to PathConstraints() : moveit_msgs::Constraints</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>410</x>
+      <y>430</y>
+      <w>210</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>CartesianCenter</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>460</x>
+      <y>90</y>
+      <w>470</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>template=ConfigType&gt;
+CircAuxiliary
+{abstract}
+--
++ toPathConstraints(): moveit_msgs::Constraints</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>470</x>
+      <y>280</y>
+      <w>230</w>
+      <h>50</h>
+    </coordinates>
+    <panel_attributes>template=ConfigType, Builder&gt;
+Center</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>730</x>
+      <y>280</y>
+      <w>220</w>
+      <h>50</h>
+    </coordinates>
+    <panel_attributes>template=ConfigType, Builder&gt;
+Interim</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>730</x>
+      <y>160</y>
+      <w>30</w>
+      <h>150</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;.</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;130.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>470</x>
+      <y>160</y>
+      <w>40</w>
+      <h>150</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;.</panel_attributes>
+    <additional_attributes>20.0;10.0;17.0;130.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>490</x>
+      <y>320</y>
+      <w>340</w>
+      <h>130</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;.
+&lt;&lt;bind&gt;&gt;
+&lt;ConfigType-&gt;CartesianConfiguration&gt;
+&lt;Builder-&gt;CartesianPathConstraintsBuilder&gt;</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;110.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>710</x>
+      <y>510</y>
+      <w>210</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>CartesianInterim</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>800</x>
+      <y>320</y>
+      <w>340</w>
+      <h>210</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;.
+
+
+
+
+
+
+&lt;&lt;bind&gt;&gt;
+&lt;ConfigType-&gt;CartesianConfiguration&gt;
+&lt;Builder-&gt;CartesianPathConstraintsBuilder&gt;</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;190.0</additional_attributes>
+  </element>
+</diagram>
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..ab33fcc08b
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_class_commands.uxf
@@ -0,0 +1,175 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>530</x>
+      <y>370</y>
+      <w>560</w>
+      <h>150</h>
+    </coordinates>
+    <panel_attributes>template=StartType, GoalType
+BaseCommand
+{abstract}
+--
++ setStartConfiguration(...)
++ getStartConfiguration(...) : StartType
+
++ setGoalConfiguration(...)
++ getGoalConfiguration(...) : GoalType
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>910</x>
+      <y>560</y>
+      <w>230</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>template=StartType, GoalType
+Ptp</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>970</x>
+      <y>510</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;60.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>710</x>
+      <y>510</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;60.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>650</x>
+      <y>560</y>
+      <w>230</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>template=StartType, GoalType
+Lin</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>530</x>
+      <y>670</y>
+      <w>300</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>template=StartType, AuxiliaryType, GoalType
+Circ</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>540</x>
+      <y>510</y>
+      <w>30</w>
+      <h>190</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;170.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>50</x>
+      <y>200</y>
+      <w>400</w>
+      <h>150</h>
+    </coordinates>
+    <panel_attributes>Sequence
+--
++ add(cmd : MotionCmdUPtr)
++ setAllBlendRadiiToZero()
++ getBlendRadius(index: int)
++ getCmd() : MotionCmd&amp;
+
++ toRequest() : moveit_msgs::MotionSequenceRequest</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>590</x>
+      <y>200</y>
+      <w>390</w>
+      <h>120</h>
+    </coordinates>
+    <panel_attributes>MotionCmd
+{abstract}
+--
++ setPlanningGroup(...)
++ setTargetLink(...)
++ setVelocityScale(...)
++ setAccelerationScale(...)</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>750</x>
+      <y>310</y>
+      <w>30</w>
+      <h>90</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;70.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>640</x>
+      <y>80</y>
+      <w>260</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>MPReqConvertible
+{abstract}
+--
+/+ toRequest(): MotionPlanRequest/</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>750</x>
+      <y>130</y>
+      <w>30</w>
+      <h>90</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;70.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>440</x>
+      <y>280</y>
+      <w>170</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;&lt;&lt;-
+m2=*</panel_attributes>
+    <additional_attributes>10.0;10.0;150.0;10.0</additional_attributes>
+  </element>
+</diagram>
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 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>330</x>
+      <y>210</y>
+      <w>280</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>RobotConfiguration
+--
++ setRobotModel()
++ setGroupName()</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>220</x>
+      <y>340</y>
+      <w>170</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>JointConfiguration</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>530</x>
+      <y>340</y>
+      <w>170</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>CartesianConfiguration</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>310</x>
+      <y>270</y>
+      <w>110</w>
+      <h>90</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>90.0;10.0;10.0;70.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>480</x>
+      <y>270</y>
+      <w>140</w>
+      <h>90</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;120.0;70.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLNote</id>
+    <coordinates>
+      <x>700</x>
+      <y>200</y>
+      <w>210</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>Defines a configuration of the robot in space</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>600</x>
+      <y>210</y>
+      <w>120</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>lt=..</panel_attributes>
+    <additional_attributes>10.0;10.0;100.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>20</x>
+      <y>20</y>
+      <w>450</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>RobotStateMsgConvertible
+{abstract}
+--
+/+ toMoveitMsgRobotState() : moveit_msgs::RobotState /</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>490</x>
+      <y>20</y>
+      <w>400</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>GoalConstraintMsgConvertible
+{abstract}
+--
+/+ toGoalConstraints() : moveit_msgs::Constraints /</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>470</x>
+      <y>90</y>
+      <w>150</w>
+      <h>140</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>130.0;10.0;10.0;120.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>270</x>
+      <y>90</y>
+      <w>160</w>
+      <h>140</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;140.0;120.0</additional_attributes>
+  </element>
+</diagram>
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..b47471023f
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner_testutils/doc/diagrams/diag_seq_testdataloader_usage.uxf
@@ -0,0 +1,215 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <help_text>// 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 &gt; New..." to create new element types
+//////////////////////////////////////////////////////////////////////////////////////////////
+
+
+// This text will be stored with each diagram;  use it for notes.</help_text>
+  <zoom_level>12</zoom_level>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>252</x>
+      <y>96</y>
+      <w>168</w>
+      <h>36</h>
+    </coordinates>
+    <panel_attributes>_:ExampleTestClass_</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>324</x>
+      <y>120</y>
+      <w>36</w>
+      <h>408</h>
+    </coordinates>
+    <panel_attributes>lt=.
+layer=-10</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;320.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>708</x>
+      <y>96</y>
+      <w>144</w>
+      <h>36</h>
+    </coordinates>
+    <panel_attributes>_:TestDataLoader_</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>768</x>
+      <y>120</y>
+      <w>36</w>
+      <h>396</h>
+    </coordinates>
+    <panel_attributes>lt=.
+layer=-10</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;310.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>324</x>
+      <y>156</y>
+      <w>24</w>
+      <h>312</h>
+    </coordinates>
+    <panel_attributes>layer=10
+transparency=0</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLSpecialState</id>
+    <coordinates>
+      <x>132</x>
+      <y>144</y>
+      <w>24</w>
+      <h>24</h>
+    </coordinates>
+    <panel_attributes>type=initial</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>144</x>
+      <y>132</y>
+      <w>204</w>
+      <h>48</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-
+myAwesomeTest()</panel_attributes>
+    <additional_attributes>150.0;20.0;10.0;20.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>768</x>
+      <y>204</y>
+      <w>24</w>
+      <h>84</h>
+    </coordinates>
+    <panel_attributes>layer=10
+transparency=0</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>336</x>
+      <y>180</y>
+      <w>456</w>
+      <h>48</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-
+getPtpCart() : PtpCart</panel_attributes>
+    <additional_attributes>360.0;20.0;10.0;20.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>612</x>
+      <y>360</y>
+      <w>24</w>
+      <h>24</h>
+    </coordinates>
+    <panel_attributes>layer=10
+transparency=0</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>348</x>
+      <y>336</y>
+      <w>288</w>
+      <h>48</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-
+toRequest() :
+moveit_msgs::MotionPlanRequest</panel_attributes>
+    <additional_attributes>220.0;20.0;10.0;20.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>576</x>
+      <y>252</y>
+      <w>96</w>
+      <h>36</h>
+    </coordinates>
+    <panel_attributes>_:PtpCart_</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>612</x>
+      <y>276</y>
+      <w>36</w>
+      <h>216</h>
+    </coordinates>
+    <panel_attributes>lt=.</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;160.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>660</x>
+      <y>252</y>
+      <w>132</w>
+      <h>36</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;90.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLGeneric</id>
+    <coordinates>
+      <x>336</x>
+      <y>324</y>
+      <w>24</w>
+      <h>96</h>
+    </coordinates>
+    <panel_attributes>layer=100
+transparency=0
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>336</x>
+      <y>276</y>
+      <w>204</w>
+      <h>72</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;-
+doAwesomeThings()</panel_attributes>
+    <additional_attributes>20.0;40.0;50.0;40.0;50.0;10.0;10.0;10.0</additional_attributes>
+  </element>
+</diagram>
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 <mutex>
+#include <condition_variable>
+#include <algorithm>
+#include <chrono>
+#include <string>
+
+#include <gmock/gmock.h>
+
+#include <ros/ros.h>
+
+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:<br>
+ * Suppose you want to test a function that calls another function asynchronously, like the following example:
+ *
+ * \code
+ * void asyncCall(std::function<void()> 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<std::string> clear_events, const int timeout_ms = -1);
+
+protected:
+  std::mutex m_;
+  std::condition_variable cv_;
+  std::set<std::string> clear_events_{};
+  std::set<std::string> 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<std::mutex> 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<std::string> clear_events, const int timeout_ms)
+{
+  std::unique_lock<std::mutex> 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 <string>
+
+#include "motioncmd.h"
+
+namespace pilz_industrial_motion_planner_testutils
+{
+template <class StartType, class GoalType>
+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 <class StartType, class GoalType>
+inline void BaseCmd<StartType, GoalType>::setStartConfiguration(StartType start)
+{
+  start_ = start;
+}
+
+template <class StartType, class GoalType>
+inline void BaseCmd<StartType, GoalType>::setGoalConfiguration(GoalType goal)
+{
+  goal_ = goal;
+}
+
+template <class StartType, class GoalType>
+inline StartType& BaseCmd<StartType, GoalType>::getStartConfiguration()
+{
+  return start_;
+}
+
+template <class StartType, class GoalType>
+inline const StartType& BaseCmd<StartType, GoalType>::getStartConfiguration() const
+{
+  return start_;
+}
+
+template <class StartType, class GoalType>
+inline GoalType& BaseCmd<StartType, GoalType>::getGoalConfiguration()
+{
+  return goal_;
+}
+
+template <class StartType, class GoalType>
+inline const GoalType& BaseCmd<StartType, GoalType>::getGoalConfiguration() const
+{
+  return goal_;
+}
+
+template <class StartType, class GoalType>
+planning_interface::MotionPlanRequest BaseCmd<StartType, GoalType>::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 <vector>
+#include <sstream>
+
+#include <boost/optional.hpp>
+
+#include <eigen_conversions/eigen_msg.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+#include <moveit/robot_state/conversions.h>
+#include <moveit/kinematic_constraints/utils.h>
+
+#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<double>& config);
+
+  CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector<double>& 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<double> getPoseTolerance() const;
+
+  void setAngleTolerance(const double tol);
+  const boost::optional<double> getAngleTolerance() const;
+
+private:
+  static geometry_msgs::Pose toPose(const std::vector<double>& 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<double> tolerance_pose_{ boost::none };
+
+  //! @brief The value to assign to the absolute tolerances of the
+  //! orientation constraint.
+  boost::optional<double> tolerance_angle_{ boost::none };
+
+  //! @brief The seed for computing the IK solution of the cartesian configuration.
+  boost::optional<JointConfiguration> 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<double> CartesianConfiguration::getPoseTolerance() const
+{
+  return tolerance_pose_;
+}
+
+inline void CartesianConfiguration::setAngleTolerance(const double tol)
+{
+  tolerance_angle_ = tol;
+}
+
+inline const boost::optional<double> 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 <string>
+
+#include <moveit_msgs/Constraints.h>
+
+#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 ConfigType, class BuilderType>
+class Center : public CircAuxiliary<ConfigType, BuilderType>
+{
+private:
+  std::string getConstraintName() const override;
+};
+
+template <class ConfigType, class BuilderType>
+std::string Center<ConfigType, BuilderType>::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..1921c8ba00
--- /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 <gtest/gtest.h>
+#include <moveit/robot_state/robot_state.h>
+
+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
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 <memory>
+
+#include "basecmd.h"
+#include "circauxiliary.h"
+
+namespace pilz_industrial_motion_planner_testutils
+{
+/**
+ * @brief Data class storing all information regarding a Circ command.
+ */
+template <class StartType, class AuxiliaryType, class GoalType>
+class Circ : public BaseCmd<StartType, GoalType>
+{
+public:
+  Circ() : BaseCmd<StartType, GoalType>()
+  {
+  }
+
+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 <class StartType, class AuxiliaryType, class GoalType>
+inline void Circ<StartType, AuxiliaryType, GoalType>::setAuxiliaryConfiguration(AuxiliaryType auxiliary)
+{
+  auxiliary_ = auxiliary;
+}
+
+template <class StartType, class AuxiliaryType, class GoalType>
+inline std::string Circ<StartType, AuxiliaryType, GoalType>::getPlannerId() const
+{
+  return "CIRC";
+}
+
+template <class StartType, class AuxiliaryType, class GoalType>
+inline planning_interface::MotionPlanRequest Circ<StartType, AuxiliaryType, GoalType>::toRequest() const
+{
+  planning_interface::MotionPlanRequest req{ BaseCmd<StartType, GoalType>::toRequest() };
+  req.path_constraints = auxiliary_.toPathConstraints();
+
+  return req;
+}
+
+template <class StartType, class AuxiliaryType, class GoalType>
+inline AuxiliaryType& Circ<StartType, AuxiliaryType, GoalType>::getAuxiliaryConfiguration()
+{
+  return auxiliary_;
+}
+
+template <class StartType, class AuxiliaryType, class GoalType>
+inline const AuxiliaryType& Circ<StartType, AuxiliaryType, GoalType>::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<CartesianConfiguration, CartesianPathConstraintsBuilder>;
+using CartesianInterim = Interim<CartesianConfiguration, CartesianPathConstraintsBuilder>;
+}  // 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 <string>
+
+#include <moveit_msgs/Constraints.h>
+
+namespace pilz_industrial_motion_planner_testutils
+{
+/**
+ * @brief Base class to define an auxiliary point needed to specify
+ * circ commands.
+ */
+template <class ConfigType, class BuilderType>
+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 <class ConfigType, class BuilderType>
+void CircAuxiliary<ConfigType, BuilderType>::setConfiguration(const ConfigType& auxiliary_config)
+{
+  auxiliary_config_ = auxiliary_config;
+}
+
+template <class ConfigType, class BuilderType>
+inline ConfigType& CircAuxiliary<ConfigType, BuilderType>::getConfiguration()
+{
+  return auxiliary_config_;
+}
+
+template <class ConfigType, class BuilderType>
+inline const ConfigType& CircAuxiliary<ConfigType, BuilderType>::getConfiguration() const
+{
+  return auxiliary_config_;
+}
+
+template <class ConfigType, class BuilderType>
+inline moveit_msgs::Constraints CircAuxiliary<ConfigType, BuilderType>::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 <boost/variant.hpp>
+
+#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<JointConfiguration, JointConfiguration> PtpJoint;
+typedef Ptp<JointConfiguration, CartesianConfiguration> PtpJointCart;
+typedef Ptp<CartesianConfiguration, CartesianConfiguration> PtpCart;
+
+typedef Lin<JointConfiguration, JointConfiguration> LinJoint;
+typedef Lin<JointConfiguration, CartesianConfiguration> LinJointCart;
+typedef Lin<CartesianConfiguration, CartesianConfiguration> LinCart;
+
+typedef Circ<CartesianConfiguration, CartesianCenter, CartesianConfiguration> CircCenterCart;
+typedef Circ<CartesianConfiguration, CartesianInterim, CartesianConfiguration> CircInterimCart;
+
+typedef Circ<JointConfiguration, CartesianCenter, JointConfiguration> CircJointCenterCart;
+typedef Circ<JointConfiguration, CartesianInterim, JointConfiguration> CircJointInterimCart;
+
+typedef boost::variant<PtpJoint, PtpJointCart, PtpCart, LinJoint, LinCart, CircCenterCart, CircInterimCart,
+                       CircJointCenterCart, CircJointInterimCart, Gripper>
+    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 <stdexcept>
+#include <string>
+
+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 <string>
+
+#include <moveit_msgs/Constraints.h>
+#include <moveit/robot_model/robot_model.h>
+
+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<JointConfiguration, JointConfiguration>
+{
+};
+}  // 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 ConfigType, class BuilderType>
+class Interim : public CircAuxiliary<ConfigType, BuilderType>
+{
+private:
+  std::string getConstraintName() const override;
+};
+
+template <class ConfigType, class BuilderType>
+std::string Interim<ConfigType, BuilderType>::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 <string>
+#include <utility>
+#include <vector>
+#include <functional>
+#include <stdexcept>
+
+#include <sensor_msgs/JointState.h>
+#include <moveit/robot_state/robot_state.h>
+#include <moveit/robot_state/conversions.h>
+
+#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<std::string(const size_t&)>;
+
+/**
+ * @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<double>& config,
+                     CreateJointNameFunc&& create_joint_name_func);
+
+  JointConfiguration(const std::string& group_name, const std::vector<double>& 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<double> 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<double> 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<double> 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 <stdexcept>
+
+#include "basecmd.h"
+
+namespace pilz_industrial_motion_planner_testutils
+{
+/**
+ * @brief Data class storing all information regarding a linear command.
+ */
+template <class StartType, class GoalType>
+class Lin : public BaseCmd<StartType, GoalType>
+{
+public:
+  Lin() : BaseCmd<StartType, GoalType>()
+  {
+  }
+
+private:
+  std::string getPlannerId() const override;
+};
+
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+template <class StartType, class GoalType>
+inline std::string Lin<StartType, GoalType>::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 <string>
+#include <memory>
+
+#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<MotionCmd>;
+}  // 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 <moveit/robot_model/robot_model.h>
+#include <moveit/planning_interface/planning_interface.h>
+
+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 <stdexcept>
+
+#include "basecmd.h"
+
+namespace pilz_industrial_motion_planner_testutils
+{
+/**
+ * @brief Data class storing all information regarding a Ptp command.
+ */
+template <class StartType, class GoalType>
+class Ptp : public BaseCmd<StartType, GoalType>
+{
+public:
+  Ptp() : BaseCmd<StartType, GoalType>()
+  {
+  }
+
+private:
+  std::string getPlannerId() const override;
+};
+
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+template <class StartType, class GoalType>
+inline std::string Ptp<StartType, GoalType>::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 <string>
+#include <utility>
+
+#include <moveit/robot_model/robot_model.h>
+
+#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 <moveit_msgs/RobotState.h>
+#include <moveit/robot_model/robot_model.h>
+
+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 <stdexcept>
+#include <vector>
+#include <utility>
+#include <typeinfo>
+
+#include <moveit_msgs/MotionSequenceRequest.h>
+
+#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 <class T>
+  T& getCmd(const size_t index_cmd);
+
+  template <class T>
+  const T& getCmd(const size_t index_cmd) const;
+
+  /**
+   * @return TRUE if the specified command is of the specified type,
+   * otherwise FALSE.
+   */
+  template <class T>
+  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<CmdVariant, double>;
+  std::vector<TCmdRadiiPair> 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 <class T>
+inline T& Sequence::getCmd(const size_t index_cmd)
+{
+  return boost::get<T>(cmds_.at(index_cmd).first);
+}
+
+template <class T>
+inline const T& Sequence::getCmd(const size_t index_cmd) const
+{
+  return boost::get<T>(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 <class T>
+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 <string>
+#include <utility>
+
+#include <moveit/robot_model/robot_model.h>
+
+#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<TestdataLoader>;
+}  // 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 <string>
+
+namespace pilz_industrial_motion_planner_testutils
+{
+const std::string EMPTY_STR{};
+
+const std::string XML_ATTR_STR{ "<xmlattr>" };
+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 <string>
+#include <vector>
+#include <functional>
+#include <map>
+#include <memory>
+
+#include <boost/property_tree/ptree.hpp>
+
+#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:
+ *
+ * <testdata>
+ *
+ *  <poses>
+ *    <pos name="MyTestPos1">
+ *      <joints group_name="manipulator">j1 j2 j3 j4 j5 j6</joints>
+ *      <xyzQuat group_name="manipulator" link_name="prbt_tcp">
+ *        x y z wx wy wz w
+ *        <seed><joints group_name="manipulator">s1 s2 s3 s4 s5 s6</joints></seed>
+ *      </xyzQuat>
+ *      <joints group_name="gripper">j_gripper</joints>
+ *    </pos>
+ *
+ *    <pos name="MyTestPos2">
+ *      <joints group_name="manipulator">j1 j2 j3 j4 j5 j6</joints>
+ *      <xyzQuat group_name="manipulator" link_name="prbt_tcp">x y z wx wy wz w</xyzQuat>
+ *      <joints group_name="gripper">j_gripper</joints>
+ *    </pos>
+ *  </poses>
+ *
+ *  <ptps>
+ *    <ptp name="MyPtp1">
+ *      <startPos>MyTestPos1</startPos>
+ *      <endPos>MyTestPos2</endPos>
+ *      <vel>0.1</vel>
+ *      <acc>0.2</acc>
+ *    </ptp>
+ *  </ptps>
+ *
+ *  <lins>
+ *    <lin name="MyTestLin1">
+ *      <planningGroup>manipulator</planningGroup>
+ *      <targetLink>prbt_tcp</targetLink>
+ *      <startPos>MyTestPos1</startPos>
+ *      <endPos>MyTestPos2</endPos>
+ *      <vel>0.3</vel>
+ *      <acc>0.4</acc>
+ *    </lin>
+ *  </lins>
+ *
+ *  <circs>
+ *    <circ name="MyTestCirc1">
+ *       <planningGroup>manipulator</planningGroup>
+ *       <targetLink>prbt_tcp</targetLink>
+ *      <startPos>MyTestPos1</startPos>
+ *      <intermediatePos>MyTestPos1</intermediatePos>
+ *      <centerPos>MyTestPos2</centerPos>
+ *      <endPos>MyTestPos1</endPos>
+ *      <vel>0.2</vel>
+ *      <acc>0.5</acc>
+ *    </circ>
+ *  </circs>
+ *
+ *  <sequences>
+ *    <blend name="TestBlend">
+ *      <sequenceCmd name="TestPtp" type="ptp" blend_radius="0.2" />
+ *      <sequenceCmd name="MyTestLin1" type="lin" blend_radius="0.01" />
+ *      <sequenceCmd name="MyTestCirc1" type="circ" blend_radius="0" />
+ *    </blend>
+ *  </sequences>
+ *
+ * </testdata>
+ */
+
+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<double> strVec2doubleVec(std::vector<std::string>& 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<AbstractCmdGetterAdapter>;
+
+  //! 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<std::string, AbstractCmdGetterUPtr> cmd_getter_funcs_;
+
+private:
+  const pt::ptree::value_type empty_value_type_{};
+  const pt::ptree empty_tree_{};
+};
+
+std::vector<double> XmlTestdataLoader::strVec2doubleVec(std::vector<std::string>& strVec)
+{
+  std::vector<double> 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<TestdataLoader>;
+}  // 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..b12152ff71
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package>
+  <name>pilz_industrial_motion_planner_testutils</name>
+  <version>1.0.11</version>
+  <description>Helper scripts and functionality to test industrial motion generation</description>
+
+  <maintainer email="a.gutenkunst@pilz.de">Alexander Gutenkunst</maintainer>
+  <maintainer email="c.henkel@pilz.de">Christian Henkel</maintainer>
+  <maintainer email="i.martini@pilz.de">Immanuel Martini</maintainer>
+  <maintainer email="j.schleicher@pilz.de">Joachim Schleicher</maintainer>
+  <maintainer email="h.slusarek@pilz.de">Hagen Slusarek</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://moveit.ros.org</url>
+  <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
+  <url type="repository">https://github.com/ros-planning/moveit</url>
+
+  <build_depend>eigen_conversions</build_depend>
+  <build_depend>moveit_core</build_depend>
+  <build_depend>moveit_msgs</build_depend>
+
+  <run_depend>moveit_core</run_depend>
+  <run_depend>moveit_msgs</run_depend>
+  <run_depend>moveit_commander</run_depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+</package>
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 <stdexcept>
+
+namespace pilz_industrial_motion_planner_testutils
+{
+CartesianConfiguration::CartesianConfiguration() : RobotConfiguration()
+{
+}
+
+CartesianConfiguration::CartesianConfiguration(const std::string& group_name, const std::string& link_name,
+                                               const std::vector<double>& 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<double>& 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<double>& 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 <moveit/kinematic_constraints/utils.h>
+
+namespace pilz_industrial_motion_planner_testutils
+{
+JointConfiguration::JointConfiguration() : RobotConfiguration()
+{
+}
+
+JointConfiguration::JointConfiguration(const std::string& group_name, const std::vector<double>& 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<double>& 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 <stdexcept>
+
+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 <algorithm>
+#include <boost/variant.hpp>
+
+namespace pilz_industrial_motion_planner_testutils
+{
+/**
+ * @brief Visitor transforming the stored command into a MotionPlanRequest.
+ */
+class ToReqVisitor : public boost::static_visitor<planning_interface::MotionPlanRequest>
+{
+public:
+  template <typename T>
+  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<MotionCmd&>
+{
+public:
+  template <typename T>
+  MotionCmd& operator()(T& cmd) const
+  {
+    return cmd;
+  }
+};
+
+moveit_msgs::MotionSequenceRequest Sequence::toRequest() const
+{
+  moveit_msgs::MotionSequenceRequest req;
+
+  std::vector<std::string> 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 <iostream>
+
+#include <boost/property_tree/xml_parser.hpp>
+#include <boost/algorithm/string.hpp>
+#include <boost/optional.hpp>
+
+#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<std::string>(PLANNING_GROUP_STR);
+}
+
+inline std::string CmdReader::getTargetLink() const
+{
+  return cmd_node_.second.get<std::string>(TARGET_LINK_STR);
+}
+
+inline std::string CmdReader::getStartPoseName() const
+{
+  return cmd_node_.second.get<std::string>(START_POS_STR);
+}
+
+inline std::string CmdReader::getEndPoseName() const
+{
+  return cmd_node_.second.get<std::string>(END_POS_STR);
+}
+
+inline double CmdReader::getVelocityScale() const
+{
+  return cmd_node_.second.get<double>(VEL_STR, default_velocity_scale_);
+}
+
+inline double CmdReader::getAccelerationScale() const
+{
+  return cmd_node_.second.get<double>(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 CmdType>
+class CmdGetterAdapter : public XmlTestdataLoader::AbstractCmdGetterAdapter
+{
+public:
+  using FuncType = std::function<CmdType(const std::string&)>;
+
+  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<PtpJoint>(std::bind(&XmlTestdataLoader::getPtpJoint, this, _1)));
+  cmd_getter_funcs_["ptp_joint_cart"] = AbstractCmdGetterUPtr(
+      new CmdGetterAdapter<PtpJointCart>(std::bind(&XmlTestdataLoader::getPtpJointCart, this, _1)));
+  cmd_getter_funcs_["ptp_cart_cart"] =
+      AbstractCmdGetterUPtr(new CmdGetterAdapter<PtpCart>(std::bind(&XmlTestdataLoader::getPtpCart, this, _1)));
+
+  cmd_getter_funcs_["lin"] =
+      AbstractCmdGetterUPtr(new CmdGetterAdapter<LinJoint>(std::bind(&XmlTestdataLoader::getLinJoint, this, _1)));
+  cmd_getter_funcs_["lin_cart"] =
+      AbstractCmdGetterUPtr(new CmdGetterAdapter<LinCart>(std::bind(&XmlTestdataLoader::getLinCart, this, _1)));
+
+  cmd_getter_funcs_["circ_center_cart"] = AbstractCmdGetterUPtr(
+      new CmdGetterAdapter<CircCenterCart>(std::bind(&XmlTestdataLoader::getCircCartCenterCart, this, _1)));
+  cmd_getter_funcs_["circ_interim_cart"] = AbstractCmdGetterUPtr(
+      new CmdGetterAdapter<CircInterimCart>(std::bind(&XmlTestdataLoader::getCircCartInterimCart, this, _1)));
+  cmd_getter_funcs_["circ_joint_interim_cart"] = AbstractCmdGetterUPtr(
+      new CmdGetterAdapter<CircJointInterimCart>(std::bind(&XmlTestdataLoader::getCircJointInterimCart, this, _1)));
+
+  cmd_getter_funcs_["gripper"] =
+      AbstractCmdGetterUPtr(new CmdGetterAdapter<Gripper>(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<std::string> 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<std::string> 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<std::string>(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<std::string>(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<double>(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/Makefile b/moveit_planners/sbpl/core/sbpl_interface/Makefile
index b75b928f20..bbd3fc6049 100644
--- a/moveit_planners/sbpl/core/sbpl_interface/Makefile
+++ b/moveit_planners/sbpl/core/sbpl_interface/Makefile
@@ -1 +1 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
+include $(shell rospack find mk)/cmake.mk
diff --git a/moveit_planners/sbpl/core/sbpl_interface/manifest.xml b/moveit_planners/sbpl/core/sbpl_interface/manifest.xml
index cf6ff29fac..bd05d28a35 100644
--- a/moveit_planners/sbpl/core/sbpl_interface/manifest.xml
+++ b/moveit_planners/sbpl/core/sbpl_interface/manifest.xml
@@ -20,5 +20,3 @@
   </export>
 
 </package>
-
-
diff --git a/moveit_planners/sbpl/ros/sbpl_interface_ros/Makefile b/moveit_planners/sbpl/ros/sbpl_interface_ros/Makefile
index b75b928f20..bbd3fc6049 100644
--- a/moveit_planners/sbpl/ros/sbpl_interface_ros/Makefile
+++ b/moveit_planners/sbpl/ros/sbpl_interface_ros/Makefile
@@ -1 +1 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
+include $(shell rospack find mk)/cmake.mk
diff --git a/moveit_planners/sbpl/ros/sbpl_interface_ros/manifest.xml b/moveit_planners/sbpl/ros/sbpl_interface_ros/manifest.xml
index 0593c47a3f..ebffd4ef22 100644
--- a/moveit_planners/sbpl/ros/sbpl_interface_ros/manifest.xml
+++ b/moveit_planners/sbpl/ros/sbpl_interface_ros/manifest.xml
@@ -19,5 +19,3 @@
   </export>
 
 </package>
-
-
diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h
new file mode 100644
index 0000000000..6ad995e4ff
--- /dev/null
+++ b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h
@@ -0,0 +1,367 @@
+#pragma once
+#include <trajopt/common.hpp>
+#include <trajopt/json_marshal.hpp>
+#include <trajopt_sco/optimizers.hpp>
+
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/planning_scene/planning_scene.h>
+
+#include <memory>
+
+namespace trajopt_interface
+{
+/**
+@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 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;
+  //  Eigen::VectorXd data_vec;
+  //  trajopt::TrajArray data_trajectory;
+  /** @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<std::string, MakerFunc> name_to_maker_;
+  int supported_term_types_;
+};
+
+struct ProblemInfo
+{
+public:
+  BasicInfo basic_info;
+  sco::BasicTrustRegionSQPParameters opt_info;
+  std::vector<TermInfoPtr> cost_infos;
+  std::vector<TermInfoPtr> 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_;
+  }
+  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;
+  }
+};
+
+void generateInitialTrajectory(const ProblemInfo& pci, const std::vector<double>& current_joint_values,
+                               trajopt::TrajArray& init_traj);
+
+}  // namespace trajopt_interface
diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h
new file mode 100644
index 0000000000..bb48692b88
--- /dev/null
+++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h
@@ -0,0 +1,77 @@
+/*********************************************************************
+ * 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 <ros/ros.h>
+#include <trajopt_sco/sco_common.hpp>
+#include <moveit/planning_interface/planning_interface.h>
+#include "problem_description.h"
+
+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(ProblemInfo& problem_info);
+  void setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name);
+  trajopt::DblVec extractStartJointValues(const planning_interface::MotionPlanRequest& req,
+                                          const std::vector<std::string>& group_joint_names);
+
+  ros::NodeHandle nh_;  /// The ROS node handle
+  sco::BasicTrustRegionSQPParameters params_;
+  std::vector<sco::Optimizer::Callback> optimizer_callbacks_;
+  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/include/trajopt_interface/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h
new file mode 100644
index 0000000000..9c06eb0f5e
--- /dev/null
+++ b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <moveit/planning_interface/planning_interface.h>
+
+#include <trajopt_interface/problem_description.h>
+#include <trajopt_interface/trajopt_interface.h>
+
+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_plugins/moveit_controller_manager_example/CHANGELOG.rst b/moveit_plugins/moveit_controller_manager_example/CHANGELOG.rst
index 96fa53d782..69c2051535 100644
--- a/moveit_plugins/moveit_controller_manager_example/CHANGELOG.rst
+++ b/moveit_plugins/moveit_controller_manager_example/CHANGELOG.rst
@@ -2,6 +2,21 @@
 Changelog for package moveit_controller_manager_example
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+
 1.0.6 (2020-08-19)
 ------------------
 
@@ -78,7 +93,7 @@ Changelog for package moveit_controller_manager_example
 
 0.9.5 (2017-03-08)
 ------------------
-* [fix][moveit_ros_warehouse] gcc6 build error `#423 <https://github.com/ros-planning/moveit/pull/423>`_ 
+* [fix][moveit_ros_warehouse] gcc6 build error `#423 <https://github.com/ros-planning/moveit/pull/423>`_
 * Contributors: Dave Coleman
 
 0.9.4 (2017-02-06)
diff --git a/moveit_plugins/moveit_controller_manager_example/package.xml b/moveit_plugins/moveit_controller_manager_example/package.xml
index 711b4b505f..5968662397 100644
--- a/moveit_plugins/moveit_controller_manager_example/package.xml
+++ b/moveit_plugins/moveit_controller_manager_example/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_controller_manager_example</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>An example controller manager plugin for MoveIt. This is not functional code.</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst
index 1a11be105f..48f9fd0b48 100644
--- a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst
+++ b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst
@@ -2,6 +2,23 @@
 Changelog for package moveit_fake_controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: Felix von Drigalski
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Migrate to clang-format-10
diff --git a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt
index 2d4c43f82b..05df4dca3a 100644
--- a/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt
+++ b/moveit_plugins/moveit_fake_controller_manager/CMakeLists.txt
@@ -36,10 +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}) 
+        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/package.xml b/moveit_plugins/moveit_fake_controller_manager/package.xml
index 533ab7940e..fcb23cde8a 100644
--- a/moveit_plugins/moveit_fake_controller_manager/package.xml
+++ b/moveit_plugins/moveit_fake_controller_manager/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_fake_controller_manager</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>A fake controller manager plugin for MoveIt.</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
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 f33ed0656a..3eaf290d83 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:
diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst
index 19caf1932f..6a7f624049 100644
--- a/moveit_plugins/moveit_plugins/CHANGELOG.rst
+++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst
@@ -2,6 +2,21 @@
 Changelog for package moveit_plugins
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+
 1.0.6 (2020-08-19)
 ------------------
 
diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml
index 7d04760de1..8f4684b61d 100644
--- a/moveit_plugins/moveit_plugins/package.xml
+++ b/moveit_plugins/moveit_plugins/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_plugins</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Metapackage for MoveIt! plugins.</description>
 
   <author email="mferguson@fetchrobotics.com">Michael Ferguson</author>
diff --git a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst
index efe7802c4a..ebfca2e474 100644
--- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst
+++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst
@@ -2,6 +2,23 @@
 Changelog for package moveit_ros_control_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: Felix von Drigalski
+
 1.0.6 (2020-08-19)
 ------------------
 
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..55358202f9 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
@@ -42,7 +42,7 @@
 
 namespace moveit_ros_control_interface
 {
-MOVEIT_CLASS_FORWARD(ControllerHandleAllocator);
+MOVEIT_CLASS_FORWARD(ControllerHandleAllocator);  // Defines ControllerHandleAllocatorPtr, ConstPtr, WeakPtr... etc
 
 /**
  * Base class for MoveItControllerHandle allocators
diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml
index 943295d0b4..7107b3ab61 100644
--- a/moveit_plugins/moveit_ros_control_interface/package.xml
+++ b/moveit_plugins/moveit_ros_control_interface/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit_ros_control_interface</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>ros_control controller manager interface for MoveIt!</description>
 
   <author email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</author>
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 c8d6e02c2b..e81065be2c 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
diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst
index cbe35ba3fb..22cdf14996 100644
--- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst
+++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst
@@ -2,6 +2,25 @@
 Changelog for package moveit_simple_controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+* feat(simple_controller_manager): add `max_effort` parameter to GripperCommand action (`#2984 <https://github.com/ros-planning/moveit/issues/2984>`_) (`#3091 <https://github.com/ros-planning/moveit/issues/3091>`_)
+* Contributors: Michael Görner, Rick Staa
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: Felix von Drigalski
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Migrate to clang-format-10
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 4a788556d5..3527ae2e7a 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
@@ -62,7 +62,8 @@ class ActionBasedControllerHandleBase : public moveit_controller_manager::MoveIt
   }
 };
 
-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
@@ -153,7 +154,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
 
 protected:
   ros::NodeHandle nh_;
-  std::string getActionName(void) const
+  std::string getActionName() const
   {
     if (namespace_.empty())
       return name_;
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..28591f41fa 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
@@ -52,10 +52,11 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
 {
 public:
   /* Topics will map to name/ns/goal, name/ns/result, etc */
-  GripperControllerHandle(const std::string& name, const std::string& ns)
+  GripperControllerHandle(const std::string& name, const std::string& ns, const double max_effort = 0.0)
     : ActionBasedControllerHandle<control_msgs::GripperCommandAction>(name, ns)
     , allow_failure_(false)
     , parallel_jaw_gripper_(false)
+    , max_effort_(max_effort)
   {
   }
 
@@ -111,7 +112,6 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
     // goal to be sent
     control_msgs::GripperCommandGoal goal;
     goal.command.position = 0.0;
-    goal.command.max_effort = 0.0;
 
     // send last point
     int tpoint = trajectory.joint_trajectory.points.size() - 1;
@@ -133,7 +133,13 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
       goal.command.position += trajectory.joint_trajectory.points[tpoint].positions[idx];
 
       if (trajectory.joint_trajectory.points[tpoint].effort.size() > idx)
+      {
         goal.command.max_effort = trajectory.joint_trajectory.points[tpoint].effort[idx];
+      }
+      else
+      {
+        goal.command.max_effort = max_effort_;
+      }
     }
 
     controller_action_client_->sendGoal(goal,
@@ -204,6 +210,12 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
    */
   bool parallel_jaw_gripper_;
 
+  /*
+   * The ``max_effort`` used in the GripperCommand message when no ``max_effort`` was
+   * specified in the requested trajectory. Defaults to ``0.0``.
+   */
+  double max_effort_;
+
   /*
    * A GripperCommand message has only a single float64 for the
    * "command", thus only a single joint angle can be sent -- however,
diff --git a/moveit_plugins/moveit_simple_controller_manager/package.xml b/moveit_plugins/moveit_simple_controller_manager/package.xml
index da405230c6..5fef0b8e79 100644
--- a/moveit_plugins/moveit_simple_controller_manager/package.xml
+++ b/moveit_plugins/moveit_simple_controller_manager/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_simple_controller_manager</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>A generic, simple controller manager plugin for MoveIt.</description>
 
   <author email="mferguson@fetchrobotics.com">Michael Ferguson</author>
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 d8055bd79b..a33f36681c 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
@@ -94,7 +94,10 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
         ActionBasedControllerHandleBasePtr new_handle;
         if (type == "GripperCommand")
         {
-          new_handle.reset(new GripperControllerHandle(name, action_ns));
+          const double max_effort =
+              controller_list[i].hasMember("max_effort") ? double(controller_list[i]["max_effort"]) : 0.0;
+
+          new_handle = std::make_shared<GripperControllerHandle>(name, action_ns, max_effort);
           if (static_cast<GripperControllerHandle*>(new_handle.get())->isConnected())
           {
             if (controller_list[i].hasMember("parallel"))
diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst
index fba9919806..f76c7649ac 100644
--- a/moveit_ros/benchmarks/CHANGELOG.rst
+++ b/moveit_ros/benchmarks/CHANGELOG.rst
@@ -2,6 +2,23 @@
 Changelog for package moveit_ros_benchmarks
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+* Provide ``MOVEIT_VERSION_CHECK`` macro (`#2997 <https://github.com/ros-planning/moveit/issues/2997>`_)
+* Contributors: Robert Haschke
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Adapt repository for splitted moveit_resources layout (`#2199 <https://github.com/ros-planning/moveit/issues/2199>`_)
diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt
index 5c2bf96808..d056e83287 100644
--- a/moveit_ros/benchmarks/CMakeLists.txt
+++ b/moveit_ros/benchmarks/CMakeLists.txt
@@ -58,7 +58,8 @@ install(
 
 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/examples/demo_obstacles.yaml b/moveit_ros/benchmarks/examples/demo_obstacles.yaml
index 70c7604e03..9ac65e96d3 100644
--- a/moveit_ros/benchmarks/examples/demo_obstacles.yaml
+++ b/moveit_ros/benchmarks/examples/demo_obstacles.yaml
@@ -30,4 +30,3 @@ benchmark_config:
         - plugin: stomp_moveit/StompPlannerManager
           planners:
             - STOMP
-
diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch b/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch
index 5ffdc5b3f1..281e80a370 100644
--- a/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch
+++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners.launch
@@ -28,4 +28,3 @@
     <rosparam command="load" file="$(arg bench_opts)"/>
   </node>
 </launch>
-
diff --git a/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml b/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml
index 00ac5066e6..eac2c21acb 100644
--- a/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml
+++ b/moveit_ros/benchmarks/examples/demo_panda_all_planners.yaml
@@ -30,4 +30,3 @@ benchmark_config:
         - plugin: stomp_moveit/StompPlannerManager
           planners:
             - STOMP
-
diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml
index c0a9614cb3..8318607fc2 100644
--- a/moveit_ros/benchmarks/package.xml
+++ b/moveit_ros/benchmarks/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_benchmarks</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
 
   <description>Enhanced tools for benchmarks in MoveIt!</description>
 
diff --git a/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py b/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py
index 50896a037d..6723858e17 100755
--- a/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py
+++ b/moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py
@@ -41,7 +41,8 @@
 import sqlite3
 import datetime
 import matplotlib
-matplotlib.use('pdf')
+
+matplotlib.use("pdf")
 from matplotlib import __version__ as matplotlibversion
 from matplotlib.backends.backend_pdf import PdfPages
 import matplotlib.pyplot as plt
@@ -52,7 +53,7 @@
 # Given a text line, split it into tokens (by space) and return the token
 # at the desired index. Additionally, test that some expected tokens exist.
 # Return None if they do not.
-def readLogValue(filevar, desired_token_index, expected_tokens) :
+def readLogValue(filevar, desired_token_index, expected_tokens):
     start_pos = filevar.tell()
     tokens = filevar.readline().split()
     for token_index in expected_tokens:
@@ -62,40 +63,45 @@ def readLogValue(filevar, desired_token_index, expected_tokens) :
             return None
     return tokens[desired_token_index]
 
-def readOptionalLogValue(filevar, desired_token_index, expected_tokens = {}) :
+
+def readOptionalLogValue(filevar, desired_token_index, expected_tokens={}):
     return readLogValue(filevar, desired_token_index, expected_tokens)
 
-def readRequiredLogValue(name, filevar, desired_token_index, expected_tokens = {}) :
+
+def readRequiredLogValue(name, filevar, desired_token_index, expected_tokens={}):
     result = readLogValue(filevar, desired_token_index, expected_tokens)
     if result == None:
         raise Exception("Unable to read " + name)
     return result
 
+
 def ensurePrefix(line, prefix):
     if not line.startswith(prefix):
         raise Exception("Expected prefix " + prefix + " was not found")
     return line
 
+
 def readOptionalMultilineValue(filevar):
     start_pos = filevar.tell()
     line = filevar.readline()
     if not line.startswith("<<<|"):
         filevar.seek(start_pos)
         return None
-    value = ''
+    value = ""
     line = filevar.readline()
-    while not line.startswith('|>>>'):
+    while not line.startswith("|>>>"):
         value = value + line
         line = filevar.readline()
         if line == None:
             raise Exception("Expected token |>>> missing")
     return value
 
+
 def readRequiredMultilineValue(filevar):
     ensurePrefix(filevar.readline(), "<<<|")
-    value = ''
+    value = ""
     line = filevar.readline()
-    while not line.startswith('|>>>'):
+    while not line.startswith("|>>>"):
         value = value + line
         line = filevar.readline()
         if line == None:
@@ -108,10 +114,11 @@ def readBenchmarkLog(dbname, filenames):
 
     conn = sqlite3.connect(dbname)
     c = conn.cursor()
-    c.execute('PRAGMA FOREIGN_KEYS = ON')
+    c.execute("PRAGMA FOREIGN_KEYS = ON")
 
     # create all tables if they don't already exist
-    c.executescript("""CREATE TABLE IF NOT EXISTS experiments
+    c.executescript(
+        """CREATE TABLE IF NOT EXISTS experiments
         (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,
@@ -128,117 +135,187 @@ def readBenchmarkLog(dbname, filenames):
         FOREIGN KEY (plannerid) REFERENCES plannerConfigs(id) ON DELETE CASCADE);
         CREATE TABLE IF NOT EXISTS progress
         (runid INTEGER, time REAL, PRIMARY KEY (runid, time),
-        FOREIGN KEY (runid) REFERENCES runs(id) ON DELETE CASCADE)""")
+        FOREIGN KEY (runid) REFERENCES runs(id) ON DELETE CASCADE)"""
+    )
 
     # 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": ""
+        "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()))
+        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')
+        print("Processing " + filename)
+        logfile = open(filename, "r")
         start_pos = logfile.tell()
-        libname = readOptionalLogValue(logfile, 0, {1 : "version"})
+        libname = readOptionalLogValue(logfile, 0, {1: "version"})
         if libname == None:
             libname = "OMPL"
         logfile.seek(start_pos)
-        version = readOptionalLogValue(logfile, -1, {1 : "version"})
+        version = readOptionalLogValue(logfile, -1, {1: "version"})
         if version == None:
             # set the version number to make Planner Arena happy
             version = "0.0.0"
-        version = ' '.join([libname, version])
-        expname = readRequiredLogValue("experiment name", logfile, -1, {0 : "Experiment"})
-        hostname = readRequiredLogValue("hostname", logfile, -1, {0 : "Running"})
-        date = ' '.join(ensurePrefix(logfile.readline(), "Starting").split()[2:])
+        version = " ".join([libname, version])
+        expname = readRequiredLogValue(
+            "experiment name", logfile, -1, {0: "Experiment"}
+        )
+        hostname = readRequiredLogValue("hostname", logfile, -1, {0: "Running"})
+        date = " ".join(ensurePrefix(logfile.readline(), "Starting").split()[2:])
         expsetup = readRequiredMultilineValue(logfile)
         cpuinfo = readOptionalMultilineValue(logfile)
-        rseed = int(readRequiredLogValue("random seed", logfile, 0, {-2 : "random", -1 : "seed"}))
-        timelimit = float(readRequiredLogValue("time limit", logfile, 0, {-3 : "seconds", -2 : "per", -1 : "run"}))
-        memorylimit = float(readRequiredLogValue("memory limit", logfile, 0, {-3 : "MB", -2 : "per", -1 : "run"}))
-        nrrunsOrNone = readOptionalLogValue(logfile, 0, {-3 : "runs", -2 : "per", -1 : "planner"})
+        rseed = int(
+            readRequiredLogValue("random seed", logfile, 0, {-2: "random", -1: "seed"})
+        )
+        timelimit = float(
+            readRequiredLogValue(
+                "time limit", logfile, 0, {-3: "seconds", -2: "per", -1: "run"}
+            )
+        )
+        memorylimit = float(
+            readRequiredLogValue(
+                "memory limit", logfile, 0, {-3: "MB", -2: "per", -1: "run"}
+            )
+        )
+        nrrunsOrNone = readOptionalLogValue(
+            logfile, 0, {-3: "runs", -2: "per", -1: "planner"}
+        )
         nrruns = -1
         if nrrunsOrNone != None:
             nrruns = int(nrrunsOrNone)
-            allExperimentsValues['runcount'] += nrruns
-        totaltime = float(readRequiredLogValue("total time", logfile, 0, {-3 : "collect", -2 : "the", -1 : "data"}))
+            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)
+        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
+        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"})
+        numEnumsOrNone = readOptionalLogValue(logfile, 0, {-2: "enum"})
         if numEnumsOrNone != None:
             numEnums = int(numEnumsOrNone)
         for i in range(numEnums):
-            enum = logfile.readline()[:-1].split('|')
+            enum = logfile.readline()[:-1].split("|")
             c.execute('SELECT * FROM enums WHERE name IS "%s"' % enum[0])
             if c.fetchone() == None:
-                for j in range(len(enum)-1):
-                    c.execute('INSERT INTO enums VALUES (?,?,?)',
-                        (enum[0],j,enum[j+1]))
-        c.execute('INSERT INTO experiments VALUES (?,?,?,?,?,?,?,?,?,?,?,?)',
-              (None, expname, totaltime, timelimit, memorylimit, nrruns,
-              version, hostname, cpuinfo, date, rseed, expsetup) )
+                for j in range(len(enum) - 1):
+                    c.execute(
+                        "INSERT INTO enums VALUES (?,?,?)", (enum[0], j, enum[j + 1])
+                    )
+        c.execute(
+            "INSERT INTO experiments VALUES (?,?,?,?,?,?,?,?,?,?,?,?)",
+            (
+                None,
+                expname,
+                totaltime,
+                timelimit,
+                memorylimit,
+                nrruns,
+                version,
+                hostname,
+                cpuinfo,
+                date,
+                rseed,
+                expsetup,
+            ),
+        )
         experimentId = c.lastrowid
-        numPlanners = int(readRequiredLogValue("planner count", logfile, 0, {-1 : "planners"}))
+        numPlanners = int(
+            readRequiredLogValue("planner count", logfile, 0, {-1: "planners"})
+        )
         for i in range(numPlanners):
             plannerName = logfile.readline()[:-1]
-            print('Parsing data for ' + plannerName)
+            print("Parsing data for " + plannerName)
 
             # read common data for planner
             numCommon = int(logfile.readline().split()[0])
-            settings = ''
+            settings = ""
             for j in range(numCommon):
-                settings = settings + logfile.readline() + ';'
+                settings = settings + logfile.readline() + ";"
 
             # find planner id
-            c.execute('SELECT id FROM plannerConfigs WHERE (name=? AND settings=?)',
-                (plannerName, settings,))
+            c.execute(
+                "SELECT id FROM plannerConfigs WHERE (name=? AND settings=?)",
+                (
+                    plannerName,
+                    settings,
+                ),
+            )
             p = c.fetchone()
-            if p==None:
-                c.execute('INSERT INTO plannerConfigs VALUES (?,?,?)',
-                    (None, plannerName, settings,))
+            if p == None:
+                c.execute(
+                    "INSERT INTO plannerConfigs VALUES (?,?,?)",
+                    (
+                        None,
+                        plannerName,
+                        settings,
+                    ),
+                )
                 plannerId = c.lastrowid
             else:
                 plannerId = p[0]
 
             # get current column names
-            c.execute('PRAGMA table_info(runs)')
+            c.execute("PRAGMA table_info(runs)")
             columnNames = [col[1] for col in c.fetchall()]
 
             # read properties and add columns as necessary
             numProperties = int(logfile.readline().split()[0])
-            propertyNames = ['experimentid', 'plannerid']
+            propertyNames = ["experimentid", "plannerid"]
             for j in range(numProperties):
                 field = logfile.readline().split()
                 propertyType = field[-1]
-                propertyName = '_'.join(field[:-1])
+                propertyName = "_".join(field[:-1])
                 if propertyName not in columnNames:
-                    c.execute('ALTER TABLE runs ADD %s %s' % (propertyName, propertyType))
+                    c.execute(
+                        "ALTER TABLE runs ADD %s %s" % (propertyName, propertyType)
+                    )
                 propertyNames.append(propertyName)
             # read measurements
-            insertFmtStr = 'INSERT INTO runs (' + ','.join(propertyNames) + \
-                ') VALUES (' + ','.join('?'*len(propertyNames)) + ')'
+            insertFmtStr = (
+                "INSERT INTO runs ("
+                + ",".join(propertyNames)
+                + ") VALUES ("
+                + ",".join("?" * len(propertyNames))
+                + ")"
+            )
             numRuns = int(logfile.readline().split()[0])
             runIds = []
             for j in range(numRuns):
-                runValues = [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
@@ -252,37 +329,49 @@ def readBenchmarkLog(dbname, filenames):
             nextLine = logfile.readline().strip()
 
             # read planner progress data if it's supplied
-            if nextLine != '.':
+            if nextLine != ".":
                 # get current column names
-                c.execute('PRAGMA table_info(progress)')
+                c.execute("PRAGMA table_info(progress)")
                 columnNames = [col[1] for col in c.fetchall()]
 
                 # read progress properties and add columns as necesary
                 numProgressProperties = int(nextLine.split()[0])
-                progressPropertyNames = ['runid']
+                progressPropertyNames = ["runid"]
                 for i in range(numProgressProperties):
                     field = logfile.readline().split()
                     progressPropertyType = field[-1]
                     progressPropertyName = "_".join(field[:-1])
                     if progressPropertyName not in columnNames:
-                        c.execute('ALTER TABLE progress ADD %s %s' %
-                            (progressPropertyName, progressPropertyType))
+                        c.execute(
+                            "ALTER TABLE progress ADD %s %s"
+                            % (progressPropertyName, progressPropertyType)
+                        )
                     progressPropertyNames.append(progressPropertyName)
                 # read progress measurements
-                insertFmtStr = 'INSERT INTO progress (' + \
-                    ','.join(progressPropertyNames) + ') VALUES (' + \
-                    ','.join('?'*len(progressPropertyNames)) + ')'
+                insertFmtStr = (
+                    "INSERT INTO progress ("
+                    + ",".join(progressPropertyNames)
+                    + ") VALUES ("
+                    + ",".join("?" * len(progressPropertyNames))
+                    + ")"
+                )
                 numRuns = int(logfile.readline().split()[0])
                 for j in range(numRuns):
-                    dataSeries = logfile.readline().split(';')[:-1]
+                    dataSeries = logfile.readline().split(";")[:-1]
                     for dataSample in dataSeries:
-                        values = tuple([runIds[j]] + \
-                            [None if len(x) == 0 or x == 'nan' or x == 'inf' else x
-                            for x in dataSample.split(',')[:-1]])
+                        values = tuple(
+                            [runIds[j]]
+                            + [
+                                None if len(x) == 0 or x == "nan" or x == "inf" else x
+                                for x in dataSample.split(",")[:-1]
+                            ]
+                        )
                         try:
                             c.execute(insertFmtStr, values)
                         except sqlite3.IntegrityError:
-                            print('Ignoring duplicate progress data. Consider increasing ompl::tools::Benchmark::Request::timeBetweenUpdates.')
+                            print(
+                                "Ignoring duplicate progress data. Consider increasing ompl::tools::Benchmark::Request::timeBetweenUpdates."
+                            )
                             pass
 
                 logfile.readline()
@@ -299,125 +388,158 @@ def readBenchmarkLog(dbname, filenames):
     conn.commit()
     c.close()
 
+
 def plotAttribute(cur, planners, attribute, typename):
     """Create a plot for a particular attribute. It will include data for
     all planners that have data for this attribute."""
     labels = []
     measurements = []
     nanCounts = []
-    if typename == 'ENUM':
+    if typename == "ENUM":
         cur.execute('SELECT description FROM enums where name IS "%s"' % attribute)
-        descriptions = [ t[0] for t in cur.fetchall() ]
+        descriptions = [t[0] for t in cur.fetchall()]
         numValues = len(descriptions)
     for planner in planners:
-        cur.execute('SELECT %s FROM runs WHERE plannerid = %s AND %s IS NOT NULL' \
-            % (attribute, planner[0], attribute))
-        measurement = [ t[0] for t in cur.fetchall() if t[0] != None ]
+        cur.execute(
+            "SELECT %s FROM runs WHERE plannerid = %s AND %s IS NOT NULL"
+            % (attribute, planner[0], attribute)
+        )
+        measurement = [t[0] for t in cur.fetchall() if t[0] != None]
         if len(measurement) > 0:
-            cur.execute('SELECT count(*) FROM runs WHERE plannerid = %s AND %s IS NULL' \
-                % (planner[0], attribute))
+            cur.execute(
+                "SELECT count(*) FROM runs WHERE plannerid = %s AND %s IS NULL"
+                % (planner[0], attribute)
+            )
             nanCounts.append(cur.fetchone()[0])
             labels.append(planner[1])
-            if typename == 'ENUM':
-                scale = 100. / len(measurement)
-                measurements.append([measurement.count(i)*scale for i in range(numValues)])
+            if typename == "ENUM":
+                scale = 100.0 / len(measurement)
+                measurements.append(
+                    [measurement.count(i) * scale for i in range(numValues)]
+                )
             else:
                 measurements.append(measurement)
 
-    if len(measurements)==0:
+    if len(measurements) == 0:
         print('Skipping "%s": no available measurements' % attribute)
         return
 
     plt.clf()
     ax = plt.gca()
-    if typename == 'ENUM':
-        width = .5
+    if typename == "ENUM":
+        width = 0.5
         measurements = np.transpose(np.vstack(measurements))
         colsum = np.sum(measurements, axis=1)
         rows = np.where(colsum != 0)[0]
-        heights = np.zeros((1,measurements.shape[1]))
+        heights = np.zeros((1, measurements.shape[1]))
         ind = range(measurements.shape[1])
         legend_labels = []
         for i in rows:
-            plt.bar(ind, measurements[i], width, bottom=heights[0],
-                color=matplotlib.cm.hot(int(floor(i*256/numValues))),
-                label=descriptions[i])
+            plt.bar(
+                ind,
+                measurements[i],
+                width,
+                bottom=heights[0],
+                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, fontsize=8, ha='right')
-        ax.set_ylabel(attribute.replace('_',' ') + ' (%)')
+        xtickNames = plt.xticks(
+            [x + width / 2.0 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])
         props = matplotlib.font_manager.FontProperties()
-        props.set_size('small')
-        ax.legend(loc='center left', bbox_to_anchor=(1, 0.5), prop = props)
-    elif typename == 'BOOLEAN':
-        width = .5
-        measurementsPercentage = [sum(m) * 100. / len(m) for m in measurements]
+        props.set_size("small")
+        ax.legend(loc="center left", bbox_to_anchor=(1, 0.5), prop=props)
+    elif typename == "BOOLEAN":
+        width = 0.5
+        measurementsPercentage = [sum(m) * 100.0 / len(m) for m in measurements]
         ind = range(len(measurements))
         plt.bar(ind, measurementsPercentage, width)
-        ### uncommenting this line will remove the term 'kConfigDefault' from the labels for OMPL Solvers. 
+        ### 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
+        # labels = [l.replace('kConfigDefault', '') for l in labels]
+
+        xtickNames = plt.xticks(
+            [x + width / 2.0 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:
-        if int(matplotlibversion.split('.')[0])<1:
-            plt.boxplot(measurements, notch=0, sym='k+', vert=1, whis=1.5)
+        if int(matplotlibversion.split(".")[0]) < 1:
+            plt.boxplot(measurements, notch=0, sym="k+", vert=1, whis=1.5)
         else:
-            plt.boxplot(measurements, notch=0, sym='k+', vert=1, whis=1.5, bootstrap=1000)
-        ax.set_ylabel(attribute.replace('_',' '))
+            plt.boxplot(
+                measurements, notch=0, sym="k+", vert=1, whis=1.5, bootstrap=1000
+            )
+        ax.set_ylabel(attribute.replace("_", " "))
 
-        #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. 
+        # 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, fontsize=8, ha='right')
-        for tick in ax.xaxis.get_major_ticks(): # shrink the font size of the x tick labels
+        # labels = [l.replace('kConfigDefault', '') for l in labels]
+
+        xtickNames = plt.setp(ax, xticklabels=labels)
+        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', fontsize=12)
-    ax.yaxis.grid(True, linestyle='-', which='major', color='lightgrey', alpha=0.5)
-    if max(nanCounts)>0:
+        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", 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
+            x = i + width / 2 if typename == "BOOLEAN" else i + 1
         ### 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):
     """Plot data for a single planner progress attribute. Will create an
-average time-plot with error bars of the attribute over all runs for
-each planner."""
+    average time-plot with error bars of the attribute over all runs for
+    each planner."""
 
     import numpy.ma as ma
 
     plt.clf()
     ax = plt.gca()
-    ax.set_xlabel('time (s)')
-    ax.set_ylabel(attribute.replace('_',' '))
+    ax.set_xlabel("time (s)")
+    ax.set_ylabel(attribute.replace("_", " "))
     plannerNames = []
     for planner in planners:
-        cur.execute("""SELECT count(progress.%s) FROM progress INNER JOIN runs
+        cur.execute(
+            """SELECT count(progress.%s) FROM progress INNER JOIN runs
             ON progress.runid = runs.id AND runs.plannerid=%s
-            AND progress.%s IS NOT NULL""" \
-            % (attribute, planner[0], attribute))
+            AND progress.%s IS NOT NULL"""
+            % (attribute, planner[0], attribute)
+        )
         if cur.fetchone()[0] > 0:
             plannerNames.append(planner[1])
-            cur.execute("""SELECT DISTINCT progress.runid FROM progress INNER JOIN runs
-            WHERE progress.runid=runs.id AND runs.plannerid=?""", (planner[0],))
+            cur.execute(
+                """SELECT DISTINCT progress.runid FROM progress INNER JOIN runs
+            WHERE progress.runid=runs.id AND runs.plannerid=?""",
+                (planner[0],),
+            )
             runids = [t[0] for t in cur.fetchall()]
             timeTable = []
             dataTable = []
             for r in runids:
                 # Select data for given run
-                cur.execute('SELECT time, %s FROM progress WHERE runid = %s ORDER BY time' % (attribute,r))
+                cur.execute(
+                    "SELECT time, %s FROM progress WHERE runid = %s ORDER BY time"
+                    % (attribute, r)
+                )
                 (time, data) = zip(*(cur.fetchall()))
                 timeTable.append(time)
                 dataTable.append(data)
@@ -428,39 +550,50 @@ def plotProgressAttribute(cur, planners, attribute):
             fewestSamples = min(len(time[:]) for time in timeTable)
             times = np.array(timeTable[0][:fewestSamples])
             dataArrays = np.array([data[:fewestSamples] for data in dataTable])
-            filteredData = ma.masked_array(dataArrays, np.equal(dataArrays, None), dtype=float)
+            filteredData = ma.masked_array(
+                dataArrays, np.equal(dataArrays, None), dtype=float
+            )
 
             means = np.mean(filteredData, axis=0)
             stddevs = np.std(filteredData, axis=0, ddof=1)
 
             # plot average with error bars
-            plt.errorbar(times, means, yerr=2*stddevs, errorevery=max(1, len(times) // 20))
+            plt.errorbar(
+                times, means, yerr=2 * stddevs, errorevery=max(1, len(times) // 20)
+            )
             ax.legend(plannerNames)
-    if len(plannerNames)>0:
+    if len(plannerNames) > 0:
         plt.show()
     else:
         plt.clf()
 
+
 def plotStatistics(dbname, fname):
     """Create a PDF file with box plots for all attributes."""
     print("Generating plots...")
     conn = sqlite3.connect(dbname)
     c = conn.cursor()
-    c.execute('PRAGMA FOREIGN_KEYS = ON')
-    c.execute('SELECT id, name FROM plannerConfigs')
-    planners = [(t[0],t[1].replace('geometric_','').replace('control_',''))
-        for t in c.fetchall()]
-    c.execute('PRAGMA table_info(runs)')
+    c.execute("PRAGMA FOREIGN_KEYS = ON")
+    c.execute("SELECT id, name FROM plannerConfigs")
+    planners = [
+        (t[0], t[1].replace("geometric_", "").replace("control_", ""))
+        for t in c.fetchall()
+    ]
+    c.execute("PRAGMA table_info(runs)")
     colInfo = c.fetchall()[3:]
 
     pp = PdfPages(fname)
     for col in colInfo:
-        if col[2] == 'BOOLEAN' or col[2] == 'ENUM' or \
-           col[2] == 'INTEGER' or col[2] == 'REAL':
-           plotAttribute(c, planners, col[1], col[2])
-           pp.savefig(plt.gcf())
-
-    c.execute('PRAGMA table_info(progress)')
+        if (
+            col[2] == "BOOLEAN"
+            or col[2] == "ENUM"
+            or col[2] == "INTEGER"
+            or col[2] == "REAL"
+        ):
+            plotAttribute(c, planners, col[1], col[2])
+            pp.savefig(plt.gcf())
+
+    c.execute("PRAGMA table_info(progress)")
     colInfo = c.fetchall()[2:]
     for col in colInfo:
         plotProgressAttribute(c, planners, col[1])
@@ -472,34 +605,41 @@ def plotStatistics(dbname, fname):
     c.execute("""SELECT id, name, timelimit, memorylimit FROM experiments""")
     experiments = c.fetchall()
     for experiment in experiments:
-        c.execute("""SELECT count(*) FROM runs WHERE runs.experimentid = %d
-            GROUP BY runs.plannerid""" % experiment[0])
+        c.execute(
+            """SELECT count(*) FROM runs WHERE runs.experimentid = %d
+            GROUP BY runs.plannerid"""
+            % experiment[0]
+        )
         numRuns = [run[0] for run in c.fetchall()]
-        numRuns = numRuns[0] if len(set(numRuns)) == 1 else ','.join(numRuns)
+        numRuns = numRuns[0] if len(set(numRuns)) == 1 else ",".join(numRuns)
 
         plt.figtext(pagex, pagey, 'Experiment "%s"' % experiment[1])
-        plt.figtext(pagex, pagey-0.05, 'Number of averaged runs: %d' % numRuns)
-        plt.figtext(pagex, pagey-0.10, "Time limit per run: %g seconds" % experiment[2])
-        plt.figtext(pagex, pagey-0.15, "Memory limit per run: %g MB" % experiment[3])
+        plt.figtext(pagex, pagey - 0.05, "Number of averaged runs: %d" % numRuns)
+        plt.figtext(
+            pagex, pagey - 0.10, "Time limit per run: %g seconds" % experiment[2]
+        )
+        plt.figtext(pagex, pagey - 0.15, "Memory limit per run: %g MB" % experiment[3])
         pagey -= 0.22
     plt.show()
     pp.savefig(plt.gcf())
     pp.close()
 
+
 def saveAsMysql(dbname, mysqldump):
     # See http://stackoverflow.com/questions/1067060/perl-to-python
     import re
+
     print("Saving as MySQL dump file...")
 
     conn = sqlite3.connect(dbname)
-    mysqldump = open(mysqldump,'w')
+    mysqldump = open(mysqldump, "w")
 
     # make sure all tables are dropped in an order that keepd foreign keys valid
     c = conn.cursor()
     c.execute("SELECT name FROM sqlite_master WHERE type='table'")
-    table_names = [ str(t[0]) for t in c.fetchall() ]
+    table_names = [str(t[0]) for t in c.fetchall()]
     c.close()
-    last = ['experiments', 'planner_configs']
+    last = ["experiments", "planner_configs"]
     for table in table_names:
         if table.startswith("sqlite"):
             continue
@@ -511,44 +651,52 @@ def saveAsMysql(dbname, mysqldump):
 
     for line in conn.iterdump():
         process = False
-        for nope in ('BEGIN TRANSACTION','COMMIT',
-            'sqlite_sequence','CREATE UNIQUE INDEX', 'CREATE VIEW'):
-            if nope in line: break
+        for nope in (
+            "BEGIN TRANSACTION",
+            "COMMIT",
+            "sqlite_sequence",
+            "CREATE UNIQUE INDEX",
+            "CREATE VIEW",
+        ):
+            if nope in line:
+                break
         else:
             process = True
-        if not process: continue
+        if not process:
+            continue
         line = re.sub(r"[\n\r\t ]+", " ", line)
-        m = re.search('CREATE TABLE ([a-zA-Z0-9_]*)(.*)', line)
+        m = re.search("CREATE TABLE ([a-zA-Z0-9_]*)(.*)", line)
         if m:
             name, sub = m.groups()
-            sub = sub.replace('"','`')
-            line = '''CREATE TABLE IF NOT EXISTS %(name)s%(sub)s'''
+            sub = sub.replace('"', "`")
+            line = """CREATE TABLE IF NOT EXISTS %(name)s%(sub)s"""
             line = line % dict(name=name, sub=sub)
             # make sure we use an engine that supports foreign keys
             line = line.rstrip("\n\t ;") + " ENGINE = InnoDB;\n"
         else:
             m = re.search('INSERT INTO "([a-zA-Z0-9_]*)"(.*)', line)
             if m:
-                line = 'INSERT INTO %s%s\n' % m.groups()
-                line = line.replace('"', r'\"')
+                line = "INSERT INTO %s%s\n" % m.groups()
+                line = line.replace('"', r"\"")
                 line = line.replace('"', "'")
 
         line = re.sub(r"([^'])'t'(.)", "\\1THIS_IS_TRUE\\2", line)
-        line = line.replace('THIS_IS_TRUE', '1')
+        line = line.replace("THIS_IS_TRUE", "1")
         line = re.sub(r"([^'])'f'(.)", "\\1THIS_IS_FALSE\\2", line)
-        line = line.replace('THIS_IS_FALSE', '0')
-        line = line.replace('AUTOINCREMENT', 'AUTO_INCREMENT')
+        line = line.replace("THIS_IS_FALSE", "0")
+        line = line.replace("AUTOINCREMENT", "AUTO_INCREMENT")
         mysqldump.write(line)
     mysqldump.close()
 
+
 def computeViews(dbname):
     conn = sqlite3.connect(dbname)
     c = conn.cursor()
-    c.execute('PRAGMA FOREIGN_KEYS = ON')
-    c.execute('PRAGMA table_info(runs)')
+    c.execute("PRAGMA FOREIGN_KEYS = ON")
+    c.execute("PRAGMA table_info(runs)")
     # kinodynamic paths cannot be simplified (or least not easily),
     # so simplification_time may not exist as a database column
-    if 'simplification_time' in [col[1] for col in c.fetchall()]:
+    if "simplification_time" in [col[1] for col in c.fetchall()]:
         s0 = """SELECT plannerid, plannerConfigs.name AS plannerName, experimentid, solved, time + simplification_time AS total_time
             FROM plannerConfigs INNER JOIN experiments INNER JOIN runs
             ON plannerConfigs.id=runs.plannerid AND experiments.id=runs.experimentid"""
@@ -556,34 +704,68 @@ def computeViews(dbname):
         s0 = """SELECT plannerid, plannerConfigs.name AS plannerName, experimentid, solved, time AS total_time
             FROM plannerConfigs INNER JOIN experiments INNER JOIN runs
             ON plannerConfigs.id=runs.plannerid AND experiments.id=runs.experimentid"""
-    s1 = """SELECT plannerid, plannerName, experimentid, AVG(solved) AS avg_solved, AVG(total_time) AS avg_total_time
-        FROM (%s) GROUP BY plannerid, experimentid""" % s0
-    s2 = """SELECT plannerid, experimentid, MIN(avg_solved) AS avg_solved, avg_total_time
-        FROM (%s) GROUP BY plannerName, experimentid ORDER BY avg_solved DESC, avg_total_time ASC""" % s1
-    c.execute('DROP VIEW IF EXISTS bestPlannerConfigsPerExperiment')
-    c.execute('CREATE VIEW IF NOT EXISTS bestPlannerConfigsPerExperiment AS %s' % s2)
-
-    s1 = """SELECT plannerid, plannerName, AVG(solved) AS avg_solved, AVG(total_time) AS avg_total_time
-        FROM (%s) GROUP BY plannerid""" % s0
-    s2 = """SELECT plannerid, MIN(avg_solved) AS avg_solved, avg_total_time
-        FROM (%s) GROUP BY plannerName ORDER BY avg_solved DESC, avg_total_time ASC""" % s1
-    c.execute('DROP VIEW IF EXISTS bestPlannerConfigs')
-    c.execute('CREATE VIEW IF NOT EXISTS bestPlannerConfigs AS %s' % s2)
+    s1 = (
+        """SELECT plannerid, plannerName, experimentid, AVG(solved) AS avg_solved, AVG(total_time) AS avg_total_time
+        FROM (%s) GROUP BY plannerid, experimentid"""
+        % s0
+    )
+    s2 = (
+        """SELECT plannerid, experimentid, MIN(avg_solved) AS avg_solved, avg_total_time
+        FROM (%s) GROUP BY plannerName, experimentid ORDER BY avg_solved DESC, avg_total_time ASC"""
+        % s1
+    )
+    c.execute("DROP VIEW IF EXISTS bestPlannerConfigsPerExperiment")
+    c.execute("CREATE VIEW IF NOT EXISTS bestPlannerConfigsPerExperiment AS %s" % s2)
+
+    s1 = (
+        """SELECT plannerid, plannerName, AVG(solved) AS avg_solved, AVG(total_time) AS avg_total_time
+        FROM (%s) GROUP BY plannerid"""
+        % s0
+    )
+    s2 = (
+        """SELECT plannerid, MIN(avg_solved) AS avg_solved, avg_total_time
+        FROM (%s) GROUP BY plannerName ORDER BY avg_solved DESC, avg_total_time ASC"""
+        % s1
+    )
+    c.execute("DROP VIEW IF EXISTS bestPlannerConfigs")
+    c.execute("CREATE VIEW IF NOT EXISTS bestPlannerConfigs AS %s" % s2)
 
     conn.commit()
     c.close()
 
+
 if __name__ == "__main__":
     usage = """%prog [options] [<benchmark.log> ...]"""
     parser = OptionParser("A script to parse benchmarking results.\n" + usage)
-    parser.add_option("-d", "--database", dest="dbname", default="benchmark.db",
-        help="Filename of benchmark database [default: %default]")
-    parser.add_option("-v", "--view", action="store_true", dest="view", default=False,
-        help="Compute the views for best planner configurations")
-    parser.add_option("-p", "--plot", dest="plot", default=None,
-        help="Create a PDF of plots with the filename provided")
-    parser.add_option("-m", "--mysql", dest="mysqldb", default=None,
-        help="Save SQLite3 database as a MySQL dump file")
+    parser.add_option(
+        "-d",
+        "--database",
+        dest="dbname",
+        default="benchmark.db",
+        help="Filename of benchmark database [default: %default]",
+    )
+    parser.add_option(
+        "-v",
+        "--view",
+        action="store_true",
+        dest="view",
+        default=False,
+        help="Compute the views for best planner configurations",
+    )
+    parser.add_option(
+        "-p",
+        "--plot",
+        dest="plot",
+        default=None,
+        help="Create a PDF of plots with the filename provided",
+    )
+    parser.add_option(
+        "-m",
+        "--mysql",
+        dest="mysqldb",
+        default=None,
+        help="Save SQLite3 database as a MySQL dump file",
+    )
     (options, args) = parser.parse_args()
 
     if len(args) == 0:
@@ -602,4 +784,3 @@ def computeViews(dbname):
 
     if options.mysqldb:
         saveAsMysql(options.dbname, options.mysqldb)
-
diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp
index 2cf5b7aec7..e6df397765 100644
--- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp
+++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp
@@ -1069,7 +1069,7 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& brequest, const std:
     return;
   }
 
-  out << "MoveIt! version " << MOVEIT_VERSION << std::endl;
+  out << "MoveIt version " << MOVEIT_VERSION_STR << std::endl;
   out << "Experiment " << brequest.name << std::endl;
   out << "Running on " << hostname << std::endl;
   out << "Starting at " << start_time << std::endl;
diff --git a/moveit_ros/manipulation/CHANGELOG.rst b/moveit_ros/manipulation/CHANGELOG.rst
index ab82336de0..cd48a2d81b 100644
--- a/moveit_ros/manipulation/CHANGELOG.rst
+++ b/moveit_ros/manipulation/CHANGELOG.rst
@@ -2,6 +2,23 @@
 Changelog for package moveit_ros_manipulation
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: Felix von Drigalski
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Migrate to clang-format-10
diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml
index a7e6c70b53..89dd4ed44f 100644
--- a/moveit_ros/manipulation/package.xml
+++ b/moveit_ros/manipulation/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_manipulation</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Components of MoveIt! used for manipulation</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
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 1cb36b9142..eb3b13861c 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
@@ -56,9 +56,9 @@ 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)
   {
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 a68e9ed523..19556386e4 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
@@ -43,7 +43,7 @@
 
 namespace pick_place
 {
-MOVEIT_CLASS_FORWARD(ManipulationStage);
+MOVEIT_CLASS_FORWARD(ManipulationStage);  // Defines ManipulationStagePtr, ConstPtr, WeakPtr... etc
 
 class ManipulationStage
 {
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 7c9af7a3d2..3b53bcd290 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
@@ -49,7 +49,7 @@
 
 namespace pick_place
 {
-MOVEIT_CLASS_FORWARD(PickPlace);
+MOVEIT_CLASS_FORWARD(PickPlace);  // Defines PickPlacePtr, ConstPtr, WeakPtr... etc
 
 class PickPlacePlanBase
 {
@@ -93,7 +93,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 +102,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
 {
diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst
index 5347c78797..5f83f5feee 100644
--- a/moveit_ros/move_group/CHANGELOG.rst
+++ b/moveit_ros/move_group/CHANGELOG.rst
@@ -2,6 +2,33 @@
 Changelog for package moveit_ros_move_group
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+* Use moveit-resources@master (`#2951 <https://github.com/ros-planning/moveit/issues/2951>`_)
+
+  - Simplify launch files to use the test_environment.launch files from moveit_resources@master
+  - Provide compatibility to the Noetic-style configuration of (multiple) planning pipelines
+    Only a single pipeline can be used at a time, specified via the ~default_planning_pipeline parameter.
+* Contributors: Robert Haschke
+
+1.0.8 (2021-05-23)
+------------------
+* Fix missing isEmpty() check in compute_ik service (`#2544 <https://github.com/ros-planning/moveit/issues/2544>`_)
+* Contributors: Michael Görner
+
+1.0.7 (2020-11-20)
+------------------
+* [feature] Start new joint_state_publisher_gui on param use_gui (`#2257 <https://github.com/ros-planning/moveit/issues/2257>`_)
+* [feature] Let the max number of contacts be the amount of world objects + link models with geometry (`#2361 <https://github.com/ros-planning/moveit/issues/2361>`_)
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: Felix von Drigalski, Loy van Beek, Michael Görner, Yoan Mollard, v4hn
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Adapt repository for splitted moveit_resources layout (`#2199 <https://github.com/ros-planning/moveit/issues/2199>`_)
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 26d9c645ef..f46558a14b 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
@@ -53,7 +53,7 @@ enum MoveGroupState
   LOOK
 };
 
-MOVEIT_CLASS_FORWARD(MoveGroupCapability);
+MOVEIT_CLASS_FORWARD(MoveGroupCapability);  // Defines MoveGroupCapabilityPtr, ConstPtr, WeakPtr... etc
 
 class MoveGroupCapability
 {
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 8ba3150481..4ba3a33d1e 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
@@ -41,23 +41,23 @@
 
 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
diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml
index 1b86d2a6cb..8b6b938750 100644
--- a/moveit_ros/move_group/package.xml
+++ b/moveit_ros/move_group/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_move_group</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>The move_group node for MoveIt</description>
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="robot.moveit@gmail.com">Sachin Chitta</author>
diff --git a/moveit_ros/move_group/scripts/load_map b/moveit_ros/move_group/scripts/load_map
index c63803f33f..862e69c558 100755
--- a/moveit_ros/move_group/scripts/load_map
+++ b/moveit_ros/move_group/scripts/load_map
@@ -5,10 +5,10 @@ from moveit_msgs.srv import LoadMap
 
 filename = sys.argv[1]
 
-rospy.init_node('load_map', anonymous=True)
+rospy.init_node("load_map", anonymous=True)
 
-load_map_service = rospy.ServiceProxy('move_group/load_map', LoadMap)
+load_map_service = rospy.ServiceProxy("move_group/load_map", LoadMap)
 if load_map_service(filename):
-    rospy.loginfo('Succeeded')
+    rospy.loginfo("Succeeded")
 else:
-    rospy.loginfo('Failed')
+    rospy.loginfo("Failed")
diff --git a/moveit_ros/move_group/scripts/save_map b/moveit_ros/move_group/scripts/save_map
index e8bcc60457..d9c7511318 100755
--- a/moveit_ros/move_group/scripts/save_map
+++ b/moveit_ros/move_group/scripts/save_map
@@ -5,11 +5,10 @@ from moveit_msgs.srv import SaveMap
 
 filename = sys.argv[1]
 
-rospy.init_node('save_map', anonymous=True)
+rospy.init_node("save_map", anonymous=True)
 
-save_map_service = rospy.ServiceProxy('move_group/save_map', SaveMap)
+save_map_service = rospy.ServiceProxy("move_group/save_map", SaveMap)
 if save_map_service(filename):
-    rospy.loginfo('Succeeded')
+    rospy.loginfo("Succeeded")
 else:
-    rospy.loginfo('Failed')
-
+    rospy.loginfo("Failed")
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 c05a6e81f7..28c0ff7488 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
@@ -75,7 +75,10 @@ void MoveGroupKinematicsService::computeIK(moveit_msgs::PositionIKRequest& req,
   const robot_state::JointModelGroup* jmg = rs.getJointModelGroup(req.group_name);
   if (jmg)
   {
-    robot_state::robotStateMsgToRobotState(req.robot_state, rs);
+    if (!planning_scene::PlanningScene::isEmpty(req.robot_state))
+    {
+      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)
diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp
index 9ab89beef6..e913a1b915 100644
--- a/moveit_ros/move_group/src/move_group.cpp
+++ b/moveit_ros/move_group/src/move_group.cpp
@@ -131,7 +131,15 @@ class MoveGroupExe
     if (node_handle_.getParam("capabilities", capability_plugins))
     {
       boost::char_separator<char> sep(" ");
-      boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
+      boost::tokenizer<boost::char_separator<char>> tok(capability_plugins, sep);
+      capabilities.insert(tok.begin(), tok.end());
+    }
+
+    // add capabilities configured for default planning pipeline (Noetic pipeline definitions)
+    if (planning_interface::getConfigNodeHandle().getParam("capabilities", capability_plugins))
+    {
+      boost::char_separator<char> sep(" ");
+      boost::tokenizer<boost::char_separator<char>> tok(capability_plugins, sep);
       capabilities.insert(tok.begin(), tok.end());
     }
 
@@ -139,8 +147,8 @@ class MoveGroupExe
     if (node_handle_.getParam("disable_capabilities", capability_plugins))
     {
       boost::char_separator<char> sep(" ");
-      boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
-      for (boost::tokenizer<boost::char_separator<char> >::iterator cap_name = tok.begin(); cap_name != tok.end();
+      boost::tokenizer<boost::char_separator<char>> tok(capability_plugins, sep);
+      for (boost::tokenizer<boost::char_separator<char>>::iterator cap_name = tok.begin(); cap_name != tok.end();
            ++cap_name)
         capabilities.erase(*cap_name);
     }
@@ -174,7 +182,7 @@ class MoveGroupExe
 
   ros::NodeHandle node_handle_;
   MoveGroupContextPtr context_;
-  std::shared_ptr<pluginlib::ClassLoader<MoveGroupCapability> > capability_plugin_loader_;
+  std::shared_ptr<pluginlib::ClassLoader<MoveGroupCapability>> capability_plugin_loader_;
   std::vector<MoveGroupCapabilityPtr> capabilities_;
 };
 }  // namespace move_group
diff --git a/moveit_ros/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp
index a36325766b..1e2d721f3d 100644
--- a/moveit_ros/move_group/src/move_group_context.cpp
+++ b/moveit_ros/move_group/src/move_group_context.cpp
@@ -40,9 +40,10 @@
 #include <moveit/plan_execution/plan_execution.h>
 #include <moveit/plan_execution/plan_with_sensing.h>
 
-move_group::MoveGroupContext::MoveGroupContext(
-    const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, bool allow_trajectory_execution,
-    bool debug)
+namespace move_group
+{
+MoveGroupContext::MoveGroupContext(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                                   bool allow_trajectory_execution, bool debug)
   : planning_scene_monitor_(planning_scene_monitor)
   , allow_trajectory_execution_(allow_trajectory_execution)
   , debug_(debug)
@@ -67,7 +68,7 @@ move_group::MoveGroupContext::MoveGroupContext(
     planning_pipeline_->publishReceivedRequests(true);
 }
 
-move_group::MoveGroupContext::~MoveGroupContext()
+MoveGroupContext::~MoveGroupContext()
 {
   plan_with_sensing_.reset();
   plan_execution_.reset();
@@ -76,7 +77,7 @@ move_group::MoveGroupContext::~MoveGroupContext()
   planning_scene_monitor_.reset();
 }
 
-bool move_group::MoveGroupContext::status() const
+bool MoveGroupContext::status() const
 {
   const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline_->getPlannerManager();
   if (planner_interface)
@@ -91,3 +92,5 @@ bool move_group::MoveGroupContext::status() const
     return false;
   }
 }
+
+}  // namespace move_group
diff --git a/moveit_ros/move_group/test/test_cancel_before_plan_execution.py b/moveit_ros/move_group/test/test_cancel_before_plan_execution.py
index 801bbe26c4..b337eff115 100755
--- a/moveit_ros/move_group/test/test_cancel_before_plan_execution.py
+++ b/moveit_ros/move_group/test/test_cancel_before_plan_execution.py
@@ -5,14 +5,19 @@
 import unittest
 from actionlib import SimpleActionClient
 import moveit_commander
-from moveit_msgs.msg import MoveItErrorCodes, MoveGroupGoal, Constraints, JointConstraint, MoveGroupAction
+from moveit_msgs.msg import (
+    MoveItErrorCodes,
+    MoveGroupGoal,
+    Constraints,
+    JointConstraint,
+    MoveGroupAction,
+)
 
 
 class TestMoveActionCancelDrop(unittest.TestCase):
-
     def setUp(self):
         # create a action client of move group
-        self._move_client = SimpleActionClient('move_group', MoveGroupAction)
+        self._move_client = SimpleActionClient("move_group", MoveGroupAction)
         self._move_client.wait_for_server()
 
         moveit_commander.roscpp_initialize(sys.argv)
@@ -57,7 +62,10 @@ def test_cancel_drop_plan_execution(self):
 
         # check the error code in result
         # error code is 0 if the server ends with RECALLED status
-        self.assertTrue(result.error_code.val == MoveItErrorCodes.PREEMPTED or result.error_code.val == 0)
+        self.assertTrue(
+            result.error_code.val == MoveItErrorCodes.PREEMPTED
+            or result.error_code.val == 0
+        )
 
     def test_cancel_drop_plan_only(self):
         # set the plan only flag
@@ -80,7 +88,10 @@ def test_cancel_drop_plan_only(self):
 
         # check the error code in result
         # error code is 0 if the server ends with RECALLED status
-        self.assertTrue(result.error_code.val == MoveItErrorCodes.PREEMPTED or result.error_code.val == 0)
+        self.assertTrue(
+            result.error_code.val == MoveItErrorCodes.PREEMPTED
+            or result.error_code.val == 0
+        )
 
     def test_cancel_resend(self):
         # send the goal
@@ -105,7 +116,12 @@ def test_cancel_resend(self):
         self.assertEqual(result.error_code.val, MoveItErrorCodes.SUCCESS)
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     import rostest
-    rospy.init_node('cancel_before_plan_execution')
-    rostest.rosrun('moveit_ros_move_group', 'test_cancel_before_plan_execution', TestMoveActionCancelDrop)
+
+    rospy.init_node("cancel_before_plan_execution")
+    rostest.rosrun(
+        "moveit_ros_move_group",
+        "test_cancel_before_plan_execution",
+        TestMoveActionCancelDrop,
+    )
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
index 784a40ae3c..5dba11031f 100755
--- 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
@@ -10,35 +10,56 @@
 
 
 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)
+        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")
-
+        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
+        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())
+        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)")
+        self.assertNotEqual(
+            len(validity_report.contacts),
+            0,
+            "When the robot collides with itself, it should have some contacts (with itself)",
+        )
 
 
-if __name__ == '__main__':
+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)
+
+    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/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst
index f516514d7c..d584285c04 100644
--- a/moveit_ros/moveit_ros/CHANGELOG.rst
+++ b/moveit_ros/moveit_ros/CHANGELOG.rst
@@ -2,6 +2,21 @@
 Changelog for package moveit_ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+
 1.0.6 (2020-08-19)
 ------------------
 
diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml
index 9652939c77..908c4f8f13 100644
--- a/moveit_ros/moveit_ros/package.xml
+++ b/moveit_ros/moveit_ros/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Components of MoveIt! that use ROS</description>
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="robot.moveit@gmail.com">Sachin Chitta</author>
diff --git a/moveit_ros/moveit_servo/CHANGELOG.rst b/moveit_ros/moveit_servo/CHANGELOG.rst
index 81b06cc007..b7b2faee6b 100644
--- a/moveit_ros/moveit_servo/CHANGELOG.rst
+++ b/moveit_ros/moveit_servo/CHANGELOG.rst
@@ -2,6 +2,48 @@
 Changelog for package moveit_servo
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+* Fix an off-by-one error in servo_calcs.cpp (`#2908 <https://github.com/ros-planning/moveit/issues/2908>`_)
+* Contributors: Michael Görner
+
+1.0.8 (2021-05-23)
+------------------
+* Avoid joint jump when SuddenHalt() is called in velocity mode (`#2594 <https://github.com/ros-planning/moveit/issues/2594>`_)
+* Halt Servo command on Pose Tracking stop (`#2501 <https://github.com/ros-planning/moveit/issues/2501>`_)
+* stop_requested\_ flag clearing fix (`#2537 <https://github.com/ros-planning/moveit/issues/2537>`_)
+* Refactor Servo velocity bounds enforcement (`#2471 <https://github.com/ros-planning/moveit/issues/2471>`_)
+* Protect paused\_ flag, for thread safety (`#2494 <https://github.com/ros-planning/moveit/issues/2494>`_)
+* Do not break out of loop -- need to update low pass filters (`#2496 <https://github.com/ros-planning/moveit/issues/2496>`_)
+* Fix initial angle error is always 0 (`#2464 <https://github.com/ros-planning/moveit/issues/2464>`_)
+* Add an important sleep in Servo pose tracking (`#2463 <https://github.com/ros-planning/moveit/issues/2463>`_)
+* Prevent moveit_servo transforms between fixed frames from causing timeout (`#2418 <https://github.com/ros-planning/moveit/issues/2418>`_)
+* Low latency mode (`#2401 <https://github.com/ros-planning/moveit/issues/2401>`_)
+* Move timer initialization down to fix potential race condition
+* Fix pose tracking race condition (`#2395 <https://github.com/ros-planning/moveit/issues/2395>`_)
+* Contributors: AdamPettinger, AndyZe, Jere Liukkonen, Michael Görner, Nathan Brooks, Tyler Weaver, parunapu
+
+1.0.7 (2020-11-20)
+------------------
+* [feature] Cleanup current state handling in servo (`#2372 <https://github.com/ros-planning/moveit/issues/2372>`_)
+* [feature] Servo namespacing logic (`#2354 <https://github.com/ros-planning/moveit/issues/2354>`_)
+* [feature] A library for servoing toward a moving pose (`#2203 <https://github.com/ros-planning/moveit/issues/2203>`_)
+* [feature] Refactor velocity limit enforcement and add a unit test (`#2260 <https://github.com/ros-planning/moveit/issues/2260>`_)
+* [feature] Add a utility to print collision pairs (`#2275 <https://github.com/ros-planning/moveit/issues/2275>`_)
+* [feature] Update last_sent_command\_ at ServoCalcs start (`#2249 <https://github.com/ros-planning/moveit/issues/2249>`_)
+* [fix] Fix servo trajectory point timestamping (`#2375 <https://github.com/ros-planning/moveit/issues/2375>`_)
+* [fix] Fix ordering of windup args to control_toolbox::Pid (`#2370 <https://github.com/ros-planning/moveit/issues/2370>`_)
+* [fix] Fix Servo thread interruption (`#2314 <https://github.com/ros-planning/moveit/issues/2314>`_)
+* [fix] Servo heap-buffer-overflow bug (`#2307 <https://github.com/ros-planning/moveit/issues/2307>`_)
+* [maint] add soname version to moveit_servo (`#2266 <https://github.com/ros-planning/moveit/issues/2266>`_)
+* Contributors: AdamPettinger, AndyZe, Jere Liukkonen, Nathan Brooks, Robert Haschke, Tyler Weaver
+
 1.0.6 (2020-08-19)
 ------------------
 * [feature] A ROS service to reset the Servo status (`#2246 <https://github.com/ros-planning/moveit/issues/2246>`_)
diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt
index 239461754a..5228a8559e 100644
--- a/moveit_ros/moveit_servo/CMakeLists.txt
+++ b/moveit_ros/moveit_servo/CMakeLists.txt
@@ -7,16 +7,17 @@ endif()
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
 
-set(LIBRARY_NAME moveit_servo_cpp_api)
+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
@@ -24,17 +25,19 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   std_msgs
   std_srvs
+  tf2_eigen
   trajectory_msgs
 )
-find_package(Eigen3 REQUIRED)
 
 catkin_package(
   INCLUDE_DIRS
     include
   LIBRARIES
-    ${LIBRARY_NAME}
+    pose_tracking
+    ${SERVO_LIB_NAME}
   CATKIN_DEPENDS
     control_msgs
+    control_toolbox
     geometry_msgs
     moveit_msgs
     moveit_ros_planning_interface
@@ -42,6 +45,7 @@ catkin_package(
     sensor_msgs
     std_msgs
     std_srvs
+    tf2_eigen
     trajectory_msgs
   DEPENDS
     EIGEN3
@@ -54,34 +58,58 @@ include_directories(
   ${Boost_INCLUDE_DIR}
 )
 
-#########################################
-## A library providing a C++ interface ##
-#########################################
+#########################################################
+## Library to process realtime twist or joint commands ##
+#########################################################
 
-add_library(${LIBRARY_NAME} SHARED
+# 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/joint_state_subscriber.cpp
   src/low_pass_filter.cpp
 )
-add_dependencies(${LIBRARY_NAME} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${LIBRARY_NAME}
+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 using the C++ library
+# 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}
-  ${Eigen_LIBRARIES}
-  ${Boost_LIBRARIES}
-  ${LIBRARY_NAME}
+  ${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}
 )
 
 ############################
@@ -93,10 +121,8 @@ add_executable(servo_server
 )
 add_dependencies(servo_server ${catkin_EXPORTED_TARGETS})
 target_link_libraries(servo_server
-  ${LIBRARY_NAME}
+  ${SERVO_LIB_NAME}
   ${catkin_LIBRARIES}
-  ${Eigen_LIBRARIES}
-  ${Boost_LIBRARIES}
 )
 
 ################################################
@@ -115,7 +141,9 @@ target_link_libraries(spacenav_to_twist ${catkin_LIBRARIES})
 
 install(
   TARGETS
-    ${LIBRARY_NAME}
+    ${SERVO_LIB_NAME}
+    pose_tracking
+    pose_tracking_example
     servo_server
     spacenav_to_twist
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@@ -127,6 +155,9 @@ 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 ##
 #############
@@ -134,15 +165,33 @@ install(DIRECTORY include/${PROJECT_NAME}/
 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
-    ${LIBRARY_NAME}
+    ${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}
-    ${Eigen_LIBRARIES}
-    ${Boost_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
index e889244cb8..de63784416 100644
--- a/moveit_ros/moveit_servo/README.md
+++ b/moveit_ros/moveit_servo/README.md
@@ -55,4 +55,4 @@ If you see a warning about "close to singularity", try changing the direction of
 
 Run tests from the moveit\_servo folder:
 
-    catkin run_tests --no-deps --this
\ No newline at end of file
+    catkin run_tests --no-deps --this
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_ros/moveit_servo/config/ur_simulated_config.yaml b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml
index 5c477ffb70..21a2c6aecd 100644
--- a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml
+++ b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml
@@ -4,7 +4,6 @@
 use_gazebo: true # Whether the robot is started in a Gazebo simulation environment
 
 ## Properties of incoming commands
-robot_link_command_frame:  world  # commands must be given in the frame of a robot link. Usually either the base or end effector
 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"
@@ -16,6 +15,7 @@ low_pass_filter_coeff: 2.  # Larger --> trust the filtered data more, trust the
 
 ## 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)
@@ -31,6 +31,10 @@ publish_joint_accelerations: false
 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.
@@ -43,11 +47,11 @@ 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: 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: joint_group_position_controller/command # Publish outgoing commands here
+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?
@@ -55,7 +59,7 @@ collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if
 # 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
+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]
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h
index 30ddaff162..08aadaad46 100644
--- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h
+++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h
@@ -46,7 +46,6 @@
 
 #include <moveit_servo/servo_parameters.h>
 #include <moveit_servo/low_pass_filter.h>
-#include <moveit_servo/joint_state_subscriber.h>
 
 namespace moveit_servo
 {
@@ -65,12 +64,15 @@ class CollisionCheck
    *                                 already started when passed into this class
    */
   CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoParameters& parameters,
-                 const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
-                 const std::shared_ptr<JointStateSubscriber>& joint_state_subscriber);
+                 const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor);
 
-  /** \brief start and stop the Timer */
+  ~CollisionCheck()
+  {
+    timer_.stop();
+  }
+
+  /** \brief start the Timer that regulates collision check rate */
   void start();
-  void stop();
 
   /** \brief Pause or unpause processing servo commands while keeping the timers alive */
   void setPaused(bool paused);
@@ -79,9 +81,6 @@ class CollisionCheck
   /** \brief Run one iteration of collision checking */
   void run(const ros::TimerEvent& timer_event);
 
-  /** \brief Print objects in collision. Useful for debugging.  */
-  void printCollisionPairs(collision_detection::CollisionResult::ContactMap& contact_map);
-
   /** \brief Get a read-only copy of the planning scene */
   planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const;
 
@@ -96,11 +95,8 @@ class CollisionCheck
   // Pointer to the collision environment
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
 
-  // Subscriber to joint states
-  const std::shared_ptr<JointStateSubscriber> joint_state_subscriber_;
-
   // Robot state and collision matrix from planning scene
-  std::unique_ptr<moveit::core::RobotState> current_state_;
+  std::shared_ptr<moveit::core::RobotState> current_state_;
   collision_detection::AllowedCollisionMatrix acm_;
 
   // Scale robot velocity according to collision proximity and user-defined thresholds.
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h
deleted file mode 100644
index 64c55367bc..0000000000
--- a/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*******************************************************************************
- * Title     : joint_state_subscriber.h
- * Project   : moveit_servo
- * Created   : 06/11/2020
- * 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 <mutex>
-
-#include <ros/ros.h>
-#include <sensor_msgs/JointState.h>
-
-#include <moveit_servo/servo_parameters.h>
-
-namespace moveit_servo
-{
-class JointStateSubscriber
-{
-public:
-  /** \brief Constructor */
-  JointStateSubscriber(ros::NodeHandle& nh, const std::string& joint_state_topic_name);
-
-  /** \brief Get the latest joint state message */
-  sensor_msgs::JointStateConstPtr getLatest() const;
-
-private:
-  void jointStateCB(const sensor_msgs::JointStateConstPtr& msg);
-
-  ros::Subscriber joint_state_sub_;
-
-  // Latest joint state, updated by ROS callback
-  mutable std::mutex joint_state_mutex_;
-  sensor_msgs::JointStateConstPtr latest_joint_state_;
-};
-}  // 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
index a593da6127..d3931dad37 100644
--- 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
@@ -38,6 +38,7 @@
 
 #pragma once
 
+#include <boost/make_shared.hpp>
 #include <boost/pool/pool_alloc.hpp>
 
 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..d705938f91
--- /dev/null
+++ b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h
@@ -0,0 +1,193 @@
+/*********************************************************************
+ * 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 <atomic>
+#include <boost/optional.hpp>
+#include <control_toolbox/pid.h>
+#include <moveit_servo/make_shared_from_pool.h>
+#include <moveit_servo/servo.h>
+#include <rosparam_shortcuts/rosparam_shortcuts.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+
+// 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<PoseTrackingStatusCode, std::string> 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();
+
+  /** \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<moveit_servo::Servo> 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<control_toolbox::Pid>& 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<control_toolbox::Pid> cartesian_position_pids_;
+  std::vector<control_toolbox::Pid> 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<bool> stop_requested_;
+
+  boost::optional<double> angular_error_;
+};
+
+// using alias
+using PoseTrackingPtr = std::shared_ptr<PoseTracking>;
+}  // 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
index 5e9f26fe64..8258c72923 100644
--- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h
+++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h
@@ -38,12 +38,16 @@
 
 #pragma once
 
+// System
 #include <memory>
 
+// MoveIt
 #include <moveit_servo/collision_check.h>
 #include <moveit_servo/servo_parameters.h>
 #include <moveit_servo/servo_calcs.h>
-#include <moveit_servo/joint_state_subscriber.h>
+
+// testing
+#include <gtest/gtest.h>
 
 namespace moveit_servo
 {
@@ -60,9 +64,6 @@ class Servo
   /** \brief start servo node */
   void start();
 
-  /** \brief stop servo node */
-  void stop();
-
   /** \brief Pause or unpause processing servo commands while keeping the timers alive */
   void setPaused(bool paused);
 
@@ -74,12 +75,31 @@ class Servo
    * @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 Get the latest joint state. */
-  sensor_msgs::JointStateConstPtr getLatestJointState() 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();
@@ -92,7 +112,6 @@ class Servo
   // Store the parameters that were read from ROS server
   ServoParameters parameters_;
 
-  std::shared_ptr<JointStateSubscriber> joint_state_subscriber_;
   std::unique_ptr<ServoCalcs> servo_calcs_;
   std::unique_ptr<CollisionCheck> collision_checker_;
 };
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h
index c67afb6e0c..dddf2ef8bc 100644
--- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h
+++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h
@@ -38,7 +38,16 @@
 
 #pragma once
 
+// C++
+#include <condition_variable>
+#include <mutex>
+#include <thread>
+#include <atomic>
+
 // ROS
+#include <control_msgs/JointJog.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/TransformStamped.h>
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <moveit/robot_model_loader/robot_model_loader.h>
 #include <moveit_msgs/ChangeDriftDimensions.h>
@@ -47,28 +56,26 @@
 #include <std_msgs/Float64.h>
 #include <std_msgs/Int8.h>
 #include <std_srvs/Empty.h>
-#include <control_msgs/JointJog.h>
-#include <geometry_msgs/TwistStamped.h>
+#include <tf2_eigen/tf2_eigen.h>
 #include <trajectory_msgs/JointTrajectory.h>
 
 // moveit_servo
 #include <moveit_servo/servo_parameters.h>
 #include <moveit_servo/status_codes.h>
 #include <moveit_servo/low_pass_filter.h>
-#include <moveit_servo/joint_state_subscriber.h>
 
 namespace moveit_servo
 {
 class ServoCalcs
 {
 public:
-  ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters,
-             const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
-             const std::shared_ptr<JointStateSubscriber>& joint_state_subscriber);
+  ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters,
+             const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor);
+
+  ~ServoCalcs();
 
-  /** \brief Start and stop the timer where we do work and publish outputs */
+  /** \brief Start the timer where we do work and publish outputs */
   void start();
-  void stop();
 
   /**
    * Get the MoveIt planning link transform.
@@ -78,13 +85,38 @@ class ServoCalcs
    * @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 Timer method */
-  void run(const ros::TimerEvent& timer_event);
+  /** \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);
@@ -93,7 +125,7 @@ class ServoCalcs
   bool jointServoCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory);
 
   /** \brief Parse the incoming joint msg for the joints of our MoveGroup */
-  bool updateJoints();
+  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.
@@ -113,10 +145,10 @@ class ServoCalcs
   void suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory);
 
   /** \brief  Scale the delta theta to match joint velocity/acceleration limits */
-  void enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta);
+  void enforceVelLimits(Eigen::ArrayXd& delta_theta);
 
   /** \brief Avoid overshooting joint limits */
-  bool enforceSRDFPositionLimits();
+  bool enforcePositionLimits();
 
   /** \brief Possibly calculate a velocity scaling factor, due to proximity of
    * singularity and direction of motion
@@ -193,14 +225,11 @@ class ServoCalcs
   ros::NodeHandle nh_;
 
   // Parameters from yaml
-  const ServoParameters& parameters_;
+  ServoParameters& parameters_;
 
   // Pointer to the collision environment
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
 
-  // Subscriber to the latest joint states
-  const std::shared_ptr<JointStateSubscriber> joint_state_subscriber_;
-
   // Track the number of cycles during which motion has not occurred.
   // Will avoid re-publishing zero velocities endlessly.
   int zero_velocity_count_ = 0;
@@ -222,7 +251,7 @@ class ServoCalcs
 
   const moveit::core::JointModelGroup* joint_model_group_;
 
-  moveit::core::RobotStatePtr kinematic_state_;
+  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)
@@ -237,8 +266,6 @@ class ServoCalcs
   trajectory_msgs::JointTrajectoryConstPtr last_sent_command_;
 
   // ROS
-  ros::Timer timer_;
-  ros::Duration period_;
   ros::Subscriber joint_state_sub_;
   ros::Subscriber twist_stamped_sub_;
   ros::Subscriber joint_cmd_sub_;
@@ -250,10 +277,13 @@ class ServoCalcs
   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;
-  bool stop_requested_ = false;
-  bool paused_ = false;
+  std::atomic<bool> paused_;
   bool twist_command_is_stale_ = false;
   bool joint_command_is_stale_ = false;
   bool ok_to_publish_ = false;
@@ -273,17 +303,19 @@ class ServoCalcs
   // The dimesions to control. In the command frame. [x, y, z, roll, pitch, yaw]
   std::array<bool, 6> control_dimensions_ = { { true, true, true, true, true, true } };
 
-  // Amount we sleep when waiting
-  ros::Rate default_sleep_rate_ = 100;
-
-  // latest_state_mutex_ is used to protect the state below it
-  mutable std::mutex latest_state_mutex_;
+  // 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
index ad0deec16f..14d993cc71 100644
--- a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h
+++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h
@@ -52,6 +52,7 @@ struct ServoParameters
   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;
@@ -70,6 +71,7 @@ struct ServoParameters
   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;
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h
index 97e92210cc..5164f46a9e 100644
--- a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h
+++ b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h
@@ -43,7 +43,7 @@
 
 namespace moveit_servo
 {
-enum StatusCode : int8_t
+enum class StatusCode : int8_t
 {
   INVALID = -1,
   NO_WARNING = 0,
@@ -54,12 +54,12 @@ enum StatusCode : int8_t
   JOINT_BOUND = 5
 };
 
-const std::unordered_map<int8_t, std::string>
-    SERVO_STATUS_CODE_MAP({ { INVALID, "Invalid" },
-                            { NO_WARNING, "No warnings" },
-                            { DECELERATE_FOR_SINGULARITY, "Close to a singularity, decelerating" },
-                            { HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" },
-                            { DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" },
-                            { HALT_FOR_COLLISION, "Collision detected, emergency stop" },
-                            { JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } });
+const std::unordered_map<StatusCode, std::string>
+    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
index c2ac9b8748..7498446ff8 100644
--- a/moveit_ros/moveit_servo/launch/cpp_interface_example.launch
+++ b/moveit_ros/moveit_servo/launch/cpp_interface_example.launch
@@ -2,8 +2,8 @@
   <!-- Launch an example that sends commands via C++ API. -->
 
   <node name="servo_server" pkg="moveit_servo" type="cpp_interface_example" output="screen" >
-    <param name="parameter_ns" type="string" value="servo_server" />
-    <rosparam command="load" file="$(find moveit_servo)/config/ur_simulated_config.yaml" />
+    <param name="parameter_ns" type="string" value="optional_parameter_namespace" />
+    <rosparam ns="optional_parameter_namespace" command="load" file="$(find moveit_servo)/config/ur_simulated_config.yaml" />
   </node>
 
 </launch>
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 @@
+<launch>
+  <!-- Launch an example that sends commands via C++ API. -->
+
+  <node name="servo_server" pkg="moveit_servo" type="pose_tracking_example" output="screen" >
+    <!-- <param name="parameter_ns" type="string" value="servo_server" /> -->
+    <rosparam command="load" file="$(find moveit_servo)/config/ur_simulated_config.yaml" />
+    <rosparam command="load" file="$(find moveit_servo)/config/pose_tracking_settings.yaml" />
+  </node>
+
+</launch>
diff --git a/moveit_ros/moveit_servo/launch/spacenav_cpp.launch b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch
index 41d81862db..b60db1fa0e 100644
--- a/moveit_ros/moveit_servo/launch/spacenav_cpp.launch
+++ b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch
@@ -13,8 +13,8 @@
   <node name="spacenav_to_twist" pkg="moveit_servo" type="spacenav_to_twist" output="screen" />
 
   <node name="servo_server" pkg="moveit_servo" type="servo_server" output="screen" >
-    <param name="parameter_ns" type="string" value="servo_server" />
-    <rosparam command="load" file="$(find moveit_servo)/config/ur_simulated_config.yaml" />
+    <param name="parameter_ns" type="string" value="optional_parameter_namespace" />
+    <rosparam ns="optional_parameter_namespace" command="load" file="$(find moveit_servo)/config/ur_simulated_config.yaml" />
   </node>
 
 </launch>
diff --git a/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch b/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch
index 93847e35a1..e3834b5e16 100644
--- a/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch
+++ b/moveit_ros/moveit_servo/launch/spacenav_teleop_tools.launch
@@ -10,8 +10,8 @@
 
   <!-- This node does the servoing calculations -->
   <node name="servo_server" pkg="moveit_servo" type="servo_server" output="screen" >
-    <param name="parameter_ns" type="string" value="servo_server" />
-    <rosparam command="load" file="$(find moveit_servo)/config/ur_simulated_config.yaml" />
+    <param name="parameter_ns" type="string" value="optional_parameter_namespace" />
+    <rosparam ns="optional_parameter_namespace" command="load" file="$(find moveit_servo)/config/ur_simulated_config.yaml" />
   </node>
 
   <!-- teleop_tools translates a joystick command into a twist message -->
diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml
index cbbc9c6701..0170c72593 100644
--- a/moveit_ros/moveit_servo/package.xml
+++ b/moveit_ros/moveit_servo/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit_servo</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
 
   <description>Provides real-time manipulator Cartesian and joint servoing.</description>
 
@@ -21,14 +21,16 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>control_msgs</depend>
+  <depend>control_toolbox</depend>
   <depend>geometry_msgs</depend>
+  <depend>moveit_msgs</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>rosparam_shortcuts</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>tf2_eigen</depend>
   <depend>trajectory_msgs</depend>
-  <depend>moveit_msgs</depend>
-  <depend>moveit_ros_planning_interface</depend>
-  <depend>rosparam_shortcuts</depend>
 
   <exec_depend>joy_teleop</exec_depend>
   <exec_depend>spacenav_node</exec_depend>
diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp
index 122ced12f1..44b89271d2 100644
--- a/moveit_ros/moveit_servo/src/collision_check.cpp
+++ b/moveit_ros/moveit_servo/src/collision_check.cpp
@@ -51,12 +51,10 @@ 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,
-                               const std::shared_ptr<JointStateSubscriber>& joint_state_subscriber)
+                               const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor)
   : nh_(nh)
   , parameters_(parameters)
   , planning_scene_monitor_(planning_scene_monitor)
-  , joint_state_subscriber_(joint_state_subscriber)
   , 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)
@@ -75,12 +73,12 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoPar
   safety_factor_ = parameters_.collision_distance_safety_factor;
 
   // Internal namespace
-  ros::NodeHandle internal_nh("~internal");
+  ros::NodeHandle internal_nh(nh_, "internal");
   collision_velocity_scale_pub_ = internal_nh.advertise<std_msgs::Float64>("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_ = std::make_unique<moveit::core::RobotState>(getLockedPlanningSceneRO()->getCurrentState());
+  current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState();
   acm_ = getLockedPlanningSceneRO()->getAllowedCollisionMatrix();
 }
 
@@ -94,11 +92,6 @@ void CollisionCheck::start()
   timer_ = nh_.createTimer(period_, &CollisionCheck::run, this);
 }
 
-void CollisionCheck::stop()
-{
-  timer_.stop();
-}
-
 void CollisionCheck::run(const ros::TimerEvent& timer_event)
 {
   // Log warning when the last loop duration was longer than the period
@@ -114,11 +107,8 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event)
     return;
   }
 
-  // Copy the latest joint state
-  auto latest_joint_state = joint_state_subscriber_->getLatest();
-  for (std::size_t i = 0; i < latest_joint_state->position.size(); ++i)
-    current_state_->setJointPositions(latest_joint_state->name[i], &latest_joint_state->position[i]);
-
+  // Update to the latest current state
+  current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState();
   current_state_->updateCollisionBodyTransforms();
   collision_detected_ = false;
 
@@ -132,7 +122,6 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event)
 
     scene_collision_distance_ = collision_result_.distance;
     collision_detected_ |= collision_result_.collision;
-    printCollisionPairs(collision_result_.contacts);
 
     collision_result_.clear();
     // Self-collisions and scene collisions are checked separately so different thresholds can be used
@@ -142,7 +131,7 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event)
 
   self_collision_distance_ = collision_result_.distance;
   collision_detected_ |= collision_result_.collision;
-  printCollisionPairs(collision_result_.contacts);
+  collision_result_.print();
 
   velocity_scale_ = 1;
   // If we're definitely in collision, stop immediately
@@ -213,25 +202,6 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event)
   }
 }
 
-void CollisionCheck::printCollisionPairs(collision_detection::CollisionResult::ContactMap& contact_map)
-{
-  if (!contact_map.empty())
-  {
-    // Throttled error message about the first contact in the list
-    ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME,
-                                   "Objects in collision (among others, possibly): "
-                                       << contact_map.begin()->first.first << ", "
-                                       << contact_map.begin()->first.second);
-    // Log all other contacts if in debug mode
-    ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Objects in collision:");
-    for (auto contact : contact_map)
-    {
-      ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME,
-                                      "\t" << contact.first.first << ", " << contact.first.second);
-    }
-  }
-}
-
 void CollisionCheck::worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg)
 {
   worst_case_stop_time_ = msg->data;
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
index 53e919ee33..d51ce11b19 100644
--- 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
@@ -75,7 +75,7 @@ class StatusMonitor
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, LOGNAME);
-  ros::NodeHandle nh;
+  ros::NodeHandle nh("~");
   ros::AsyncSpinner spinner(8);
   spinner.start();
 
@@ -150,6 +150,6 @@ int main(int argc, char** argv)
     ++num_commands;
   }
 
-  servo.stop();
+  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 <std_msgs/Int8.h>
+#include <geometry_msgs/TransformStamped.h>
+
+#include <moveit_servo/servo.h>
+#include <moveit_servo/pose_tracking.h>
+#include <moveit_servo/status_codes.h>
+#include <moveit_servo/make_shared_from_pool.h>
+#include <thread>
+
+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<moveit_servo::StatusCode>(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<planning_scene_monitor::PlanningSceneMonitor>("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<geometry_msgs::PoseStamped>("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/joint_state_subscriber.cpp b/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp
deleted file mode 100644
index c60b331119..0000000000
--- a/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*******************************************************************************
- * 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     : joint_state_subscriber.cpp
- *      Project   : moveit_servo
- *      Created   : 06/11/2020
- *      Author    : Tyler Weaver
- */
-
-#include <moveit_servo/joint_state_subscriber.h>
-
-namespace moveit_servo
-{
-constexpr char LOGNAME[] = "joint_state_subscriber";
-
-// Constructor for the class that handles collision checking
-JointStateSubscriber::JointStateSubscriber(ros::NodeHandle& nh, const std::string& joint_state_topic_name)
-{
-  // subscribe to joints
-  joint_state_sub_ = nh.subscribe(joint_state_topic_name, ROS_QUEUE_SIZE, &JointStateSubscriber::jointStateCB, this);
-
-  // Wait for initial messages
-  ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg.");
-  ros::topic::waitForMessage<sensor_msgs::JointState>(joint_state_topic_name);
-  ROS_INFO_NAMED(LOGNAME, "Received first joint msg.");
-}
-
-sensor_msgs::JointStateConstPtr JointStateSubscriber::getLatest() const
-{
-  const std::lock_guard<std::mutex> lock(joint_state_mutex_);
-  return latest_joint_state_;
-}
-
-void JointStateSubscriber::jointStateCB(const sensor_msgs::JointStateConstPtr& msg)
-{
-  const std::lock_guard<std::mutex> lock(joint_state_mutex_);
-  latest_joint_state_ = msg;
-}
-
-}  // 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..f56182e44f
--- /dev/null
+++ b/moveit_ros/moveit_servo/src/pose_tracking.cpp
@@ -0,0 +1,392 @@
+/*********************************************************************
+ * 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<moveit_servo::Servo>(nh_, planning_scene_monitor_);
+  servo_->start();
+
+  // Connect to Servo ROS interfaces
+  target_pose_sub_ =
+      nh_.subscribe<geometry_msgs::PoseStamped>("target_pose", 1, &PoseTracking::targetPoseCallback, this);
+
+  // Publish outgoing twist commands to the Servo object
+  twist_stamped_pub_ =
+      nh_.advertise<geometry_msgs::TwistStamped>(servo_->getParameters().cartesian_command_in_topic, 1);
+}
+
+PoseTrackingStatusCode PoseTracking::moveToPose(const Eigen::Vector3d& positional_tolerance,
+                                                const double angular_tolerance, const double target_pose_timeout)
+{
+  // Reset stop requested flag before starting motions
+  stop_requested_ = false;
+  // 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<control_toolbox::Pid>& 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<geometry_msgs::TwistStamped>();
+
+  // 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<std::mutex> 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::stopMotion()
+{
+  stop_requested_ = true;
+
+  // Send a 0 command to Servo to halt arm motion
+  auto msg = moveit::util::make_shared_from_pool<geometry_msgs::TwistStamped>();
+  {
+    std::lock_guard<std::mutex> lock(target_pose_mtx_);
+    msg->header.frame_id = target_pose_.header.frame_id;
+  }
+  msg->header.stamp = ros::Time::now();
+  twist_stamped_pub_.publish(msg);
+}
+
+void PoseTracking::doPostMotionReset()
+{
+  stopMotion();
+  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<std::mutex> 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
index ee567f8bb3..aba61fc1fe 100644
--- a/moveit_ros/moveit_servo/src/servo.cpp
+++ b/moveit_ros/moveit_servo/src/servo.cpp
@@ -39,12 +39,18 @@
 
 #include <rosparam_shortcuts/rosparam_shortcuts.h>
 
+#include <moveit_servo/make_shared_from_pool.h>
 #include <moveit_servo/servo.h>
 
 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)
 {
@@ -52,12 +58,27 @@ Servo::Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMon
   if (!readParameters())
     exit(EXIT_FAILURE);
 
-  joint_state_subscriber_ = std::make_shared<JointStateSubscriber>(nh_, parameters_.joint_topic);
+  // 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<ServoCalcs>(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_);
+  servo_calcs_ = std::make_unique<ServoCalcs>(nh_, parameters_, planning_scene_monitor_);
 
-  collision_checker_ =
-      std::make_unique<CollisionCheck>(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_);
+  collision_checker_ = std::make_unique<CollisionCheck>(nh_, parameters_, planning_scene_monitor_);
 }
 
 // Read ROS parameters, typically from YAML file
@@ -65,67 +86,58 @@ bool Servo::readParameters()
 {
   std::size_t error = 0;
 
-  // Specified in the launch file. All other parameters will be read from this namespace.
+  // 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 (parameter_ns.empty())
-  {
-    ROS_ERROR_STREAM_NAMED(LOGNAME, "A namespace must be specified in the launch file, like:");
-    ROS_ERROR_STREAM_NAMED(LOGNAME, "<param name=\"parameter_ns\" "
-                                    "type=\"string\" "
-                                    "value=\"left_servo_server\" />");
-    return false;
-  }
 
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_period", parameters_.publish_period);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_check_rate", parameters_.collision_check_rate);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/num_outgoing_halt_msgs_to_publish",
+  // 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("", nh_, parameter_ns + "/scale/linear", parameters_.linear_scale);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/scale/rotational", parameters_.rotational_scale);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/scale/joint", parameters_.joint_scale);
+  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("", nh_, parameter_ns + "/low_pass_filter_coeff", parameters_.low_pass_filter_coeff);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/joint_topic", parameters_.joint_topic);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/command_in_type", parameters_.command_in_type);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/cartesian_command_in_topic",
-                                    parameters_.cartesian_command_in_topic);
-  error +=
-      !rosparam_shortcuts::get("", nh_, parameter_ns + "/joint_command_in_topic", parameters_.joint_command_in_topic);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/robot_link_command_frame",
-                                    parameters_.robot_link_command_frame);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/incoming_command_timeout",
-                                    parameters_.incoming_command_timeout);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/lower_singularity_threshold",
-                                    parameters_.lower_singularity_threshold);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/hard_stop_singularity_threshold",
+      !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("", nh_, parameter_ns + "/move_group_name", parameters_.move_group_name);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/planning_frame", parameters_.planning_frame);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/use_gazebo", parameters_.use_gazebo);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/joint_limit_margin", parameters_.joint_limit_margin);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/command_out_topic", parameters_.command_out_topic);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/command_out_type", parameters_.command_out_type);
+  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("", nh_, parameter_ns + "/publish_joint_positions", parameters_.publish_joint_positions);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_joint_velocities",
-                                    parameters_.publish_joint_velocities);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_joint_accelerations",
-                                    parameters_.publish_joint_accelerations);
+      !rosparam_shortcuts::get(LOGNAME, nh, "publish_joint_accelerations", parameters_.publish_joint_accelerations);
 
   // Parameters for collision checking
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/check_collisions", parameters_.check_collisions);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_check_type", parameters_.collision_check_type);
+  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(
-      "", nh_, parameter_ns + "/self_collision_proximity_threshold", parameters_.self_collision_proximity_threshold);
+      LOGNAME, nh, "self_collision_proximity_threshold", parameters_.self_collision_proximity_threshold);
   bool have_scene_collision_proximity_threshold = rosparam_shortcuts::get(
-      "", nh_, parameter_ns + "/scene_collision_proximity_threshold", parameters_.scene_collision_proximity_threshold);
+      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(parameter_ns + "/collision_proximity_threshold") &&
-      rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_proximity_threshold", collision_proximity_threshold))
+  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' "
@@ -143,22 +155,33 @@ bool Servo::readParameters()
   }
   error += !have_self_collision_proximity_threshold;
   error += !have_scene_collision_proximity_threshold;
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_distance_safety_factor",
+  error += !rosparam_shortcuts::get(LOGNAME, nh, "collision_distance_safety_factor",
                                     parameters_.collision_distance_safety_factor);
-  error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/min_allowable_collision_distance",
+  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("", nh_, parameter_ns + "/status_topic", parameters_.status_topic))
+  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("", nh_, parameter_ns + "/warning_topic", parameters_.status_topic);
+    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(parameter_ns, error);
+  rosparam_shortcuts::shutdownIfError(LOGNAME, error);
 
   // Input checking
   if (parameters_.publish_period <= 0.)
@@ -196,7 +219,7 @@ bool Servo::readParameters()
   if (parameters_.joint_limit_margin < 0.)
   {
     ROS_WARN_NAMED(LOGNAME, "Parameter 'joint_limit_margin' should be "
-                            "greater than zero. Check yaml file.");
+                            "greater than or equal to zero. Check yaml file.");
     return false;
   }
   if (parameters_.command_in_type != "unitless" && parameters_.command_in_type != "speed_units")
@@ -286,15 +309,9 @@ void Servo::start()
     collision_checker_->start();
 }
 
-void Servo::stop()
-{
-  servo_calcs_->stop();
-  collision_checker_->stop();
-}
-
 Servo::~Servo()
 {
-  stop();
+  setPaused(true);
 }
 
 void Servo::setPaused(bool paused)
@@ -308,14 +325,24 @@ bool Servo::getCommandFrameTransform(Eigen::Isometry3d& transform)
   return servo_calcs_->getCommandFrameTransform(transform);
 }
 
-const ServoParameters& Servo::getParameters() const
+bool Servo::getCommandFrameTransform(geometry_msgs::TransformStamped& transform)
 {
-  return parameters_;
+  return servo_calcs_->getEEFrameTransform(transform);
 }
 
-sensor_msgs::JointStateConstPtr Servo::getLatestJointState() const
+bool Servo::getEEFrameTransform(Eigen::Isometry3d& transform)
 {
-  return joint_state_subscriber_->getLatest();
+  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
index f2282ef69c..e9d990ffd7 100644
--- a/moveit_ros/moveit_servo/src/servo_calcs.cpp
+++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp
@@ -37,11 +37,13 @@
  *      Author    : Brian O'Neil, Andy Zelenak, Blake Anderson
  */
 
+#include <cassert>
+
 #include <std_msgs/Bool.h>
 #include <std_msgs/Float64MultiArray.h>
 
-#include <moveit_servo/servo_calcs.h>
 #include <moveit_servo/make_shared_from_pool.h>
+#include <moveit_servo/servo_calcs.h>
 
 static const std::string LOGNAME = "servo_calcs";
 constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30;  // Seconds to throttle logs inside loops
@@ -67,30 +69,32 @@ bool isNonZero(const control_msgs::JointJog& msg)
   };
   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, const ServoParameters& parameters,
-                       const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
-                       const std::shared_ptr<JointStateSubscriber>& joint_state_subscriber)
+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)
-  , joint_state_subscriber_(joint_state_subscriber)
-  , period_(parameters.publish_period)
+  , stop_requested_(true)
+  , paused_(false)
 {
   // MoveIt Setup
-  const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr = planning_scene_monitor_->getRobotModelLoader();
-  while (ros::ok() && !model_loader_ptr)
-  {
-    ROS_WARN_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Waiting for a non-null robot_model_loader pointer");
-    default_sleep_rate_.sleep();
-  }
-  const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel();
-  kinematic_state_ = std::make_shared<moveit::core::RobotState>(kinematic_model);
-  kinematic_state_->setToDefaultValues();
-
-  joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name);
+  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
@@ -99,22 +103,19 @@ ServoCalcs::ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters,
   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(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions",
-                           &ServoCalcs::changeDriftDimensions, this);
+  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(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions",
-                           &ServoCalcs::changeControlDimensions, this);
+  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(nh_.getNamespace() + "/" + ros::this_node::getName() + "/reset_servo_status",
-                           &ServoCalcs::resetServoStatus, this);
+  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("~internal");
+  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<std_msgs::Float64>("worst_case_stop_time", ROS_QUEUE_SIZE);
@@ -134,16 +135,49 @@ ServoCalcs::ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters,
   internal_joint_state_.position.resize(num_joints_);
   internal_joint_state_.velocity.resize(num_joints_);
 
-  // Set up the "last" published message, in case we need to send it first
+  // 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<trajectory_msgs::JointTrajectory>();
-  auto latest_joints = joint_state_subscriber_->getLatest();
+
+  // 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->header.stamp = ros::Time::now();
   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)
-    point.positions = latest_joints->position;
+    planning_scene_monitor_->getStateMonitor()->getCurrentState()->copyJointGroupPositions(joint_model_group_,
+                                                                                           point.positions);
   if (parameters_.publish_joint_velocities)
   {
     std::vector<double> velocity(num_joints_);
@@ -154,47 +188,85 @@ ServoCalcs::ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters,
     // 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<double> acceleration(num_joints_);
-    point.accelerations = acceleration;
+    point.accelerations.resize(num_joints_);
   }
   initial_joint_trajectory->points.push_back(point);
   last_sent_command_ = initial_joint_trajectory;
 
-  // 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);
-  }
-}
+  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);
 
-void ServoCalcs::start()
-{
   stop_requested_ = false;
-  timer_ = nh_.createTimer(period_, &ServoCalcs::run, this);
+  thread_ = std::thread([this] { mainCalcLoop(); });
+  new_input_cmd_ = false;
 }
 
 void ServoCalcs::stop()
 {
+  // Request stop
   stop_requested_ = true;
-  timer_.stop();
+
+  // 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<std::mutex> lock(input_mutex_);
+    new_input_cmd_ = false;
+    input_cv_.notify_all();
+  }
+
+  // Join the thread
+  if (thread_.joinable())
+  {
+    thread_.join();
+  }
 }
 
-void ServoCalcs::run(const ros::TimerEvent& timer_event)
+void ServoCalcs::mainCalcLoop()
 {
-  // Log warning when the last loop duration was longer than the period
-  if (timer_event.profile.last_duration.toSec() > period_.toSec())
+  ros::Rate rate(1.0 / parameters_.publish_period);
+
+  while (ros::ok() && !stop_requested_)
   {
-    ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME,
-                                   "last_duration: " << timer_event.profile.last_duration.toSec() << " ("
-                                                     << period_.toSec() << ")");
+    // lock the input state mutex
+    std::unique_lock<std::mutex> 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<std_msgs::Int8>();
   status_msg->data = static_cast<int8_t>(status_);
@@ -203,39 +275,36 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event)
   // 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
-  while (!updateJoints() && ros::ok())
-  {
-    if (stop_requested_)
-      return;
-    default_sleep_rate_.sleep();
-  }
+  updateJoints();
 
   // Update from latest state
-  sensor_msgs::JointStateConstPtr latest_joint_state = joint_state_subscriber_->getLatest();
-  kinematic_state_->setVariableValues(*latest_joint_state);
-  {
-    const std::lock_guard<std::mutex> lock(latest_state_mutex_);
-    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_;
-  }
+  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_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() *
-                                  kinematic_state_->getGlobalLinkTransform(parameters_.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_;
 
@@ -331,13 +400,15 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event)
     zero_velocity_count_ = 0;
   }
 
-  if (ok_to_publish_)
+  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")
     {
-      joint_trajectory->header.stamp = ros::Time::now();
+      // 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")
@@ -408,13 +479,19 @@ bool ServoCalcs::cartesianServoCalcs(geometry_msgs::TwistStamped& cmd,
       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 =
-          kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() *
-          kinematic_state_->getGlobalLinkTransform(cmd.header.frame_id);
+          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;
@@ -433,7 +510,7 @@ bool ServoCalcs::cartesianServoCalcs(geometry_msgs::TwistStamped& cmd,
   Eigen::VectorXd delta_x = scaleCartesianCommand(cmd);
 
   // Convert from cartesian commands to joint commands
-  Eigen::MatrixXd jacobian = kinematic_state_->getJacobian(joint_model_group_);
+  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.
@@ -454,7 +531,7 @@ bool ServoCalcs::cartesianServoCalcs(geometry_msgs::TwistStamped& cmd,
 
   delta_theta_ = pseudo_inverse * delta_x;
 
-  enforceSRDFAccelVelLimits(delta_theta_);
+  enforceVelLimits(delta_theta_);
 
   // If close to a collision or a singularity, decelerate
   applyVelocityScaling(delta_theta_, velocityScalingFactorForSingularity(delta_x, svd, pseudo_inverse));
@@ -480,7 +557,7 @@ bool ServoCalcs::jointServoCalcs(const control_msgs::JointJog& cmd, trajectory_m
   // Apply user-defined scaling
   delta_theta_ = scaleJointCommand(cmd);
 
-  enforceSRDFAccelVelLimits(delta_theta_);
+  enforceVelLimits(delta_theta_);
 
   // If close to a collision, decelerate
   applyVelocityScaling(delta_theta_, 1.0 /* scaling for singularities -- ignore for joint motions */);
@@ -503,7 +580,7 @@ bool ServoCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& jo
 
   composeJointTrajMessage(internal_joint_state_, joint_trajectory);
 
-  if (!enforceSRDFPositionLimits())
+  if (!enforcePositionLimits())
   {
     suddenHalt(joint_trajectory);
     status_ = StatusCode::JOINT_BOUND;
@@ -520,15 +597,15 @@ bool ServoCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& jo
 
 // 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)
+  // Start from 2nd point (i = 1) because we already have the first point.
+  // The timestamps are shifted up one period since first point is at 1 * publish_period, not 0.
+  for (int i = 1; i < count; ++i)
   {
-    point.time_from_start = ros::Duration(i * parameters_.publish_period);
+    point.time_from_start = ros::Duration((i + 1) * parameters_.publish_period);
     joint_trajectory.points[i] = point;
   }
 }
@@ -564,8 +641,10 @@ void ServoCalcs::calculateJointVelocities(sensor_msgs::JointState& joint_state,
 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.header.stamp = ros::Time::now();
   joint_trajectory.joint_names = joint_state.name;
 
   trajectory_msgs::JointTrajectoryPoint point;
@@ -635,10 +714,10 @@ double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& co
 
   // Calculate a small change in joints
   Eigen::VectorXd new_theta;
-  kinematic_state_->copyJointGroupPositions(joint_model_group_, new_theta);
+  current_state_->copyJointGroupPositions(joint_model_group_, new_theta);
   new_theta += pseudo_inverse * delta_x;
-  kinematic_state_->setJointGroupPositions(joint_model_group_, new_theta);
-  auto new_jacobian = kinematic_state_->getJacobian(joint_model_group_);
+  current_state_->setJointGroupPositions(joint_model_group_, new_theta);
+  Eigen::MatrixXd new_jacobian = current_state_->getJacobian(joint_model_group_);
 
   Eigen::JacobiSVD<Eigen::MatrixXd> new_svd(new_jacobian);
   double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1);
@@ -676,81 +755,31 @@ double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& co
   return velocity_scale;
 }
 
-void ServoCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta)
+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;
-  const Eigen::ArrayXd acceleration = (velocity - prev_joint_velocity_) / parameters_.publish_period;
 
-  std::size_t joint_delta_index = 0;
-  for (auto joint : joint_model_group_->getActiveJointModels())
+  std::size_t joint_delta_index{ 0 };
+  double velocity_scaling_factor{ 1.0 };
+  for (const moveit::core::JointModel* joint : joint_model_group_->getActiveJointModels())
   {
-    // Some joints do not have bounds defined
-    const auto bounds = joint->getVariableBounds(joint->getName());
-    if (bounds.acceleration_bounded_)
+    const auto& bounds = joint->getVariableBounds(joint->getName());
+    if (bounds.velocity_bounded_ && velocity(joint_delta_index) != 0.0)
     {
-      bool clip_acceleration = false;
-      double acceleration_limit = 0.0;
-      if (acceleration(joint_delta_index) < bounds.min_acceleration_)
-      {
-        clip_acceleration = true;
-        acceleration_limit = bounds.min_acceleration_;
-      }
-      else if (acceleration(joint_delta_index) > bounds.max_acceleration_)
-      {
-        clip_acceleration = true;
-        acceleration_limit = bounds.max_acceleration_;
-      }
-
-      // Apply acceleration bounds
-      if (clip_acceleration)
-      {
-        // accel = (vel - vel_prev) / delta_t = ((delta_theta / delta_t) - vel_prev) / delta_t
-        // --> delta_theta = (accel * delta_t _ + vel_prev) * delta_t
-        const double relative_change =
-            ((acceleration_limit * parameters_.publish_period + prev_joint_velocity_(joint_delta_index)) *
-             parameters_.publish_period) /
-            delta_theta(joint_delta_index);
-        // Avoid nan
-        if (fabs(relative_change) < 1)
-          delta_theta(joint_delta_index) = relative_change * delta_theta(joint_delta_index);
-      }
-    }
-
-    if (bounds.velocity_bounded_)
-    {
-      velocity(joint_delta_index) = delta_theta(joint_delta_index) / parameters_.publish_period;
-
-      bool clip_velocity = false;
-      double velocity_limit = 0.0;
-      if (velocity(joint_delta_index) < bounds.min_velocity_)
-      {
-        clip_velocity = true;
-        velocity_limit = bounds.min_velocity_;
-      }
-      else if (velocity(joint_delta_index) > bounds.max_velocity_)
-      {
-        clip_velocity = true;
-        velocity_limit = bounds.max_velocity_;
-      }
-
-      // Apply velocity bounds
-      if (clip_velocity)
-      {
-        // delta_theta = joint_velocity * delta_t
-        const double relative_change = (velocity_limit * parameters_.publish_period) / delta_theta(joint_delta_index);
-        // Avoid nan
-        if (fabs(relative_change) < 1)
-        {
-          delta_theta(joint_delta_index) = relative_change * delta_theta(joint_delta_index);
-          velocity(joint_delta_index) = relative_change * velocity(joint_delta_index);
-        }
-      }
+      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::enforceSRDFPositionLimits()
+bool ServoCalcs::enforcePositionLimits()
 {
   bool halting = false;
 
@@ -766,16 +795,16 @@ bool ServoCalcs::enforceSRDFPositionLimits()
         break;
       }
     }
-    if (!kinematic_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin))
+    if (!current_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin))
     {
       const std::vector<moveit_msgs::JointLimits> limits = joint->getVariableBoundsMsg();
 
       // Joint limits are not defined for some joints. Skip them.
       if (!limits.empty())
       {
-        if ((kinematic_state_->getJointVelocities(joint)[0] < 0 &&
+        if ((current_state_->getJointVelocities(joint)[0] < 0 &&
              (joint_angle < (limits[0].min_position + parameters_.joint_limit_margin))) ||
-            (kinematic_state_->getJointVelocities(joint)[0] > 0 &&
+            (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,
@@ -794,51 +823,45 @@ bool ServoCalcs::enforceSRDFPositionLimits()
 // Is handled differently for position vs. velocity control.
 void ServoCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory)
 {
-  if (joint_trajectory.points.empty())
-  {
-    joint_trajectory.points.push_back(trajectory_msgs::JointTrajectoryPoint());
-    joint_trajectory.points[0].positions.resize(num_joints_);
-    joint_trajectory.points[0].velocities.resize(num_joints_);
-  }
+  // 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);
+
+  if (parameters_.publish_joint_positions)
+    point.positions.resize(num_joints_);
+  if (parameters_.publish_joint_velocities)
+    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)
-      joint_trajectory.points[0].positions[i] = original_joint_state_.position[i];
+      point.positions[i] = original_joint_state_.position[i];
 
     // For velocity-controlled robots, stop
     if (parameters_.publish_joint_velocities)
-      joint_trajectory.points[0].velocities[i] = 0;
+      point.velocities[i] = 0;
   }
 }
 
 // Parse the incoming joint msg for the joints of our MoveGroup
-bool ServoCalcs::updateJoints()
+void ServoCalcs::updateJoints()
 {
-  sensor_msgs::JointStateConstPtr latest_joint_state = joint_state_subscriber_->getLatest();
-
-  // Check that the msg contains enough joints
-  if (latest_joint_state->name.size() < num_joints_)
-    return false;
-
-  // Store joints in a member variable
-  for (std::size_t m = 0; m < latest_joint_state->name.size(); ++m)
-  {
-    std::size_t c;
-    try
-    {
-      c = joint_state_name_map_.at(latest_joint_state->name[m]);
-    }
-    catch (const std::out_of_range& e)
-    {
-      ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME,
-                                      "Ignoring joint " << latest_joint_state->name[m]);
-      continue;
-    }
-
-    internal_joint_state_.position[c] = latest_joint_state->position[m];
-  }
+  // 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_;
@@ -849,9 +872,9 @@ bool ServoCalcs::updateJoints()
   double accel_limit = 0;
   double joint_velocity = 0;
   double worst_case_stop_time = 0;
-  for (size_t jt_state_idx = 0; jt_state_idx < latest_joint_state->velocity.size(); ++jt_state_idx)
+  for (size_t jt_state_idx = 0; jt_state_idx < internal_joint_state_.velocity.size(); ++jt_state_idx)
   {
-    joint_name = latest_joint_state->name[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())
@@ -877,7 +900,7 @@ bool ServoCalcs::updateJoints()
     }
 
     // Get the current joint velocity
-    joint_velocity = latest_joint_state->velocity[jt_state_idx];
+    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));
@@ -889,8 +912,6 @@ bool ServoCalcs::updateJoints()
     msg->data = worst_case_stop_time;
     worst_case_stop_time_pub_.publish(msg);
   }
-
-  return true;
 }
 
 // Scale the incoming servo command
@@ -938,8 +959,7 @@ Eigen::VectorXd ServoCalcs::scaleJointCommand(const control_msgs::JointJog& comm
     }
     catch (const std::out_of_range& e)
     {
-      ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME,
-                                     "Ignoring joint " << joint_state_subscriber_->getLatest()->name[m]);
+      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]
@@ -994,31 +1014,75 @@ void ServoCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& del
 
 bool ServoCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform)
 {
-  const std::lock_guard<std::mutex> lock(latest_state_mutex_);
+  const std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> lock(latest_state_mutex_);
+  const std::lock_guard<std::mutex> 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<std::mutex> lock(latest_state_mutex_);
+  const std::lock_guard<std::mutex> 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)
@@ -1054,7 +1118,7 @@ bool ServoCalcs::changeControlDimensions(moveit_msgs::ChangeControlDimensions::R
   return true;
 }
 
-bool ServoCalcs::resetServoStatus(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
+bool ServoCalcs::resetServoStatus(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
 {
   status_ = StatusCode::NO_WARNING;
   return true;
@@ -1065,4 +1129,9 @@ 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
index 590061609f..5c630f3082 100644
--- a/moveit_ros/moveit_servo/src/servo_server.cpp
+++ b/moveit_ros/moveit_servo/src/servo_server.cpp
@@ -52,7 +52,7 @@ int main(int argc, char** argv)
   ros::AsyncSpinner spinner(ROS_THREADS);
   spinner.start();
 
-  ros::NodeHandle nh;
+  ros::NodeHandle nh("~");
 
   // Load the planning scene monitor
   auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
@@ -80,7 +80,7 @@ int main(int argc, char** argv)
   ros::waitForShutdown();
 
   // Stop the servo server
-  servo.stop();
+  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
index cec304711a..7f5c685d6a 100644
--- a/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp
+++ b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp
@@ -104,4 +104,4 @@ int main(int argc, char** argv)
   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 <string>
+
+// ROS
+#include <ros/ros.h>
+
+// Testing
+#include <gtest/gtest.h>
+
+// Servo
+#include <moveit_servo/make_shared_from_pool.h>
+#include <moveit_servo/servo.h>
+
+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<sensor_msgs::JointState>("/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<planning_scene_monitor::PlanningSceneMonitor>("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<Servo>(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<void(const trajectory_msgs::JointTrajectoryConstPtr&)> 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<geometry_msgs::TwistStamped>(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<size_t>(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<geometry_msgs::TwistStamped>();
+    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<void(const trajectory_msgs::JointTrajectoryConstPtr&)> 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<control_msgs::JointJog>(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<size_t>(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<control_msgs::JointJog>();
+    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 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load URDF, SRDF -->
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch" >
+    <arg name="load_robot_description" value="true"/>
+  </include>
+
+  <!-- Initial joint positions -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <rosparam command="load" file="$(find moveit_servo)/test/config/initial_position.yaml" />
+    <param name="publish_frequency" type="double" value="50.0"/>
+  </node>
+
+  <test pkg="moveit_servo" type="basic_servo_tests" test-name="basic_servo_tests" time-limit="60" args="">
+    <param name="parameter_ns" type="string" value="optional_parameter_namespace" />
+    <rosparam command="load" file="$(find moveit_servo)/test/config/servo_settings_low_latency.yaml" ns="optional_parameter_namespace"/>
+  </test>
+</launch>
diff --git a/moveit_ros/moveit_servo/test/config/initial_position.yaml b/moveit_ros/moveit_servo/test/config/initial_position.yaml
index 7a86cc0aa7..38ac0fcecf 100644
--- a/moveit_ros/moveit_servo/test/config/initial_position.yaml
+++ b/moveit_ros/moveit_servo/test/config/initial_position.yaml
@@ -8,4 +8,3 @@ zeros:
   panda_joint6: 1.571
   panda_joint7: 0.785
 publish_default_positions: True
-
diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml
index 9a674425b6..b081f28c73 100644
--- a/moveit_ros/moveit_servo/test/config/servo_settings.yaml
+++ b/moveit_ros/moveit_servo/test/config/servo_settings.yaml
@@ -4,8 +4,7 @@
 use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
 
 ## Properties of incoming commands
-robot_link_command_frame:  panda_link0  # commands must be given in the frame of a robot link. Usually either the base or end effector
-command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+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.
@@ -15,6 +14,7 @@ scale:
 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?
@@ -31,6 +31,10 @@ publish_joint_accelerations: false
 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.
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 <string>
+#include <thread>
+
+// ROS
+#include <ros/ros.h>
+
+// Testing
+#include <gtest/gtest.h>
+
+// Servo
+#include <moveit_servo/pose_tracking.h>
+#include <moveit_servo/make_shared_from_pool.h>
+
+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<sensor_msgs::JointState>("/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<planning_scene_monitor::PlanningSceneMonitor>("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<moveit_servo::PoseTracking>(nh_, planning_scene_monitor_);
+
+    target_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("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<void(const trajectory_msgs::JointTrajectoryConstPtr&)> 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 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load URDF, SRDF -->
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch" >
+    <arg name="load_robot_description" value="true"/>
+  </include>
+
+  <!-- Initial joint positions -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <rosparam command="load" file="$(find moveit_servo)/test/config/initial_position.yaml" />
+    <param name="publish_frequency" type="double" value="50.0"/>
+  </node>
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="tool_state_publisher">
+    <param name="publish_frequency" type="double" value="50.0"/>
+  </node>
+
+  <test pkg="moveit_servo" type="pose_tracking_test" test-name="pose_tracking_test" time-limit="60
+    " args="">
+    <param name="parameter_ns" type="string" value="" />
+    <rosparam command="load" file="$(find moveit_servo)/test/config/servo_settings.yaml"/>
+    <rosparam command="load" file="$(find moveit_servo)/config/pose_tracking_settings.yaml"/>
+  </test>
+</launch>
diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp
index 4fd01fc73e..fa61c32e0a 100644
--- a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp
+++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp
@@ -46,10 +46,11 @@
 #include <gtest/gtest.h>
 
 // Servo
-#include <moveit_servo/servo.h>
 #include <moveit_servo/make_shared_from_pool.h>
+#include <moveit_servo/servo.h>
 
 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
 {
@@ -58,6 +59,13 @@ class ServoFixture : public ::testing::Test
 public:
   void SetUp() override
   {
+    // Wait for several key topics / parameters
+    ros::topic::waitForMessage<sensor_msgs::JointState>("/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<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
     planning_scene_monitor_->startSceneMonitor();
@@ -76,12 +84,12 @@ class ServoFixture : public ::testing::Test
 
   bool waitForFirstStatus()
   {
-    auto msg = ros::topic::waitForMessage<std_msgs::Int8>(servo_->getParameters().status_topic, nh_, ros::Duration(15));
+    auto msg = ros::topic::waitForMessage<std_msgs::Int8>(servo_->getParameters().status_topic, nh_, ros::Duration(1));
     return static_cast<bool>(msg);
   }
 
 protected:
-  ros::NodeHandle nh_;
+  ros::NodeHandle nh_{ "~" };
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
   moveit_servo::ServoPtr servo_;
 };  // class ServoFixture
@@ -90,14 +98,14 @@ TEST_F(ServoFixture, StartStopTest)
 {
   servo_->start();
   EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message";
-  servo_->stop();
+  servo_->setPaused(true);
 
   ros::Duration(1.0).sleep();
 
   // Start and stop again
-  servo_->start();
+  servo_->setPaused(false);
   EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message";
-  servo_->stop();
+  servo_->setPaused(true);
 }
 
 TEST_F(ServoFixture, SendTwistStampedTest)
@@ -110,7 +118,7 @@ TEST_F(ServoFixture, SendTwistStampedTest)
   // count trajectory messages sent by servo
   size_t received_count = 0;
   boost::function<void(const trajectory_msgs::JointTrajectoryConstPtr&)> traj_callback =
-      [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { received_count++; };
+      [&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
@@ -136,8 +144,9 @@ TEST_F(ServoFixture, SendTwistStampedTest)
   }
 
   EXPECT_GT(received_count, num_commands - 20);
+  EXPECT_GT(received_count, (unsigned)0);
   EXPECT_LT(received_count, num_commands + 20);
-  servo_->stop();
+  servo_->setPaused(true);
 }
 
 TEST_F(ServoFixture, SendJointServoTest)
@@ -150,7 +159,7 @@ TEST_F(ServoFixture, SendJointServoTest)
   // count trajectory messages sent by servo
   size_t received_count = 0;
   boost::function<void(const trajectory_msgs::JointTrajectoryConstPtr&)> traj_callback =
-      [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { received_count++; };
+      [&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
@@ -177,9 +186,74 @@ TEST_F(ServoFixture, SendJointServoTest)
 
   EXPECT_GT(received_count, num_commands - 20);
   EXPECT_LT(received_count, num_commands + 20);
-  servo_->stop();
+  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<void(const trajectory_msgs::JointTrajectoryConstPtr&)> 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<geometry_msgs::TwistStamped>(parameters.cartesian_command_in_topic, 1);
+
+  constexpr double test_duration = 1.0;
+  const size_t num_commands = static_cast<size_t>(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<geometry_msgs::TwistStamped>();
+    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)
diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test
index f165250d82..94c6a2a7f4 100644
--- a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test
+++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test
@@ -12,7 +12,7 @@
   </node>
 
   <test pkg="moveit_servo" type="servo_cpp_interface_test" test-name="servo_cpp_interface_test" time-limit="60" args="">
-    <param name="parameter_ns" type="string" value="servo_cpp_interface_test" />
-    <rosparam command="load" file="$(find moveit_servo)/test/config/servo_settings.yaml"/>
+    <param name="parameter_ns" type="string" value="optional_parameter_namespace" />
+    <rosparam command="load" file="$(find moveit_servo)/test/config/servo_settings.yaml" ns="optional_parameter_namespace"/>
   </test>
 </launch>
diff --git a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst
index 9c255ed440..7c777d91b1 100644
--- a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst
+++ b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst
@@ -2,6 +2,26 @@
 Changelog for package moveit_ros_occupancy_map_monitor
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+* Document solution in ROS_ERROR on failed self-filtering (`#2627 <https://github.com/ros-planning/moveit/issues/2627>`_)
+* It's not an error not to define a plugin (`#2521 <https://github.com/ros-planning/moveit/issues/2521>`_)
+* Contributors: Michael Görner
+
+1.0.7 (2020-11-20)
+------------------
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: Felix von Drigalski
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Migrate to clang-format-10
diff --git a/moveit_ros/occupancy_map_monitor/CMakeLists.txt b/moveit_ros/occupancy_map_monitor/CMakeLists.txt
index 1390c52719..4f9cd70fc0 100644
--- a/moveit_ros/occupancy_map_monitor/CMakeLists.txt
+++ b/moveit_ros/occupancy_map_monitor/CMakeLists.txt
@@ -50,6 +50,7 @@ include_directories(include
                     )
 include_directories(SYSTEM
                     ${EIGEN3_INCLUDE_DIRS}
+                    ${OCTOMAP_INCLUDE_DIRS}
                     ${X11_INCLUDE_DIR}
                     )
 
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 210f1ebea6..376a3639ff 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
@@ -97,7 +97,7 @@ class OccMapTree : public octomap::OcTree
     return WriteLock(tree_mutex_);
   }
 
-  void triggerUpdateCallback(void)
+  void triggerUpdateCallback()
   {
     if (update_callback_)
       update_callback_();
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 a0c5410563..17748b4a13 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
@@ -54,7 +54,7 @@ typedef boost::function<bool(const std::string& target_frame, const ros::Time& t
 
 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.
  */
diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml
index 86b515af0b..6ee9808645 100644
--- a/moveit_ros/occupancy_map_monitor/package.xml
+++ b/moveit_ros/occupancy_map_monitor/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit_ros_occupancy_map_monitor</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Components of MoveIt! connecting to occupancy map</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
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 441538e468..34a2581ff1 100644
--- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp
+++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp
@@ -174,7 +174,9 @@ void OccupancyMapMonitor::initialize()
     }
   }
   else
-    ROS_ERROR("Failed to find 3D sensor plugin parameters for octomap generation");
+  {
+    ROS_INFO("No 3D sensor plugin(s) defined for octomap updates");
+  }
 
   /* advertise a service for loading octomaps from disk */
   save_map_srv_ = nh_.advertiseService("save_map", &OccupancyMapMonitor::saveMapCallback, this);
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..456615e2d2 100644
--- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp
+++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp
@@ -72,7 +72,14 @@ bool OccupancyMapUpdater::updateTransformCache(const std::string& target_frame,
 {
   transform_cache_.clear();
   if (transform_provider_callback_)
-    return transform_provider_callback_(target_frame, target_time, transform_cache_);
+  {
+    bool success = transform_provider_callback_(target_frame, target_time, transform_cache_);
+    if (!success)
+      ROS_ERROR_THROTTLE(
+          1, "Transform cache was not updated. Self-filtering may fail. If transforms were not available yet, consider "
+             "setting robot_description_planning/shape_transform_cache_lookup_wait_time to wait longer for transforms");
+    return success;
+  }
   else
   {
     ROS_WARN_THROTTLE(1, "No callback provided for updating the transform cache for octomap updaters");
diff --git a/moveit_ros/perception/CHANGELOG.rst b/moveit_ros/perception/CHANGELOG.rst
index 839eca6690..6c701852c9 100644
--- a/moveit_ros/perception/CHANGELOG.rst
+++ b/moveit_ros/perception/CHANGELOG.rst
@@ -2,6 +2,27 @@
 Changelog for package moveit_ros_perception
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+* Document solution in ROS_ERROR on failed self-filtering (`#2627 <https://github.com/ros-planning/moveit/issues/2627>`_)
+* Fixed flood of errors on startup for `mesh_filter` (`#2550 <https://github.com/ros-planning/moveit/issues/2550>`_)
+* Enable mesh filter (`#2448 <https://github.com/ros-planning/moveit/issues/2448>`_)
+* Contributors: Jafar Abdi, John Stechschulte, Michael Görner
+
+1.0.7 (2020-11-20)
+------------------
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: Felix von Drigalski, Robert Haschke
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Migrate to clang-format-10
diff --git a/moveit_ros/perception/CMakeLists.txt b/moveit_ros/perception/CMakeLists.txt
index 44353476bb..d3457f7b6e 100644
--- a/moveit_ros/perception/CMakeLists.txt
+++ b/moveit_ros/perception/CMakeLists.txt
@@ -49,12 +49,28 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   moveit_msgs
   moveit_ros_occupancy_map_monitor
+  moveit_ros_planning
+  nodelet
 )
 
 find_package(Eigen3 REQUIRED)
 find_package(OpenMP REQUIRED)
 find_package(OpenCV)
 
+set(PACKAGE_LIBRARIES
+  moveit_lazy_free_space_updater
+  moveit_point_containment_filter
+  moveit_pointcloud_octomap_updater_core
+  moveit_semantic_world
+)
+
+if (WITH_OPENGL)
+  list(APPEND PACKAGE_LIBRARIES
+          moveit_mesh_filter
+          moveit_depth_self_filter
+          moveit_depth_image_octomap_updater)
+endif(WITH_OPENGL)
+
 catkin_package(
   INCLUDE_DIRS
     lazy_free_space_updater/include
@@ -63,15 +79,14 @@ catkin_package(
     semantic_world/include
     ${perception_GL_INCLUDE_DIRS}
   LIBRARIES
-    moveit_lazy_free_space_updater
-    moveit_point_containment_filter
-    moveit_pointcloud_octomap_updater_core
-    moveit_semantic_world
+    ${PACKAGE_LIBRARIES}
   CATKIN_DEPENDS
     image_transport
     moveit_core
     moveit_msgs
     moveit_ros_occupancy_map_monitor
+    moveit_ros_planning
+    nodelet
     object_recognition_msgs
     roscpp
     sensor_msgs
@@ -109,5 +124,6 @@ install(
   FILES
     pointcloud_octomap_updater_plugin_description.xml
     depth_image_octomap_updater_plugin_description.xml
+    moveit_depth_self_filter.xml
   DESTINATION
     ${CATKIN_PACKAGE_SHARE_DESTINATION})
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 11ca20cfa8..733a8e5ca9 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
@@ -309,10 +309,7 @@ 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.");
     return;
-  }
 
   if (depth_msg->is_bigendian && !HOST_IS_BIG_ENDIAN)
     ROS_ERROR_THROTTLE(1, "endian problem: received image data does not match host");
diff --git a/moveit_ros/perception/mesh_filter/CMakeLists.txt b/moveit_ros/perception/mesh_filter/CMakeLists.txt
index 8d8a00743b..3abb380bfd 100644
--- a/moveit_ros/perception/mesh_filter/CMakeLists.txt
+++ b/moveit_ros/perception/mesh_filter/CMakeLists.txt
@@ -6,23 +6,33 @@ add_library(${MOVEIT_LIB_NAME}
   src/stereo_camera_model.cpp
   src/gl_renderer.cpp
   src/gl_mesh.cpp
-  )
+)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
 target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${gl_LIBS} GLUT::GLUT ${GLEW_LIBRARIES})
 
+add_library(moveit_depth_self_filter
+  src/depth_self_filter_nodelet.cpp
+  src/transform_provider.cpp
+)
+set_target_properties(moveit_depth_self_filter PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+
+target_link_libraries(moveit_depth_self_filter ${catkin_LIBRARIES} ${MOVEIT_LIB_NAME})
+
 if (CATKIN_ENABLE_TESTING)
   #catkin_lint: ignore_once env_var
   # Can only run this test if we have a display
   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}
+install(TARGETS ${MOVEIT_LIB_NAME} moveit_depth_self_filter
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_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..af50c4b7ea 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
@@ -39,8 +39,6 @@
 
 #include <nodelet/nodelet.h>
 #include <image_transport/image_transport.h>
-#include <boost/thread.hpp>
-#include <boost/thread/condition_variable.hpp>
 #include <moveit/mesh_filter/transform_provider.h>
 #include <moveit/mesh_filter/mesh_filter.h>
 #include <moveit/mesh_filter/stereo_camera_model.h>
@@ -58,10 +56,10 @@ class DepthSelfFiltering : public nodelet::Nodelet
 {
 public:
   /** \brief Nodelet init callback*/
-  virtual void onInit();
+  void onInit() override;
 
 private:
-  ~DepthSelfFiltering();
+  ~DepthSelfFiltering() override;
 
   /**
    * \brief adding the meshes to a given mesh filter object.
@@ -104,7 +102,7 @@ class DepthSelfFiltering : public nodelet::Nodelet
   image_transport::CameraPublisher pub_model_label_image_;
 
   /** \brief required to avoid listener registration before we are all set*/
-  boost::mutex connect_mutex_;
+  std::mutex connect_mutex_;
   int queue_size_;
   TransformProvider transform_provider_;
 
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 0846896384..5a0aea9207 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
@@ -38,13 +38,10 @@
 #define MOVEIT_MESH_FILTER_FILTER_JOB_
 
 #include <moveit/macros/class_forward.h>
-#include <boost/thread/condition.hpp>
-#include <boost/thread/mutex.hpp>
-#include <boost/thread/locks.hpp>
 
 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.
@@ -65,20 +62,20 @@ class Job
 
 protected:
   bool done_;
-  mutable boost::condition_variable condition_;
-  mutable boost::mutex mutex_;
+  mutable std::condition_variable condition_;
+  mutable std::mutex mutex_;
 };
 
 void Job::wait() const
 {
-  boost::unique_lock<boost::mutex> lock(mutex_);
+  std::unique_lock<std::mutex> lock(mutex_);
   while (!done_)
     condition_.wait(lock);
 }
 
 void Job::cancel()
 {
-  boost::unique_lock<boost::mutex> lock(mutex_);
+  std::unique_lock<std::mutex> lock(mutex_);
   done_ = true;
   condition_.notify_all();
 }
@@ -92,21 +89,21 @@ template <typename ReturnType>
 class FilterJob : public Job
 {
 public:
-  FilterJob(const boost::function<ReturnType()>& exec) : Job(), exec_(exec)
+  FilterJob(const std::function<ReturnType()>& exec) : Job(), exec_(exec)
   {
   }
   void execute() override;
   const ReturnType& getResult() const;
 
 private:
-  boost::function<ReturnType()> exec_;
+  std::function<ReturnType()> exec_;
   ReturnType result_;
 };
 
 template <typename ReturnType>
 void FilterJob<ReturnType>::execute()
 {
-  boost::unique_lock<boost::mutex> lock(mutex_);
+  std::unique_lock<std::mutex> lock(mutex_);
   if (!done_)  // not canceled !
     result_ = exec_();
 
@@ -125,12 +122,12 @@ template <>
 class FilterJob<void> : public Job
 {
 public:
-  FilterJob(const boost::function<void()>& exec) : Job(), exec_(exec)
+  FilterJob(const std::function<void()>& exec) : Job(), exec_(exec)
   {
   }
   void execute() override
   {
-    boost::unique_lock<boost::mutex> lock(mutex_);
+    std::unique_lock<std::mutex> lock(mutex_);
     if (!done_)  // not canceled !
       exec_();
 
@@ -139,7 +136,7 @@ class FilterJob<void> : public Job
   }
 
 private:
-  boost::function<void()> exec_;
+  std::function<void()> exec_;
 };
 }  // 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..7d15cf5852 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
@@ -45,11 +45,13 @@
 #include <GL/gl.h>
 #endif
 #include <string>
-#include <boost/thread.hpp>
+#include <thread>
+#include <mutex>
+#include <map>
 
 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.
@@ -295,10 +297,10 @@ class GLRenderer
   float cy_;
 
   /** \brief map from thread id to OpenGL context */
-  static std::map<boost::thread::id, std::pair<unsigned, GLuint> > context_;
+  static std::map<std::thread::id, std::pair<unsigned, GLuint> > context_;
 
   /* \brief lock for context map */
-  static boost::mutex context_lock_;
+  static std::mutex context_lock_;
 
   static bool glutInitialized_;
 };
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 7d482b21b2..e3c0ba0a78 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
@@ -41,7 +41,6 @@
 #include <moveit/macros/declare_ptr.h>
 #include <moveit/mesh_filter/gl_renderer.h>
 #include <moveit/mesh_filter/mesh_filter_base.h>
-#include <boost/function.hpp>
 
 // forward declarations
 namespace shapes
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 bb538bf47c..44c9573a9d 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
@@ -41,10 +41,11 @@
 #include <moveit/macros/class_forward.h>
 #include <moveit/mesh_filter/gl_renderer.h>
 #include <moveit/mesh_filter/sensor_model.h>
-#include <boost/function.hpp>
-#include <boost/thread/mutex.hpp>
 #include <Eigen/Geometry>  // for Isometry3d
 #include <queue>
+#include <thread>
+#include <condition_variable>
+#include <mutex>
 
 // forward declarations
 namespace shapes
@@ -54,8 +55,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;
@@ -64,7 +65,7 @@ class MeshFilterBase
 {
   // inner types and typedefs
 public:
-  typedef boost::function<bool(MeshHandle, Eigen::Isometry3d&)> TransformCallback;
+  typedef std::function<bool(MeshHandle, Eigen::Isometry3d&)> TransformCallback;
   // \todo @suat: to avoid a few comparisons, it would be much nicer if background = 14 and shadow = 15 (near/far clip
   // can be anything below that)
   // this would allow me to do a single comparison instead of 3, in the code i write
@@ -246,22 +247,22 @@ class MeshFilterBase
   MeshHandle min_handle_;
 
   /** \brief the filtering thread that also holds the OpenGL context*/
-  boost::thread filter_thread_;
+  std::thread filter_thread_;
 
   /** \brief condition variable to notify the filtering thread if a new image arrived*/
-  mutable boost::condition_variable jobs_condition_;
+  mutable std::condition_variable jobs_condition_;
 
   /** \brief mutex required for synchronization of condition states*/
-  mutable boost::mutex jobs_mutex_;
+  mutable std::mutex jobs_mutex_;
 
   /** \brief OpenGL job queue that need to be processed by the worker thread*/
   mutable std::queue<JobPtr> jobs_queue_;
 
   /** \brief mutex for synchronization of updating filtered meshes */
-  mutable boost::mutex meshes_mutex_;
+  mutable std::mutex meshes_mutex_;
 
   /** \brief mutex for synchronization of setting/calling transform_callback_ */
-  mutable boost::mutex transform_callback_mutex_;
+  mutable std::mutex transform_callback_mutex_;
 
   /** \brief indicates whether the filtering loop should stop*/
   bool stop_;
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 2c01c42761..33b6303e51 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
@@ -52,7 +52,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.
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 2d3ad2ab1d..386f293be2 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
@@ -38,8 +38,6 @@
 #define MOVEIT_MESH_FILTER_TRANSFORM_PROVIDER_
 
 #include <string>
-#include <boost/thread/thread.hpp>
-#include <boost/thread/mutex.hpp>
 #include <moveit/macros/class_forward.h>
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <moveit/mesh_filter/mesh_filter_base.h>
@@ -61,9 +59,9 @@ class TransformProvider
   /**
    * \brief Constructor
    * \author Suat Gedikli (gedikli@willowgarage.com)
-   * \param[in] interval_us update interval in micro seconds
+   * \param[in] update_rate update rate in Hz
    */
-  TransformProvider(unsigned long interval_us = 30000);
+  TransformProvider(double update_rate = 30.);
 
   /** \brief Destructor */
   ~TransformProvider();
@@ -81,7 +79,7 @@ class TransformProvider
    * \brief registers a mesh with its handle
    * \author Suat Gedikli (gedikli@willowgarage.com)
    * \param[in] handle handle of the mesh
-   * \param[in] name frame_id_ of teh mesh
+   * \param[in] name frame_id_ of the mesh
    */
   void addHandle(mesh_filter::MeshHandle handle, const std::string& name);
 
@@ -105,13 +103,12 @@ class TransformProvider
   void stop();
 
   /**
-   * \brief sets the update interval in micro seconds. This should be low enough to reduce the system load but high
-   * enough
-   * to get up-to-date transformations. For PSDK compatible devices this value should be around 30000 = 30ms
+   * \brief sets the update rate in Hz. This should be slow enough to reduce the system load but fast enough to get
+   * up-to-date transformations. For PSDK compatible devices this value should be around 30 Hz.
    * \author Suat Gedikli (gedikli@willowgarage.com)
-   * \param[in] usecs interval in micro seconds
+   * \param[in] update_rate update rate in Hz
    */
-  void setUpdateInterval(unsigned long usecs);
+  void setUpdateRate(double update_rate);
 
 private:
   /**
@@ -121,14 +118,15 @@ class TransformProvider
    */
   void updateTransforms();
 
-  MOVEIT_CLASS_FORWARD(TransformContext);
+  MOVEIT_CLASS_FORWARD(TransformContext);  // Defines TransformContextPtr, ConstPtr, WeakPtr... etc
 
   /**
    * \brief Context Object for registered frames
    * \author Suat Gedikli (gedikli@willowgarage.com)
    */
-  struct TransformContext
+  class TransformContext
   {
+  public:
     TransformContext(const std::string& name) : frame_id_(name)
     {
       transformation_.matrix().setZero();
@@ -136,7 +134,7 @@ class TransformProvider
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     std::string frame_id_;
     Eigen::Isometry3d transformation_;
-    boost::mutex mutex_;
+    std::mutex mutex_;
   };
 
   /**
@@ -159,12 +157,12 @@ class TransformProvider
   std::string frame_id_;
 
   /** \brief thread object*/
-  boost::thread thread_;
+  std::thread thread_;
 
   /** \flag to leave the update loop*/
   bool stop_;
 
-  /** \brief update interval in micro seconds*/
-  unsigned long interval_us_;
+  /** \brief update rate in Hz*/
+  ros::Rate update_rate_;
 };
 #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 20dc29f101..dc91e5b884 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
@@ -45,8 +45,6 @@
 #include <cv_bridge/cv_bridge.h>
 
 namespace enc = sensor_msgs::image_encodings;
-using namespace std;
-using namespace boost;
 
 mesh_filter::DepthSelfFiltering::~DepthSelfFiltering()
 {
@@ -70,14 +68,14 @@ void mesh_filter::DepthSelfFiltering::onInit()
   private_nh.param("shadow_threshold", shadow_threshold_, 0.3);
   private_nh.param("padding_scale", padding_scale_, 1.0);
   private_nh.param("padding_offset", padding_offset_, 0.005);
-  double tf_update_rate = 30;
+  double tf_update_rate = 30.;
   private_nh.param("tf_update_rate", tf_update_rate, 30.0);
-  transform_provider_.setUpdateInterval(long(1000000.0 / tf_update_rate));
+  transform_provider_.setUpdateRate(tf_update_rate);
 
-  image_transport::SubscriberStatusCallback itssc = bind(&DepthSelfFiltering::connectCb, this);
-  ros::SubscriberStatusCallback rssc = bind(&DepthSelfFiltering::connectCb, this);
+  image_transport::SubscriberStatusCallback itssc = std::bind(&DepthSelfFiltering::connectCb, this);
+  ros::SubscriberStatusCallback rssc = std::bind(&DepthSelfFiltering::connectCb, this);
 
-  lock_guard<mutex> lock(connect_mutex_);
+  std::lock_guard<std::mutex> lock(connect_mutex_);
   pub_filtered_depth_image_ =
       filtered_depth_transport_->advertiseCamera("/filtered/depth", queue_size_, itssc, itssc, rssc, rssc);
   pub_filtered_label_image_ =
@@ -94,7 +92,7 @@ void mesh_filter::DepthSelfFiltering::onInit()
 
   mesh_filter_.reset(
       new MeshFilter<StereoCameraModel>(bind(&TransformProvider::getTransform, &transform_provider_, _1, _2),
-                                        mesh_filter::StereoCameraModel::RegisteredPSDKParams));
+                                        mesh_filter::StereoCameraModel::REGISTERED_PSDK_PARAMS));
   mesh_filter_->parameters().setDepthRange(near_clipping_plane_distance_, far_clipping_plane_distance_);
   mesh_filter_->setShadowThreshold(shadow_threshold_);
   mesh_filter_->setPaddingOffset(padding_offset_);
@@ -115,9 +113,9 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d
 
   const float* src = (const float*)&depth_msg->data[0];
   //*
-  static unsigned dataSize = 0;
-  static unsigned short* data = 0;
-  if (dataSize < depth_msg->width * depth_msg->height)
+  static unsigned data_size = 0;
+  static unsigned short* data = nullptr;
+  if (data_size < depth_msg->width * depth_msg->height)
     data = new unsigned short[depth_msg->width * depth_msg->height];
   for (unsigned idx = 0; idx < depth_msg->width * depth_msg->height; ++idx)
     data[idx] = (unsigned short)(src[idx] * 1000.0);
@@ -132,7 +130,8 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d
     filtered_depth_ptr_->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
     filtered_depth_ptr_->header = depth_msg->header;
 
-    if (filtered_depth_ptr_->image.cols != depth_msg->width || filtered_depth_ptr_->image.rows != depth_msg->height)
+    if (static_cast<uint32_t>(filtered_depth_ptr_->image.cols) != depth_msg->width ||
+        static_cast<uint32_t>(filtered_depth_ptr_->image.rows) != depth_msg->height)
       filtered_depth_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_32FC1);
     mesh_filter_->getFilteredDepth((float*)filtered_depth_ptr_->image.data);
     pub_filtered_depth_image_.publish(filtered_depth_ptr_->toImageMsg(), info_msg);
@@ -144,7 +143,8 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d
     model_depth_ptr_->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
     model_depth_ptr_->header = depth_msg->header;
 
-    if (model_depth_ptr_->image.cols != depth_msg->width || model_depth_ptr_->image.rows != depth_msg->height)
+    if (static_cast<uint32_t>(model_depth_ptr_->image.cols) != depth_msg->width ||
+        static_cast<uint32_t>(model_depth_ptr_->image.rows) != depth_msg->height)
       model_depth_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_32FC1);
     mesh_filter_->getModelDepth((float*)model_depth_ptr_->image.data);
     pub_model_depth_image_.publish(model_depth_ptr_->toImageMsg(), info_msg);
@@ -155,7 +155,8 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d
     filtered_label_ptr_->encoding = sensor_msgs::image_encodings::RGBA8;
     filtered_label_ptr_->header = depth_msg->header;
 
-    if (filtered_label_ptr_->image.cols != depth_msg->width || filtered_label_ptr_->image.rows != depth_msg->height)
+    if (static_cast<uint32_t>(filtered_label_ptr_->image.cols) != depth_msg->width ||
+        static_cast<uint32_t>(filtered_label_ptr_->image.rows) != depth_msg->height)
       filtered_label_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_8UC4);
     mesh_filter_->getFilteredLabels((unsigned int*)filtered_label_ptr_->image.data);
     pub_filtered_label_image_.publish(filtered_label_ptr_->toImageMsg(), info_msg);
@@ -165,7 +166,8 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d
   {
     model_label_ptr_->encoding = sensor_msgs::image_encodings::RGBA8;
     model_label_ptr_->header = depth_msg->header;
-    if (model_label_ptr_->image.cols != depth_msg->width || model_label_ptr_->image.rows != depth_msg->height)
+    if (static_cast<uint32_t>(model_label_ptr_->image.cols) != depth_msg->width ||
+        static_cast<uint32_t>(model_label_ptr_->image.rows) != depth_msg->height)
       model_label_ptr_->image = cv::Mat(depth_msg->height, depth_msg->width, CV_8UC4);
     mesh_filter_->getModelLabels((unsigned int*)model_label_ptr_->image.data);
     pub_model_label_image_.publish(model_label_ptr_->toImageMsg(), info_msg);
@@ -174,17 +176,20 @@ void mesh_filter::DepthSelfFiltering::filter(const sensor_msgs::ImageConstPtr& d
 
 void mesh_filter::DepthSelfFiltering::addMeshes(MeshFilter<StereoCameraModel>& mesh_filter)
 {
-  robot_model_loader::RobotModelLoader robotModelLoader("robot_description");
-  robot_model::RobotModelConstPtr robotModel = robotModelLoader.getModel();
-  const vector<robot_model::LinkModel*>& links = robotModel->getLinkModelsWithCollisionGeometry();
-  for (size_t i = 0; i < links.size(); ++i)
+  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
+  robot_model::RobotModelConstPtr robot_model = robot_model_loader.getModel();
+  const auto& links = robot_model->getLinkModelsWithCollisionGeometry();
+  for (auto link : links)
   {
-    shapes::ShapeConstPtr shape = links[i]->getShape();
-    if (shape->type == shapes::MESH)
+    const auto& shapes = link->getShapes();
+    for (const auto& shape : shapes)
     {
-      const shapes::Mesh& m = static_cast<const shapes::Mesh&>(*shape);
-      MeshHandle mesh_handle = mesh_filter.addMesh(m);
-      transform_provider_.addHandle(mesh_handle, links[i]->getName());
+      if (shape->type == shapes::MESH)
+      {
+        const shapes::Mesh& m = static_cast<const shapes::Mesh&>(*shape);
+        MeshHandle mesh_handle = mesh_filter.addMesh(m);
+        transform_provider_.addHandle(mesh_handle, link->getName());
+      }
     }
   }
 }
@@ -192,7 +197,7 @@ void mesh_filter::DepthSelfFiltering::addMeshes(MeshFilter<StereoCameraModel>& m
 // Handles (un)subscribing when clients (un)subscribe
 void mesh_filter::DepthSelfFiltering::connectCb()
 {
-  lock_guard<mutex> lock(connect_mutex_);
+  std::lock_guard<std::mutex> lock(connect_mutex_);
   if (pub_filtered_depth_image_.getNumSubscribers() == 0 && pub_filtered_label_image_.getNumSubscribers() == 0 &&
       pub_model_depth_image_.getNumSubscribers() == 0 && pub_model_label_image_.getNumSubscribers() == 0)
   {
diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp
index 4f351b53e6..834d2b4edd 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 <fstream>
 #include <stdexcept>
 #include <vector>
-#include <boost/thread.hpp>
 #include <ros/console.h>
 
 using namespace std;
@@ -352,8 +351,8 @@ GLuint mesh_filter::GLRenderer::loadShaders(const string& vertex_source, const s
   return program_id;
 }
 
-map<boost::thread::id, pair<unsigned, GLuint> > mesh_filter::GLRenderer::context_;
-boost::mutex mesh_filter::GLRenderer::context_lock_;
+map<std::thread::id, pair<unsigned, GLuint> > mesh_filter::GLRenderer::context_;
+std::mutex mesh_filter::GLRenderer::context_lock_;
 bool mesh_filter::GLRenderer::glutInitialized_ = false;
 
 namespace
@@ -363,7 +362,7 @@ void nullDisplayFunction(){};
 
 void mesh_filter::GLRenderer::createGLContext()
 {
-  boost::mutex::scoped_lock _(context_lock_);
+  std::unique_lock<std::mutex> _(context_lock_);
   if (!glutInitialized_)
   {
     char buffer[1];
@@ -376,8 +375,8 @@ void mesh_filter::GLRenderer::createGLContext()
   }
 
   // check if our thread is initialized
-  boost::thread::id thread_id = boost::this_thread::get_id();
-  map<boost::thread::id, pair<unsigned, GLuint> >::iterator context_it = context_.find(thread_id);
+  std::thread::id thread_id = std::this_thread::get_id();
+  map<std::thread::id, pair<unsigned, GLuint> >::iterator context_it = context_.find(thread_id);
 
   if (context_it == context_.end())
   {
@@ -410,9 +409,9 @@ void mesh_filter::GLRenderer::createGLContext()
 
 void mesh_filter::GLRenderer::deleteGLContext()
 {
-  boost::mutex::scoped_lock _(context_lock_);
-  boost::thread::id thread_id = boost::this_thread::get_id();
-  map<boost::thread::id, pair<unsigned, GLuint> >::iterator context_it = context_.find(thread_id);
+  std::unique_lock<std::mutex> _(context_lock_);
+  std::thread::id thread_id = std::this_thread::get_id();
+  map<std::thread::id, pair<unsigned, GLuint> >::iterator context_it = context_.find(thread_id);
   if (context_it == context_.end())
   {
     stringstream error_msg;
diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp
index 98ed797ae4..007e9e8397 100644
--- a/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp
+++ b/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp
@@ -52,7 +52,7 @@ using namespace std;
 using namespace Eigen;
 using shapes::Mesh;
 
-mesh_filter::MeshFilter::MeshFilter(const boost::function<bool(MeshFilter::MeshHandle, Isometry3d&)>& transform_callback)
+mesh_filter::MeshFilter::MeshFilter(const std::function<bool(MeshFilter::MeshHandle, Isometry3d&)>& 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!
@@ -117,7 +117,7 @@ void mesh_filter::MeshFilter::setSize(unsigned width, unsigned height)
 }
 
 void mesh_filter::MeshFilter::setTransformCallback(
-    const boost::function<bool(mesh_filter::MeshFilter::MeshHandle, Isometry3d&)>& transform_callback)
+    const std::function<bool(mesh_filter::MeshFilter::MeshHandle, Isometry3d&)>& transform_callback)
 {
   transform_callback_ = transform_callback;
 }
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 edc97ceb91..9ce1f2da88 100644
--- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp
+++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp
@@ -66,8 +66,8 @@ mesh_filter::MeshFilterBase::MeshFilterBase(const TransformCallback& transform_c
   , padding_offset_(0.01)
   , shadow_threshold_(0.5)
 {
-  filter_thread_ = boost::thread(boost::bind(&MeshFilterBase::run, this, render_vertex_shader, render_fragment_shader,
-                                             filter_vertex_shader, filter_fragment_shader));
+  filter_thread_ = std::thread(std::bind(&MeshFilterBase::run, this, render_vertex_shader, render_fragment_shader,
+                                         filter_vertex_shader, filter_fragment_shader));
 }
 
 void mesh_filter::MeshFilterBase::initialize(const std::string& render_vertex_shader,
@@ -121,7 +121,7 @@ void mesh_filter::MeshFilterBase::initialize(const std::string& render_vertex_sh
 mesh_filter::MeshFilterBase::~MeshFilterBase()
 {
   {
-    boost::unique_lock<boost::mutex> lock(jobs_mutex_);
+    std::unique_lock<std::mutex> lock(jobs_mutex_);
     stop_ = true;
     while (!jobs_queue_.empty())
     {
@@ -136,7 +136,7 @@ mesh_filter::MeshFilterBase::~MeshFilterBase()
 void mesh_filter::MeshFilterBase::addJob(const JobPtr& job) const
 {
   {
-    boost::unique_lock<boost::mutex> _(jobs_mutex_);
+    std::unique_lock<std::mutex> _(jobs_mutex_);
     jobs_queue_.push(job);
   }
   jobs_condition_.notify_one();
@@ -163,15 +163,15 @@ void mesh_filter::MeshFilterBase::setSize(unsigned int width, unsigned int heigh
 
 void mesh_filter::MeshFilterBase::setTransformCallback(const TransformCallback& transform_callback)
 {
-  boost::mutex::scoped_lock _(transform_callback_mutex_);
+  std::unique_lock<std::mutex> _(transform_callback_mutex_);
   transform_callback_ = transform_callback;
 }
 
 mesh_filter::MeshHandle mesh_filter::MeshFilterBase::addMesh(const shapes::Mesh& mesh)
 {
-  boost::mutex::scoped_lock _(meshes_mutex_);
+  std::unique_lock<std::mutex> _(meshes_mutex_);
 
-  JobPtr job(new FilterJob<void>(boost::bind(&MeshFilterBase::addMeshHelper, this, next_handle_, &mesh)));
+  JobPtr job(new FilterJob<void>(std::bind(&MeshFilterBase::addMeshHelper, this, next_handle_, &mesh)));
   addJob(job);
   job->wait();
   mesh_filter::MeshHandle ret = next_handle_;
@@ -193,8 +193,8 @@ void mesh_filter::MeshFilterBase::addMeshHelper(MeshHandle handle, const shapes:
 
 void mesh_filter::MeshFilterBase::removeMesh(MeshHandle handle)
 {
-  boost::mutex::scoped_lock _(meshes_mutex_);
-  FilterJob<bool>* remover = new FilterJob<bool>(boost::bind(&MeshFilterBase::removeMeshHelper, this, handle));
+  std::unique_lock<std::mutex> _(meshes_mutex_);
+  FilterJob<bool>* remover = new FilterJob<bool>(std::bind(&MeshFilterBase::removeMeshHelper, this, handle));
   JobPtr job(remover);
   addJob(job);
   job->wait();
@@ -217,19 +217,18 @@ void mesh_filter::MeshFilterBase::setShadowThreshold(float threshold)
 
 void mesh_filter::MeshFilterBase::getModelLabels(LabelType* labels) const
 {
-  JobPtr job(
-      new FilterJob<void>(boost::bind(&GLRenderer::getColorBuffer, mesh_renderer_.get(), (unsigned char*)labels)));
+  JobPtr job(new FilterJob<void>(std::bind(&GLRenderer::getColorBuffer, mesh_renderer_.get(), (unsigned char*)labels)));
   addJob(job);
   job->wait();
 }
 
 void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const
 {
-  JobPtr job1(new FilterJob<void>(boost::bind(&GLRenderer::getDepthBuffer, mesh_renderer_.get(), depth)));
+  JobPtr job1(new FilterJob<void>(std::bind(&GLRenderer::getDepthBuffer, mesh_renderer_.get(), depth)));
   JobPtr job2(new FilterJob<void>(
-      boost::bind(&SensorModel::Parameters::transformModelDepthToMetricDepth, sensor_parameters_.get(), depth)));
+      std::bind(&SensorModel::Parameters::transformModelDepthToMetricDepth, sensor_parameters_.get(), depth)));
   {
-    boost::unique_lock<boost::mutex> lock(jobs_mutex_);
+    std::unique_lock<std::mutex> lock(jobs_mutex_);
     jobs_queue_.push(job1);
     jobs_queue_.push(job2);
   }
@@ -240,11 +239,11 @@ void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const
 
 void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const
 {
-  JobPtr job1(new FilterJob<void>(boost::bind(&GLRenderer::getDepthBuffer, depth_filter_.get(), depth)));
+  JobPtr job1(new FilterJob<void>(std::bind(&GLRenderer::getDepthBuffer, depth_filter_.get(), depth)));
   JobPtr job2(new FilterJob<void>(
-      boost::bind(&SensorModel::Parameters::transformFilteredDepthToMetricDepth, sensor_parameters_.get(), depth)));
+      std::bind(&SensorModel::Parameters::transformFilteredDepthToMetricDepth, sensor_parameters_.get(), depth)));
   {
-    boost::unique_lock<boost::mutex> lock(jobs_mutex_);
+    std::unique_lock<std::mutex> lock(jobs_mutex_);
     jobs_queue_.push(job1);
     jobs_queue_.push(job2);
   }
@@ -255,7 +254,7 @@ void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const
 
 void mesh_filter::MeshFilterBase::getFilteredLabels(LabelType* labels) const
 {
-  JobPtr job(new FilterJob<void>(boost::bind(&GLRenderer::getColorBuffer, depth_filter_.get(), (unsigned char*)labels)));
+  JobPtr job(new FilterJob<void>(std::bind(&GLRenderer::getColorBuffer, depth_filter_.get(), (unsigned char*)labels)));
   addJob(job);
   job->wait();
 }
@@ -269,7 +268,7 @@ void mesh_filter::MeshFilterBase::run(const std::string& render_vertex_shader,
 
   while (!stop_)
   {
-    boost::unique_lock<boost::mutex> lock(jobs_mutex_);
+    std::unique_lock<std::mutex> lock(jobs_mutex_);
     // check if we have new sensor data to be processed. If not, wait until we get notified.
     if (jobs_queue_.empty())
       jobs_condition_.wait(lock);
@@ -295,7 +294,7 @@ void mesh_filter::MeshFilterBase::filter(const void* sensor_data, GLushort type,
     throw std::runtime_error(msg.str());
   }
 
-  JobPtr job(new FilterJob<void>(boost::bind(&MeshFilterBase::doFilter, this, sensor_data, type)));
+  JobPtr job(new FilterJob<void>(std::bind(&MeshFilterBase::doFilter, this, sensor_data, type)));
   addJob(job);
   if (wait)
     job->wait();
@@ -303,7 +302,7 @@ void mesh_filter::MeshFilterBase::filter(const void* sensor_data, GLushort type,
 
 void mesh_filter::MeshFilterBase::doFilter(const void* sensor_data, const int encoding) const
 {
-  boost::mutex::scoped_lock _(transform_callback_mutex_);
+  std::unique_lock<std::mutex> _(transform_callback_mutex_);
 
   mesh_renderer_->begin();
   sensor_parameters_->setRenderParameters(*mesh_renderer_);
diff --git a/moveit_ros/perception/mesh_filter/src/stereo_camera_model.cpp b/moveit_ros/perception/mesh_filter/src/stereo_camera_model.cpp
index 5881faffd8..eaa9d54118 100644
--- a/moveit_ros/perception/mesh_filter/src/stereo_camera_model.cpp
+++ b/moveit_ros/perception/mesh_filter/src/stereo_camera_model.cpp
@@ -37,8 +37,6 @@
 #include <moveit/mesh_filter/stereo_camera_model.h>
 #include <moveit/mesh_filter/gl_renderer.h>
 
-using namespace std;
-
 mesh_filter::StereoCameraModel::Parameters::Parameters(unsigned width, unsigned height,
                                                        float near_clipping_plane_distance,
                                                        float far_clipping_plane_distance, float fx, float fy, float cx,
@@ -112,7 +110,7 @@ void mesh_filter::StereoCameraModel::Parameters::setFilterParameters(GLRenderer&
 const mesh_filter::StereoCameraModel::Parameters& mesh_filter::StereoCameraModel::REGISTERED_PSDK_PARAMS =
     mesh_filter::StereoCameraModel::Parameters(640, 480, 0.4, 10.0, 525, 525, 319.5, 239.5, 0.075, 0.125);
 
-const string mesh_filter::StereoCameraModel::RENDER_VERTEX_SHADER_SOURCE =
+const std::string mesh_filter::StereoCameraModel::RENDER_VERTEX_SHADER_SOURCE =
     "#version 120\n"
     "uniform vec3 padding_coefficients;"
     "void main()"
@@ -127,22 +125,23 @@ const string mesh_filter::StereoCameraModel::RENDER_VERTEX_SHADER_SOURCE =
     "  gl_Position.y = -gl_Position.y;"
     "}";
 
-const string mesh_filter::StereoCameraModel::RENDER_FRAGMENT_SHADER_SOURCE = "#version 120\n"
-                                                                             "void main()"
-                                                                             "{"
-                                                                             "  gl_FragColor = gl_Color;"
-                                                                             "}";
-
-const string mesh_filter::StereoCameraModel::FILTER_VERTEX_SHADER_SOURCE = "#version 120\n"
-                                                                           "void main ()"
-                                                                           "{"
-                                                                           "     gl_FrontColor = gl_Color;"
-                                                                           "     gl_TexCoord[0] = gl_MultiTexCoord0;"
-                                                                           "     gl_Position = gl_Vertex;"
-                                                                           "  gl_Position.w = 1.0;"
-                                                                           "}";
-
-const string mesh_filter::StereoCameraModel::FILTER_FRAGMENT_SHADER_SOURCE =
+const std::string mesh_filter::StereoCameraModel::RENDER_FRAGMENT_SHADER_SOURCE = "#version 120\n"
+                                                                                  "void main()"
+                                                                                  "{"
+                                                                                  "  gl_FragColor = gl_Color;"
+                                                                                  "}";
+
+const std::string mesh_filter::StereoCameraModel::FILTER_VERTEX_SHADER_SOURCE =
+    "#version 120\n"
+    "void main ()"
+    "{"
+    "     gl_FrontColor = gl_Color;"
+    "     gl_TexCoord[0] = gl_MultiTexCoord0;"
+    "     gl_Position = gl_Vertex;"
+    "  gl_Position.w = 1.0;"
+    "}";
+
+const std::string mesh_filter::StereoCameraModel::FILTER_FRAGMENT_SHADER_SOURCE =
     "#version 120\n"
     "uniform sampler2D sensor;"
     "uniform sampler2D depth;"
diff --git a/moveit_ros/perception/mesh_filter/src/transform_provider.cpp b/moveit_ros/perception/mesh_filter/src/transform_provider.cpp
index cc5e8bda85..e16fb34de8 100644
--- a/moveit_ros/perception/mesh_filter/src/transform_provider.cpp
+++ b/moveit_ros/perception/mesh_filter/src/transform_provider.cpp
@@ -39,13 +39,7 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_eigen/tf2_eigen.h>
 
-using namespace std;
-using namespace boost;
-using namespace Eigen;
-using namespace tf;
-using namespace mesh_filter;
-
-TransformProvider::TransformProvider(unsigned long interval_us) : stop_(true), interval_us_(interval_us)
+TransformProvider::TransformProvider(double update_rate) : stop_(true), update_rate_(update_rate)
 {
   tf_buffer_ = std::make_shared<tf2_ros::Buffer>();
   tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_));
@@ -61,7 +55,7 @@ TransformProvider::~TransformProvider()
 void TransformProvider::start()
 {
   stop_ = false;
-  thread_ = thread(&TransformProvider::run, this);
+  thread_ = std::thread(&TransformProvider::run, this);
 }
 
 void TransformProvider::stop()
@@ -70,103 +64,107 @@ void TransformProvider::stop()
   thread_.join();
 }
 
-void TransformProvider::addHandle(MeshHandle handle, const string& name)
+void TransformProvider::addHandle(mesh_filter::MeshHandle handle, const std::string& name)
 {
   if (!stop_)
-    throw runtime_error("Can not add handles if TransformProvider is running");
+    throw std::runtime_error("Can not add handles if TransformProvider is running");
 
   handle2context_[handle].reset(new TransformContext(name));
 }
 
-void TransformProvider::setFrame(const string& frame)
+void TransformProvider::setFrame(const std::string& frame)
 {
   if (frame_id_ != frame)
   {
     frame_id_ = frame;
-    for (map<MeshHandle, shared_ptr<TransformContext> >::iterator contextIt = handle2context_.begin();
-         contextIt != handle2context_.end(); ++contextIt)
+    for (auto& context_it : handle2context_)
     {
       // invalidate transformations
-      contextIt->second->mutex_.lock();
-      contextIt->second->transformation_.matrix().setZero();
-      contextIt->second->mutex_.unlock();
+      context_it.second->mutex_.lock();
+      context_it.second->transformation_.matrix().setZero();
+      context_it.second->mutex_.unlock();
     }
   }
 }
 
-bool TransformProvider::getTransform(MeshHandle handle, Isometry3d& transform) const
+bool TransformProvider::getTransform(mesh_filter::MeshHandle handle, Eigen::Isometry3d& transform) const
 {
-  map<MeshHandle, shared_ptr<TransformContext> >::const_iterator contextIt = handle2context_.find(handle);
+  auto context_it = handle2context_.find(handle);
 
-  if (contextIt == handle2context_.end())
+  if (context_it == handle2context_.end())
   {
     ROS_ERROR("Unable to find mesh with handle %d", handle);
     return false;
   }
-  contextIt->second->mutex_.lock();
-  transform = contextIt->second->transformation_;
-  contextIt->second->mutex_.unlock();
+  context_it->second->mutex_.lock();
+  transform = context_it->second->transformation_;
+  context_it->second->mutex_.unlock();
   return !(transform.matrix().isZero(0));
 }
 
 void TransformProvider::run()
 {
   if (handle2context_.empty())
-    throw runtime_error("TransformProvider is listening to empty list of frames!");
+    throw std::runtime_error("TransformProvider is listening to empty list of frames!");
 
-  while (!stop_)
+  while (ros::ok() && !stop_)
   {
     updateTransforms();
-    usleep(interval_us_);
+    update_rate_.sleep();
   }
 }
 
-void TransformProvider::setUpdateInterval(unsigned long usecs)
+void TransformProvider::setUpdateRate(double update_rate)
 {
-  interval_us_ = usecs;
+  update_rate_ = ros::Rate(update_rate);
 }
 
 void TransformProvider::updateTransforms()
 {
-  static tf2::Stamped<Isometry3d> input_transform, output_transform;
+  // Don't bother if frame_id_ is empty (true initially)
+  if (frame_id_.empty())
+  {
+    ROS_DEBUG_THROTTLE(2., "Not updating transforms because frame_id_ is empty.");
+    return;
+  }
+
+  static tf2::Stamped<Eigen::Isometry3d> input_transform, output_transform;
   static robot_state::RobotStatePtr robot_state;
   robot_state = psm_->getStateMonitor()->getCurrentState();
   try
   {
     geometry_msgs::TransformStamped common_tf =
         tf_buffer_->lookupTransform(frame_id_, psm_->getPlanningScene()->getPlanningFrame(), ros::Time(0.0));
+    input_transform.stamp_ = common_tf.header.stamp;
   }
   catch (tf2::TransformException& ex)
   {
     ROS_ERROR("TF Problem: %s", ex.what());
     return;
   }
-  input_transform.stamp_ = common_tf.header.stamp;
   input_transform.frame_id_ = psm_->getPlanningScene()->getPlanningFrame();
 
-  for (map<MeshHandle, shared_ptr<TransformContext> >::const_iterator contextIt = handle2context_.begin();
-       contextIt != handle2context_.end(); ++contextIt)
+  for (auto& context_it : handle2context_)
   {
     try
     {
       // TODO: check logic here - which global collision body's transform should be used?
-      input_transform.setData(
-          robot_state->getAttachedBody(contextIt->second->frame_id_)->getGlobalCollisionBodyTransforms()[0]);
+      input_transform.setData(robot_state->getGlobalLinkTransform(context_it.second->frame_id_));
       tf_buffer_->transform(input_transform, output_transform, frame_id_);
     }
     catch (const tf2::TransformException& ex)
     {
-      handle2context_[contextIt->first]->mutex_.lock();
-      handle2context_[contextIt->first]->transformation_.matrix().setZero();
-      handle2context_[contextIt->first]->mutex_.unlock();
+      handle2context_[context_it.first]->mutex_.lock();
+      handle2context_[context_it.first]->transformation_.matrix().setZero();
+      handle2context_[context_it.first]->mutex_.unlock();
       continue;
     }
     catch (std::exception& ex)
     {
       ROS_ERROR("Caught %s while updating transforms", ex.what());
     }
-    handle2context_[contextIt->first]->mutex_.lock();
-    handle2context_[contextIt->first]->transformation_ = static_cast<Isometry3d>(output_transform);
-    handle2context_[contextIt->first]->mutex_.unlock();
+    handle2context_[context_it.first]->mutex_.lock();
+    handle2context_[context_it.first]->transformation_ = static_cast<Eigen::Isometry3d>(output_transform);
+    handle2context_[context_it.first]->mutex_.unlock();
   }
 }
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 e0a0c628a0..47fe8fbc4e 100644
--- a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp
+++ b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp
@@ -43,7 +43,7 @@
 using namespace mesh_filter;
 using namespace Eigen;
 using namespace std;
-using namespace boost;
+using namespace std::placeholders;
 
 namespace mesh_filter_test
 {
@@ -118,7 +118,7 @@ MeshFilterTest<Type>::MeshFilterTest(unsigned width, unsigned height, double nea
   , shadow_(shadow)
   , epsilon_(epsilon)
   , sensor_parameters_(width, height, near_, far_, width >> 1, height >> 1, width >> 1, height >> 1, 0.1, 0.1)
-  , filter_(boost::bind(&MeshFilterTest<Type>::transformCallback, this, _1, _2), sensor_parameters_)
+  , filter_(std::bind(&MeshFilterTest<Type>::transformCallback, this, _1, _2), sensor_parameters_)
   , sensor_data_(width_ * height_)
   , distance_(0.0)
 {
diff --git a/moveit_ros/perception/moveit_depth_self_filter.xml b/moveit_ros/perception/moveit_depth_self_filter.xml
new file mode 100644
index 0000000000..4153868295
--- /dev/null
+++ b/moveit_ros/perception/moveit_depth_self_filter.xml
@@ -0,0 +1,7 @@
+<library path="lib/libmoveit_depth_self_filter">
+    <class name="mesh_filter/DepthSelfFiltering" type="mesh_filter::DepthSelfFiltering" base_class_type="nodelet::Nodelet">
+        <description>
+            Nodelet for filtering meshes from depth images. e.g. meshes of the robot or any attached object where a transformation can be provided for.
+        </description>
+    </class>
+</library>
diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml
index 22cd6c877c..5c1ed4ccc9 100644
--- a/moveit_ros/perception/package.xml
+++ b/moveit_ros/perception/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_perception</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Components of MoveIt! connecting to perception</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
@@ -27,6 +27,7 @@
   <depend>image_transport</depend>
   <depend>glut</depend>
   <depend>libglew-dev</depend>
+  <depend>libomp-dev</depend>
   <depend>opengl</depend>
   <depend>cv_bridge</depend>
   <depend>sensor_msgs</depend>
@@ -37,6 +38,8 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>moveit_ros_occupancy_map_monitor</depend>
+  <depend>moveit_ros_planning</depend>
+  <depend>nodelet</depend>
 
   <build_depend>eigen</build_depend>
 
@@ -45,6 +48,7 @@
   <export>
     <moveit_ros_perception plugin="${prefix}/pointcloud_octomap_updater_plugin_description.xml"/>
     <moveit_ros_perception plugin="${prefix}/depth_image_octomap_updater_plugin_description.xml"/>
+    <nodelet plugin="${prefix}/moveit_depth_self_filter.xml"/>
   </export>
 
 </package>
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 782525c283..a4aa0cb15d 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
@@ -95,7 +95,7 @@ class ShapeMask
   {
     SeeShape()
     {
-      body = NULL;
+      body = nullptr;
     }
 
     bodies::Body* body;
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 870340ec90..fc86b23b4d 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
@@ -213,10 +213,7 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::
   Eigen::Vector3d sensor_origin_eigen(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());
 
   if (!updateTransformCache(cloud_msg->header.frame_id, cloud_msg->header.stamp))
-  {
-    ROS_ERROR_THROTTLE(1, "Transform cache was not updated. Self-filtering may fail.");
     return;
-  }
 
   /* mask out points on the robot */
   shape_mask_->maskContainment(*cloud_msg, sensor_origin_eigen, 0.0, max_range_, mask_);
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 b298bc654f..db0e31a631 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
@@ -47,14 +47,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.
diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst
index 7c5dc53ad0..7b1340540c 100644
--- a/moveit_ros/planning/CHANGELOG.rst
+++ b/moveit_ros/planning/CHANGELOG.rst
@@ -2,6 +2,37 @@
 Changelog for package moveit_ros_planning
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+* Use moveit-resources@master (`#2951 <https://github.com/ros-planning/moveit/issues/2951>`_)
+
+  - Simplify launch files to use the test_environment.launch files from moveit_resources@master
+  - Provide compatibility to the Noetic-style configuration of (multiple) planning pipelines
+    Only a single pipeline can be used at a time, specified via the ~default_planning_pipeline parameter.
+* Contributors: Robert Haschke
+
+1.0.8 (2021-05-23)
+------------------
+* Unify and simplify CSM::haveCompleteState overloads (`#2663 <https://github.com/ros-planning/moveit/issues/2663>`_)
+* FixStartStateBounds: Copy attached bodies when adapting the start state (`#2398 <https://github.com/ros-planning/moveit/issues/2398>`_)
+* PlanExecution: Correctly handle preempt-requested flag (`#2554 <https://github.com/ros-planning/moveit/issues/2554>`_)
+* thread safety in clear octomap & only update geometry (`#2500 <https://github.com/ros-planning/moveit/issues/2500>`_)
+* Fix some typos in comments (`#2466 <https://github.com/ros-planning/moveit/issues/2466>`_)
+* Simplify logic in PSM (`#2632 <https://github.com/ros-planning/moveit/issues/2632>`_) (`#2637 <https://github.com/ros-planning/moveit/issues/2637>`_)
+* Contributors: Jafar Abdi, Michael Görner, Robert Haschke, Simon Schmeisser, Tyler Weaver, Udbhavbisarya23
+
+1.0.7 (2020-11-20)
+------------------
+* [fix] Fix "Clear Octomap" button, disable when no octomap is published (`#2320 <https://github.com/ros-planning/moveit/issues/2320>`_)
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: Felix von Drigalski
+
 1.0.6 (2020-08-19)
 ------------------
 * [fix]   Fix segfault in PSM::clearOctomap() (`#2193 <https://github.com/ros-planning/moveit/issues/2193>`_)
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..2dacbdb662 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
@@ -63,7 +63,7 @@ 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_;
 };
 
diff --git a/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp b/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp
index 0a08260f49..c4141111a5 100644
--- a/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp
+++ b/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp
@@ -36,6 +36,7 @@
 #include <pluginlib/class_loader.hpp>
 #include <memory>
 
+static const std::string LOGNAME = "collision_detection";
 namespace collision_detection
 {
 class CollisionPluginLoader::CollisionPluginLoaderImpl
@@ -108,7 +109,10 @@ bool CollisionPluginLoader::activate(const std::string& name, const planning_sce
 void CollisionPluginLoader::setupScene(ros::NodeHandle& nh, const planning_scene::PlanningScenePtr& scene)
 {
   if (!scene)
+  {
+    ROS_WARN_NAMED(LOGNAME, "Cannot setup scene, PlanningScenePtr is null.");
     return;
+  }
 
   std::string param_name;
   std::string collision_detector_name;
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 edda1398c7..84129f862d 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
@@ -42,7 +42,8 @@
 
 namespace constraint_sampler_manager_loader
 {
-MOVEIT_CLASS_FORWARD(ConstraintSamplerManagerLoader);
+MOVEIT_CLASS_FORWARD(
+    ConstraintSamplerManagerLoader);  // Defines ConstraintSamplerManagerLoaderPtr, ConstPtr, WeakPtr... etc
 
 class ConstraintSamplerManagerLoader
 {
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 67c68562b8..ddb03259aa 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
@@ -44,7 +44,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
@@ -104,7 +104,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<std::string> groups_;
diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml
index 01435e5ffd..9cec2ce55d 100644
--- a/moveit_ros/planning/package.xml
+++ b/moveit_ros/planning/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="utf-8"?>
 <package format="2">
   <name>moveit_ros_planning</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Planning components of MoveIt! that use ROS</description>
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="robot.moveit@gmail.com">Sachin Chitta</author>
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 9ba9db39c3..5933ce5b5b 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
@@ -45,10 +45,12 @@
 #include <moveit/sensor_manager/sensor_manager.h>
 #include <pluginlib/class_loader.hpp>
 
+#include <atomic>
+
 /** \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
 {
@@ -134,7 +136,7 @@ class PlanExecution
 
       In case there is no \e planning_scene or \e planning_scene_monitor set in the \e plan they will be set at the
       start of the method. They are then used to monitor the execution. */
-  moveit_msgs::MoveItErrorCodes executeAndMonitor(ExecutableMotionPlan& plan);
+  moveit_msgs::MoveItErrorCodes executeAndMonitor(ExecutableMotionPlan& plan, bool reset_preempted = true);
 
   void stop();
 
@@ -156,7 +158,25 @@ class PlanExecution
 
   unsigned int default_max_replan_attempts_;
 
-  bool preempt_requested_;
+  class
+  {
+  private:
+    std::atomic<bool> preemption_requested{ false };
+
+  public:
+    /** \brief check *and clear* the preemption flag
+
+        A true return value has to trigger full execution stop, as it consumes the request */
+    inline bool checkAndClear()
+    {
+      return preemption_requested.exchange(false);
+    }
+    inline void request()
+    {
+      preemption_requested.store(true);
+    }
+  } preempt_;
+
   bool new_scene_update_;
 
   bool execution_complete_;
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 cba80504d6..5400495367 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
@@ -49,7 +49,7 @@
 
 namespace plan_execution
 {
-MOVEIT_CLASS_FORWARD(PlanWithSensing);
+MOVEIT_CLASS_FORWARD(PlanWithSensing);  // Defines PlanWithSensingPtr, ConstPtr, WeakPtr... etc
 
 class PlanWithSensing
 {
diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp
index b8caaedc95..814e301bda 100644
--- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp
+++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp
@@ -82,7 +82,6 @@ plan_execution::PlanExecution::PlanExecution(
 
   default_max_replan_attempts_ = 5;
 
-  preempt_requested_ = false;
   new_scene_update_ = false;
 
   // we want to be notified when new information is available
@@ -99,7 +98,7 @@ plan_execution::PlanExecution::~PlanExecution()
 
 void plan_execution::PlanExecution::stop()
 {
-  preempt_requested_ = true;
+  preempt_.request();
 }
 
 std::string plan_execution::PlanExecution::getErrorCodeString(const moveit_msgs::MoveItErrorCodes& error_code)
@@ -160,7 +159,9 @@ void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan& plan,
 void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& plan, const Options& opt)
 {
   // perform initial configuration steps & various checks
-  preempt_requested_ = false;
+  preempt_.checkAndClear();  // clear any previous preempt_ request
+
+  bool preempt_requested = false;
 
   // run the actual motion plan & execution
   unsigned int max_replan_attempts =
@@ -188,7 +189,8 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p
             opt.plan_callback_(plan) :
             opt.repair_plan_callback_(plan, trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex());
 
-    if (preempt_requested_)
+    preempt_requested = preempt_.checkAndClear();
+    if (preempt_requested)
       break;
 
     // if planning fails in a manner that is not recoverable, we exit the loop,
@@ -216,23 +218,23 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p
       if (opt.before_execution_callback_)
         opt.before_execution_callback_();
 
-      if (preempt_requested_)
+      preempt_requested = preempt_.checkAndClear();
+      if (preempt_requested)
         break;
 
-      // execute the trajectory, and monitor its executionm
-      plan.error_code_ = executeAndMonitor(plan);
+      // execute the trajectory, and monitor its execution
+      plan.error_code_ = executeAndMonitor(plan, false);
     }
 
-    // if we are done, then we exit the loop
-    if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
-      break;
+    if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
+      preempt_requested = true;
 
-    // if execution failed in a manner that we do not consider recoverable, we exit the loop (with failure)
+    // if execution succeeded or failed in a manner that we do not consider recoverable, we exit the loop (with failure)
     if (plan.error_code_.val != moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
       break;
     else
     {
-      // othewrise, we wait (if needed)
+      // otherwise, we wait (if needed)
       if (opt.replan_delay_ > 0.0)
       {
         ROS_INFO_NAMED("plan_execution", "Waiting for a %lf seconds before attempting a new plan ...",
@@ -242,9 +244,14 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p
         ROS_INFO_NAMED("plan_execution", "Done waiting");
       }
     }
-  } while (!preempt_requested_ && max_replan_attempts > replan_attempts);
 
-  if (preempt_requested_)
+    preempt_requested = preempt_.checkAndClear();
+    if (preempt_requested)
+      break;
+
+  } while (replan_attempts < max_replan_attempts);
+
+  if (preempt_requested)
   {
     ROS_DEBUG_NAMED("plan_execution", "PlanExecution was preempted");
     plan.error_code_.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
@@ -313,8 +320,12 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
   return true;
 }
 
-moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(ExecutableMotionPlan& plan)
+moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(ExecutableMotionPlan& plan,
+                                                                               bool reset_preempted)
 {
+  if (reset_preempted)
+    preempt_.checkAndClear();
+
   if (!plan.planning_scene_monitor_)
     plan.planning_scene_monitor_ = planning_scene_monitor_;
   if (!plan.planning_scene_)
@@ -409,7 +420,9 @@ moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(E
   // wait for path to be done, while checking that the path does not become invalid
   ros::Rate r(100);
   path_became_invalid_ = false;
-  while (node_handle_.ok() && !execution_complete_ && !preempt_requested_ && !path_became_invalid_)
+  bool preempt_requested = false;
+
+  while (node_handle_.ok() && !execution_complete_ && !path_became_invalid_)
   {
     r.sleep();
     // check the path if there was an environment update in the meantime
@@ -422,10 +435,14 @@ moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(E
         break;
       }
     }
+
+    preempt_requested = preempt_.checkAndClear();
+    if (preempt_requested)
+      break;
   }
 
   // stop execution if needed
-  if (preempt_requested_)
+  if (preempt_requested)
   {
     ROS_INFO_NAMED("plan_execution", "Stopping execution due to preempt request");
     trajectory_execution_manager_->stopExecution();
@@ -457,7 +474,7 @@ moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(E
     result.val = moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE;
   else
   {
-    if (preempt_requested_)
+    if (preempt_requested)
     {
       result.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
     }
@@ -505,7 +522,7 @@ void plan_execution::PlanExecution::successfulTrajectorySegmentExecution(const E
     {
       // execution of side-effect failed
       ROS_ERROR_NAMED("plan_execution", "Execution of path-completion side-effect failed. Preempting.");
-      preempt_requested_ = true;
+      preempt_.request();
       return;
     }
 
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 dfce531e83..b7bf116b5d 100644
--- a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp
+++ b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp
@@ -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)
diff --git a/moveit_ros/planning/planning_components_tools/results/State_Operations_i2600K_PR2_Groovy.txt b/moveit_ros/planning/planning_components_tools/results/State_Operations_i2600K_PR2_Groovy.txt
index c85b727771..cfadbf5271 100644
--- a/moveit_ros/planning/planning_components_tools/results/State_Operations_i2600K_PR2_Groovy.txt
+++ b/moveit_ros/planning/planning_components_tools/results/State_Operations_i2600K_PR2_Groovy.txt
@@ -66,4 +66,3 @@ left_gripper:FK Default: 0.017607s (0.401701%), [1e-06s --> 3.5e-05 s], 10000 pa
 right_gripper:FK Random: 0.011327s (0.258424%), [1e-06s --> 1.2e-05 s], 10000 parts, 1.1327e-06 s on average (882846 /s)
 left_gripper:FK Random: 0.011019s (0.251397%), [1e-06s --> 9e-06 s], 10000 parts, 1.1019e-06 s on average (907523 /s)
 Unaccounted time : 0.061805 (1.41007 %)
-
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 282cb6c33b..138abb7b87 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
@@ -195,7 +195,7 @@ class PlanningPipeline
   ros::Publisher contacts_publisher_;
 };
 
-MOVEIT_CLASS_FORWARD(PlanningPipeline);
+MOVEIT_CLASS_FORWARD(PlanningPipeline);  // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc
 }  // namespace planning_pipeline
 
 #endif
diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
index 18cb516806..26e52294b8 100644
--- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
+++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
@@ -55,11 +55,12 @@ planning_pipeline::PlanningPipeline::PlanningPipeline(const robot_model::RobotMo
   : nh_(nh), robot_model_(model)
 {
   std::string planner;
-  if (nh_.getParam(planner_plugin_param_name, planner))
+  ros::NodeHandle cnh = planning_interface::getConfigNodeHandle(nh);
+  if (cnh.getParam(planner_plugin_param_name, planner))
     planner_plugin_name_ = planner;
 
   std::string adapters;
-  if (nh_.getParam(adapter_plugins_param_name, adapters))
+  if (cnh.getParam(adapter_plugins_param_name, adapters))
   {
     boost::char_separator<char> sep(" ");
     boost::tokenizer<boost::char_separator<char> > tok(adapters, sep);
@@ -114,7 +115,7 @@ void planning_pipeline::PlanningPipeline::configure()
   try
   {
     planner_instance_ = planner_plugin_loader_->createUniqueInstance(planner_plugin_name_);
-    if (!planner_instance_->initialize(robot_model_, nh_.getNamespace()))
+    if (!planner_instance_->initialize(robot_model_, planning_interface::getConfigNodeHandle(nh_).getNamespace()))
       throw std::runtime_error("Unable to initialize planning plugin");
     ROS_INFO_STREAM("Using planning interface '" << planner_instance_->getDescription() << "'");
   }
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 4ee721ac86..f41a5edd47 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
@@ -49,7 +49,8 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap
   static const std::string BOUNDS_PARAM_NAME;
   static const std::string DT_PARAM_NAME;
 
-  FixStartStateBounds() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
+  FixStartStateBounds()
+    : planning_request_adapter::PlanningRequestAdapter(), nh_(planning_interface::getConfigNodeHandle())
   {
     if (!nh_.getParam(BOUNDS_PARAM_NAME, bounds_dist_))
     {
@@ -178,7 +179,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
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 237b59fe51..87ef1e6f01 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
@@ -49,7 +49,8 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA
   static const std::string JIGGLE_PARAM_NAME;
   static const std::string ATTEMPTS_PARAM_NAME;
 
-  FixStartStateCollision() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
+  FixStartStateCollision()
+    : planning_request_adapter::PlanningRequestAdapter(), nh_(planning_interface::getConfigNodeHandle())
   {
     if (!nh_.getParam(DT_PARAM_NAME, max_dt_offset_))
     {
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 15fbce16fa..16b5ae0e19 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
@@ -45,7 +45,8 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt
 public:
   static const std::string WBOUNDS_PARAM_NAME;
 
-  FixWorkspaceBounds() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
+  FixWorkspaceBounds()
+    : planning_request_adapter::PlanningRequestAdapter(), nh_(planning_interface::getConfigNodeHandle())
   {
     if (!nh_.getParam(WBOUNDS_PARAM_NAME, workspace_extent_))
     {
diff --git a/moveit_ros/planning/planning_request_adapters_plugin_description.xml b/moveit_ros/planning/planning_request_adapters_plugin_description.xml
index 50b5f0b1c0..d883f2a717 100644
--- a/moveit_ros/planning/planning_request_adapters_plugin_description.xml
+++ b/moveit_ros/planning/planning_request_adapters_plugin_description.xml
@@ -37,7 +37,7 @@
 
   <class name="default_planner_request_adapters/AddTimeOptimalParameterization" type="default_planner_request_adapters::AddTimeOptimalParameterization" base_class_type="planning_request_adapter::PlanningRequestAdapter">
     <description>
-      This is an improved time parameterization using Time-Optimal Trajectory Generation for Path Following With Bounded Acceleration and Velocity (Kunz and Stilman, RSS 2008) 
+      This is an improved time parameterization using Time-Optimal Trajectory Generation for Path Following With Bounded Acceleration and Velocity (Kunz and Stilman, RSS 2008)
     </description>
   </class>
 
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 306e67055b..74c8bbdab4 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
@@ -98,26 +98,58 @@ class CurrentStateMonitor
   /** @brief Query whether we have joint state information for all DOFs in the kinematic model
    *  @return False if we have no joint state information for one or more of the joints
    */
-  bool haveCompleteState() const;
+  inline bool haveCompleteState() const
+  {
+    return haveCompleteStateHelper(ros::Time(0), nullptr);
+  }
+
+  /** @brief Query whether we have joint state information for all DOFs in the kinematic model
+   *  @param oldest_allowed_update_time All joint information must be from this time or more current
+   *  @return False if we have no joint state information for one of the joints or if our state
+   *  information is more than \e age old*/
+  inline bool haveCompleteState(const ros::Time& oldest_allowed_update_time) const
+  {
+    return haveCompleteStateHelper(oldest_allowed_update_time, nullptr);
+  }
 
   /** @brief Query whether we have joint state information for all DOFs in the kinematic model
+   *  @param age Joint information must be at most this old
    *  @return False if we have no joint state information for one of the joints or if our state
    *  information is more than \e age old
    */
-  bool haveCompleteState(const ros::Duration& age) const;
+  inline bool haveCompleteState(const ros::Duration& age) const
+  {
+    return haveCompleteStateHelper(ros::Time::now() - age, nullptr);
+  }
 
   /** @brief Query whether we have joint state information for all DOFs in the kinematic model
    *  @param missing_joints Returns the list of joints that are missing
    *  @return False if we have no joint state information for one or more of the joints
    */
-  bool haveCompleteState(std::vector<std::string>& missing_joints) const;
+  inline bool haveCompleteState(std::vector<std::string>& missing_joints) const
+  {
+    return haveCompleteStateHelper(ros::Time(0), &missing_joints);
+  }
 
   /** @brief Query whether we have joint state information for all DOFs in the kinematic model
-   *  @param age The max allowed age of the joint state information
-   *  @param missing_states Returns the list of joints that are missing
+   *  @param oldest_allowed_update_time All joint information must be from this time or more current
+   *  @param missing_joints Returns the list of joints that are missing
    *  @return False if we have no joint state information for one of the joints or if our state
    *  information is more than \e age old*/
-  bool haveCompleteState(const ros::Duration& age, std::vector<std::string>& missing_states) const;
+  inline bool haveCompleteState(const ros::Time& oldest_allowed_update_time,
+                                std::vector<std::string>& missing_joints) const
+  {
+    return haveCompleteStateHelper(oldest_allowed_update_time, &missing_joints);
+  }
+
+  /** @brief Query whether we have joint state information for all DOFs in the kinematic model
+   *  @return False if we have no joint state information for one of the joints or if our state
+   *  information is more than \e age old
+   */
+  inline bool haveCompleteState(const ros::Duration& age, std::vector<std::string>& missing_joints) const
+  {
+    return haveCompleteStateHelper(ros::Time::now() - age, &missing_joints);
+  }
 
   /** @brief Get the current state
    *  @return Returns the current state */
@@ -189,6 +221,9 @@ class CurrentStateMonitor
   }
 
 private:
+  bool haveCompleteStateHelper(const ros::Time& oldest_allowed_update_time,
+                               std::vector<std::string>* missing_joints) const;
+
   void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
   void tfCallback();
 
@@ -211,7 +246,7 @@ class CurrentStateMonitor
   std::shared_ptr<TFConnection> tf_connection_;
 };
 
-MOVEIT_CLASS_FORWARD(CurrentStateMonitor);
+MOVEIT_CLASS_FORWARD(CurrentStateMonitor);  // Defines CurrentStateMonitorPtr, ConstPtr, WeakPtr... etc
 }  // namespace planning_scene_monitor
 
 #endif
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 f2fc78a5c2..d84f80823b 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
@@ -54,7 +54,7 @@
 
 namespace planning_scene_monitor
 {
-MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);
+MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);  // Defines PlanningSceneMonitorPtr, ConstPtr, WeakPtr... etc
 
 /**
  * @brief PlanningSceneMonitor
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 be9997432d..51823c6d00 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
@@ -48,7 +48,7 @@ namespace planning_scene_monitor
 typedef boost::function<void(const robot_state::RobotStateConstPtr& state, const ros::Time& stamp)>
     TrajectoryStateAddedCallback;
 
-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. */
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 9f1cf9b8f1..099c0f9479 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,15 +41,18 @@
 
 #include <limits>
 
-planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
-                                                                 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
+constexpr char LOGNAME[] = "current_state_monitor";
+
+namespace planning_scene_monitor
+{
+CurrentStateMonitor::CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model,
+                                         const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
   : CurrentStateMonitor(robot_model, tf_buffer, ros::NodeHandle())
 {
 }
 
-planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
-                                                                 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
-                                                                 const ros::NodeHandle& nh)
+CurrentStateMonitor::CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model,
+                                         const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const ros::NodeHandle& nh)
   : nh_(nh)
   , tf_buffer_(tf_buffer)
   , robot_model_(robot_model)
@@ -61,33 +64,32 @@ planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_mod
   robot_state_.setToDefaultValues();
 }
 
-planning_scene_monitor::CurrentStateMonitor::~CurrentStateMonitor()
+CurrentStateMonitor::~CurrentStateMonitor()
 {
   stopStateMonitor();
 }
 
-robot_state::RobotStatePtr planning_scene_monitor::CurrentStateMonitor::getCurrentState() const
+moveit::core::RobotStatePtr 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);
 }
 
-ros::Time planning_scene_monitor::CurrentStateMonitor::getCurrentStateTime() const
+ros::Time CurrentStateMonitor::getCurrentStateTime() const
 {
   boost::mutex::scoped_lock slock(state_update_lock_);
   return current_state_time_;
 }
 
-std::pair<robot_state::RobotStatePtr, ros::Time>
-planning_scene_monitor::CurrentStateMonitor::getCurrentStateAndTime() const
+std::pair<moveit::core::RobotStatePtr, ros::Time> 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_);
 }
 
-std::map<std::string, double> planning_scene_monitor::CurrentStateMonitor::getCurrentStateValues() const
+std::map<std::string, double> CurrentStateMonitor::getCurrentStateValues() const
 {
   std::map<std::string, double> m;
   boost::mutex::scoped_lock slock(state_update_lock_);
@@ -98,7 +100,7 @@ std::map<std::string, double> planning_scene_monitor::CurrentStateMonitor::getCu
   return m;
 }
 
-void planning_scene_monitor::CurrentStateMonitor::setToCurrentState(robot_state::RobotState& upd) const
+void CurrentStateMonitor::setToCurrentState(moveit::core::RobotState& upd) const
 {
   boost::mutex::scoped_lock slock(state_update_lock_);
   const double* pos = robot_state_.getVariablePositions();
@@ -123,24 +125,24 @@ void planning_scene_monitor::CurrentStateMonitor::setToCurrentState(robot_state:
   }
 }
 
-void planning_scene_monitor::CurrentStateMonitor::addUpdateCallback(const JointStateUpdateCallback& fn)
+void CurrentStateMonitor::addUpdateCallback(const JointStateUpdateCallback& fn)
 {
   if (fn)
     update_callbacks_.push_back(fn);
 }
 
-void planning_scene_monitor::CurrentStateMonitor::clearUpdateCallbacks()
+void CurrentStateMonitor::clearUpdateCallbacks()
 {
   update_callbacks_.clear();
 }
 
-void planning_scene_monitor::CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topic)
+void CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topic)
 {
   if (!state_monitor_started_ && robot_model_)
   {
     joint_time_.clear();
     if (joint_states_topic.empty())
-      ROS_ERROR("The joint states topic cannot be an empty string");
+      ROS_ERROR_NAMED(LOGNAME, "The joint states topic cannot be an empty string");
     else
       joint_state_subscriber_ = nh_.subscribe(joint_states_topic, 25, &CurrentStateMonitor::jointStateCallback, this);
     if (tf_buffer_ && !robot_model_->getMultiDOFJointModels().empty())
@@ -150,16 +152,16 @@ void planning_scene_monitor::CurrentStateMonitor::startStateMonitor(const std::s
     }
     state_monitor_started_ = true;
     monitor_start_time_ = ros::Time::now();
-    ROS_DEBUG("Listening to joint states on topic '%s'", nh_.resolveName(joint_states_topic).c_str());
+    ROS_DEBUG_NAMED(LOGNAME, "Listening to joint states on topic '%s'", nh_.resolveName(joint_states_topic).c_str());
   }
 }
 
-bool planning_scene_monitor::CurrentStateMonitor::isActive() const
+bool CurrentStateMonitor::isActive() const
 {
   return state_monitor_started_;
 }
 
-void planning_scene_monitor::CurrentStateMonitor::stopStateMonitor()
+void CurrentStateMonitor::stopStateMonitor()
 {
   if (state_monitor_started_)
   {
@@ -169,12 +171,12 @@ void planning_scene_monitor::CurrentStateMonitor::stopStateMonitor()
       tf_buffer_->_removeTransformsChangedListener(*tf_connection_);
       tf_connection_.reset();
     }
-    ROS_DEBUG("No longer listening for joint states");
+    ROS_DEBUG_NAMED(LOGNAME, "No longer listening for joint states");
     state_monitor_started_ = false;
   }
 }
 
-std::string planning_scene_monitor::CurrentStateMonitor::getMonitoredTopic() const
+std::string CurrentStateMonitor::getMonitoredTopic() const
 {
   if (joint_state_subscriber_)
     return joint_state_subscriber_.getTopic();
@@ -182,96 +184,35 @@ std::string planning_scene_monitor::CurrentStateMonitor::getMonitoredTopic() con
     return "";
 }
 
-bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState() const
+bool CurrentStateMonitor::haveCompleteStateHelper(const ros::Time& oldest_allowed_update_time,
+                                                  std::vector<std::string>* missing_joints) const
 {
-  bool result = true;
-  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
+  const std::vector<const moveit::core::JointModel*>& active_joints = robot_model_->getActiveJointModels();
   boost::mutex::scoped_lock slock(state_update_lock_);
-  for (const moveit::core::JointModel* joint : joints)
-    if (joint_time_.find(joint) == joint_time_.end())
-    {
-      if (!joint->isPassive() && !joint->getMimic())
-      {
-        ROS_DEBUG("Joint '%s' has never been updated", joint->getName().c_str());
-        result = false;
-      }
-    }
-  return result;
-}
-
-bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(std::vector<std::string>& missing_states) const
-{
-  bool result = true;
-  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
-  boost::mutex::scoped_lock slock(state_update_lock_);
-  for (const moveit::core::JointModel* joint : joints)
-    if (joint_time_.find(joint) == joint_time_.end())
-      if (!joint->isPassive() && !joint->getMimic())
-      {
-        missing_states.push_back(joint->getName());
-        result = false;
-      }
-  return result;
-}
-
-bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(const ros::Duration& age) const
-{
-  bool result = true;
-  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
-  ros::Time now = ros::Time::now();
-  ros::Time old = now - age;
-  boost::mutex::scoped_lock slock(state_update_lock_);
-  for (const moveit::core::JointModel* joint : joints)
+  for (const moveit::core::JointModel* joint : active_joints)
   {
-    if (joint->isPassive() || joint->getMimic())
-      continue;
     std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it = joint_time_.find(joint);
     if (it == joint_time_.end())
     {
-      ROS_DEBUG("Joint '%s' has never been updated", joint->getName().c_str());
-      result = false;
+      ROS_DEBUG_NAMED(LOGNAME, "Joint '%s' has never been updated", joint->getName().c_str());
     }
-    else if (it->second < old)
+    else if (it->second < oldest_allowed_update_time)
     {
-      ROS_DEBUG("Joint '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
-                joint->getName().c_str(), (now - it->second).toSec(), age.toSec());
-      result = false;
+      ROS_DEBUG_NAMED(LOGNAME, "Joint '%s' was last updated %0.3lf seconds before requested time",
+                      joint->getName().c_str(), (oldest_allowed_update_time - it->second).toSec());
     }
-  }
-  return result;
-}
-
-bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(const ros::Duration& age,
-                                                                    std::vector<std::string>& missing_states) const
-{
-  bool result = true;
-  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
-  ros::Time now = ros::Time::now();
-  ros::Time old = now - age;
-  boost::mutex::scoped_lock slock(state_update_lock_);
-  for (const moveit::core::JointModel* joint : joints)
-  {
-    if (joint->isPassive() || joint->getMimic())
+    else
       continue;
-    std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it = joint_time_.find(joint);
-    if (it == joint_time_.end())
-    {
-      ROS_DEBUG("Joint '%s' has never been updated", joint->getName().c_str());
-      missing_states.push_back(joint->getName());
-      result = false;
-    }
-    else if (it->second < old)
-    {
-      ROS_DEBUG("Joint '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
-                joint->getName().c_str(), (now - it->second).toSec(), age.toSec());
-      missing_states.push_back(joint->getName());
-      result = false;
-    }
+
+    if (missing_joints)
+      missing_joints->push_back(joint->getName());
+    else
+      return false;
   }
-  return result;
+  return (missing_joints == nullptr) || missing_joints->empty();
 }
 
-bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const ros::Time t, double wait_time) const
+bool CurrentStateMonitor::waitForCurrentState(const ros::Time t, double wait_time) const
 {
   ros::WallTime start = ros::WallTime::now();
   ros::WallDuration elapsed(0, 0);
@@ -284,16 +225,17 @@ bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const ros:
     elapsed = ros::WallTime::now() - start;
     if (elapsed > timeout)
     {
-      ROS_INFO_STREAM("Didn't received robot state (joint angles) with recent timestamp within "
-                      << wait_time << " seconds.\n"
-                      << "Check clock synchronization if your are running ROS across multiple machines!");
+      ROS_INFO_STREAM_NAMED(LOGNAME,
+                            "Didn't receive robot state (joint angles) with recent timestamp within "
+                                << wait_time << " seconds.\n"
+                                << "Check clock synchronization if your are running ROS across multiple machines!");
       return false;
     }
   }
   return true;
 }
 
-bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState(double wait_time) const
+bool CurrentStateMonitor::waitForCompleteState(double wait_time) const
 {
   double slept_time = 0.0;
   double sleep_step_s = std::min(0.05, wait_time / 10.0);
@@ -306,7 +248,7 @@ bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState(double wa
   return haveCompleteState();
 }
 
-bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState(const std::string& group, double wait_time) const
+bool CurrentStateMonitor::waitForCompleteState(const std::string& group, double wait_time) const
 {
   if (waitForCompleteState(wait_time))
     return true;
@@ -333,12 +275,14 @@ bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState(const std
   return ok;
 }
 
-void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state)
+void CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state)
 {
   if (joint_state->name.size() != joint_state->position.size())
   {
-    ROS_ERROR_THROTTLE(1, "State monitor received invalid joint state (number of joint names does not match number of "
-                          "positions)");
+    ROS_ERROR_THROTTLE_NAMED(
+        1, LOGNAME,
+        "State monitor received invalid joint state (number of joint names does not match number of "
+        "positions)");
     return;
   }
   bool update = false;
@@ -411,7 +355,7 @@ void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const senso
   state_update_condition_.notify_all();
 }
 
-void planning_scene_monitor::CurrentStateMonitor::tfCallback()
+void CurrentStateMonitor::tfCallback()
 {
   // read multi-dof joint states from TF, if needed
   const std::vector<const moveit::core::JointModel*>& multi_dof_joints = robot_model_->getMultiDOFJointModels();
@@ -437,9 +381,10 @@ void planning_scene_monitor::CurrentStateMonitor::tfCallback()
       }
       catch (tf2::TransformException& ex)
       {
-        ROS_WARN_STREAM_ONCE("Unable to update multi-DOF joint '"
-                             << joint->getName() << "': Failure to lookup transform between '" << parent_frame.c_str()
-                             << "' and '" << child_frame.c_str() << "' with TF exception: " << ex.what());
+        ROS_WARN_STREAM_ONCE_NAMED(LOGNAME, "Unable to update multi-DOF joint '"
+                                                << joint->getName() << "': Failure to lookup transform between '"
+                                                << parent_frame.c_str() << "' and '" << child_frame.c_str()
+                                                << "' with TF exception: " << ex.what());
         continue;
       }
 
@@ -482,3 +427,5 @@ void planning_scene_monitor::CurrentStateMonitor::tfCallback()
     state_update_condition_.notify_all();
   }
 }
+
+}  // namespace planning_scene_monitor
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 5f75d60794..044bd197ea 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
@@ -190,15 +190,11 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc
   {
     robot_model_ = rm_loader_->getModel();
     scene_ = scene;
-    collision_loader_.setupScene(nh_, scene_);
-    scene_const_ = scene_;
     if (!scene_)
     {
       try
       {
         scene_.reset(new planning_scene::PlanningScene(rm_loader_->getModel()));
-        collision_loader_.setupScene(nh_, scene_);
-        scene_const_ = scene_;
         configureCollisionMatrix(scene_);
         configureDefaultPadding();
 
@@ -220,11 +216,14 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc
       {
         ROS_ERROR_NAMED(LOGNAME, "Configuration of planning scene failed");
         scene_.reset();
-        scene_const_ = scene_;
       }
     }
+    // scene_const_ is set regardless if scene_ is null or not
+    scene_const_ = scene_;
     if (scene_)
     {
+      // The scene_ is loaded on the collision loader only if it was correctly instantiated
+      collision_loader_.setupScene(nh_, scene_);
       scene_->setAttachedBodyUpdateCallback(
           boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
       scene_->setCollisionObjectUpdateCallback(
@@ -537,16 +536,25 @@ void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningS
 
 void PlanningSceneMonitor::clearOctomap()
 {
-  if (octomap_monitor_)
-  {
-    octomap_monitor_->getOcTreePtr()->lockWrite();
-    octomap_monitor_->getOcTreePtr()->clear();
-    octomap_monitor_->getOcTreePtr()->unlockWrite();
-  }
-  else
+  bool removed = false;
   {
-    ROS_WARN_NAMED(LOGNAME, "Unable to clear octomap since no octomap monitor has been initialized");
+    boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
+    removed = scene_->getWorldNonConst()->removeObject(scene_->OCTOMAP_NS);
+
+    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");
+    }  // Lift the scoped lock before calling triggerSceneUpdateEvent to avoid deadlock
   }
+
+  if (removed)
+    triggerSceneUpdateEvent(UPDATE_GEOMETRY);
 }
 
 bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
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 85c12e1d49..67573fe67a 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
@@ -43,7 +43,7 @@
 
 namespace rdf_loader
 {
-MOVEIT_CLASS_FORWARD(RDFLoader);
+MOVEIT_CLASS_FORWARD(RDFLoader);  // Defines RDFLoaderPtr, ConstPtr, WeakPtr... etc
 
 /** @class RDFLoader
  *  @brief Default constructor
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 bf4aa2c973..e2d6aa75c7 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
@@ -44,7 +44,7 @@
 
 namespace robot_model_loader
 {
-MOVEIT_CLASS_FORWARD(RobotModelLoader);
+MOVEIT_CLASS_FORWARD(RobotModelLoader);  // Defines RobotModelLoaderPtr, ConstPtr, WeakPtr... etc
 
 /** @class RobotModelLoader */
 class RobotModelLoader
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..8f578b6af6 100644
--- a/moveit_ros/planning/trajectory_execution_manager/dox/trajectory_execution.dox
+++ b/moveit_ros/planning/trajectory_execution_manager/dox/trajectory_execution.dox
@@ -8,4 +8,4 @@ The trajectory_execution_manager::TrajectoryExecutionManager class allows two ma
 
 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 7d94940b36..c4bfeab3f6 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
@@ -52,7 +52,7 @@
 
 namespace trajectory_execution_manager
 {
-MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager);
+MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager);  // Defines TrajectoryExecutionManagerPtr, ConstPtr, WeakPtr... etc
 
 // Two modes:
 // Managed controllers
diff --git a/moveit_ros/planning/trajectory_execution_manager/lib/plugin_description.xml b/moveit_ros/planning/trajectory_execution_manager/lib/plugin_description.xml
index 686b38e1f2..faf679d629 100644
--- a/moveit_ros/planning/trajectory_execution_manager/lib/plugin_description.xml
+++ b/moveit_ros/planning/trajectory_execution_manager/lib/plugin_description.xml
@@ -4,4 +4,3 @@
     </description>
   </class>
 </library>
-
diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst
index dd57124213..1dacff1b6a 100644
--- a/moveit_ros/planning_interface/CHANGELOG.rst
+++ b/moveit_ros/planning_interface/CHANGELOG.rst
@@ -2,6 +2,46 @@
 Changelog for package moveit_ros_planning_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+* Use moveit-resources@master (`#2951 <https://github.com/ros-planning/moveit/issues/2951>`_)
+
+  - Simplify launch files to use the test_environment.launch files from moveit_resources@master
+  - Provide compatibility to the Noetic-style configuration of (multiple) planning pipelines
+    Only a single pipeline can be used at a time, specified via the ~default_planning_pipeline parameter.
+* Add missing noexcept declarations (`#3016 <https://github.com/ros-planning/moveit/issues/3016>`_)
+* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 <https://github.com/ros-planning/moveit/issues/3009>`_)
+* Revert "Fix scaling factor parameter names" (`#2907 <https://github.com/ros-planning/moveit/issues/2907>`_)
+* Contributors: Michael Görner, Robert Haschke
+
+1.0.8 (2021-05-23)
+------------------
+* planning_interface: synchronize async interfaces in test (`#2640 <https://github.com/ros-planning/moveit/issues/2640>`_)
+* Print an error indicating that the planning pipelines are empty before returning (`#2639 <https://github.com/ros-planning/moveit/issues/2639>`_)
+* Fix docstring in MGI API (`#2626 <https://github.com/ros-planning/moveit/issues/2626>`_)
+* Enable mesh filter (`#2448 <https://github.com/ros-planning/moveit/issues/2448>`_)
+* add get_active_joint_names (`#2533 <https://github.com/ros-planning/moveit/issues/2533>`_)
+* Improve robustness of move group interface test (`#2484 <https://github.com/ros-planning/moveit/issues/2484>`_)
+* Fix scaling factor parameter names (`#2452 <https://github.com/ros-planning/moveit/issues/2452>`_)
+* Contributors: Luc Bettaieb, Boston Cleek, Jafar Abdi, Michael Görner, Peter Mitrano, Shota Aoki, Tyler Weaver
+
+1.0.7 (2020-11-20)
+------------------
+* [feature] Python interface improvements. (`#2356 <https://github.com/ros-planning/moveit/issues/2356>`_)
+* [feature] moveit_cpp: more informative error message, cover another potential failure condition. (`#2336 <https://github.com/ros-planning/moveit/issues/2336>`_)
+* [feature] Unit Test for ByteString-based ROS msg conversion (`#2362 <https://github.com/ros-planning/moveit/issues/2362>`_)
+* [feature] Make GILReleaser exception-safe (`#2363 <https://github.com/ros-planning/moveit/issues/2363>`_)
+* [feature] Added GILRelease to pick and place (`#2272 <https://github.com/ros-planning/moveit/issues/2272>`_)
+* [feature] Add missing variants of place from list of PlaceLocations and Poses in the python interface (`#2231 <https://github.com/ros-planning/moveit/issues/2231>`_)
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: AndyZe, Bjar Ne, Felix von Drigalski, Gerard Canal, Peter Mitrano, Robert Haschke
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint]   Adapt repository for splitted moveit_resources layout (`#2199 <https://github.com/ros-planning/moveit/issues/2199>`_)
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 7fca23fb17..6f16527b34 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
@@ -40,6 +40,7 @@
 
 #include <moveit/macros/class_forward.h>
 #include <moveit/robot_state/robot_state.h>
+#include <moveit/utils/moveit_error_code.h>
 #include <moveit_msgs/RobotTrajectory.h>
 #include <moveit_msgs/RobotState.h>
 #include <moveit_msgs/PlannerInterfaceDescription.h>
@@ -60,36 +61,9 @@ namespace moveit
 /** \brief Simple interface to MoveIt! components */
 namespace planning_interface
 {
-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 c) const
-  {
-    return val == c;
-  }
-  bool operator!=(const int c) const
-  {
-    return val != c;
-  }
-};
+using MoveItErrorCode = moveit::core::MoveItErrorCode;
 
-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
 
@@ -177,8 +151,8 @@ 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;
@@ -228,7 +202,15 @@ class MoveGroupInterface
   void setPlannerParams(const std::string& planner_id, const std::string& group,
                         const std::map<std::string, std::string>& params, bool bReplace = false);
 
-  /** \brief Get the default planner for a given group (or global default) */
+  std::string getDefaultPlanningPipelineId() const;
+
+  /** \brief Specify a planning pipeline to be used for further planning */
+  void setPlanningPipelineId(const std::string& pipeline_id);
+
+  /** \brief Get the current planning_pipeline_id */
+  const std::string& getPlanningPipelineId() const;
+
+  /** \brief Get the default planner of the current planning pipeline for the given group (or the pipeline's default) */
   std::string getDefaultPlannerId(const std::string& group = "") const;
 
   /** \brief Specify a planner to be used for further planning */
@@ -689,7 +671,7 @@ class MoveGroupInterface
   /** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified
      target.
       This call is not blocking (does not wait for the execution of the trajectory to complete). */
-  MoveItErrorCode asyncMove();
+  moveit::core::MoveItErrorCode asyncMove();
 
   /** \brief Get the move_group action client used by the \e MoveGroupInterface.
       The client can be used for querying the execution state of the trajectory and abort trajectory execution
@@ -700,24 +682,24 @@ class MoveGroupInterface
      target.
       This call is always blocking (waits for the execution of the trajectory to complete) and requires an asynchronous
      spinner to be started.*/
-  MoveItErrorCode move();
+  moveit::core::MoveItErrorCode move();
 
   /** \brief Compute a motion plan that takes the group declared in the constructor from the current state to the
      specified
       target. No execution is performed. The resulting plan is stored in \e plan*/
-  MoveItErrorCode plan(Plan& plan);
+  moveit::core::MoveItErrorCode plan(Plan& plan);
 
   /** \brief Given a \e plan, execute it without waiting for completion. */
-  MoveItErrorCode asyncExecute(const Plan& plan);
+  moveit::core::MoveItErrorCode asyncExecute(const Plan& plan);
 
   /** \brief Given a \e robot trajectory, execute it without waiting for completion. */
-  MoveItErrorCode asyncExecute(const moveit_msgs::RobotTrajectory& trajectory);
+  moveit::core::MoveItErrorCode asyncExecute(const moveit_msgs::RobotTrajectory& trajectory);
 
   /** \brief Given a \e plan, execute it while waiting for completion. */
-  MoveItErrorCode execute(const Plan& plan);
+  moveit::core::MoveItErrorCode execute(const Plan& plan);
 
   /** \brief Given a \e robot trajectory, execute it while waiting for completion. */
-  MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory);
+  moveit::core::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
@@ -730,7 +712,7 @@ class MoveGroupInterface
       Return -1.0 in case of error. */
   double computeCartesianPath(const std::vector<geometry_msgs::Pose>& 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
@@ -747,7 +729,7 @@ class MoveGroupInterface
   double computeCartesianPath(const std::vector<geometry_msgs::Pose>& 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();
@@ -784,19 +766,20 @@ class MoveGroupInterface
   /** \brief Pick up an object
 
       This applies a number of hard-coded default grasps */
-  MoveItErrorCode pick(const std::string& object, bool plan_only = false)
+  moveit::core::MoveItErrorCode pick(const std::string& object, bool plan_only = false)
   {
     return pick(constructPickupGoal(object, std::vector<moveit_msgs::Grasp>(), plan_only));
   }
 
   /** \brief Pick up an object given a grasp pose */
-  MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only = false)
+  moveit::core::MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only = false)
   {
     return pick(constructPickupGoal(object, { grasp }, plan_only));
   }
 
   /** \brief Pick up an object given possible grasp poses */
-  MoveItErrorCode pick(const std::string& object, std::vector<moveit_msgs::Grasp> grasps, bool plan_only = false)
+  moveit::core::MoveItErrorCode pick(const std::string& object, std::vector<moveit_msgs::Grasp> grasps,
+                                     bool plan_only = false)
   {
     return pick(constructPickupGoal(object, std::move(grasps), plan_only));
   }
@@ -805,40 +788,41 @@ class MoveGroupInterface
 
       Use as follows: first create the goal with constructPickupGoal(), then set \e possible_grasps and any other
       desired variable in the goal, and finally pass it on to this function */
-  MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal);
+  moveit::core::MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal);
 
   /** \brief Pick up an object
 
       calls the external moveit_msgs::GraspPlanning service "plan_grasps" to compute possible grasps */
-  MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false);
+  moveit::core::MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false);
 
   /** \brief Pick up an object
 
       calls the external moveit_msgs::GraspPlanning service "plan_grasps" to compute possible grasps */
-  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false);
+  moveit::core::MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false);
 
   /** \brief Place an object somewhere safe in the world (a safe location will be detected) */
-  MoveItErrorCode place(const std::string& object, bool plan_only = false)
+  moveit::core::MoveItErrorCode place(const std::string& object, bool plan_only = false)
   {
     return place(constructPlaceGoal(object, std::vector<moveit_msgs::PlaceLocation>(), plan_only));
   }
 
   /** \brief Place an object at one of the specified possible locations */
-  MoveItErrorCode place(const std::string& object, std::vector<moveit_msgs::PlaceLocation> locations,
-                        bool plan_only = false)
+  moveit::core::MoveItErrorCode place(const std::string& object, std::vector<moveit_msgs::PlaceLocation> locations,
+                                      bool plan_only = false)
   {
     return place(constructPlaceGoal(object, std::move(locations), plan_only));
   }
 
   /** \brief Place an object at one of the specified possible locations */
-  MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses,
-                        bool plan_only = false)
+  moveit::core::MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses,
+                                      bool plan_only = false)
   {
     return place(constructPlaceGoal(object, posesToPlaceLocations(poses), plan_only));
   }
 
   /** \brief Place an object at one of the specified possible location */
-  MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only = false)
+  moveit::core::MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose,
+                                      bool plan_only = false)
   {
     return place(constructPlaceGoal(object, posesToPlaceLocations({ pose }), plan_only));
   }
@@ -847,7 +831,7 @@ class MoveGroupInterface
 
       Use as follows: first create the goal with constructPlaceGoal(), then set \e place_locations and any other
       desired variable in the goal, and finally pass it on to this function */
-  MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal);
+  moveit::core::MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal);
 
   /** \brief Given the name of an object in the planning scene, make
       the object attached to a link of the robot.  If no link name is
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 1802b80f30..ca4be411b5 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
@@ -602,17 +602,17 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     return locations;
   }
 
-  MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal)
+  moveit::core::MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal)
   {
     if (!place_action_client_)
     {
       ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
     if (!place_action_client_->isServerConnected())
     {
       ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
 
     place_action_client_->sendGoal(goal);
@@ -623,27 +623,27 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     }
     if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
     {
-      return MoveItErrorCode(place_action_client_->getResult()->error_code);
+      return place_action_client_->getResult()->error_code;
     }
     else
     {
       ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": "
                                                              << place_action_client_->getState().getText());
-      return MoveItErrorCode(place_action_client_->getResult()->error_code);
+      return place_action_client_->getResult()->error_code;
     }
   }
 
-  MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal)
+  moveit::core::MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal)
   {
     if (!pick_action_client_)
     {
       ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
     if (!pick_action_client_->isServerConnected())
     {
       ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
 
     pick_action_client_->sendGoal(goal);
@@ -653,17 +653,17 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     }
     if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
     {
-      return MoveItErrorCode(pick_action_client_->getResult()->error_code);
+      return pick_action_client_->getResult()->error_code;
     }
     else
     {
       ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": "
                                                              << pick_action_client_->getState().getText());
-      return MoveItErrorCode(pick_action_client_->getResult()->error_code);
+      return pick_action_client_->getResult()->error_code;
     }
   }
 
-  MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false)
+  moveit::core::MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false)
   {
     if (object.empty())
     {
@@ -677,13 +677,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     {
       ROS_ERROR_STREAM_NAMED("move_group_interface",
                              "Asked for grasps for the object '" << object << "', but the object could not be found");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
+      return moveit::core::MoveItErrorCode::INVALID_OBJECT_NAME;
     }
 
     return planGraspsAndPick(objects[object], plan_only);
   }
 
-  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false)
+  moveit::core::MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false)
   {
     if (!plan_grasps_service_.exists())
     {
@@ -691,7 +691,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
                                                          << GRASP_PLANNING_SERVICE_NAME
                                                          << "' is not available."
                                                             " This has to be implemented and started separately.");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
 
     moveit_msgs::GraspPlanning::Request request;
@@ -706,21 +706,21 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
         response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
     {
       ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick.");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
 
     return pick(constructPickupGoal(object.id, std::move(response.grasps), plan_only));
   }
 
-  MoveItErrorCode plan(Plan& plan)
+  moveit::core::MoveItErrorCode plan(Plan& plan)
   {
     if (!move_action_client_)
     {
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
     if (!move_action_client_->isServerConnected())
     {
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
 
     moveit_msgs::MoveGroupGoal goal;
@@ -741,25 +741,25 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
       plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
       plan.start_state_ = move_action_client_->getResult()->trajectory_start;
       plan.planning_time_ = move_action_client_->getResult()->planning_time;
-      return MoveItErrorCode(move_action_client_->getResult()->error_code);
+      return move_action_client_->getResult()->error_code;
     }
     else
     {
       ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": "
                                                              << move_action_client_->getState().getText());
-      return MoveItErrorCode(move_action_client_->getResult()->error_code);
+      return move_action_client_->getResult()->error_code;
     }
   }
 
-  MoveItErrorCode move(bool wait)
+  moveit::core::MoveItErrorCode move(bool wait)
   {
     if (!move_action_client_)
     {
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
     if (!move_action_client_->isServerConnected())
     {
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
 
     moveit_msgs::MoveGroupGoal goal;
@@ -774,7 +774,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     move_action_client_->sendGoal(goal);
     if (!wait)
     {
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
+      return moveit::core::MoveItErrorCode::SUCCESS;
     }
 
     if (!move_action_client_->waitForResult())
@@ -784,27 +784,27 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
 
     if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
     {
-      return MoveItErrorCode(move_action_client_->getResult()->error_code);
+      return move_action_client_->getResult()->error_code;
     }
     else
     {
       ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString()
                                                         << ": " << move_action_client_->getState().getText());
-      return MoveItErrorCode(move_action_client_->getResult()->error_code);
+      return move_action_client_->getResult()->error_code;
     }
   }
 
-  MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait)
+  moveit::core::MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait)
   {
     if (!execute_action_client_)
     {
       ROS_ERROR_STREAM_NAMED("move_group_interface", "execute action client not found");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
     if (!execute_action_client_->isServerConnected())
     {
       ROS_WARN_STREAM_NAMED("move_group_interface", "execute action server not connected");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
 
     moveit_msgs::ExecuteTrajectoryGoal goal;
@@ -813,7 +813,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     execute_action_client_->sendGoal(goal);
     if (!wait)
     {
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
+      return moveit::core::MoveItErrorCode::SUCCESS;
     }
 
     if (!execute_action_client_->waitForResult())
@@ -823,13 +823,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
 
     if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
     {
-      return MoveItErrorCode(execute_action_client_->getResult()->error_code);
+      return execute_action_client_->getResult()->error_code;
     }
     else
     {
       ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString()
                                                         << ": " << execute_action_client_->getState().getText());
-      return MoveItErrorCode(execute_action_client_->getResult()->error_code);
+      return execute_action_client_->getResult()->error_code;
     }
   }
 
@@ -1314,14 +1314,14 @@ moveit::planning_interface::MoveGroupInterface::~MoveGroupInterface()
   delete impl_;
 }
 
-moveit::planning_interface::MoveGroupInterface::MoveGroupInterface(MoveGroupInterface&& other)
+moveit::planning_interface::MoveGroupInterface::MoveGroupInterface(MoveGroupInterface&& other) noexcept
   : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_)
 {
   other.impl_ = nullptr;
 }
 
 moveit::planning_interface::MoveGroupInterface&
-moveit::planning_interface::MoveGroupInterface::operator=(MoveGroupInterface&& other)
+moveit::planning_interface::MoveGroupInterface::operator=(MoveGroupInterface&& other) noexcept
 {
   if (this != &other)
   {
@@ -1415,7 +1415,7 @@ void moveit::planning_interface::MoveGroupInterface::setMaxAccelerationScalingFa
   impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
 }
 
-moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::asyncMove()
+moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::asyncMove()
 {
   return impl_->move(false);
 }
@@ -1426,35 +1426,34 @@ moveit::planning_interface::MoveGroupInterface::getMoveGroupClient() const
   return impl_->getMoveGroupClient();
 }
 
-moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::move()
+moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::move()
 {
   return impl_->move(true);
 }
 
-moveit::planning_interface::MoveItErrorCode
-moveit::planning_interface::MoveGroupInterface::asyncExecute(const Plan& plan)
+moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::asyncExecute(const Plan& plan)
 {
   return impl_->execute(plan.trajectory_, false);
 }
 
-moveit::planning_interface::MoveItErrorCode
+moveit::core::MoveItErrorCode
 moveit::planning_interface::MoveGroupInterface::asyncExecute(const moveit_msgs::RobotTrajectory& trajectory)
 {
   return impl_->execute(trajectory, false);
 }
 
-moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::execute(const Plan& plan)
+moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::execute(const Plan& plan)
 {
   return impl_->execute(plan.trajectory_, true);
 }
 
-moveit::planning_interface::MoveItErrorCode
+moveit::core::MoveItErrorCode
 moveit::planning_interface::MoveGroupInterface::execute(const moveit_msgs::RobotTrajectory& trajectory)
 {
   return impl_->execute(trajectory, true);
 }
 
-moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::plan(Plan& plan)
+moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::plan(Plan& plan)
 {
   return impl_->plan(plan);
 }
@@ -1477,27 +1476,25 @@ std::vector<moveit_msgs::PlaceLocation> moveit::planning_interface::MoveGroupInt
   return impl_->posesToPlaceLocations(poses);
 }
 
-moveit::planning_interface::MoveItErrorCode
-moveit::planning_interface::MoveGroupInterface::pick(const moveit_msgs::PickupGoal& goal)
+moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::pick(const moveit_msgs::PickupGoal& goal)
 {
   return impl_->pick(goal);
 }
 
-moveit::planning_interface::MoveItErrorCode
+moveit::core::MoveItErrorCode
 moveit::planning_interface::MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only)
 {
   return impl_->planGraspsAndPick(object, plan_only);
 }
 
-moveit::planning_interface::MoveItErrorCode
+moveit::core::MoveItErrorCode
 moveit::planning_interface::MoveGroupInterface::planGraspsAndPick(const moveit_msgs::CollisionObject& object,
                                                                   bool plan_only)
 {
   return impl_->planGraspsAndPick(object, plan_only);
 }
 
-moveit::planning_interface::MoveItErrorCode
-moveit::planning_interface::MoveGroupInterface::place(const moveit_msgs::PlaceGoal& goal)
+moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::place(const moveit_msgs::PlaceGoal& goal)
 {
   return impl_->place(goal);
 }
@@ -1610,8 +1607,14 @@ bool moveit::planning_interface::MoveGroupInterface::setNamedTarget(const std::s
 
 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector<double>& joint_values)
 {
-  if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
+  auto const n_group_joints = impl_->getJointModelGroup()->getVariableCount();
+  if (joint_values.size() != n_group_joints)
+  {
+    ROS_DEBUG_STREAM("Provided joint value list has length " << joint_values.size() << " but group "
+                                                             << impl_->getJointModelGroup()->getName() << " has "
+                                                             << n_group_joints << " joints");
     return false;
+  }
   impl_->setTargetType(JOINT);
   impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
   return impl_->getJointStateTarget().satisfiesBounds(impl_->getJointModelGroup(), impl_->getGoalJointTolerance());
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 7d421ac016..09f1f22ddf 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
@@ -271,7 +271,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     msg.header.frame_id = getPoseReferenceFrame();
     msg.header.stamp = ros::Time::now();
     GILReleaser gr;
-    return place(object_name, msg, plan_only) == MoveItErrorCode::SUCCESS;
+    return place(object_name, msg, plan_only) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool placePoses(const std::string& object_name, const bp::list& poses_list, bool plan_only = false)
@@ -281,7 +281,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     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;
+    return place(object_name, poses, plan_only) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool placeLocation(const std::string& object_name, const py_bindings_tools::ByteString& location_str,
@@ -290,7 +290,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     std::vector<moveit_msgs::PlaceLocation> locations(1);
     py_bindings_tools::deserializeMsg(location_str, locations[0]);
     GILReleaser gr;
-    return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS;
+    return place(object_name, std::move(locations), plan_only) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool placeLocations(const std::string& object_name, const bp::list& location_list, bool plan_only = false)
@@ -300,13 +300,13 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     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;
+    return place(object_name, std::move(locations), plan_only) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool placeAnywhere(const std::string& object_name, bool plan_only = false)
   {
     GILReleaser gr;
-    return place(object_name, plan_only) == MoveItErrorCode::SUCCESS;
+    return place(object_name, plan_only) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   void convertListToArrayOfPoses(const bp::list& poses, std::vector<geometry_msgs::Pose>& msg)
@@ -350,6 +350,14 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     return output;
   }
 
+  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;
@@ -436,12 +444,12 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
   bool movePython()
   {
     GILReleaser gr;
-    return move() == MoveItErrorCode::SUCCESS;
+    return move() == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool asyncMovePython()
   {
-    return asyncMove() == MoveItErrorCode::SUCCESS;
+    return asyncMove() == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool attachObjectPython(const std::string& object_name, const std::string& link_name, const bp::list& touch_links)
@@ -454,22 +462,23 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     MoveGroupInterface::Plan plan;
     py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_);
     GILReleaser gr;
-    return execute(plan) == MoveItErrorCode::SUCCESS;
+    return execute(plan) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool asyncExecutePython(const py_bindings_tools::ByteString& plan_str)
   {
     MoveGroupInterface::Plan plan;
     py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_);
-    return asyncExecute(plan) == MoveItErrorCode::SUCCESS;
+    return asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   py_bindings_tools::ByteString getPlanPython()
   {
-    GILReleaser gr;
     MoveGroupInterface::Plan plan;
-    MoveGroupInterface::plan(plan);
-    gr.reacquire();
+    {
+      GILReleaser gr;
+      MoveGroupInterface::plan(plan);
+    }
     return py_bindings_tools::serializeMsg(plan.trajectory_);
   }
 
@@ -495,10 +504,11 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     std::vector<geometry_msgs::Pose> poses;
     convertListToArrayOfPoses(waypoints, poses);
     moveit_msgs::RobotTrajectory trajectory;
-    GILReleaser gr;
-    double fraction =
-        computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions);
-    gr.reacquire();
+    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);
   }
 
@@ -547,36 +557,39 @@ 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);
-      GILReleaser gr;
-      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")
-      {
-        trajectory_processing::IterativeSplineParameterization time_param;
-        time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor);
-      }
-      else if (algorithm == "time_optimal_trajectory_generation")
+      bool algorithm_found = true;
       {
-        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);
-        gr.reacquire();
-        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);
-      gr.reacquire();
+        // 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();
+        }
+
+        if (algorithm_found)
+          // Convert the retimed trajectory back into a message
+          traj_obj.getRobotTrajectoryMsg(traj_msg);
+      }
       return py_bindings_tools::serializeMsg(traj_msg);
     }
     else
@@ -603,6 +616,24 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     state.setJointGroupPositions(group, v);
     return state.getJacobian(group, Eigen::Map<Eigen::Vector3d>(&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)
@@ -618,7 +649,7 @@ static void wrap_move_group_interface()
   move_group_interface_class.def("move", &MoveGroupInterfaceWrapper::movePython);
   move_group_interface_class.def("execute", &MoveGroupInterfaceWrapper::executePython);
   move_group_interface_class.def("async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython);
-  moveit::planning_interface::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) =
+  moveit::core::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) =
       &MoveGroupInterfaceWrapper::pick;
   move_group_interface_class.def("pick", pick_1);
   move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasp);
@@ -744,8 +775,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_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/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h
index affeceb1a7..1c481e473b 100644
--- 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
@@ -50,7 +50,7 @@ namespace moveit
 {
 namespace planning_interface
 {
-MOVEIT_CLASS_FORWARD(MoveItCpp);
+MOVEIT_CLASS_FORWARD(MoveItCpp);  // Defines MoveItCppPtr, ConstPtr, WeakPtr... etc
 
 class MoveItCpp
 {
@@ -120,8 +120,8 @@ class MoveItCpp
   MoveItCpp(const MoveItCpp&) = delete;
   MoveItCpp& operator=(const MoveItCpp&) = delete;
 
-  MoveItCpp(MoveItCpp&& other);
-  MoveItCpp& operator=(MoveItCpp&& other);
+  MoveItCpp(MoveItCpp&& other) noexcept;
+  MoveItCpp& operator=(MoveItCpp&& other) noexcept;
 
   /** \brief Destructor */
   ~MoveItCpp();
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
index 3d1475da6b..03c2f73760 100644
--- 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
@@ -44,46 +44,20 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <moveit/robot_state/conversions.h>
 #include <moveit_msgs/MoveItErrorCodes.h>
+#include <moveit/utils/moveit_error_code.h>
 
 namespace moveit
 {
 namespace planning_interface
 {
-MOVEIT_CLASS_FORWARD(PlanningComponent);
+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;
-    }
-  };
+  using MoveItErrorCode = moveit::core::MoveItErrorCode;
 
   /// The representation of a plan solution
   struct PlanSolution
@@ -95,7 +69,7 @@ class PlanningComponent
     robot_trajectory::RobotTrajectoryPtr trajectory;
 
     /// Reason why the plan failed
-    MoveItErrorCode error_code;
+    moveit::core::MoveItErrorCode error_code;
 
     explicit operator bool() const
     {
@@ -137,8 +111,8 @@ class PlanningComponent
   PlanningComponent(const PlanningComponent&) = delete;
   PlanningComponent& operator=(const PlanningComponent&) = delete;
 
-  PlanningComponent(PlanningComponent&& other);
-  PlanningComponent& operator=(PlanningComponent&& other);
+  PlanningComponent(PlanningComponent&& other) = delete;
+  PlanningComponent& operator=(PlanningComponent&& other) noexcept;
 
   /** \brief Destructor */
   ~PlanningComponent();
diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp
index 6bf1eccc89..2f5aa6b384 100644
--- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp
+++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp
@@ -96,7 +96,7 @@ MoveItCpp::MoveItCpp(const Options& options, const ros::NodeHandle& nh,
   ROS_INFO_NAMED(LOGNAME, "MoveItCpp running");
 }
 
-MoveItCpp::MoveItCpp(MoveItCpp&& other)
+MoveItCpp::MoveItCpp(MoveItCpp&& other) noexcept
 {
   other.clearContents();
 }
@@ -107,7 +107,7 @@ MoveItCpp::~MoveItCpp()
   clearContents();
 }
 
-MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other)
+MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) noexcept
 {
   if (this != &other)
   {
@@ -177,8 +177,8 @@ bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options)
 
   if (planning_pipelines_.empty())
   {
-    return false;
     ROS_ERROR_NAMED(LOGNAME, "Failed to load any planning pipelines.");
+    return false;
   }
 
   // Retrieve group/pipeline mapping for faster lookup
@@ -244,7 +244,9 @@ std::set<std::string> MoveItCpp::getPlanningPipelineNames(const std::string& gro
   std::set<std::string> result_names;
   if (!group_name.empty() && groups_pipelines_map_.count(group_name) == 0)
   {
-    ROS_ERROR_NAMED(LOGNAME, "There are no planning pipelines loaded for group '%s'.", group_name.c_str());
+    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_)
@@ -259,6 +261,11 @@ std::set<std::string> MoveItCpp::getPlanningPipelineNames(const std::string& gro
     }
     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;
 }
 
diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp
index 086f3d9e9b..198c274551 100644
--- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp
+++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp
@@ -92,7 +92,7 @@ PlanningComponent::~PlanningComponent()
   clearContents();
 }
 
-PlanningComponent& PlanningComponent::operator=(PlanningComponent&& other)
+PlanningComponent& PlanningComponent::operator=(PlanningComponent&& other) noexcept
 {
   if (this != &other)
   {
@@ -130,7 +130,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet
   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);
+    last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME;
     return *last_plan_solution_;
   }
 
@@ -167,7 +167,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet
   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);
+    last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS;
     return *last_plan_solution_;
   }
   req.goal_constraints = current_goal_constraints_;
@@ -177,7 +177,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet
   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);
+    last_plan_solution_->error_code = moveit::core::MoveItErrorCode::FAILURE;
     return *last_plan_solution_;
   }
   const planning_pipeline::PlanningPipelinePtr pipeline =
diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml
index b936a4acec..44532a43da 100644
--- a/moveit_ros/planning_interface/package.xml
+++ b/moveit_ros/planning_interface/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_planning_interface</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Components of MoveIt! that offer simpler interfaces to planning and execution</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
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 c0d056dcb9..2c86938409 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
@@ -48,7 +48,7 @@ namespace moveit
 {
 namespace planning_interface
 {
-MOVEIT_CLASS_FORWARD(PlanningSceneInterface);
+MOVEIT_CLASS_FORWARD(PlanningSceneInterface);  // Defines PlanningSceneInterfacePtr, ConstPtr, WeakPtr... etc
 
 class PlanningSceneInterface
 {
diff --git a/moveit_ros/planning_interface/py_bindings_tools/dox/py_bindings.dox b/moveit_ros/planning_interface/py_bindings_tools/dox/py_bindings.dox
index 9546ad12f0..061314d0d8 100644
--- a/moveit_ros/planning_interface/py_bindings_tools/dox/py_bindings.dox
+++ b/moveit_ros/planning_interface/py_bindings_tools/dox/py_bindings.dox
@@ -45,4 +45,4 @@
    But to forward these parameters an explicit function call needs to be made.
    For that we provide a function call
 
-*/
\ No newline at end of file
+*/
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
index 697a0a446e..215a470810 100644
--- 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
@@ -59,13 +59,16 @@ class GILReleaser
   /** \brief Release the GIL on construction  */
   GILReleaser() noexcept
   {
-    m_thread_state = nullptr;
-    release();
+    m_thread_state = PyEval_SaveThread();
   }
   /** \brief Reacquire the GIL on destruction  */
   ~GILReleaser() noexcept
   {
-    reacquire();
+    if (m_thread_state)
+    {
+      PyEval_RestoreThread(m_thread_state);
+      m_thread_state = nullptr;
+    }
   }
 
   GILReleaser(const GILReleaser&) = delete;
@@ -87,24 +90,6 @@ class GILReleaser
   {
     std::swap(other.m_thread_state, m_thread_state);
   }
-
-  /** \brief Reacquire the GIL, noop if already acquired */
-  void reacquire() noexcept
-  {
-    if (m_thread_state)
-    {
-      PyEval_RestoreThread(m_thread_state);
-      m_thread_state = nullptr;
-    }
-  }
-  /** \brief Release the GIL (again), noop if already released */
-  void release() noexcept
-  {
-    if (!m_thread_state)
-    {
-      m_thread_state = PyEval_SaveThread();
-    }
-  }
 };
 
 }  // namespace py_bindings_tools
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 126d7f0cd3..86c6be8ac8 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
@@ -72,6 +72,20 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer
     return robot_model_->getName().c_str();
   }
 
+  bp::list getActiveJointNames() const
+  {
+    return py_bindings_tools::listFromString(robot_model_->getActiveJointModelNames());
+  }
+
+  bp::list getGroupActiveJointNames(const std::string& group) const
+  {
+    const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
+    if (jmg)
+      return py_bindings_tools::listFromString(jmg->getActiveJointModelNames());
+    else
+      return bp::list();
+  }
+
   bp::list getJointNames() const
   {
     return py_bindings_tools::listFromString(robot_model_->getJointModelNames());
@@ -380,6 +394,8 @@ static void wrap_robot_interface()
 
   robot_class.def("get_joint_names", &RobotInterfacePython::getJointNames);
   robot_class.def("get_group_joint_names", &RobotInterfacePython::getGroupJointNames);
+  robot_class.def("get_active_joint_names", &RobotInterfacePython::getActiveJointNames);
+  robot_class.def("get_group_active_joint_names", &RobotInterfacePython::getGroupActiveJointNames);
   robot_class.def("get_group_default_states", &RobotInterfacePython::getDefaultStateNames);
   robot_class.def("get_group_joint_tips", &RobotInterfacePython::getGroupJointTips);
   robot_class.def("get_group_names", &RobotInterfacePython::getGroupNames);
diff --git a/moveit_ros/planning_interface/setup.py b/moveit_ros/planning_interface/setup.py
index 685bfa2d77..60f5f44547 100644
--- a/moveit_ros/planning_interface/setup.py
+++ b/moveit_ros/planning_interface/setup.py
@@ -2,8 +2,8 @@
 from catkin_pkg.python_setup import generate_distutils_setup
 
 d = generate_distutils_setup()
-d['packages'] = ['moveit_ros_planning_interface']
-d['scripts'] = []
-d['package_dir'] = {'': 'python'}
+d["packages"] = ["moveit_ros_planning_interface"]
+d["scripts"] = []
+d["package_dir"] = {"": "python"}
 
 setup(**d)
diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt
index 871dddebe6..61609181cb 100644
--- a/moveit_ros/planning_interface/test/CMakeLists.txt
+++ b/moveit_ros/planning_interface/test/CMakeLists.txt
@@ -15,4 +15,16 @@ if (CATKIN_ENABLE_TESTING)
   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.py b/moveit_ros/planning_interface/test/cleanup.py
index bca6618a96..07cf98caa1 100755
--- a/moveit_ros/planning_interface/test/cleanup.py
+++ b/moveit_ros/planning_interface/test/cleanup.py
@@ -8,8 +8,8 @@
 import rospkg
 import roslib.packages
 
-PKGNAME = 'moveit_ros_planning_interface'
-NODENAME = 'moveit_cleanup_tests'
+PKGNAME = "moveit_ros_planning_interface"
+NODENAME = "moveit_cleanup_tests"
 
 # As issue #592 is related to a crash during program exit,
 # we cannot perform a standard unit test.
@@ -21,7 +21,7 @@ def __init__(self, *args, **kwargs):
         super(CleanupTest, self).__init__(*args, **kwargs)
         self._rospack = rospkg.RosPack()
 
-    def run_cmd(self, cmd, num = 5):
+    def run_cmd(self, cmd, num=5):
         failures = 0
         for i in range(num):
             if subprocess.call(cmd) != 0:
@@ -29,10 +29,13 @@ def run_cmd(self, cmd, num = 5):
         self.assertEqual(failures, 0, "%d of %d runs failed" % (failures, num))
 
     def test_py(self):
-        self.run_cmd(roslib.packages.find_node(PKGNAME, "test_cleanup.py", self._rospack))
+        self.run_cmd(
+            roslib.packages.find_node(PKGNAME, "test_cleanup.py", self._rospack)
+        )
 
     def test_cpp(self):
         self.run_cmd(roslib.packages.find_node(PKGNAME, "test_cleanup", self._rospack))
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     rostest.rosrun(PKGNAME, NODENAME, CleanupTest)
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
index ceae107b73..0155e75a68 100644
--- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp
+++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp
@@ -33,7 +33,7 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-/* Author: Tyler Weaver */
+/* 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
@@ -43,6 +43,7 @@
 #include <string>
 #include <vector>
 #include <map>
+#include <future>
 
 // ROS
 #include <ros/ros.h>
@@ -53,6 +54,8 @@
 // MoveIt
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
 #include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/common_planning_interface_objects/common_objects.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 
 // TF2
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -84,6 +87,32 @@ class MoveGroupTestFixture : public ::testing::Test
 
     // set the tolerance for the goals to be smaller than epsilon
     move_group_->setGoalTolerance(GOAL_TOLERANCE);
+
+    /* the tf buffer is not strictly needed,
+       but it's a simple way to add the codepaths to the tests */
+    psm_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description",
+                                                                          moveit::planning_interface::getSharedTF());
+    psm_->startSceneMonitor("/move_group/monitored_planning_scene");
+    psm_->requestPlanningSceneState();
+
+    // give move_group_, planning_scene_interface_ and psm_ time to connect their topics
+    ros::Duration(0.5).sleep();
+  }
+
+  // run updater() and ensure at least one geometry update was processed by the `move_group` node after doing so
+  void synchronizeGeometryUpdate(const std::function<void()>& updater)
+  {
+    SCOPED_TRACE("synchronizeGeometryUpdate");
+    std::promise<void> promise;
+    std::future<void> future = promise.get_future();
+    psm_->addUpdateCallback([this, &promise](planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType t) {
+      if (t & planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY)
+        promise.set_value();
+      psm_->clearUpdateCallbacks();
+    });
+    updater();
+    // the updater must have triggered a geometry update, otherwise we can't be sure about the state of the scene anymore
+    ASSERT_EQ(future.wait_for(std::chrono::seconds(5)), std::future_status::ready);
   }
 
   void planAndMoveToPose(const geometry_msgs::Pose& pose)
@@ -97,8 +126,8 @@ class MoveGroupTestFixture : public ::testing::Test
   {
     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);
+    ASSERT_EQ(move_group_->plan(my_plan), moveit::core::MoveItErrorCode::SUCCESS);
+    ASSERT_EQ(move_group_->move(), moveit::core::MoveItErrorCode::SUCCESS);
   }
 
   void testEigenPose(const Eigen::Isometry3d& expected, const Eigen::Isometry3d& actual)
@@ -148,18 +177,30 @@ class MoveGroupTestFixture : public ::testing::Test
   ros::NodeHandle nh_;
   moveit::planning_interface::MoveGroupInterfacePtr move_group_;
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface_;
+  planning_scene_monitor::PlanningSceneMonitorPtr psm_;
 };
 
-TEST_F(MoveGroupTestFixture, MoveToPoseTest)
+TEST_F(MoveGroupTestFixture, PathConstraintCollisionTest)
 {
-  SCOPED_TRACE("MoveToPoseTest");
+  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.28;
-  target_pose.position.y = -0.2;
-  target_pose.position.z = 0.5;
+  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;
@@ -174,46 +215,7 @@ TEST_F(MoveGroupTestFixture, MoveToPoseTest)
   // expect that they are identical
   testEigenPose(eigen_target_pose, eigen_set_target_pose);
 
-  // plan and move
-  planAndMove();
-
-  // get the pose after the movement
-  testPose(eigen_target_pose);
-}
-
-TEST_F(MoveGroupTestFixture, JointSpaceGoalTest)
-{
-  SCOPED_TRACE("JointSpaceGoalTest");
-
-  // Next get the current set of joint values for the group.
-  std::vector<double> 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);
-}
-
-TEST_F(MoveGroupTestFixture, PathConstraintTest)
-{
-  SCOPED_TRACE("PathConstraintTest");
-
-  // set a custom start state
-  geometry_msgs::Pose start_pose;
-  start_pose.orientation.w = 1.0;
-  start_pose.position.x = 0.55;
-  start_pose.position.y = -0.05;
-  start_pose.position.z = 0.8;
-  planAndMoveToPose(start_pose);
-
+  ////////////////////////////////////////////////////////////////////
   // create an orientation constraint
   moveit_msgs::OrientationConstraint ocm;
   ocm.link_name = move_group_->getEndEffectorLink();
@@ -227,72 +229,20 @@ TEST_F(MoveGroupTestFixture, PathConstraintTest)
   test_constraints.orientation_constraints.push_back(ocm);
   move_group_->setPathConstraints(test_constraints);
 
-  // move to a custom target pose
-  geometry_msgs::Pose target_pose;
-  target_pose.orientation.w = 1.0;
-  target_pose.position.x = 0.28;
-  target_pose.position.y = -0.2;
-  target_pose.position.z = 0.5;
-  planAndMoveToPose(target_pose);
-
-  // clear path constraints
-  move_group_->clearPathConstraints();
+  ////////////////////////////////////////////////////////////////////
+  // plan and move
+  planAndMove();
 
   // get the pose after the movement
-  testPose(target_pose);
-}
-
-TEST_F(MoveGroupTestFixture, CartPathTest)
-{
-  SCOPED_TRACE("CartPathTest");
-
-  // set a custom start state
-  geometry_msgs::Pose start_pose;
-  start_pose.orientation.w = 1.0;
-  start_pose.position.x = 0.55;
-  start_pose.position.y = -0.05;
-  start_pose.position.z = 0.8;
-  planAndMoveToPose(start_pose);
-
-  std::vector<geometry_msgs::Pose> waypoints;
-  waypoints.push_back(start_pose);
-
-  geometry_msgs::Pose target_waypoint = start_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 double jump_threshold = 0.0;
-  const double 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);
+  testPose(eigen_target_pose);
 
-  // get the pose after the movement
-  testPose(target_waypoint);
+  // clear path constraints
+  move_group_->clearPathConstraints();
 }
 
-TEST_F(MoveGroupTestFixture, CollisionObjectsTest)
+TEST_F(MoveGroupTestFixture, ModifyPlanningSceneAsyncInterfaces)
 {
-  SCOPED_TRACE("CollisionObjectsTest");
-
-  // set a custom start state
-  geometry_msgs::Pose start_pose;
-  start_pose.orientation.w = 1.0;
-  start_pose.position.x = 0.28;
-  start_pose.position.y = -0.2;
-  start_pose.position.z = 0.5;
-  planAndMoveToPose(start_pose);
-
+  ////////////////////////////////////////////////////////////////////
   // Define a collision object ROS message.
   moveit_msgs::CollisionObject collision_object;
   collision_object.header.frame_id = move_group_->getPlanningFrame();
@@ -304,16 +254,16 @@ TEST_F(MoveGroupTestFixture, CollisionObjectsTest)
   shape_msgs::SolidPrimitive primitive;
   primitive.type = primitive.BOX;
   primitive.dimensions.resize(3);
-  primitive.dimensions[0] = 0.4;
-  primitive.dimensions[1] = 0.1;
-  primitive.dimensions[2] = 0.1;
+  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.4;
-  box_pose.position.y = -0.2;
-  box_pose.position.z = 0.8;
+  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);
@@ -323,33 +273,78 @@ TEST_F(MoveGroupTestFixture, CollisionObjectsTest)
   collision_objects.push_back(collision_object);
 
   // Now, let's add the collision object into the world
-  planning_scene_interface_.addCollisionObjects(collision_objects);
-
-  // plan trajectory avoiding object
-  geometry_msgs::Pose target_pose;
-  target_pose.orientation.w = 0.0;
-  target_pose.position.x = 0.4;
-  target_pose.position.y = -0.4;
-  target_pose.position.z = 0.7;
-  planAndMoveToPose(target_pose);
-
-  // get the pose after the movement
-  testPose(target_pose);
+  synchronizeGeometryUpdate([&]() { planning_scene_interface_.addCollisionObjects(collision_objects); });
 
   // attach and detach collision object
-  EXPECT_TRUE(move_group_->attachObject(collision_object.id));
+  synchronizeGeometryUpdate([&]() { 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));
+  synchronizeGeometryUpdate([&]() { EXPECT_TRUE(move_group_->detachObject(collision_object.id)); });
   EXPECT_EQ(planning_scene_interface_.getAttachedObjects().size(), std::size_t(0));
 
   // remove object from world
-  std::vector<std::string> object_ids;
-  object_ids.push_back(collision_object.id);
+  const std::vector<std::string> object_ids = { collision_object.id };
   EXPECT_EQ(planning_scene_interface_.getObjects().size(), std::size_t(1));
-  planning_scene_interface_.removeCollisionObjects(object_ids);
+  synchronizeGeometryUpdate([&]() { 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<geometry_msgs::Pose> 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;
+
+  // test below is meaningless if Cartesian planning did not succeed
+  ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory), 1.0);
+
+  // Execute trajectory
+  EXPECT_EQ(move_group_->execute(trajectory), moveit::core::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<double> 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");
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
index 9975f257d2..02d1f7201c 100644
--- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test
+++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test
@@ -1,27 +1,5 @@
 <launch>
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
-
-  <!-- If needed, broadcast static tf for robot root -->
-  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world panda_link0" />
-
-  <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
-  </node>
-  <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
-
-  <!-- Given the published joint states, publish tf for the robot links -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
-
-  <!-- Run the main MoveIt executable with fake trajectory execution -->
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
-  </include>
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch" />
 
   <!-- test -->
   <test pkg="moveit_ros_planning_interface" type="move_group_interface_cpp_test" test-name="move_group_interface_cpp_test"
diff --git a/moveit_ros/planning_interface/test/moveit_cpp.yaml b/moveit_ros/planning_interface/test/moveit_cpp.yaml
index 6d622662c9..9a69f0dd1d 100644
--- a/moveit_ros/planning_interface/test/moveit_cpp.yaml
+++ b/moveit_ros/planning_interface/test/moveit_cpp.yaml
@@ -15,4 +15,4 @@ 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
+  max_acceleration_scaling_factor: 1.0
diff --git a/moveit_ros/planning_interface/test/python_move_group.py b/moveit_ros/planning_interface/test/python_move_group.py
index 187e7bfebb..4dc9b81ae2 100755
--- a/moveit_ros/planning_interface/test/python_move_group.py
+++ b/moveit_ros/planning_interface/test/python_move_group.py
@@ -6,7 +6,9 @@
 import rostest
 import os
 
-from moveit_ros_planning_interface._moveit_move_group_interface import MoveGroupInterface
+from moveit_ros_planning_interface._moveit_move_group_interface import (
+    MoveGroupInterface,
+)
 
 
 class PythonMoveGroupTest(unittest.TestCase):
@@ -14,7 +16,9 @@ class PythonMoveGroupTest(unittest.TestCase):
 
     @classmethod
     def setUpClass(self):
-        self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace())
+        self.group = MoveGroupInterface(
+            self.PLANNING_GROUP, "robot_description", rospy.get_namespace()
+        )
 
     @classmethod
     def tearDown(self):
@@ -25,16 +29,20 @@ def check_target_setting(self, expect, *args):
             args = [expect]
         self.group.set_joint_value_target(*args)
         res = self.group.get_joint_value_target()
-        self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)),
-                        "Setting failed for %s, values: %s" % (type(args[0]), res))
+        self.assertTrue(
+            np.all(np.asarray(res) == np.asarray(expect)),
+            "Setting failed for %s, values: %s" % (type(args[0]), res),
+        )
 
     def test_target_setting(self):
         n = self.group.get_variable_count()
         self.check_target_setting([0.1] * n)
         self.check_target_setting((0.2,) * n)
         self.check_target_setting(np.zeros(n))
-        self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()})
-        self.check_target_setting([0.5] + [0.3]*(n-1), "joint_1", 0.5)
+        self.check_target_setting(
+            [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}
+        )
+        self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5)
 
     def plan(self, target):
         self.group.set_joint_value_target(target)
@@ -60,24 +68,33 @@ def test_get_jacobian_matrix(self):
         current = self.group.get_current_joint_values()
         result = self.group.get_jacobian_matrix(current)
         # Value check by known value at the initial pose
-        expected = np.array([[ 0.  ,  0.8 , -0.2 ,  0.  ,  0.  ,  0.  ],
-                             [ 0.89,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
-                             [ 0.  , -0.74,  0.74,  0.  ,  0.1 ,  0.  ],
-                             [ 0.  ,  0.  ,  0.  , -1.  ,  0.  , -1.  ],
-                             [ 0.  ,  1.  , -1.  ,  0.  , -1.  ,  0.  ],
-                             [ 1.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ]])
+        expected = np.array(
+            [
+                [0.0, 0.8, -0.2, 0.0, 0.0, 0.0],
+                [0.89, 0.0, 0.0, 0.0, 0.0, 0.0],
+                [0.0, -0.74, 0.74, 0.0, 0.1, 0.0],
+                [0.0, 0.0, 0.0, -1.0, 0.0, -1.0],
+                [0.0, 1.0, -1.0, 0.0, -1.0, 0.0],
+                [1.0, 0.0, 0.0, 0.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'
-    NODENAME = 'moveit_test_python_move_group'
+        expected = np.array(
+            [
+                [1.0, 1.8, -1.2, 0.0, -1.0, 0.0],
+                [1.89, 0.0, 0.0, 1.0, 0.0, 1.0],
+                [0.0, -1.74, 1.74, 1.0, 1.1, 1.0],
+                [0.0, 0.0, 0.0, -1.0, 0.0, -1.0],
+                [0.0, 1.0, -1.0, 0.0, -1.0, 0.0],
+                [1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            ]
+        )
+
+
+if __name__ == "__main__":
+    PKGNAME = "moveit_ros_planning_interface"
+    NODENAME = "moveit_test_python_move_group"
     rospy.init_node(NODENAME)
     rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupTest)
diff --git a/moveit_ros/planning_interface/test/python_move_group_ns.py b/moveit_ros/planning_interface/test/python_move_group_ns.py
index 13a5c21fed..2e85e4f6c3 100755
--- a/moveit_ros/planning_interface/test/python_move_group_ns.py
+++ b/moveit_ros/planning_interface/test/python_move_group_ns.py
@@ -43,7 +43,9 @@
 import rostest
 import os
 
-from moveit_ros_planning_interface._moveit_move_group_interface import MoveGroupInterface
+from moveit_ros_planning_interface._moveit_move_group_interface import (
+    MoveGroupInterface,
+)
 
 
 class PythonMoveGroupNsTest(unittest.TestCase):
@@ -52,7 +54,11 @@ class PythonMoveGroupNsTest(unittest.TestCase):
 
     @classmethod
     def setUpClass(self):
-        self.group = MoveGroupInterface(self.PLANNING_GROUP, "%srobot_description"%self.PLANNING_NS, self.PLANNING_NS)
+        self.group = MoveGroupInterface(
+            self.PLANNING_GROUP,
+            "%srobot_description" % self.PLANNING_NS,
+            self.PLANNING_NS,
+        )
 
     @classmethod
     def tearDown(self):
@@ -63,16 +69,20 @@ def check_target_setting(self, expect, *args):
             args = [expect]
         self.group.set_joint_value_target(*args)
         res = self.group.get_joint_value_target()
-        self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)),
-                        "Setting failed for %s, values: %s" % (type(args[0]), res))
+        self.assertTrue(
+            np.all(np.asarray(res) == np.asarray(expect)),
+            "Setting failed for %s, values: %s" % (type(args[0]), res),
+        )
 
     def test_target_setting(self):
         n = self.group.get_variable_count()
         self.check_target_setting([0.1] * n)
         self.check_target_setting((0.2,) * n)
         self.check_target_setting(np.zeros(n))
-        self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()})
-        self.check_target_setting([0.5] + [0.3]*(n-1), "joint_1", 0.5)
+        self.check_target_setting(
+            [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}
+        )
+        self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5)
 
     def plan(self, target):
         self.group.set_joint_value_target(target)
@@ -95,8 +105,8 @@ def test_validation(self):
         self.assertTrue(self.group.execute(plan3))
 
 
-if __name__ == '__main__':
-    PKGNAME = 'moveit_ros_planning_interface'
-    NODENAME = 'moveit_test_python_move_group'
+if __name__ == "__main__":
+    PKGNAME = "moveit_ros_planning_interface"
+    NODENAME = "moveit_test_python_move_group"
     rospy.init_node(NODENAME)
     rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupNsTest)
diff --git a/moveit_ros/planning_interface/test/robot_state_update.py b/moveit_ros/planning_interface/test/robot_state_update.py
index f85c5328e1..394c7af3ec 100755
--- a/moveit_ros/planning_interface/test/robot_state_update.py
+++ b/moveit_ros/planning_interface/test/robot_state_update.py
@@ -6,7 +6,9 @@
 import rostest
 import os
 
-from moveit_ros_planning_interface._moveit_move_group_interface import MoveGroupInterface
+from moveit_ros_planning_interface._moveit_move_group_interface import (
+    MoveGroupInterface,
+)
 
 
 class RobotStateUpdateTest(unittest.TestCase):
@@ -14,7 +16,9 @@ class RobotStateUpdateTest(unittest.TestCase):
 
     @classmethod
     def setUpClass(self):
-        self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace())
+        self.group = MoveGroupInterface(
+            self.PLANNING_GROUP, "robot_description", rospy.get_namespace()
+        )
 
     @classmethod
     def tearDown(self):
@@ -27,20 +31,20 @@ def plan(self, target):
     def test(self):
         current = np.asarray(self.group.get_current_joint_values())
         for i in range(30):
-            target = current + np.random.uniform(-0.5, 0.5, size = current.shape)
+            target = current + np.random.uniform(-0.5, 0.5, size=current.shape)
             # if plan was successfully executed, current state should be reported at target
             if self.group.execute(self.plan(target)):
-                 actual = np.asarray(self.group.get_current_joint_values())
-                 self.assertTrue(np.allclose(target, actual, atol=1e-4, rtol=0.0))
+                actual = np.asarray(self.group.get_current_joint_values())
+                self.assertTrue(np.allclose(target, actual, atol=1e-4, rtol=0.0))
             # otherwise current state should be still the same
             else:
-               actual = np.asarray(self.group.get_current_joint_values())
-               self.assertTrue(np.allclose(current, actual, atol=1e-4, rtol=0.0))
+                actual = np.asarray(self.group.get_current_joint_values())
+                self.assertTrue(np.allclose(current, actual, atol=1e-4, rtol=0.0))
 
 
-if __name__ == '__main__':
-    PKGNAME = 'moveit_ros_planning_interface'
-    NODENAME = 'moveit_test_robot_state_update'
+if __name__ == "__main__":
+    PKGNAME = "moveit_ros_planning_interface"
+    NODENAME = "moveit_test_robot_state_update"
     rospy.init_node(NODENAME)
     rostest.rosrun(PKGNAME, NODENAME, RobotStateUpdateTest)
 
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..212c0905eb
--- /dev/null
+++ b/moveit_ros/planning_interface/test/serialize_msg.py
@@ -0,0 +1,116 @@
+#!/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 <Python.h>
+#include <boost/python.hpp>
+#include <moveit/py_bindings_tools/serialize_msg.h>
+#include <geometry_msgs/Vector3.h>
+#include <cstring>
+
+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<unsigned char>& 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<unsigned char>::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<unsigned char> testdata{ 0xff, 0xef, 0x00, 0x10 };
+    return doCompare(testdata, s.ptr());
+  }
+  bool compareTuple(const bp::tuple& t)
+  {
+    const ByteString s(t[0]);
+    const std::vector<unsigned char> 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_<ByteStringTestHelper> 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/test_cleanup.py b/moveit_ros/planning_interface/test/test_cleanup.py
index 1567c1ea3c..bb0aad1f72 100755
--- a/moveit_ros/planning_interface/test/test_cleanup.py
+++ b/moveit_ros/planning_interface/test/test_cleanup.py
@@ -1,5 +1,8 @@
 #!/usr/bin/env python
 
 import rospy
-from moveit_ros_planning_interface._moveit_move_group_interface import MoveGroupInterface
+from moveit_ros_planning_interface._moveit_move_group_interface import (
+    MoveGroupInterface,
+)
+
 group = MoveGroupInterface("manipulator", "robot_description", rospy.get_namespace())
diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst
index a843d0ca51..0fbd86179f 100644
--- a/moveit_ros/robot_interaction/CHANGELOG.rst
+++ b/moveit_ros/robot_interaction/CHANGELOG.rst
@@ -2,6 +2,25 @@
 Changelog for package moveit_ros_robot_interaction
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+* Suppress warnings "mesh_use_embedded_materials is ignored"
+* Contributors: Robert Haschke
+
+1.0.7 (2020-11-20)
+------------------
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: Felix von Drigalski
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Migrate to clang-format-10
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 4841fab90a..6bd22ae98d 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
@@ -47,9 +47,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;
@@ -217,7 +217,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,
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 022042d78a..dd0862562b 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
@@ -45,7 +45,7 @@
 
 namespace robot_interaction
 {
-MOVEIT_CLASS_FORWARD(LockedRobotState);
+MOVEIT_CLASS_FORWARD(LockedRobotState);  // Defines LockedRobotStatePtr, ConstPtr, WeakPtr... etc
 
 /// Maintain a RobotState in a multithreaded environment.
 //
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 bbbd9585cc..7a912ff0ec 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
@@ -58,9 +58,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.
 //
@@ -79,7 +79,7 @@ class RobotInteraction
   RobotInteraction(const robot_model::RobotModelConstPtr& robot_model, const std::string& ns = "");
   virtual ~RobotInteraction();
 
-  const std::string& getServerTopic(void) const
+  const std::string& getServerTopic() const
   {
     return topic_;
   }
diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml
index 0a8a3519f2..28a50e62d2 100644
--- a/moveit_ros/robot_interaction/package.xml
+++ b/moveit_ros/robot_interaction/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_robot_interaction</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Components of MoveIt! that offer interaction via interactive markers</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp
index fddf7962b6..0bf627cf07 100644
--- a/moveit_ros/robot_interaction/src/robot_interaction.cpp
+++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp
@@ -414,7 +414,7 @@ void RobotInteraction::addEndEffectorMarkers(const InteractionHandlerPtr& handle
   for (std::size_t i = 0; i < marker_array.markers.size(); ++i)
   {
     marker_array.markers[i].header = im.header;
-    marker_array.markers[i].mesh_use_embedded_materials = true;
+    marker_array.markers[i].mesh_use_embedded_materials = !marker_array.markers[i].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);
diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst
index d37e2e9a52..b8040cabba 100644
--- a/moveit_ros/visualization/CHANGELOG.rst
+++ b/moveit_ros/visualization/CHANGELOG.rst
@@ -2,6 +2,34 @@
 Changelog for package moveit_ros_visualization
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 <https://github.com/ros-planning/moveit/issues/3009>`_)
+* Contributors: Michael Görner, Robert Haschke
+
+1.0.8 (2021-05-23)
+------------------
+* Remove unused model_is_loading\_
+* Fix deadlock in PlanningSceneDisplay
+* Call renderPlanningScene() only if planning_scene_render\_ is valid
+* Keep MotionPlanningFrame hidden on Display::reset()
+* Catch exceptions during RobotModel loading in rviz (`#2468 <https://github.com/ros-planning/moveit/issues/2468>`_)
+* Fix QObject::connect: Cannot queue arguments of type 'QVector<int>' (`#2392 <https://github.com/ros-planning/moveit/issues/2392>`_)
+* Contributors: Robert Haschke, Simon Schmeisser, Tyler Weaver
+
+1.0.7 (2020-11-20)
+------------------
+* [feature] Clean up Rviz Motion Planning plugin, add tooltips (`#2310 <https://github.com/ros-planning/moveit/issues/2310>`_)
+* [fix] Fix "Clear Octomap" button, disable when no octomap is published (`#2320 <https://github.com/ros-planning/moveit/issues/2320>`_)
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: Felix von Drigalski, Robert Haschke
+
 1.0.6 (2020-08-19)
 ------------------
 * [feature] MP display: add units to joints tab (`#2264 <https://github.com/ros-planning/moveit/issues/2264>`_)
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 f39c15afdb..b2d43331e8 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
@@ -74,9 +74,9 @@ 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
@@ -103,7 +103,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();
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
index 3f52a306f8..984f776926 100644
--- 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
@@ -124,7 +124,7 @@ class MotionPlanningFrameJointsWidget : public QWidget
 public:
   MotionPlanningFrameJointsWidget(const MotionPlanningFrameJointsWidget&) = delete;
   MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent = nullptr);
-  ~MotionPlanningFrameJointsWidget();
+  ~MotionPlanningFrameJointsWidget() override;
 
   void changePlanningGroup(const std::string& group_name,
                            const robot_interaction::InteractionHandlerPtr& start_state_handler,
@@ -168,7 +168,7 @@ class ProgressBarDelegate : public QStyledItemDelegate
     VariableBoundsRole
   };
 
-  ProgressBarDelegate(QWidget* parent = 0) : QStyledItemDelegate(parent)
+  ProgressBarDelegate(QWidget* parent = nullptr) : QStyledItemDelegate(parent)
   {
   }
 
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 d631b582ea..c721089b54 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
@@ -43,7 +43,7 @@ namespace moveit
 {
 namespace planning_interface
 {
-MOVEIT_CLASS_FORWARD(MoveGroupInterface);
+MOVEIT_CLASS_FORWARD(MoveGroupInterface);  // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc
 }
 }  // namespace moveit
 
@@ -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);
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 f27a54f86a..2388518d10 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
@@ -298,11 +298,14 @@ void MotionPlanningDisplay::reset()
   // Planned Path Display
   trajectory_visual_->reset();
 
+  bool enabled = this->isEnabled();
   frame_->disable();
-  frame_->enable();
-
-  query_robot_start_->setVisible(query_start_state_property_->getBool());
-  query_robot_goal_->setVisible(query_goal_state_property_->getBool());
+  if (enabled)
+  {
+    frame_->enable();
+    query_robot_start_->setVisible(query_start_state_property_->getBool());
+    query_robot_goal_->setVisible(query_goal_state_property_->getBool());
+  }
 }
 
 void MotionPlanningDisplay::setName(const QString& name)
@@ -1329,8 +1332,6 @@ void MotionPlanningDisplay::load(const rviz::Config& config)
       frame_->ui_->velocity_scaling_factor->setValue(d);
     if (config.mapGetFloat("Acceleration_Scaling_Factor", &d))
       frame_->ui_->acceleration_scaling_factor->setValue(d);
-    if (config.mapGetFloat("MoveIt_Goal_Tolerance", &d))
-      frame_->ui_->goal_tolerance->setValue(d);
 
     bool b;
     if (config.mapGetBool("MoveIt_Allow_Replanning", &b))
@@ -1389,8 +1390,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());
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 16311397d8..ab5bc6e208 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
@@ -156,8 +156,6 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::
   connect(ui_->database_host, SIGNAL(textChanged(QString)), this, SIGNAL(configChanged()));
   connect(ui_->database_port, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 
-  connect(ui_->goal_tolerance, SIGNAL(valueChanged(double)), 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()));
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
index 7f4782f66c..d33563a76d 100644
--- 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
@@ -50,7 +50,7 @@ JMGItemModel::JMGItemModel(const moveit::core::RobotState& robot_state, const st
     jmg_ = robot_state_.getRobotModel()->getJointModelGroup(group_name);
 }
 
-int JMGItemModel::rowCount(const QModelIndex& parent) const
+int JMGItemModel::rowCount(const QModelIndex& /*parent*/) const
 {
   if (!jmg_)
     return robot_state_.getVariableCount();
@@ -58,7 +58,7 @@ int JMGItemModel::rowCount(const QModelIndex& parent) const
     return jmg_->getVariableCount();
 }
 
-int JMGItemModel::columnCount(const QModelIndex& parent) const
+int JMGItemModel::columnCount(const QModelIndex& /*parent*/) const
 {
   return 2;
 }
@@ -461,7 +461,7 @@ JointsWidgetEventFilter::JointsWidgetEventFilter(QAbstractItemView* view) : QObj
 {
 }
 
-bool JointsWidgetEventFilter::eventFilter(QObject* target, QEvent* event)
+bool JointsWidgetEventFilter::eventFilter(QObject* /*target*/, QEvent* event)
 {
   if (event->type() == QEvent::MouseButtonPress)
   {
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 dd21924e7c..b056969c13 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
@@ -58,7 +58,7 @@
 
 namespace moveit_rviz_plugin
 {
-void MotionPlanningFrame::shapesComboBoxChanged(const QString& text)
+void MotionPlanningFrame::shapesComboBoxChanged(const QString& /*text*/)
 {
   switch (ui_->shapes_combo_box->currentData().toInt())  // fetch shape ID from current combobox item
   {
@@ -890,6 +890,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<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
@@ -907,7 +908,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);
@@ -939,6 +943,7 @@ 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();
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 e7fbd57865..b85ca3e6bd 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
@@ -110,6 +110,7 @@ void MotionPlanningFrame::onClearOctomapClicked()
 {
   std_srvs::Empty srv;
   clear_octomap_service_client_.call(srv);
+  ui_->clear_octomap_button->setEnabled(false);
 }
 
 bool MotionPlanningFrame::computeCartesianPlan()
@@ -162,7 +163,7 @@ bool MotionPlanningFrame::computeCartesianPlan()
 bool MotionPlanningFrame::computeJointSpacePlan()
 {
   current_plan_.reset(new moveit::planning_interface::MoveGroupInterface::Plan());
-  return move_group_->plan(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
+  return move_group_->plan(*current_plan_) == moveit::core::MoveItErrorCode::SUCCESS;
 }
 
 void MotionPlanningFrame::computePlanButtonClicked()
@@ -199,7 +200,7 @@ void MotionPlanningFrame::computeExecuteButtonClicked()
   if (mgi && current_plan_)
   {
     ui_->stop_button->setEnabled(true);  // enable stopping
-    bool success = mgi->execute(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
+    bool success = mgi->execute(*current_plan_) == moveit::core::MoveItErrorCode::SUCCESS;
     onFinishedExecution(success);
   }
 }
@@ -221,7 +222,7 @@ void MotionPlanningFrame::computePlanAndExecuteButtonClicked()
   }
   else
   {
-    bool success = move_group_->move() == moveit::planning_interface::MoveItErrorCode::SUCCESS;
+    bool success = move_group_->move() == moveit::core::MoveItErrorCode::SUCCESS;
     onFinishedExecution(success);
   }
   ui_->plan_and_execute_button->setEnabled(true);
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 22154c05da..e99f8bd2b9 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,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>532</width>
-    <height>389</height>
+    <width>507</width>
+    <height>380</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -26,7 +26,7 @@
       <bool>false</bool>
      </property>
      <property name="currentIndex">
-      <number>3</number>
+      <number>1</number>
      </property>
      <widget class="QWidget" name="context">
       <attribute name="title">
@@ -96,6 +96,9 @@
             </item>
             <item>
              <widget class="moveit_rviz_plugin::MotionPlanningParamWidget" name="planner_param_treeview">
+              <property name="autoScroll">
+               <bool>false</bool>
+              </property>
               <property name="indentation">
                <number>0</number>
               </property>
@@ -157,6 +160,12 @@
           </item>
           <item>
            <widget class="QPushButton" name="reset_db_button">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
             <property name="styleSheet">
              <string notr="true">color:red</string>
             </property>
@@ -401,7 +410,7 @@
               <item>
                <widget class="QPushButton" name="plan_and_execute_button">
                 <property name="text">
-                 <string>Plan and E&amp;xecute</string>
+                 <string>Plan &amp;&amp; E&amp;xecute</string>
                 </property>
                </widget>
               </item>
@@ -410,6 +419,9 @@
                 <property name="enabled">
                  <bool>false</bool>
                 </property>
+                <property name="toolTip">
+                 <string>Stop execution by sending a stop command to the move_group.</string>
+                </property>
                 <property name="text">
                  <string>&amp;Stop</string>
                 </property>
@@ -433,6 +445,9 @@
               </item>
               <item>
                <widget class="QPushButton" name="clear_octomap_button">
+                <property name="enabled">
+                 <bool>false</bool>
+                </property>
                 <property name="text">
                  <string>Clear octomap</string>
                 </property>
@@ -509,12 +524,12 @@
             <enum>Qt::Vertical</enum>
            </property>
            <property name="sizeType">
-            <enum>QSizePolicy::Fixed</enum>
+            <enum>QSizePolicy::Expanding</enum>
            </property>
            <property name="sizeHint" stdset="0">
             <size>
              <width>20</width>
-             <height>6</height>
+             <height>32</height>
             </size>
            </property>
           </spacer>
@@ -538,38 +553,15 @@
              <number>6</number>
             </property>
             <item>
-             <widget class="QComboBox" name="path_constraints_combo_box"/>
-            </item>
-            <item>
-             <layout class="QHBoxLayout" name="horizontalLayout_11">
-              <item>
-               <widget class="QLabel" name="label_6">
-                <property name="text">
-                 <string>Goal Tolerance:</string>
-                </property>
-               </widget>
-              </item>
-              <item>
-               <widget class="QDoubleSpinBox" name="goal_tolerance"/>
-              </item>
-             </layout>
+             <widget class="QComboBox" name="path_constraints_combo_box">
+              <property name="toolTip">
+               <string>Select path constraints from the database. </string>
+              </property>
+             </widget>
             </item>
            </layout>
           </widget>
          </item>
-         <item>
-          <spacer name="verticalSpacer_3">
-           <property name="orientation">
-            <enum>Qt::Vertical</enum>
-           </property>
-           <property name="sizeHint" stdset="0">
-            <size>
-             <width>20</width>
-             <height>40</height>
-            </size>
-           </property>
-          </spacer>
-         </item>
         </layout>
        </item>
        <item>
@@ -584,7 +576,7 @@
            </property>
            <layout class="QVBoxLayout" name="verticalLayout_10">
             <property name="spacing">
-             <number>2</number>
+             <number>1</number>
             </property>
             <property name="bottomMargin">
              <number>4</number>
@@ -596,6 +588,9 @@
               </property>
               <item>
                <widget class="QLabel" name="label_9">
+                <property name="toolTip">
+                 <string>Total time allowed for planning.  Planning stops if time or number of attempts is exceeded, or goal is reached.</string>
+                </property>
                 <property name="text">
                  <string>Planning Time (s):</string>
                 </property>
@@ -609,6 +604,18 @@
                   <verstretch>0</verstretch>
                  </sizepolicy>
                 </property>
+                <property name="minimumSize">
+                 <size>
+                  <width>40</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="toolTip">
+                 <string>Total time allowed for planning.  Planning stops if time or number of attempts is exceeded, or goal is reached.</string>
+                </property>
+                <property name="decimals">
+                 <number>1</number>
+                </property>
                 <property name="maximum">
                  <double>300.000000000000000</double>
                 </property>
@@ -626,6 +633,9 @@
               </property>
               <item>
                <widget class="QLabel" name="label_10">
+                <property name="toolTip">
+                 <string>Allowed number of planning attempts. Planning stops if time or number of attempts is exceeded, or goal is reached.</string>
+                </property>
                 <property name="text">
                  <string>Planning Attempts:</string>
                 </property>
@@ -639,6 +649,15 @@
                   <verstretch>0</verstretch>
                  </sizepolicy>
                 </property>
+                <property name="minimumSize">
+                 <size>
+                  <width>40</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="toolTip">
+                 <string>Allowed number of planning attempts. Planning stops if time or number of attempts is exceeded, or goal is reached.</string>
+                </property>
                 <property name="maximum">
                  <number>1000</number>
                 </property>
@@ -656,6 +675,9 @@
               </property>
               <item>
                <widget class="QLabel" name="label_11">
+                <property name="toolTip">
+                 <string>Factor (between 0 and 1) to scale down maximum joint velocities</string>
+                </property>
                 <property name="text">
                  <string>Velocity Scaling:</string>
                 </property>
@@ -663,11 +685,20 @@
               </item>
               <item>
                <widget class="QDoubleSpinBox" name="velocity_scaling_factor">
+                <property name="minimumSize">
+                 <size>
+                  <width>40</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="toolTip">
+                 <string>Factor (between 0 and 1) to scale down maximum joint velocities</string>
+                </property>
                 <property name="maximum">
                  <double>1.000000000000000</double>
                 </property>
                 <property name="singleStep">
-                 <double>0.010000000000000</double>
+                 <double>0.100000000000000</double>
                 </property>
                 <property name="value">
                  <double>1.000000000000000</double>
@@ -683,18 +714,30 @@
               </property>
               <item>
                <widget class="QLabel" name="label_20">
+                <property name="toolTip">
+                 <string>Factor (between 0 and 1) to scale down maximum joint accelerations</string>
+                </property>
                 <property name="text">
-                 <string>Acceleration Scaling:</string>
+                 <string>Accel. Scaling:</string>
                 </property>
                </widget>
               </item>
               <item>
                <widget class="QDoubleSpinBox" name="acceleration_scaling_factor">
+                <property name="minimumSize">
+                 <size>
+                  <width>40</width>
+                  <height>0</height>
+                 </size>
+                </property>
+                <property name="toolTip">
+                 <string>Factor (between 0 and 1) to scale down maximum joint accelerations</string>
+                </property>
                 <property name="maximum">
                  <double>1.000000000000000</double>
                 </property>
                 <property name="singleStep">
-                 <double>0.010000000000000</double>
+                 <double>0.100000000000000</double>
                 </property>
                 <property name="value">
                  <double>1.000000000000000</double>
@@ -704,9 +747,39 @@
              </layout>
             </item>
             <item>
-             <widget class="QCheckBox" name="allow_replanning">
+             <spacer name="verticalSpacer_2">
+              <property name="orientation">
+               <enum>Qt::Vertical</enum>
+              </property>
+              <property name="sizeHint" stdset="0">
+               <size>
+                <width>20</width>
+                <height>40</height>
+               </size>
+              </property>
+             </spacer>
+            </item>
+            <item>
+             <widget class="QCheckBox" name="use_cartesian_path">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="minimumSize">
+               <size>
+                <width>30</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="toolTip">
+               <string>Check this to generate a linear path in Cartesian (3D) space.
+
+ This does not plan around obstacles. </string>
+              </property>
               <property name="text">
-               <string>Allow Replanning</string>
+               <string>Use Cartesian Path</string>
               </property>
               <property name="checked">
                <bool>false</bool>
@@ -714,9 +787,26 @@
              </widget>
             </item>
             <item>
-             <widget class="QCheckBox" name="allow_looking">
+             <widget class="QCheckBox" name="collision_aware_ik">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="minimumSize">
+               <size>
+                <width>30</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="toolTip">
+               <string>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.</string>
+              </property>
               <property name="text">
-               <string>Allow Sensor Positioning</string>
+               <string>Collision-aware IK</string>
               </property>
               <property name="checked">
                <bool>false</bool>
@@ -724,19 +814,46 @@
              </widget>
             </item>
             <item>
-             <widget class="QCheckBox" name="allow_external_program">
-              <property name="text">
-               <string>Allow External Comm.</string>
+             <widget class="QCheckBox" name="approximate_ik">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
               </property>
-              <property name="checked">
-               <bool>false</bool>
+              <property name="minimumSize">
+               <size>
+                <width>30</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="toolTip">
+               <string>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.</string>
+              </property>
+              <property name="text">
+               <string>Approx IK Solutions</string>
               </property>
              </widget>
             </item>
             <item>
-             <widget class="QCheckBox" name="use_cartesian_path">
+             <widget class="QCheckBox" name="allow_external_program">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="minimumSize">
+               <size>
+                <width>30</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="toolTip">
+               <string>Allow other nodes to control start and end poses as well as planning and execution, using ROS topics (/rviz/moveit/ ).</string>
+              </property>
               <property name="text">
-               <string>Use Cartesian Path</string>
+               <string>External Comm.</string>
               </property>
               <property name="checked">
                <bool>false</bool>
@@ -744,38 +861,61 @@
              </widget>
             </item>
             <item>
-             <widget class="QCheckBox" name="collision_aware_ik">
+             <widget class="QCheckBox" name="allow_replanning">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="minimumSize">
+               <size>
+                <width>30</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="toolTip">
+               <string>EXPERIMENTAL: Trigger replanning if new collisions are detected during plan execution. Only works for the &quot;Plan &amp; Execute&quot; button. </string>
+              </property>
+              <property name="autoFillBackground">
+               <bool>false</bool>
+              </property>
               <property name="text">
-               <string>Use Collision-Aware IK</string>
+               <string>Replanning</string>
               </property>
               <property name="checked">
-               <bool>true</bool>
+               <bool>false</bool>
               </property>
              </widget>
             </item>
             <item>
-             <widget class="QCheckBox" name="approximate_ik">
+             <widget class="QCheckBox" name="allow_looking">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="minimumSize">
+               <size>
+                <width>30</width>
+                <height>0</height>
+               </size>
+              </property>
+              <property name="toolTip">
+               <string>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.</string>
+              </property>
               <property name="text">
-               <string>Allow Approx IK Solutions</string>
+               <string>Sensor Positioning</string>
+              </property>
+              <property name="checked">
+               <bool>false</bool>
               </property>
              </widget>
             </item>
            </layout>
           </widget>
          </item>
-         <item>
-          <spacer name="verticalSpacer_2">
-           <property name="orientation">
-            <enum>Qt::Vertical</enum>
-           </property>
-           <property name="sizeHint" stdset="0">
-            <size>
-             <width>20</width>
-             <height>40</height>
-            </size>
-           </property>
-          </spacer>
-         </item>
         </layout>
        </item>
       </layout>
@@ -857,6 +997,18 @@
          <layout class="QGridLayout" name="gridLayout_14">
           <item row="0" column="2">
            <widget class="QDoubleSpinBox" name="roi_center_y">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>40</width>
+              <height>0</height>
+             </size>
+            </property>
             <property name="minimum">
              <double>-99.000000000000000</double>
             </property>
@@ -867,6 +1019,18 @@
           </item>
           <item row="1" column="1">
            <widget class="QDoubleSpinBox" name="roi_size_x">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>40</width>
+              <height>0</height>
+             </size>
+            </property>
             <property name="singleStep">
              <double>0.010000000000000</double>
             </property>
@@ -877,6 +1041,18 @@
           </item>
           <item row="1" column="3">
            <widget class="QDoubleSpinBox" name="roi_size_z">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>40</width>
+              <height>0</height>
+             </size>
+            </property>
             <property name="singleStep">
              <double>0.010000000000000</double>
             </property>
@@ -887,6 +1063,18 @@
           </item>
           <item row="1" column="2">
            <widget class="QDoubleSpinBox" name="roi_size_y">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>40</width>
+              <height>0</height>
+             </size>
+            </property>
             <property name="singleStep">
              <double>0.010000000000000</double>
             </property>
@@ -904,6 +1092,18 @@
           </item>
           <item row="0" column="1">
            <widget class="QDoubleSpinBox" name="roi_center_x">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>40</width>
+              <height>0</height>
+             </size>
+            </property>
             <property name="minimum">
              <double>-99.000000000000000</double>
             </property>
@@ -921,6 +1121,18 @@
           </item>
           <item row="0" column="3">
            <widget class="QDoubleSpinBox" name="roi_center_z">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>40</width>
+              <height>0</height>
+             </size>
+            </property>
             <property name="minimum">
              <double>-99.000000000000000</double>
             </property>
@@ -1009,6 +1221,18 @@
              <layout class="QHBoxLayout" name="horizontalLayout_16">
               <item>
                <widget class="QDoubleSpinBox" name="shape_size_x_spin_box">
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+                <property name="minimumSize">
+                 <size>
+                  <width>20</width>
+                  <height>0</height>
+                 </size>
+                </property>
                 <property name="toolTip">
                  <string>x dimension</string>
                 </property>
@@ -1022,6 +1246,18 @@
               </item>
               <item>
                <widget class="QDoubleSpinBox" name="shape_size_z_spin_box">
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+                <property name="minimumSize">
+                 <size>
+                  <width>20</width>
+                  <height>0</height>
+                 </size>
+                </property>
                 <property name="toolTip">
                  <string>y dimension</string>
                 </property>
@@ -1035,6 +1271,18 @@
               </item>
               <item>
                <widget class="QDoubleSpinBox" name="shape_size_y_spin_box">
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+                <property name="minimumSize">
+                 <size>
+                  <width>20</width>
+                  <height>0</height>
+                 </size>
+                </property>
                 <property name="toolTip">
                  <string>z dimension</string>
                 </property>
@@ -1051,7 +1299,14 @@
             <item>
              <layout class="QHBoxLayout" name="horizontalLayout_13">
               <item>
-               <widget class="QComboBox" name="shapes_combo_box"/>
+               <widget class="QComboBox" name="shapes_combo_box">
+                <property name="minimumSize">
+                 <size>
+                  <width>30</width>
+                  <height>0</height>
+                 </size>
+                </property>
+               </widget>
               </item>
               <item>
                <widget class="QToolButton" name="add_object_button">
@@ -1124,8 +1379,11 @@
         <layout class="QVBoxLayout" name="verticalLayout_20">
          <item>
           <widget class="QGroupBox" name="pose_scale_group_box">
+           <property name="toolTip">
+            <string>Change the pose and scale of the selected object</string>
+           </property>
            <property name="title">
-            <string>Change pose and scale of selected object</string>
+            <string>Change object pose/scale</string>
            </property>
            <layout class="QGridLayout" name="gridLayout">
             <property name="leftMargin">
@@ -1159,6 +1417,12 @@
             </item>
             <item row="1" column="1">
              <widget class="QDoubleSpinBox" name="object_x">
+              <property name="minimumSize">
+               <size>
+                <width>20</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="minimum">
                <double>-999.990000000000009</double>
               </property>
@@ -1172,6 +1436,12 @@
             </item>
             <item row="2" column="3">
              <widget class="QDoubleSpinBox" name="object_rz">
+              <property name="minimumSize">
+               <size>
+                <width>20</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="minimum">
                <double>-3.140000000000000</double>
               </property>
@@ -1185,6 +1455,12 @@
             </item>
             <item row="2" column="1">
              <widget class="QDoubleSpinBox" name="object_rx">
+              <property name="minimumSize">
+               <size>
+                <width>20</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="minimum">
                <double>-3.140000000000000</double>
               </property>
@@ -1198,6 +1474,12 @@
             </item>
             <item row="2" column="2">
              <widget class="QDoubleSpinBox" name="object_ry">
+              <property name="minimumSize">
+               <size>
+                <width>20</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="minimum">
                <double>-3.140000000000000</double>
               </property>
@@ -1211,6 +1493,12 @@
             </item>
             <item row="1" column="2">
              <widget class="QDoubleSpinBox" name="object_y">
+              <property name="minimumSize">
+               <size>
+                <width>20</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="minimum">
                <double>-999.990000000000009</double>
               </property>
@@ -1242,7 +1530,7 @@
                <widget class="QSlider" name="scene_scale">
                 <property name="minimumSize">
                  <size>
-                  <width>160</width>
+                  <width>40</width>
                   <height>0</height>
                  </size>
                 </property>
@@ -1280,6 +1568,12 @@
             </item>
             <item row="1" column="3">
              <widget class="QDoubleSpinBox" name="object_z">
+              <property name="minimumSize">
+               <size>
+                <width>20</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="minimum">
                <double>-999.990000000000009</double>
               </property>
@@ -1296,8 +1590,11 @@
          </item>
          <item>
           <widget class="QGroupBox" name="groupBox_5">
+           <property name="toolTip">
+            <string>Describes the selected object's pose and geometry.</string>
+           </property>
            <property name="title">
-            <string>Status of selected object</string>
+            <string>Object status</string>
            </property>
            <layout class="QVBoxLayout" name="verticalLayout_12">
             <property name="leftMargin">
@@ -1329,6 +1626,9 @@
               <property name="textFormat">
                <enum>Qt::AutoText</enum>
               </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
               <property name="WordWrap" stdset="0">
                <bool>true</bool>
               </property>
@@ -1371,11 +1671,17 @@
             <item>
              <widget class="QPushButton" name="publish_current_scene_button">
               <property name="sizePolicy">
-               <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+               <sizepolicy hsizetype="Maximum" vsizetype="Fixed">
                 <horstretch>0</horstretch>
                 <verstretch>0</verstretch>
                </sizepolicy>
               </property>
+              <property name="minimumSize">
+               <size>
+                <width>40</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="text">
                <string>&amp;Publish</string>
               </property>
@@ -1396,6 +1702,18 @@
             </item>
             <item>
              <widget class="QPushButton" name="export_scene_geometry_text_button">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Maximum" vsizetype="Fixed">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="minimumSize">
+               <size>
+                <width>40</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="toolTip">
                <string>Export scene geometry to a .scene file (only geometry information is preserved)</string>
               </property>
@@ -1406,6 +1724,18 @@
             </item>
             <item>
              <widget class="QPushButton" name="import_scene_geometry_text_button">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Maximum" vsizetype="Fixed">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="minimumSize">
+               <size>
+                <width>40</width>
+                <height>0</height>
+               </size>
+              </property>
               <property name="toolTip">
                <string>Import scene geometry from a .scene file (only geometry information is preserved)</string>
               </property>
@@ -1820,25 +2150,35 @@
   <tabstop>database_port</tabstop>
   <tabstop>reset_db_button</tabstop>
   <tabstop>database_connect_button</tabstop>
+  <tabstop>wcenter_x</tabstop>
+  <tabstop>wcenter_y</tabstop>
+  <tabstop>wcenter_z</tabstop>
+  <tabstop>wsize_x</tabstop>
+  <tabstop>wsize_y</tabstop>
+  <tabstop>wsize_z</tabstop>
   <tabstop>plan_button</tabstop>
   <tabstop>execute_button</tabstop>
   <tabstop>plan_and_execute_button</tabstop>
   <tabstop>stop_button</tabstop>
+  <tabstop>clear_octomap_button</tabstop>
   <tabstop>planning_group_combo_box</tabstop>
   <tabstop>start_state_combo_box</tabstop>
   <tabstop>goal_state_combo_box</tabstop>
-  <tabstop>clear_octomap_button</tabstop>
+  <tabstop>path_constraints_combo_box</tabstop>
   <tabstop>planning_time</tabstop>
   <tabstop>planning_attempts</tabstop>
   <tabstop>velocity_scaling_factor</tabstop>
   <tabstop>acceleration_scaling_factor</tabstop>
-  <tabstop>allow_replanning</tabstop>
-  <tabstop>allow_looking</tabstop>
-  <tabstop>allow_external_program</tabstop>
+  <tabstop>use_cartesian_path</tabstop>
   <tabstop>collision_aware_ik</tabstop>
   <tabstop>approximate_ik</tabstop>
-  <tabstop>path_constraints_combo_box</tabstop>
-  <tabstop>goal_tolerance</tabstop>
+  <tabstop>allow_external_program</tabstop>
+  <tabstop>allow_replanning</tabstop>
+  <tabstop>allow_looking</tabstop>
+  <tabstop>detected_objects_list</tabstop>
+  <tabstop>detect_objects_button</tabstop>
+  <tabstop>support_surfaces_list</tabstop>
+  <tabstop>pick_button</tabstop>
   <tabstop>place_button</tabstop>
   <tabstop>roi_center_x</tabstop>
   <tabstop>roi_center_y</tabstop>
@@ -1847,6 +2187,13 @@
   <tabstop>roi_size_y</tabstop>
   <tabstop>roi_size_z</tabstop>
   <tabstop>collision_objects_list</tabstop>
+  <tabstop>shape_size_x_spin_box</tabstop>
+  <tabstop>shape_size_z_spin_box</tabstop>
+  <tabstop>shape_size_y_spin_box</tabstop>
+  <tabstop>shapes_combo_box</tabstop>
+  <tabstop>add_object_button</tabstop>
+  <tabstop>remove_object_button</tabstop>
+  <tabstop>clear_scene_button</tabstop>
   <tabstop>object_x</tabstop>
   <tabstop>object_y</tabstop>
   <tabstop>object_z</tabstop>
@@ -1874,16 +2221,6 @@
   <tabstop>save_goal_state_button</tabstop>
   <tabstop>status_text</tabstop>
   <tabstop>tabWidget</tabstop>
-  <tabstop>wcenter_x</tabstop>
-  <tabstop>wcenter_y</tabstop>
-  <tabstop>wcenter_z</tabstop>
-  <tabstop>wsize_x</tabstop>
-  <tabstop>wsize_y</tabstop>
-  <tabstop>wsize_z</tabstop>
-  <tabstop>pick_button</tabstop>
-  <tabstop>support_surfaces_list</tabstop>
-  <tabstop>detected_objects_list</tabstop>
-  <tabstop>detect_objects_button</tabstop>
  </tabstops>
  <resources/>
  <connections/>
diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml
index ffae042382..389d71eac2 100644
--- a/moveit_ros/visualization/package.xml
+++ b/moveit_ros/visualization/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_visualization</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Components of MoveIt! that offer visualization</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
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 57b474eea8..142e9c3271 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
@@ -131,6 +131,7 @@ private Q_SLOTS:
   void changedSceneDisplayTime();
   void changedOctreeRenderMode();
   void changedOctreeColorMode();
+  void setSceneName(const QString& name);
 
 protected Q_SLOTS:
   virtual void changedAttachedBodyColor();
@@ -178,7 +179,6 @@ protected Q_SLOTS:
   virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type);
 
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
-  bool model_is_loading_;
   boost::mutex robot_model_loading_lock_;
 
   moveit::tools::BackgroundProcessing background_process_;
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 f644c424bb..4718a5253a 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
@@ -64,7 +64,7 @@ namespace moveit_rviz_plugin
 // Base class contructor
 // ******************************************************************************************
 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)
+  : Display(), 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 "
@@ -203,13 +203,13 @@ void PlanningSceneDisplay::onInitialize()
 
 void PlanningSceneDisplay::reset()
 {
-  planning_scene_render_.reset();
   if (planning_scene_robot_)
     planning_scene_robot_->clear();
-
-  addBackgroundJob(boost::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel");
   Display::reset();
 
+  if (isEnabled())
+    addBackgroundJob(boost::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel");
+
   if (planning_scene_robot_)
   {
     planning_scene_robot_->setVisible(true);
@@ -523,7 +523,6 @@ void PlanningSceneDisplay::loadRobotModel()
 {
   // wait for other robot loadRobotModel() calls to complete;
   boost::mutex::scoped_lock _(robot_model_loading_lock_);
-  model_is_loading_ = true;
 
   // we need to make sure the clearing of the robot model is in the main thread,
   // so that rendering operations do not have data removed from underneath,
@@ -538,15 +537,12 @@ void PlanningSceneDisplay::loadRobotModel()
     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();
   }
   else
   {
-    setStatus(rviz::StatusProperty::Error, "PlanningScene", "No Planning Scene Loaded");
+    addMainLoopJob([this]() { setStatus(rviz::StatusProperty::Error, "PlanningScene", "No Planning Scene Loaded"); });
   }
-
-  model_is_loading_ = false;
 }
 
 // This should always run in the main GUI thread!
@@ -568,6 +564,8 @@ void PlanningSceneDisplay::onRobotModelLoaded()
   bool old_state = scene_name_property_->blockSignals(true);
   scene_name_property_->setStdString(ps->getName());
   scene_name_property_->blockSignals(old_state);
+
+  setStatus(rviz::StatusProperty::Ok, "PlanningScene", "Planning Scene Loaded Successfully");
 }
 
 void PlanningSceneDisplay::onNewPlanningSceneState()
@@ -583,14 +581,17 @@ void PlanningSceneDisplay::sceneMonitorReceivedUpdate(
 void PlanningSceneDisplay::onSceneMonitorReceivedUpdate(
     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();
@@ -644,9 +645,9 @@ void PlanningSceneDisplay::update(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_ &&
-       robot_state_needs_render_) ||
-      planning_scene_needs_render_)
+  if (planning_scene_render_ &&
+      ((current_scene_time_ > scene_display_time_property_->getFloat() && robot_state_needs_render_) ||
+       planning_scene_needs_render_))
   {
     renderPlanningScene();
     calculateOffsetPosition();
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 beb49e28b8..4efb92a360 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
@@ -366,20 +366,27 @@ void RobotStateDisplay::loadRobotModel()
 
   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, "RobotModel", "Loaded successfully");
-
-    changedEnableVisualVisible();
-    changedEnableCollisionVisible();
+    try
+    {
+      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, "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, "RobotModel", "Loading failed");
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 1923d8d54f..73296be1e7 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
@@ -55,9 +55,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
 {
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 52ae8d685c..045b4b51fe 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
@@ -57,8 +57,8 @@ class Shape;
 
 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
 {
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 2b3208b942..bab2a74af4 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
@@ -44,8 +44,8 @@
 
 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 */
 class RobotStateVisualization
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..ec41081283 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
@@ -54,7 +54,7 @@ class TrajectoryPanel : public rviz::Panel
   Q_OBJECT
 
 public:
-  TrajectoryPanel(QWidget* parent = 0);
+  TrajectoryPanel(QWidget* parent = nullptr);
 
   ~TrajectoryPanel() override;
 
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 412c64251b..62b83fd2da 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
@@ -69,7 +69,7 @@ class MovableText;
 
 namespace moveit_rviz_plugin
 {
-MOVEIT_CLASS_FORWARD(TrajectoryVisualization);
+MOVEIT_CLASS_FORWARD(TrajectoryVisualization);  // Defines TrajectoryVisualizationPtr, ConstPtr, WeakPtr... etc
 
 class TrajectoryVisualization : public QObject
 {
diff --git a/moveit_ros/visualization/setup.py b/moveit_ros/visualization/setup.py
index 8b428f692c..65f309f202 100644
--- a/moveit_ros/visualization/setup.py
+++ b/moveit_ros/visualization/setup.py
@@ -2,6 +2,6 @@
 from catkin_pkg.python_setup import generate_distutils_setup
 
 d = generate_distutils_setup()
-d['packages'] = ['moveit_ros_visualization']
-d['package_dir'] = {'': 'src'}
+d["packages"] = ["moveit_ros_visualization"]
+d["package_dir"] = {"": "src"}
 setup(**d)
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 5d503b8f4b..4476a22dee 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)
 #
@@ -55,17 +54,19 @@
 from geometry_msgs.msg import PoseStamped
 from visualization_msgs.msg import InteractiveMarkerInit
 
+
 def signedSquare(val):
-  if val > 0:
-    sign = 1
-  else:
-    sign = -1
-  return val * val * sign
+    if val > 0:
+        sign = 1
+    else:
+        sign = -1
+    return val * val * sign
 
 
 # classes to use joystick of xbox, ps3(wired) and ps3(wireless).
 
-class JoyStatus():
+
+class JoyStatus:
     def __init__(self):
         self.center = False
         self.select = False
@@ -89,6 +90,7 @@ def __init__(self):
         self.right_analog_x = 0.0
         self.right_analog_y = 0.0
 
+
 class XBoxStatus(JoyStatus):
     def __init__(self, msg):
         JoyStatus.__init__(self)
@@ -166,6 +168,7 @@ def __init__(self, msg):
         self.right_analog_y = msg.axes[4]
         self.orig_msg = msg
 
+
 class PS3Status(JoyStatus):
     def __init__(self, msg):
         JoyStatus.__init__(self)
@@ -244,6 +247,7 @@ def __init__(self, msg):
         self.right_analog_y = msg.axes[3]
         self.orig_msg = msg
 
+
 class PS3WiredStatus(JoyStatus):
     def __init__(self, msg):
         JoyStatus.__init__(self)
@@ -322,10 +326,11 @@ 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
+        # creating from sensor_msg/Joy
         if msg.buttons[12] == 1:
             self.center = True
         else:
@@ -400,10 +405,11 @@ def __init__(self, msg):
         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
+        # creating from sensor_msg/Joy
         if msg.buttons[10] == 1:
             self.center = True
         else:
@@ -479,31 +485,37 @@ def __init__(self, msg):
         self.orig_msg = msg
 
 
-class StatusHistory():
-  def __init__(self, max_length=10):
-    self.max_length = max_length
-    self.buffer = []
-  def add(self, status):
-    self.buffer.append(status)
-    if len(self.buffer) > self.max_length:
-      self.buffer = self.buffer[1:self.max_length+1]
-  def all(self, proc):
-    for status in self.buffer:
-      if not proc(status):
-        return False
-    return True
-  def latest(self):
-    if len(self.buffer) > 0:
-      return self.buffer[-1]
-    else:
-      return None
-  def length(self):
-    return len(self.buffer)
-  def new(self, status, attr):
-    if len(self.buffer) == 0:
-      return getattr(status, attr)
-    else:
-      return getattr(status, attr) and not getattr(self.latest(), attr)
+class StatusHistory:
+    def __init__(self, max_length=10):
+        self.max_length = max_length
+        self.buffer = []
+
+    def add(self, status):
+        self.buffer.append(status)
+        if len(self.buffer) > self.max_length:
+            self.buffer = self.buffer[1 : self.max_length + 1]
+
+    def all(self, proc):
+        for status in self.buffer:
+            if not proc(status):
+                return False
+        return True
+
+    def latest(self):
+        if len(self.buffer) > 0:
+            return self.buffer[-1]
+        else:
+            return None
+
+    def length(self):
+        return len(self.buffer)
+
+    def new(self, status, attr):
+        if len(self.buffer) == 0:
+            return getattr(status, attr)
+        else:
+            return getattr(status, attr) and not getattr(self.latest(), attr)
+
 
 class MoveitJoy:
     def parseSRDF(self):
@@ -511,16 +523,21 @@ def parseSRDF(self):
         planning_groups = {}
         for g in ri.get_group_names():
             self.planning_groups_tips[g] = ri.get_group_joint_tips(g)
-            planning_groups[g] = ["/rviz/moveit/move_marker/goal_" + l
-                                  for l in self.planning_groups_tips[g]]
+            planning_groups[g] = [
+                "/rviz/moveit/move_marker/goal_" + l
+                for l in self.planning_groups_tips[g]
+            ]
         for name in planning_groups.keys():
             if len(planning_groups[name]) == 0:
                 del planning_groups[name]
             else:
                 print(name, planning_groups[name])
         self.planning_groups = planning_groups
-        self.planning_groups_keys = planning_groups.keys()   #we'd like to store the 'order'
+        self.planning_groups_keys = (
+            planning_groups.keys()
+        )  # we'd like to store the 'order'
         self.frame_id = ri.get_planning_frame()
+
     def __init__(self):
         self.initial_poses = {}
         self.planning_groups_tips = {}
@@ -536,18 +553,28 @@ def __init__(self):
         self.initialize_poses = False
         self.initialized = False
         self.parseSRDF()
-        self.plan_group_pub = rospy.Publisher('/rviz/moveit/select_planning_group', String, queue_size=5)
+        self.plan_group_pub = rospy.Publisher(
+            "/rviz/moveit/select_planning_group", String, queue_size=5
+        )
         self.updatePlanningGroup(0)
         self.updatePoseTopic(0, False)
         self.joy_pose_pub = rospy.Publisher("/joy_pose", PoseStamped, queue_size=1)
         self.plan_pub = rospy.Publisher("/rviz/moveit/plan", Empty, queue_size=5)
         self.execute_pub = rospy.Publisher("/rviz/moveit/execute", Empty, queue_size=5)
-        self.update_start_state_pub = rospy.Publisher("/rviz/moveit/update_start_state", Empty, queue_size=5)
-        self.update_goal_state_pub = rospy.Publisher("/rviz/moveit/update_goal_state", Empty, queue_size=5)
-        self.interactive_marker_sub = rospy.Subscriber("/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full",
-                                                       InteractiveMarkerInit,
-                                                       self.markerCB, queue_size=1)
+        self.update_start_state_pub = rospy.Publisher(
+            "/rviz/moveit/update_start_state", Empty, queue_size=5
+        )
+        self.update_goal_state_pub = rospy.Publisher(
+            "/rviz/moveit/update_goal_state", Empty, queue_size=5
+        )
+        self.interactive_marker_sub = rospy.Subscriber(
+            "/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full",
+            InteractiveMarkerInit,
+            self.markerCB,
+            queue_size=1,
+        )
         self.sub = rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=1)
+
     def updatePlanningGroup(self, next_index):
         if next_index >= len(self.planning_groups_keys):
             self.current_planning_group_index = 0
@@ -557,13 +584,16 @@ def updatePlanningGroup(self, next_index):
             self.current_planning_group_index = next_index
         next_planning_group = None
         try:
-            next_planning_group = self.planning_groups_keys[self.current_planning_group_index]
+            next_planning_group = self.planning_groups_keys[
+                self.current_planning_group_index
+            ]
         except IndexError:
-            msg = 'Check if you started movegroups. Exiting.'
+            msg = "Check if you started movegroups. Exiting."
             rospy.logfatal(msg)
             raise rospy.ROSInitException(msg)
         rospy.loginfo("Changed planning group to " + next_planning_group)
         self.plan_group_pub.publish(next_planning_group)
+
     def updatePoseTopic(self, next_index, wait=True):
         planning_group = self.planning_groups_keys[self.current_planning_group_index]
         topics = self.planning_groups[planning_group]
@@ -575,11 +605,15 @@ def updatePoseTopic(self, next_index, wait=True):
             self.current_eef_index = next_index
         next_topic = topics[self.current_eef_index]
 
-        rospy.loginfo("Changed controlled end effector to " + self.planning_groups_tips[planning_group][self.current_eef_index])
+        rospy.loginfo(
+            "Changed controlled end effector to "
+            + self.planning_groups_tips[planning_group][self.current_eef_index]
+        )
         self.pose_pub = rospy.Publisher(next_topic, PoseStamped, queue_size=5)
         if wait:
             self.waitForInitialPose(next_topic)
         self.current_pose_topic = next_topic
+
     def markerCB(self, msg):
         try:
             self.marker_lock.acquire()
@@ -592,14 +626,24 @@ def markerCB(self, msg):
                     if marker.header.frame_id != self.frame_id:
                         ps = PoseStamped(header=marker.header, pose=marker.pose)
                         try:
-                            transformed_pose = self.tf_listener.transformPose(self.frame_id, ps)
+                            transformed_pose = self.tf_listener.transformPose(
+                                self.frame_id, ps
+                            )
                             self.initial_poses[marker.name[3:]] = transformed_pose.pose
-                        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, e):
+                        except (
+                            tf.LookupException,
+                            tf.ConnectivityException,
+                            tf.ExtrapolationException,
+                            e,
+                        ):
                             rospy.logerr("tf error when resolving tf: %s" % e)
                     else:
-                        self.initial_poses[marker.name[3:]] = marker.pose   #tf should be resolved
+                        self.initial_poses[
+                            marker.name[3:]
+                        ] = marker.pose  # tf should be resolved
         finally:
             self.marker_lock.release()
+
     def waitForInitialPose(self, next_topic, timeout=None):
         counter = 0
         while not rospy.is_shutdown():
@@ -616,11 +660,13 @@ def waitForInitialPose(self, next_topic, timeout=None):
                     return True
                 else:
                     rospy.logdebug(self.initial_poses.keys())
-                    rospy.loginfo("Waiting for pose topic of '%s' to be initialized",
-                                  topic_suffix)
+                    rospy.loginfo(
+                        "Waiting for pose topic of '%s' to be initialized", topic_suffix
+                    )
                     rospy.sleep(1)
             finally:
                 self.marker_lock.release()
+
     def joyCB(self, msg):
         if len(msg.axes) == 27 and len(msg.buttons) == 19:
             status = PS3WiredStatus(msg)
@@ -636,12 +682,16 @@ def joyCB(self, msg):
             raise Exception("Unknown joystick")
         self.run(status)
         self.history.add(status)
+
     def computePoseFromJoy(self, pre_pose, status):
         new_pose = PoseStamped()
         new_pose.header.frame_id = self.frame_id
         new_pose.header.stamp = rospy.Time(0.0)
         # move in local
-        dist = status.left_analog_y * status.left_analog_y + status.left_analog_x * status.left_analog_x
+        dist = (
+            status.left_analog_y * status.left_analog_y
+            + status.left_analog_x * status.left_analog_x
+        )
         scale = 200.0
         x_diff = signedSquare(status.left_analog_y) / scale
         y_diff = signedSquare(status.left_analog_x) / scale
@@ -656,15 +706,16 @@ def computePoseFromJoy(self, pre_pose, status):
             z_scale = 4.0
         else:
             z_scale = 2.0
-        local_move = numpy.array((x_diff, y_diff,
-                                  z_diff * z_scale,
-                                  1.0))
-        q = numpy.array((pre_pose.pose.orientation.x,
-                         pre_pose.pose.orientation.y,
-                         pre_pose.pose.orientation.z,
-                         pre_pose.pose.orientation.w))
-        xyz_move = numpy.dot(tf.transformations.quaternion_matrix(q),
-                         local_move)
+        local_move = numpy.array((x_diff, y_diff, z_diff * z_scale, 1.0))
+        q = numpy.array(
+            (
+                pre_pose.pose.orientation.x,
+                pre_pose.pose.orientation.y,
+                pre_pose.pose.orientation.z,
+                pre_pose.pose.orientation.w,
+            )
+        )
+        xyz_move = numpy.dot(tf.transformations.quaternion_matrix(q), local_move)
         new_pose.pose.position.x = pre_pose.pose.position.x + xyz_move[0]
         new_pose.pose.position.y = pre_pose.pose.position.y + xyz_move[1]
         new_pose.pose.position.z = pre_pose.pose.position.z + xyz_move[2]
@@ -709,17 +760,26 @@ def computePoseFromJoy(self, pre_pose, status):
         new_pose.pose.orientation.z = new_q[2]
         new_pose.pose.orientation.w = new_q[3]
         return new_pose
+
     def run(self, status):
         if not self.initialized:
             # when not initialized, we will force to change planning_group
             while True:
                 self.updatePlanningGroup(self.current_planning_group_index)
-                planning_group = self.planning_groups_keys[self.current_planning_group_index]
+                planning_group = self.planning_groups_keys[
+                    self.current_planning_group_index
+                ]
                 topics = self.planning_groups[planning_group]
                 next_topic = topics[self.current_eef_index]
                 if not self.waitForInitialPose(next_topic, timeout=3):
-                    rospy.logwarn("Unable to initialize planning group " + planning_group + ". Trying different group.")
-                    rospy.logwarn("Is 'Allow External Comm.' enabled in Rviz? Is the 'Query Goal State' robot enabled?")
+                    rospy.logwarn(
+                        "Unable to initialize planning group "
+                        + planning_group
+                        + ". Trying different group."
+                    )
+                    rospy.logwarn(
+                        "Is 'Allow External Comm.' enabled in Rviz? Is the 'Query Goal State' robot enabled?"
+                    )
                 else:
                     rospy.loginfo("Initialized planning group")
                     self.initialized = True
@@ -728,15 +788,15 @@ def run(self, status):
                 # Try to initialize with different planning group
                 self.current_planning_group_index += 1
                 if self.current_planning_group_index >= len(self.planning_groups_keys):
-                    self.current_planning_group_index = 0 # reset loop
-        if self.history.new(status, "select"):   #increment planning group
+                    self.current_planning_group_index = 0  # reset loop
+        if self.history.new(status, "select"):  # increment planning group
             self.updatePlanningGroup(self.current_planning_group_index + 1)
-            self.current_eef_index = 0    # force to reset
+            self.current_eef_index = 0  # force to reset
             self.updatePoseTopic(self.current_eef_index)
             return
-        elif self.history.new(status, "start"):   #decrement planning group
+        elif self.history.new(status, "start"):  # decrement planning group
             self.updatePlanningGroup(self.current_planning_group_index - 1)
-            self.current_eef_index = 0    # force to reset
+            self.current_eef_index = 0  # force to reset
             self.updatePoseTopic(self.current_eef_index)
             return
         elif self.history.new(status, "triangle"):
@@ -745,11 +805,11 @@ def run(self, status):
         elif self.history.new(status, "cross"):
             self.updatePoseTopic(self.current_eef_index - 1)
             return
-        elif self.history.new(status, "square"):   #plan
+        elif self.history.new(status, "square"):  # plan
             rospy.loginfo("Plan")
             self.plan_pub.publish(Empty())
             return
-        elif self.history.new(status, "circle"):   #execute
+        elif self.history.new(status, "circle"):  # execute
             rospy.loginfo("Execute")
             self.execute_pub.publish(Empty())
             return
diff --git a/moveit_ros/visualization/test/test_moveit_joy.py b/moveit_ros/visualization/test/test_moveit_joy.py
index b3974e8e52..9f36914a73 100755
--- a/moveit_ros/visualization/test/test_moveit_joy.py
+++ b/moveit_ros/visualization/test/test_moveit_joy.py
@@ -42,8 +42,9 @@
 import rostest
 import os
 
-_PKGNAME = 'moveit_ros_visualization'
-_NODENAME = 'test_moveit_joy'
+_PKGNAME = "moveit_ros_visualization"
+_NODENAME = "test_moveit_joy"
+
 
 class TestMoveitJoy(unittest.TestCase):
     def __init__(self, *args, **kwargs):
@@ -54,7 +55,8 @@ def test_constructor(self):
         # However, at least we tested, that MoveitJoy constructor can be called...
         self.assertRaises(RuntimeError, MoveitJoy)
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     rospy.init_node(_NODENAME)
     rostest.rosrun(_PKGNAME, _NODENAME, TestMoveitJoy)
     # Don't get trapped by https://github.com/ros/ros_comm/issues/870
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 db0884d95a..cdea836eed 100644
--- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp
+++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp
@@ -64,22 +64,29 @@ void TrajectoryDisplay::onInitialize()
 
 void TrajectoryDisplay::loadRobotModel()
 {
-  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 robot_model::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_);
 }
 
 void TrajectoryDisplay::reset()
diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst
index c29d1eed7d..43a8f11a57 100644
--- a/moveit_ros/warehouse/CHANGELOG.rst
+++ b/moveit_ros/warehouse/CHANGELOG.rst
@@ -2,6 +2,23 @@
 Changelog for package moveit_ros_warehouse
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: Felix von Drigalski
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Migrate to clang-format-10
diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml
index 6e98a0ff0d..413b58a77f 100644
--- a/moveit_ros/warehouse/package.xml
+++ b/moveit_ros/warehouse/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_warehouse</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>Components of MoveIt! connecting to MongoDB</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_ros/warehouse/warehouse/Makefile b/moveit_ros/warehouse/warehouse/Makefile
index b75b928f20..bbd3fc6049 100644
--- a/moveit_ros/warehouse/warehouse/Makefile
+++ b/moveit_ros/warehouse/warehouse/Makefile
@@ -1 +1 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
+include $(shell rospack find mk)/cmake.mk
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 f7d82bad7b..128b9b3469 100644
--- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h
+++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h
@@ -46,7 +46,7 @@ namespace moveit_warehouse
 typedef warehouse_ros::MessageWithMetadata<moveit_msgs::Constraints>::ConstPtr ConstraintsWithMetadata;
 typedef warehouse_ros::MessageCollection<moveit_msgs::Constraints>::Ptr ConstraintsCollection;
 
-MOVEIT_CLASS_FORWARD(ConstraintsStorage);
+MOVEIT_CLASS_FORWARD(ConstraintsStorage);  // Defines ConstraintsStoragePtr, ConstPtr, WeakPtr... etc
 
 class ConstraintsStorage : public MoveItMessageStorage
 {
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 239f531212..17a0e98dd2 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
@@ -53,7 +53,7 @@ typedef warehouse_ros::MessageCollection<moveit_msgs::PlanningScene>::Ptr Planni
 typedef warehouse_ros::MessageCollection<moveit_msgs::MotionPlanRequest>::Ptr MotionPlanRequestCollection;
 typedef warehouse_ros::MessageCollection<moveit_msgs::RobotTrajectory>::Ptr RobotTrajectoryCollection;
 
-MOVEIT_CLASS_FORWARD(PlanningSceneStorage);
+MOVEIT_CLASS_FORWARD(PlanningSceneStorage);  // Defines PlanningSceneStoragePtr, ConstPtr, WeakPtr... etc
 
 class PlanningSceneStorage : public MoveItMessageStorage
 {
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 1c31e68825..feded144f0 100644
--- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h
+++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h
@@ -46,7 +46,7 @@ namespace moveit_warehouse
 typedef warehouse_ros::MessageWithMetadata<moveit_msgs::RobotState>::ConstPtr RobotStateWithMetadata;
 typedef warehouse_ros::MessageCollection<moveit_msgs::RobotState>::Ptr RobotStateCollection;
 
-MOVEIT_CLASS_FORWARD(RobotStateStorage);
+MOVEIT_CLASS_FORWARD(RobotStateStorage);  // Defines RobotStateStoragePtr, ConstPtr, WeakPtr... etc
 
 class RobotStateStorage : public MoveItMessageStorage
 {
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 ed2c5393f5..689a9677ed 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
@@ -47,7 +47,7 @@ typedef warehouse_ros::MessageWithMetadata<moveit_msgs::TrajectoryConstraints>::
     TrajectoryConstraintsWithMetadata;
 typedef warehouse_ros::MessageCollection<moveit_msgs::TrajectoryConstraints>::Ptr TrajectoryConstraintsCollection;
 
-MOVEIT_CLASS_FORWARD(TrajectoryConstraintsStorage);
+MOVEIT_CLASS_FORWARD(TrajectoryConstraintsStorage);  // Defines TrajectoryConstraintsStoragePtr, ConstPtr, WeakPtr... etc
 
 class TrajectoryConstraintsStorage : public MoveItMessageStorage
 {
@@ -79,10 +79,10 @@ 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_;
 };
diff --git a/moveit_ros/warehouse/warehouse/src/db_path_config.py b/moveit_ros/warehouse/warehouse/src/db_path_config.py
index 7b0cc11e76..15356ea79e 100755
--- a/moveit_ros/warehouse/warehouse/src/db_path_config.py
+++ b/moveit_ros/warehouse/warehouse/src/db_path_config.py
@@ -4,12 +4,14 @@
 # This is used in conjunction with ROS_HOME & roslaunch
 # to set the default path for database storage
 
-import roslib; roslib.load_manifest('moveit_warehouse')
+import roslib
+
+roslib.load_manifest("moveit_warehouse")
 import rospy
 import os
 
-if __name__ == '__main__':
-    rospy.init_node('moveit_warehouse')
+if __name__ == "__main__":
+    rospy.init_node("moveit_warehouse")
     path_base = os.getcwd() + "/moveit_warehouse/"
-    rospy.set_param('~database_path_base', path_base)
-    rospy.set_param('~default_database', path_base + "default")
+    rospy.set_param("~database_path_base", path_base)
+    rospy.set_param("~default_database", path_base + "default")
diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst
index c16e8925d4..26cab1162b 100644
--- a/moveit_runtime/CHANGELOG.rst
+++ b/moveit_runtime/CHANGELOG.rst
@@ -2,6 +2,21 @@
 Changelog for package moveit_runtime
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+
+1.0.8 (2021-05-23)
+------------------
+
+1.0.7 (2020-11-20)
+------------------
+
 1.0.6 (2020-08-19)
 ------------------
 
diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml
index 486a511cf0..dfd95e22fe 100644
--- a/moveit_runtime/package.xml
+++ b/moveit_runtime/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit_runtime</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
   <description>moveit_runtime meta package contains MoveIt! packages that are essential for its runtime (e.g. running MoveIt! on robots).</description>
 
   <author email="gm130s@gmail.com">Isaac I. Y. Saito</author>
diff --git a/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/CHANGELOG.rst
index d3332a8d34..7fb5cac8bf 100644
--- a/moveit_setup_assistant/CHANGELOG.rst
+++ b/moveit_setup_assistant/CHANGELOG.rst
@@ -2,6 +2,48 @@
 Changelog for package moveit_setup_assistant
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.11 (2022-09-13)
+-------------------
+* Pass xacro_args to both, urdf and srdf loading (`#3200 <https://github.com/ros-planning/moveit/issues/3200>`_)
+* Contributors: Robert Haschke
+
+1.0.10 (2022-03-06)
+-------------------
+
+1.0.9 (2022-01-09)
+------------------
+* Use moveit-resources@master (`#2951 <https://github.com/ros-planning/moveit/issues/2951>`_)
+
+  - Simplify launch files to use the test_environment.launch files from moveit_resources@master
+  - Provide compatibility to the Noetic-style configuration of (multiple) planning pipelines
+    Only a single pipeline can be used at a time, specified via the ~default_planning_pipeline parameter.
+  - Rename launch argument execution_type -> fake_execution_type
+* Templates: Fix passing capabilities arguments (`#2729 <https://github.com/ros-planning/moveit/issues/2729>`_)
+* Fix writing an empty sensor config (`#2708 <https://github.com/ros-planning/moveit/issues/2708>`_)
+* Contributors: David V. Lu!!, G.A. vd. Hoorn, Michael Görner, Robert Haschke
+
+1.0.8 (2021-05-23)
+------------------
+* Let users override fake execution type from demo.launch (`#2602 <https://github.com/ros-planning/moveit/issues/2602>`_)
+* Fix segfault in MSA when a group was defined without ``ns`` attribute (`#2564 <https://github.com/ros-planning/moveit/issues/2564>`_)
+* Missing RViz and moveit_simple_controller_manager dependencies in MSA template (`#2455 <https://github.com/ros-planning/moveit/issues/2455>`_)
+* Fix empty sequence in moveit_setup_assistant (`#2406 <https://github.com/ros-planning/moveit/issues/2406>`_)
+* Add pilz_industrial_motion_planner to moveit_planners (`#2507 <https://github.com/ros-planning/moveit/issues/2507>`_)
+* Contributors: David V. Lu!!, Immanuel Martini, Michael Görner, Tyler Weaver
+
+1.0.7 (2020-11-20)
+------------------
+* [feature] MSA: Fix group editing (`#2350 <https://github.com/ros-planning/moveit/issues/2350>`_)
+* [feature] MSA: Allow showing both, visual and collision geometry (`#2352 <https://github.com/ros-planning/moveit/issues/2352>`_)
+* [feature] Start new joint_state_publisher_gui on param use_gui (`#2257 <https://github.com/ros-planning/moveit/issues/2257>`_)
+* [fix] MSA launch files: fix indentation (`#2371 <https://github.com/ros-planning/moveit/issues/2371>`_)
+* [fix] Segfault when editing pose in moveit_setup_assistant (`#2340 <https://github.com/ros-planning/moveit/issues/2340>`_)
+* [fix] robot_description is already loaded in move_group.launch (`#2313 <https://github.com/ros-planning/moveit/issues/2313>`_)
+* [fix] MSA: only write default_planner_config field if any is selected (`#2293 <https://github.com/ros-planning/moveit/issues/2293>`_)
+* [fix] MSA: Fix disappearing robot on change of reference frame (`#2335 <https://github.com/ros-planning/moveit/issues/2335>`_)
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/2315>`_)
+* Contributors: David V. Lu!!, Felix von Drigalski, Michael Görner, Robert Haschke, Tyler Weaver, Yoan Mollard
+
 1.0.6 (2020-08-19)
 ------------------
 * [maint] Adapt repository for splitted moveit_resources layout (`#2199 <https://github.com/ros-planning/moveit/issues/2199>`_)
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 0a2c29caec..a13030efc4 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
@@ -41,7 +41,7 @@
 #include <moveit/macros/class_forward.h>
 namespace planning_scene
 {
-MOVEIT_CLASS_FORWARD(PlanningScene);
+MOVEIT_CLASS_FORWARD(PlanningScene);  // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc
 }
 
 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 9c4654df72..a2af4d5316 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
@@ -44,6 +44,8 @@
 #include <urdf/model.h>                                               // to share throughout app
 #include <srdfdom/srdf_writer.h>                                      // for writing srdf data
 
+#include <utility>
+
 namespace moveit_setup_assistant
 {
 // ******************************************************************************************
@@ -55,8 +57,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;  // NOLINT(readability-identifier-naming)
+static const double DEFAULT_KIN_SOLVER_TIMEOUT_ = 0.005;            // NOLINT(readability-identifier-naming)
 
 // ******************************************************************************************
 // Structs
@@ -144,25 +146,25 @@ 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()
+  const std::string& getName() const
   {
     return name_;
   };
-  std::string getValue()
+  const std::string& getValue() const
   {
     return value_;
   };
-  std::string getComment()
+  const std::string& getComment() const
   {
     return comment_;
   };
@@ -173,10 +175,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
@@ -204,7 +206,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
@@ -304,7 +306,7 @@ class MoveItConfigData
   // ******************************************************************************************
   // Public Functions for outputting configuration and setting files
   // ******************************************************************************************
-  std::vector<OMPLPlannerDescription> getOMPLPlanners();
+  std::vector<OMPLPlannerDescription> getOMPLPlanners() const;
   bool outputSetupAssistantFile(const std::string& file_path);
   bool outputOMPLPlanningYAML(const std::string& file_path);
   bool outputCHOMPPlanningYAML(const std::string& file_path);
@@ -511,7 +513,7 @@ class MoveItConfigData
    * \param jm2 - a pointer to the second joint model to compare
    * \return bool of alphabetical sorting comparison
    */
-  struct joint_model_compare
+  struct joint_model_compare  // NOLINT(readability-identifier-naming)
   {
     bool operator()(const robot_model::JointModel* jm1, const robot_model::JointModel* jm2) const
     {
diff --git a/moveit_setup_assistant/launch/setup_assistant.launch b/moveit_setup_assistant/launch/setup_assistant.launch
index 4d908ba325..d3446da716 100644
--- a/moveit_setup_assistant/launch/setup_assistant.launch
+++ b/moveit_setup_assistant/launch/setup_assistant.launch
@@ -8,7 +8,7 @@
 
   <!-- Run -->
   <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
-	launch-prefix="$(arg launch_prefix)"
+        launch-prefix="$(arg launch_prefix)"
         output="screen"  required="true" />
 
 </launch>
diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml
index 585ed83ba3..06009bdb34 100644
--- a/moveit_setup_assistant/package.xml
+++ b/moveit_setup_assistant/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_setup_assistant</name>
-  <version>1.0.6</version>
+  <version>1.0.11</version>
 
   <description>Generates a configuration package that makes it easy to use MoveIt!</description>
 
diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp
index b5f16d98c4..edc15610be 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 <ros/ros.h>
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
 #include <moveit/rdf_loader/rdf_loader.h>
 
diff --git a/moveit_setup_assistant/src/setup_assistant_main.cpp b/moveit_setup_assistant/src/setup_assistant_main.cpp
index a514696622..0f4485dd2d 100644
--- a/moveit_setup_assistant/src/setup_assistant_main.cpp
+++ b/moveit_setup_assistant/src/setup_assistant_main.cpp
@@ -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.h b/moveit_setup_assistant/src/tools/collision_linear_model.h
index 9744170243..3b8b266c6e 100644
--- a/moveit_setup_assistant/src/tools/collision_linear_model.h
+++ b/moveit_setup_assistant/src/tools/collision_linear_model.h
@@ -52,7 +52,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 +82,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);
diff --git a/moveit_setup_assistant/src/tools/collision_matrix_model.h b/moveit_setup_assistant/src/tools/collision_matrix_model.h
index dccbc2eed1..9783abf3e2 100644
--- a/moveit_setup_assistant/src/tools/collision_matrix_model.h
+++ b/moveit_setup_assistant/src/tools/collision_matrix_model.h
@@ -50,7 +50,7 @@ class CollisionMatrixModel : public QAbstractTableModel
   Q_OBJECT
 public:
   CollisionMatrixModel(moveit_setup_assistant::LinkPairMap& pairs, const std::vector<std::string>& 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 +59,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);
 
diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
index 63ad7d59fc..d6491c9dc5 100644
--- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp
+++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
@@ -241,8 +241,9 @@ bool MoveItConfigData::outputOMPLPlanningYAML(const std::string& file_path)
     emitter << YAML::Key << group_it->name_;
     emitter << YAML::Value << YAML::BeginMap;
     // Output associated planners
-    emitter << YAML::Key << "default_planner_config" << YAML::Value
-            << group_meta_data_[group_it->name_].default_planner_;
+    if (!group_meta_data_[group_it->name_].default_planner_.empty())
+      emitter << YAML::Key << "default_planner_config" << YAML::Value
+              << group_meta_data_[group_it->name_].default_planner_;
     emitter << YAML::Key << "planner_configs";
     emitter << YAML::Value << YAML::BeginSeq;
     for (std::size_t i = 0; i < pconfigs.size(); ++i)
@@ -272,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
@@ -517,6 +518,8 @@ bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path)
     const std::vector<const robot_model::JointModel*>& joint_models = joint_model_group->getActiveJointModels();
     emitter << YAML::Key << "name";
     emitter << YAML::Value << "fake_" + group_it->name_ + "_controller";
+    emitter << YAML::Key << "type";
+    emitter << YAML::Value << "$(arg fake_execution_type)";
     emitter << YAML::Key << "joints";
     emitter << YAML::Value << YAML::BeginSeq;
 
@@ -571,6 +574,10 @@ bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path)
     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;
@@ -588,7 +595,7 @@ bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path)
   return true;  // file created successfully
 }
 
-std::vector<OMPLPlannerDescription> MoveItConfigData::getOMPLPlanners()
+std::vector<OMPLPlannerDescription> MoveItConfigData::getOMPLPlanners() const
 {
   std::vector<OMPLPlannerDescription> planner_des;
 
@@ -1065,21 +1072,18 @@ bool MoveItConfigData::output3DSensorPluginYAML(const std::string& file_path)
   emitter << YAML::Key << "sensors";
   emitter << YAML::Value << YAML::BeginSeq;
 
-  // Can we have more than one plugin config?
-  emitter << YAML::BeginMap;
-
-  // Make sure sensors_plugin_config_parameter_list_ is not empty
-  if (!sensors_plugin_config_parameter_list_.empty())
+  for (auto& sensors_plugin_config : sensors_plugin_config_parameter_list_)
   {
-    for (auto& parameter : sensors_plugin_config_parameter_list_[0])
+    emitter << YAML::BeginMap;
+
+    for (auto& parameter : sensors_plugin_config)
     {
       emitter << YAML::Key << parameter.first;
       emitter << YAML::Value << parameter.second.getValue();
     }
+    emitter << YAML::EndMap;
   }
 
-  emitter << YAML::EndMap;
-
   emitter << YAML::EndSeq;
 
   emitter << YAML::EndMap;
@@ -1388,7 +1392,8 @@ bool MoveItConfigData::inputPlanningContextLaunch(const std::string& file_path)
   // 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"))
+  while (kinematics_group && kinematics_group->Attribute("ns") &&
+         kinematics_group->Attribute("ns") != std::string("$(arg robot_description)_kinematics"))
   {
     kinematics_group = kinematics_group->NextSiblingElement("group");
   }
diff --git a/moveit_setup_assistant/src/tools/rotated_header_view.h b/moveit_setup_assistant/src/tools/rotated_header_view.h
index 6c518b7cf5..9081a64c5a 100644
--- a/moveit_setup_assistant/src/tools/rotated_header_view.h
+++ b/moveit_setup_assistant/src/tools/rotated_header_view.h
@@ -44,7 +44,7 @@ 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;
diff --git a/moveit_setup_assistant/src/widgets/author_information_widget.cpp b/moveit_setup_assistant/src/widgets/author_information_widget.cpp
index fc9dc50727..1ca1d6cbbb 100644
--- a/moveit_setup_assistant/src/widgets/author_information_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/author_information_widget.cpp
@@ -39,10 +39,10 @@
 #include <QPushButton>
 #include <QMessageBox>
 #include <QApplication>
-#include <QSplitter>
-// ROS
+#include <QLabel>
+#include <QLineEdit>
 #include "author_information_widget.h"
-#include <ros/ros.h>
+#include "header_widget.h"
 
 namespace moveit_setup_assistant
 {
diff --git a/moveit_setup_assistant/src/widgets/author_information_widget.h b/moveit_setup_assistant/src/widgets/author_information_widget.h
index 15ed0036ec..68f32e122e 100644
--- a/moveit_setup_assistant/src/widgets/author_information_widget.h
+++ b/moveit_setup_assistant/src/widgets/author_information_widget.h
@@ -37,15 +37,12 @@
 #ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_AUTHOR_INFORMATION_WIDGET_
 #define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_AUTHOR_INFORMATION_WIDGET_
 
-#include <QWidget>
-#include <QString>
-#include <QLineEdit>
+class QLineEdit;
 
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
 #endif
 
-#include "header_widget.h"
 #include "setup_screen_widget.h"  // a base class for screens in the setup assistant
 
 namespace moveit_setup_assistant
diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
index 689aa91601..10eb45dd6b 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 <QVBoxLayout>
-#include <QPushButton>
-#include <QMessageBox>
 #include <QApplication>
-#include <QSplitter>
+#include <QLabel>
+#include <QList>
+#include <QListWidget>
+#include <QMessageBox>
+#include <QProgressBar>
+#include <QPushButton>
+#include <QPushButton>
 #include <QRegExp>
-// ROS
+#include <QSplitter>
+#include <QVBoxLayout>
+
 #include "configuration_files_widget.h"
-#include <ros/ros.h>
+#include "header_widget.h"
+
 // Boost
 #include <boost/algorithm/string.hpp>       // for string find and replace in templates
 #include <boost/filesystem/path.hpp>        // for creating folders/files
@@ -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_);
@@ -375,6 +392,18 @@ 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);
+
   // chomp_planning_pipeline.launch
   // --------------------------------------------------------------------------------------
   file.file_name_ = "chomp_planning_pipeline.launch.xml";
diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.h b/moveit_setup_assistant/src/widgets/configuration_files_widget.h
index f0787af004..aa7aa6c1c7 100644
--- a/moveit_setup_assistant/src/widgets/configuration_files_widget.h
+++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.h
@@ -37,23 +37,23 @@
 #ifndef MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_CONFIGURATION_FILES_WIDGET_
 #define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_CONFIGURATION_FILES_WIDGET_
 
-#include <QWidget>
-#include <QPushButton>
-#include <QString>
-#include <QProgressBar>
-#include <QLabel>
-#include <QListWidget>
 #include <QList>
+class QLabel;
+class QListWidget;
+class QListWidgetItem;
+class QProgressBar;
+class QPushButton;
 
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
 #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
 {
diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp b/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp
index f86d0f7d5a..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 <QVBoxLayout>
+#include <QComboBox>
+#include <QFormLayout>
+#include <QGroupBox>
 #include <QHBoxLayout>
+#include <QLabel>
+#include <QLineEdit>
 #include <QMessageBox>
-#include <QFormLayout>
+#include <QPushButton>
 #include <QString>
-#include <QGroupBox>
+#include <QVBoxLayout>
 #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);
diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.h b/moveit_setup_assistant/src/widgets/controller_edit_widget.h
index 76ef935831..de1545b563 100644
--- a/moveit_setup_assistant/src/widgets/controller_edit_widget.h
+++ b/moveit_setup_assistant/src/widgets/controller_edit_widget.h
@@ -37,10 +37,10 @@
 #define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_CONTROLLER_EDIT_WIDGET_H
 
 #include <QWidget>
-#include <QLabel>
-#include <QLineEdit>
-#include <QComboBox>
-#include <QPushButton>
+class QComboBox;
+class QLabel;
+class QLineEdit;
+class QPushButton;
 
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp
index 480ab150f2..4454db176e 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 <QHBoxLayout>
-#include <QMessageBox>
-#include <QProgressDialog>
-#include <QString>
-#include <QFont>
+#include <QAction>
 #include <QApplication>
 #include <QButtonGroup>
-#include <QRadioButton>
+#include <QCheckBox>
+#include <QFont>
+#include <QGroupBox>
+#include <QHBoxLayout>
 #include <QKeyEvent>
+#include <QLabel>
+#include <QLineEdit>
 #include <QMenu>
+#include <QMessageBox>
+#include <QProgressBar>
+#include <QPushButton>
+#include <QRadioButton>
+#include <QSlider>
+#include <QSpinBox>
+#include <QString>
+#include <QTableView>
+#include <QVBoxLayout>
 
 #include "default_collisions_widget.h"
 #include "header_widget.h"
diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.h b/moveit_setup_assistant/src/widgets/default_collisions_widget.h
index 37c5cad980..ae14fdfe97 100644
--- a/moveit_setup_assistant/src/widgets/default_collisions_widget.h
+++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.h
@@ -34,22 +34,26 @@
 
 /* Author: Dave Coleman */
 
-#ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_DEFAULT_COLLISIONS_WIDGET__
-#define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_DEFAULT_COLLISIONS_WIDGET__
-
-#include <QLabel>
-#include <QVBoxLayout>
-#include <QTableView>
-#include <QSlider>
-#include <QPushButton>
-#include <QGroupBox>
-#include <QProgressBar>
-#include <QCheckBox>
-#include <QRadioButton>
-#include <QSpinBox>
+#pragma once
+
 #include <QThread>
-#include <QLineEdit>
-#include <QAction>
+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 <boost/thread/thread.hpp>
@@ -237,7 +241,7 @@ class MonitorThread : public QThread
   Q_OBJECT
 
 public:
-  MonitorThread(const boost::function<void(unsigned int*)>& f, QProgressBar* progress_bar = NULL);
+  MonitorThread(const boost::function<void(unsigned int*)>& f, QProgressBar* progress_bar = nullptr);
   void run() override;
   void cancel()
   {
@@ -257,5 +261,3 @@ class MonitorThread : public QThread
   bool canceled_;
 };
 }  // namespace moveit_setup_assistant
-
-#endif
diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.cpp b/moveit_setup_assistant/src/widgets/double_list_widget.cpp
index 59e550ba29..edcf029058 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 <QVBoxLayout>
-#include <QHBoxLayout>
-#include <QGroupBox>
-#include <QPushButton>
 #include <QFormLayout>
+#include <QGroupBox>
+#include <QHBoxLayout>
+#include <QHeaderView>
+#include <QLabel>
 #include <QLineEdit>
 #include <QMessageBox>
+#include <QPushButton>
 #include <QString>
-#include <QHeaderView>
+#include <QTableWidget>
+#include <QVBoxLayout>
 #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);
diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.h b/moveit_setup_assistant/src/widgets/double_list_widget.h
index c4e511bd14..afa80c2379 100644
--- a/moveit_setup_assistant/src/widgets/double_list_widget.h
+++ b/moveit_setup_assistant/src/widgets/double_list_widget.h
@@ -38,8 +38,10 @@
 #define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_DOUBLE_LIST_WIDGET_
 
 #include <QWidget>
-#include <QLabel>
-#include <QTableWidget>
+class QLabel;
+class QTableWidget;
+class QTableWidgetItem;
+class QItemSelection;
 
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
@@ -69,7 +71,7 @@ class DoubleListWidget : public QWidget
   /// Set the right box
   void setSelected(const std::vector<std::string>& items);
 
-  void clearContents(void);
+  void clearContents();
 
   /// Convenience function for reusing set table code
   void setTable(const std::vector<std::string>& items, QTableWidget* table);
diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp
index a38c5f3140..eb092e6a1b 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 <QApplication>
+#include <QComboBox>
 #include <QFormLayout>
+#include <QGroupBox>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QLineEdit>
 #include <QMessageBox>
-#include <QApplication>
+#include <QPushButton>
+#include <QScrollArea>
+#include <QStackedWidget>
+#include <QString>
+#include <QTableWidget>
+#include <QVBoxLayout>
+#include <QWidget>
 
 namespace moveit_setup_assistant
 {
@@ -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);
@@ -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);
@@ -574,7 +579,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);
@@ -586,7 +591,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
@@ -654,7 +659,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 dd75249b4e..69be8c18f2 100644
--- a/moveit_setup_assistant/src/widgets/end_effectors_widget.h
+++ b/moveit_setup_assistant/src/widgets/end_effectors_widget.h
@@ -38,24 +38,17 @@
 #define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_END_EFFECTORS_WIDGET_
 
 // Qt
-#include <QWidget>
-#include <QVBoxLayout>
-#include <QHBoxLayout>
-#include <QScrollArea>
-#include <QGroupBox>
-#include <QLabel>
-#include <QPushButton>
-#include <QTableWidget>
-#include <QStackedLayout>
-#include <QString>
-#include <QComboBox>
+class QComboBox;
+class QLineEdit;
+class QPushButton;
+class QStackedWidget;
+class QTableWidget;
 
 // SA
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
 #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 +75,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_;
diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp
index 4a0f75ba7f..a3d84bd178 100644
--- a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/group_edit_widget.cpp
@@ -34,13 +34,18 @@
 
 /* Author: Dave Coleman */
 
-#include <QVBoxLayout>
-#include <QHBoxLayout>
-#include <QMessageBox>
+#include <QComboBox>
 #include <QFileDialog>
 #include <QFormLayout>
-#include <QString>
 #include <QGroupBox>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QLineEdit>
+#include <QMessageBox>
+#include <QPushButton>
+#include <QString>
+#include <QVBoxLayout>
+
 #include "group_edit_widget.h"
 #include <pluginlib/class_loader.hpp>  // for loading all avail kinematic planners
 
@@ -142,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);
@@ -153,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);
@@ -180,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();
@@ -196,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);
@@ -265,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
@@ -320,7 +321,8 @@ void GroupEditWidget::loadKinematicPlannersComboBox()
   std::unique_ptr<pluginlib::ClassLoader<kinematics::KinematicsBase>> loader;
   try
   {
-    loader.reset(new pluginlib::ClassLoader<kinematics::KinematicsBase>("moveit_core", "kinematics::KinematicsBase"));
+    loader = std::make_unique<pluginlib::ClassLoader<kinematics::KinematicsBase>>("moveit_core",
+                                                                                  "kinematics::KinematicsBase");
   }
   catch (pluginlib::PluginlibException& ex)
   {
@@ -338,22 +340,22 @@ void GroupEditWidget::loadKinematicPlannersComboBox()
   if (classes.empty())
   {
     QMessageBox::warning(this, "Missing Kinematic Solvers",
-                         "No MoveIt!-compatible kinematics solvers found. Try "
+                         "No MoveIt-compatible kinematics solvers found. Try "
                          "installing moveit_kinematics (sudo apt-get install "
                          "ros-${ROS_DISTRO}-moveit-kinematics)");
     return;
   }
 
   // Loop through all planners and add to combo box
-  for (std::vector<std::string>::const_iterator plugin_it = classes.begin(); plugin_it != classes.end(); ++plugin_it)
+  for (const std::string& kinematics_plugin_name : classes)
   {
-    kinematics_solver_field_->addItem(plugin_it->c_str());
+    kinematics_solver_field_->addItem(kinematics_plugin_name.c_str());
   }
 
   std::vector<OMPLPlannerDescription> planners = config_data_->getOMPLPlanners();
-  for (std::size_t i = 0; i < planners.size(); ++i)
+  for (OMPLPlannerDescription& planner : planners)
   {
-    std::string planner_name = planners[i].name_;
+    std::string planner_name = planner.name_;
     default_planner_field_->addItem(planner_name.c_str());
   }
 }
diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.h b/moveit_setup_assistant/src/widgets/group_edit_widget.h
index 76b33ccd57..23970afe81 100644
--- a/moveit_setup_assistant/src/widgets/group_edit_widget.h
+++ b/moveit_setup_assistant/src/widgets/group_edit_widget.h
@@ -38,10 +38,10 @@
 #define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_GROUP_EDIT_WIDGET_
 
 #include <QWidget>
-#include <QLabel>
-#include <QLineEdit>
-#include <QComboBox>
-#include <QPushButton>
+class QComboBox;
+class QLabel;
+class QLineEdit;
+class QPushButton;
 
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
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 <QPushButton>
-#include <QFont>
+#include "header_widget.h"
 #include <QFileDialog>
+#include <QFont>
+#include <QLabel>
+#include <QLineEdit>
+#include <QPushButton>
 #include <QVBoxLayout>
-#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 e8ac0ea777..c79157d247 100644
--- a/moveit_setup_assistant/src/widgets/header_widget.h
+++ b/moveit_setup_assistant/src/widgets/header_widget.h
@@ -38,8 +38,9 @@
 #define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_HEADER_WIDGET_
 
 #include <QWidget>
-#include <QLabel>
-#include <QLineEdit>
+#include <QFrame>
+class QLabel;
+class QLineEdit;
 
 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..70550744a1 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 <QGridLayout>
 #include <QLineEdit>
 #include <QMessageBox>
-#include <QString>
+#include <QLabel>
+#include <QTreeWidget>
 #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);
diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h
index ccfb26f987..2e5e7f3b35 100644
--- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h
+++ b/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h
@@ -38,12 +38,13 @@
 #define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_KINEMATIC_CHAIN_WIDGET_
 
 #include <QWidget>
-#include <QLabel>
-#include <QTreeWidget>
+class QLabel;
+class QLineEdit;
+class QTreeWidget;
+class QTreeWidgetItem;
 
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
-#include <ros/ros.h>
 #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 d9cef1fa43..0f2ca8dd5f 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 <QApplication>
+#include <QPainter>
+#include <QScrollBar>
+#include <QStandardItemModel>
 #include <iostream>
 
 namespace moveit_setup_assistant
diff --git a/moveit_setup_assistant/src/widgets/navigation_widget.h b/moveit_setup_assistant/src/widgets/navigation_widget.h
index 241355988b..382ba6223c 100644
--- a/moveit_setup_assistant/src/widgets/navigation_widget.h
+++ b/moveit_setup_assistant/src/widgets/navigation_widget.h
@@ -38,11 +38,8 @@
 #define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_NAVIGATION_WIDGET_
 
 #include <QListView>
-#include <QStandardItemModel>
-#include <QScrollBar>
-#include <QString>
 #include <QStyledItemDelegate>
-#include <QPainter>
+class QStandardItemModel;
 
 namespace moveit_setup_assistant
 {
@@ -57,7 +54,7 @@ class NavigationWidget : public QListView
 {
   Q_OBJECT
 public:
-  explicit NavigationWidget(QWidget* parent = 0);
+  explicit NavigationWidget(QWidget* parent = nullptr);
 
   void setNavs(const QList<QString>& navs);
   void setEnabled(const int& index, bool enabled);
@@ -78,7 +75,7 @@ class NavDelegate : public QStyledItemDelegate
 {
   Q_OBJECT
 public:
-  explicit NavDelegate(QObject* parent = 0);
+  explicit NavDelegate(QObject* parnullptrnt = nullptr);
 
   QSize sizeHint(const QStyleOptionViewItem& option, const QModelIndex& index) const override;
   void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override;
diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp b/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp
index 72fa2a45c7..8f2b2e2d35 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 <QFormLayout>
+#include <QLabel>
 #include <QMessageBox>
+#include <QTableWidget>
 
 namespace moveit_setup_assistant
 {
diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.h b/moveit_setup_assistant/src/widgets/passive_joints_widget.h
index 8e23450eef..e4da1f2878 100644
--- a/moveit_setup_assistant/src/widgets/passive_joints_widget.h
+++ b/moveit_setup_assistant/src/widgets/passive_joints_widget.h
@@ -37,28 +37,16 @@
 #ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PASSIVE_JOINTS_WIDGET_
 #define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PASSIVE_JOINTS_WIDGET_
 
-// Qt
-#include <QWidget>
-#include <QVBoxLayout>
-#include <QHBoxLayout>
-#include <QLabel>
-#include <QPushButton>
-#include <QTableWidget>
-#include <QStackedLayout>
-#include <QString>
-#include <QComboBox>
-
 // SA
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
 #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
diff --git a/moveit_setup_assistant/src/widgets/perception_widget.cpp b/moveit_setup_assistant/src/widgets/perception_widget.cpp
index 7a496cabac..f4a94df94d 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 <QVBoxLayout>
-#include <QFormLayout>
 #include <QApplication>
+#include <QComboBox>
+#include <QFormLayout>
+#include <QGroupBox>
+#include <QLabel>
+#include <QLineEdit>
+#include <QVBoxLayout>
 
 namespace moveit_setup_assistant
 {
@@ -203,7 +208,7 @@ bool PerceptionWidget::focusLost()
   // Save the sensor plugin configuration to sensors_plugin_config data structure
   if (sensor_plugin_field_->currentIndex() == 1)
   {
-    // Point Cloud plugin feilds
+    // Point Cloud plugin fields
     config_data_->addGenericParameterToSensorPluginConfig("sensor_plugin", "occupancy_map_monitor/"
                                                                            "PointCloudOctomapUpdater");
     config_data_->addGenericParameterToSensorPluginConfig("point_cloud_topic",
@@ -224,7 +229,7 @@ bool PerceptionWidget::focusLost()
   }
   else if (sensor_plugin_field_->currentIndex() == 2)
   {
-    // Depth Map plugin feilds
+    // Depth Map plugin fields
     config_data_->addGenericParameterToSensorPluginConfig("sensor_plugin", "occupancy_map_monitor/"
                                                                            "DepthImageOctomapUpdater");
     config_data_->addGenericParameterToSensorPluginConfig("image_topic",
diff --git a/moveit_setup_assistant/src/widgets/perception_widget.h b/moveit_setup_assistant/src/widgets/perception_widget.h
index 336fe8a80e..9111121051 100644
--- a/moveit_setup_assistant/src/widgets/perception_widget.h
+++ b/moveit_setup_assistant/src/widgets/perception_widget.h
@@ -37,18 +37,15 @@
 #define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_PERCEPTION_WIDGET_
 
 // Qt
-#include <QWidget>
-#include <QComboBox>
-#include <QGroupBox>
-#include <QString>
-#include <QLineEdit>
+class QComboBox;
+class QGroupBox;
+class QLineEdit;
 
 // SA
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
 #endif
 
-#include "header_widget.h"
 #include "setup_screen_widget.h"  // a base class for screens in the setup assistant
 
 namespace moveit_setup_assistant
diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp
index 697d916269..fc54f9bafc 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 <QApplication>
-#include <QVBoxLayout>
+#include <QComboBox>
 #include <QHBoxLayout>
-#include <QStackedLayout>
-#include <QPushButton>
+#include <QLabel>
+#include <QLineEdit>
 #include <QMessageBox>
+#include <QPushButton>
+#include <QStackedWidget>
+#include <QTableWidget>
 #include <QTreeWidget>
 #include <QTreeWidgetItem>
+#include <QVBoxLayout>
 
 //// Cycle checking
 #include <boost/graph/adjacency_list.hpp>
@@ -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);
@@ -471,50 +467,29 @@ void PlanningGroupsWidget::editSelected()
   // Get the user custom properties of the currently selected row
   PlanGroupType plan_group = item->data(0, Qt::UserRole).value<PlanGroupType>();
 
-  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)
+  switch (plan_group.type_)
   {
-    // 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)
-  {
-    // 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_);
 }
 
 // ******************************************************************************************
@@ -546,7 +521,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;
 }
 
 // ******************************************************************************************
@@ -578,7 +552,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;
 }
 
 // ******************************************************************************************
@@ -611,7 +584,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;
 }
 
 // ******************************************************************************************
@@ -645,7 +617,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;
 }
 
 // ******************************************************************************************
@@ -676,9 +647,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;
 }
 
 // ******************************************************************************************
@@ -849,7 +817,7 @@ void PlanningGroupsWidget::addGroup()
   loadGroupScreen(nullptr);  // NULL indicates this is a new group, not an existing one
 
   // Switch to screen
-  changeScreen(5);
+  changeScreen(GROUP);
 }
 
 // ******************************************************************************************
@@ -1250,7 +1218,7 @@ bool PlanningGroupsWidget::saveGroupScreen()
   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].kinematics_parameters_file_ = kinematics_parameters_file;
-  config_data_->group_meta_data_[group_name].default_planner_ = default_planner;
+  config_data_->group_meta_data_[group_name].default_planner_ = (default_planner == "None" ? "" : default_planner);
   config_data_->changes |= MoveItConfigData::GROUP_KINEMATICS;
 
   // Reload main screen table
@@ -1286,9 +1254,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);
 }
 
 // ******************************************************************************************
@@ -1302,9 +1271,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);
 }
 
 // ******************************************************************************************
@@ -1318,9 +1288,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);
 }
 
 // ******************************************************************************************
@@ -1334,9 +1305,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);
 }
 
 // ******************************************************************************************
@@ -1344,6 +1316,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_);
@@ -1396,9 +1374,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);
 }
 
@@ -1407,7 +1385,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);
diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.h b/moveit_setup_assistant/src/widgets/planning_groups_widget.h
index 5b850dc3db..5404e00c1a 100644
--- a/moveit_setup_assistant/src/widgets/planning_groups_widget.h
+++ b/moveit_setup_assistant/src/widgets/planning_groups_widget.h
@@ -39,7 +39,7 @@
 
 // Qt
 class QPushButton;
-class QStackedLayout;
+class QStackedWidget;
 class QTreeWidget;
 class QTreeWidgetItem;
 
@@ -62,11 +62,11 @@ class GroupEditWidget;
 // Custom Type
 enum GroupType
 {
-  JOINT,
-  LINK,
-  CHAIN,
-  SUBGROUP,
-  GROUP
+  JOINT = 1,
+  LINK = 2,
+  CHAIN = 3,
+  SUBGROUP = 4,
+  GROUP = 5
 };
 
 // ******************************************************************************************
@@ -147,7 +147,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 +173,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_;
diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp
index a17c6365ae..eb1605b948 100644
--- a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp
@@ -36,15 +36,25 @@
 
 // SA
 #include "robot_poses_widget.h"
+#include "header_widget.h"
 #include <moveit_msgs/JointLimits.h>
 // Qt
+#include <QApplication>
+#include <QComboBox>
+#include <QDoubleValidator>
 #include <QFormLayout>
+#include <QLabel>
+#include <QLineEdit>
 #include <QMessageBox>
-#include <QDoubleValidator>
-#include <QApplication>
+#include <QPushButton>
+#include <QScrollArea>
+#include <QSlider>
+#include <QStackedWidget>
+#include <QTableWidget>
 
 #include <moveit/robot_state/conversions.h>
 #include <moveit_msgs/DisplayRobotState.h>
+#include <moveit/planning_scene/planning_scene.h>
 
 namespace moveit_setup_assistant
 {
@@ -75,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);
@@ -154,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);
@@ -258,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);
@@ -291,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;
@@ -321,13 +322,17 @@ void RobotPosesWidget::editDoubleClicked(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);
+  }
 }
 
 // ******************************************************************************************
@@ -449,7 +454,7 @@ void RobotPosesWidget::edit(int row)
   publishJoints();
 
   // 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);
@@ -481,7 +486,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
@@ -708,7 +713,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);
@@ -720,7 +725,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);
@@ -777,7 +782,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();
diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.h b/moveit_setup_assistant/src/widgets/robot_poses_widget.h
index ffdbb73217..a24e826a23 100644
--- a/moveit_setup_assistant/src/widgets/robot_poses_widget.h
+++ b/moveit_setup_assistant/src/widgets/robot_poses_widget.h
@@ -38,26 +38,21 @@
 #define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROBOT_POSES_WIDGET_
 
 // Qt
-#include <QWidget>
-#include <QVBoxLayout>
-#include <QHBoxLayout>
-#include <QScrollArea>
-#include <QGroupBox>
-#include <QLabel>
-#include <QPushButton>
-#include <QTableWidget>
-#include <QStackedLayout>
-#include <QString>
-#include <QComboBox>
+class QComboBox;
+class QLabel;
+class QLineEdit;
+class QPushButton;
+class QScrollArea;
+class QSlider;
+class QStackedWidget;
+class QTableWidget;
+class QVBoxLayout;
 
 // SA
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
-#include <moveit/planning_scene/planning_scene.h>  // for collision stuff
-#include <ros/ros.h>
 #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 +79,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_;
diff --git a/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp b/moveit_setup_assistant/src/widgets/ros_controllers_widget.cpp
index cc70aab17a..ce9bb1c869 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 <moveit_msgs/JointLimits.h>
 // Qt
+#include <QApplication>
+#include <QDoubleValidator>
 #include <QFormLayout>
+#include <QGroupBox>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QLineEdit>
 #include <QMessageBox>
-#include <QDoubleValidator>
+#include <QPushButton>
+#include <QScrollArea>
+#include <QStackedWidget>
+#include <QString>
+#include <QTableWidget>
+#include <QTreeWidget>
 #include <QTreeWidgetItem>
-#include <QApplication>
+#include <QVBoxLayout>
 
 #include <regex>
 #include <moveit/robot_state/conversions.h>
@@ -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);
@@ -805,7 +810,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);
@@ -816,7 +821,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..b93a791b3e 100644
--- a/moveit_setup_assistant/src/widgets/ros_controllers_widget.h
+++ b/moveit_setup_assistant/src/widgets/ros_controllers_widget.h
@@ -37,27 +37,24 @@
 #define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_ROS_CONTROLLERS_WIDGET_H
 
 // Qt
-#include <QVBoxLayout>
-#include <QHBoxLayout>
-#include <QScrollArea>
-#include <QGroupBox>
-#include <QTableWidget>
-#include <QTreeWidget>
-#include <QStackedLayout>
-#include <QString>
+class QHBoxLayout;
+class QPushButton;
+class QStackedWidget;
+class QTreeWidget;
+class QTreeWidgetItem;
 
 // SA
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
 #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
@@ -129,7 +126,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_;
diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp
index 8feffda315..4737db016e 100644
--- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp
@@ -37,15 +37,21 @@
 // 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 <QStackedLayout>
-#include <QListWidget>
-#include <QListWidgetItem>
+#include <QApplication>
+#include <QCloseEvent>
 #include <QFont>
+#include <QHBoxLayout>
 #include <QLabel>
 #include <QPushButton>
+#include <QCheckBox>
 #include <QCloseEvent>
 #include <QMessageBox>
+#include <QPushButton>
+#include <QSplitter>
+#include <QStackedWidget>
 #include <QString>
 #include <pluginlib/class_loader.hpp>  // for loading all avail kinematic planners
 // Rviz
@@ -83,14 +89,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
@@ -145,7 +146,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
@@ -404,10 +405,25 @@ void SetupAssistantWidget::loadRviz()
   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();
 }
 
diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h
index 98c7878ddb..d7e853fcf2 100644
--- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h
+++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.h
@@ -38,18 +38,9 @@
 #define MOVEIT_ROS_MOVEIT_SETUP_ASSISTANT_WIDGETS_SETUP_ASSISTANT_WIDGET_
 
 // Qt
-#include <QWidget>
-#include <QVBoxLayout>
-#include <QHBoxLayout>
-#include <QString>
-#include <QApplication>
-#include <QObject>
-#include <QEvent>
-#include <QListWidget>
-#include <QTimer>
-#include <QSplitter>
-#include <QStringList>
-// Setup Asst
+class QSplitter;
+
+// Setup Assistant
 #include "navigation_widget.h"
 #include "start_screen_widget.h"
 #include "default_collisions_widget.h"
@@ -68,8 +59,7 @@
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
 
 // Other
-#include <ros/ros.h>
-#include <boost/program_options.hpp>  // for parsing input arguments
+#include <boost/program_options/variables_map.hpp>  // for parsing input arguments
 #include <boost/thread/mutex.hpp>
 #endif
 
@@ -203,10 +193,9 @@ private Q_SLOTS:
   QList<QString> 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_;
 
diff --git a/moveit_setup_assistant/src/widgets/simulation_widget.cpp b/moveit_setup_assistant/src/widgets/simulation_widget.cpp
index 14542fa7f2..bba4b5bd1a 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 <QVBoxLayout>
+#include <QColor>
+#include <QLabel>
 #include <QMessageBox>
 #include <QPushButton>
-#include <QColor>
+#include <QTextEdit>
+#include <QVBoxLayout>
 
 #include <moveit/robot_state/conversions.h>
 #include <moveit_msgs/DisplayRobotState.h>
diff --git a/moveit_setup_assistant/src/widgets/simulation_widget.h b/moveit_setup_assistant/src/widgets/simulation_widget.h
index 2b3ac58331..ed1ae0b509 100644
--- a/moveit_setup_assistant/src/widgets/simulation_widget.h
+++ b/moveit_setup_assistant/src/widgets/simulation_widget.h
@@ -37,16 +37,14 @@
 #define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_SIMULATION_WIDGET_H
 
 // Qt
-#include <QScrollArea>
-#include <QTextEdit>
-#include <QString>
+class QLabel;
+class QTextEdit;
 
 // SA
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
 #endif
 
-#include "header_widget.h"
 #include "setup_screen_widget.h"  // a base class for screens in the setup assistant
 
 namespace moveit_setup_assistant
diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp
index 337f0c7c79..4ec835ffc9 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 <QLabel>
-#include <QTimer>
-#include <QVBoxLayout>
+#include <QApplication>
+#include <QFileDialog>
+#include <QFont>
 #include <QHBoxLayout>
+#include <QLabel>
+#include <QLineEdit>
 #include <QMessageBox>
+#include <QProgressBar>
+#include <QPushButton>
 #include <QString>
-#include <QApplication>
-#include <QFont>
-#include <QFileDialog>
+#include <QTextEdit>
+#include <QTimer>
+#include <QVBoxLayout>
+
 // ROS
-#include <ros/ros.h>
 #include <ros/package.h>  // for getting file path for loadng images
 // SA
 #include "header_widget.h"  // title and instructions
@@ -188,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_);
@@ -393,7 +395,7 @@ bool StartScreenWidget::loadExistingFiles()
   QApplication::processEvents();
 
   // Load the SRDF
-  if (!loadSRDFFile(config_data_->srdf_path_))
+  if (!loadSRDFFile(config_data_->srdf_path_, config_data_->xacro_args_))
     return false;  // error occured
 
   // Progress Indicator
@@ -544,9 +546,7 @@ bool StartScreenWidget::loadNewFiles()
 // ******************************************************************************************
 bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args)
 {
-  const std::vector<std::string> vec_xacro_args = { xacro_args };
-
-  if (!rdf_loader::RDFLoader::loadXmlFileToString(config_data_->urdf_string_, urdf_file_path, vec_xacro_args))
+  if (!rdf_loader::RDFLoader::loadXmlFileToString(config_data_->urdf_string_, urdf_file_path, { xacro_args }))
   {
     QMessageBox::warning(this, "Error Loading Files",
                          QString("URDF/COLLADA file not found: ").append(urdf_file_path.c_str()));
@@ -590,12 +590,10 @@ bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const st
 // ******************************************************************************************
 // Load SRDF File to Parameter Server
 // ******************************************************************************************
-bool StartScreenWidget::loadSRDFFile(const std::string& srdf_file_path)
+bool StartScreenWidget::loadSRDFFile(const std::string& srdf_file_path, const std::string& xacro_args)
 {
-  const std::vector<std::string> xacro_args;
-
   std::string srdf_string;
-  if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, srdf_file_path, xacro_args))
+  if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, srdf_file_path, { xacro_args }))
   {
     QMessageBox::warning(this, "Error Loading Files", QString("SRDF file not found: ").append(srdf_file_path.c_str()));
     return false;
diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.h b/moveit_setup_assistant/src/widgets/start_screen_widget.h
index 116f3b987e..293c9e5c71 100644
--- a/moveit_setup_assistant/src/widgets/start_screen_widget.h
+++ b/moveit_setup_assistant/src/widgets/start_screen_widget.h
@@ -37,12 +37,9 @@
 #ifndef MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_START_SCREEN_WIDGET_
 #define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_START_SCREEN_WIDGET_
 
-#include <QWidget>
-#include <QLineEdit>
-#include <QPushButton>
-#include <QLabel>
-#include <QProgressBar>
-#include <QTextEdit>
+class QLabel;
+class QProgressBar;
+class QPushButton;
 
 #ifndef Q_MOC_RUN
 #include <urdf/model.h>                                       // for testing a valid urdf is loaded
@@ -150,7 +147,7 @@ private Q_SLOTS:
   bool loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args);
 
   /// Load SRDF File
-  bool loadSRDFFile(const std::string& srdf_file_path);
+  bool loadSRDFFile(const std::string& srdf_file_path, const std::string& xacro_args);
 
   /// Put SRDF File on Parameter Server
   bool setSRDFFile(const std::string& srdf_string);
diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp
index 7a56df09f3..9c926b1088 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 <QApplication>
+#include <QComboBox>
 #include <QFormLayout>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QLineEdit>
 #include <QMessageBox>
-#include <QApplication>
+#include <QPushButton>
+#include <QStackedWidget>
+#include <QString>
+#include <QTableWidget>
+#include <QVBoxLayout>
 
 namespace moveit_setup_assistant
 {
@@ -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);
@@ -255,29 +257,6 @@ void VirtualJointsWidget::editDoubleClicked(int row, int column)
 // ******************************************************************************************
 void VirtualJointsWidget::previewClicked(int row, int column)
 {
-  // TODO: highlight the virtual joint?
-
-  /*  // Get list of all selected items
-  QList<QTableWidgetItem*> 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<std::string, std::vector<double> >::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);
@@ -537,7 +516,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);
@@ -555,7 +534,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);
@@ -620,7 +599,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 c8e7361037..99ee02d262 100644
--- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h
+++ b/moveit_setup_assistant/src/widgets/virtual_joints_widget.h
@@ -38,22 +38,18 @@
 #define MOVEIT_MOVEIT_SETUP_ASSISTANT_WIDGETS_VIRTUAL_JOINTS_WIDGET_
 
 // Qt
-#include <QWidget>
-#include <QVBoxLayout>
-#include <QHBoxLayout>
-#include <QLabel>
-#include <QPushButton>
-#include <QTableWidget>
-#include <QStackedLayout>
-#include <QString>
-#include <QComboBox>
+class QLabel;
+class QLineEdit;
+class QPushButton;
+class QTableWidget;
+class QStackedWidget;
+class QComboBox;
 
 // SA
 #ifndef Q_MOC_RUN
 #include <moveit/setup_assistant/tools/moveit_config_data.h>
 #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 +76,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_;
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 1ec94ef5ae..a28ad6d52b 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,20 +1,30 @@
 <launch>
-   <!-- CHOMP Plugin for MoveIt! -->
-   <arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
-   <arg name="start_state_max_bounds_error" value="0.1" />
-   <!-- The request adapters (plugins) used when planning.
-        ORDER MATTERS -->
-   <arg name="planning_adapters"
-        value="default_planner_request_adapters/AddTimeParameterization
-               default_planner_request_adapters/FixWorkspaceBounds
-               default_planner_request_adapters/FixStartStateBounds
-               default_planner_request_adapters/FixStartStateCollision
-               default_planner_request_adapters/FixStartStatePathConstraints"
-               />
+  <!-- CHOMP Plugin for MoveIt! -->
+  <arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
 
-   <param name="planning_plugin" value="$(arg planning_plugin)" />
-   <param name="request_adapters" value="$(arg planning_adapters)" />
-   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
 
-   <rosparam command="load" file="$(find [GENERATED_PACKAGE_NAME])/config/chomp_planning.yaml" />
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+  <!-- The request adapters (plugins) used when planning.
+       ORDER MATTERS -->
+  <arg name="planning_adapters"
+       value="default_planner_request_adapters/AddTimeParameterization
+              default_planner_request_adapters/ResolveConstraintFrames
+              default_planner_request_adapters/FixWorkspaceBounds
+              default_planner_request_adapters/FixStartStateBounds
+              default_planner_request_adapters/FixStartStateCollision
+              default_planner_request_adapters/FixStartStatePathConstraints"
+              />
+
+  <param name="planning_plugin" value="$(arg planning_plugin)" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="capabilities" value="$(arg capabilities)" />
+  <param name="disable_capabilities" value="$(arg disable_capabilities)" />
+
+  <rosparam command="load" file="$(find [GENERATED_PACKAGE_NAME])/config/chomp_planning.yaml" />
 </launch>
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 ef52573466..73817c359d 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,6 +11,12 @@
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />
 
+  <!-- By default, we will load or override the robot_description -->
+  <arg name="load_robot_description" default="true"/>
+
+  <!-- Set execution mode for fake execution controllers -->
+  <arg name="fake_execution_type" default="interpolate" />
+
   <!--
   By default, hide joint_state_publisher's GUI
 
@@ -22,17 +28,14 @@
   <arg name="use_gui" default="false" />
   <arg name="use_rviz" default="true" />
 
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find [GENERATED_PACKAGE_NAME])/launch/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
-
   <!-- If needed, broadcast static tf for robot root -->
-  [VIRTUAL_JOINT_BROADCASTER]
+[VIRTUAL_JOINT_BROADCASTER]
 
   <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="use_gui" value="$(arg use_gui)"/>
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
+    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
+  </node>
+  <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
     <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
   </node>
 
@@ -43,9 +46,11 @@
   <include file="$(find [GENERATED_PACKAGE_NAME])/launch/move_group.launch">
     <arg name="allow_trajectory_execution" value="true"/>
     <arg name="fake_execution" value="true"/>
+    <arg name="fake_execution_type" value="$(arg fake_execution_type)" />
     <arg name="info" value="true"/>
     <arg name="debug" value="$(arg debug)"/>
     <arg name="pipeline" value="$(arg pipeline)"/>
+    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
   </include>
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
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 6249188164..e4183aa164 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,6 +8,9 @@
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />
 
+  <!-- By default, we won't load or override the robot_description -->
+  <arg name="load_robot_description" default="false"/>
+
   <!--
   By default, hide joint_state_publisher's GUI
 
@@ -31,17 +34,16 @@
     <arg name="urdf_path" value="$(arg urdf_path)"/>
   </include>
 
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find [GENERATED_PACKAGE_NAME])/launch/planning_context.launch">
-    <arg name="load_robot_description" value="false"/>
-  </include>
-
   <!-- If needed, broadcast static tf for robot root -->
   [VIRTUAL_JOINT_BROADCASTER]
 
   <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="use_gui" value="$(arg use_gui)"/>
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
+    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
+    <rosparam param="source_list">[/joint_states]</rosparam>
+  </node>
+  <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
+    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
     <rosparam param="source_list">[/joint_states]</rosparam>
   </node>
 
@@ -54,7 +56,8 @@
     <arg name="fake_execution" value="false"/>
     <arg name="info" value="true"/>
     <arg name="debug" value="$(arg debug)"/>
-  </include>  
+    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+  </include>
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
   <include file="$(find [GENERATED_PACKAGE_NAME])/launch/moveit_rviz.launch">
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..613028d317 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
@@ -8,8 +8,8 @@
 
   <!-- Run -->
   <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
-	args="--config_pkg=[GENERATED_PACKAGE_NAME]"
-	launch-prefix="$(arg launch_prefix)"
+        args="--config_pkg=[GENERATED_PACKAGE_NAME]"
+        launch-prefix="$(arg launch_prefix)"
         output="screen" />
 
 </launch>
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..b0a803df1f 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 @@
 <launch>
 
+  <!-- execute the trajectory in 'interpolate' mode or jump to goal position in 'last point' mode -->
+  <arg name="fake_execution_type" default="interpolate" />
+
   <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
 
   <!-- The rest of the params are specific to this plugin -->
-  <rosparam file="$(find [GENERATED_PACKAGE_NAME])/config/fake_controllers.yaml"/>
+  <rosparam subst_value="true" file="$(find [GENERATED_PACKAGE_NAME])/config/fake_controllers.yaml"/>
 
 </launch>
diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gdb_settings.gdb b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gdb_settings.gdb
index 9eeb4cb95b..1c207c8e0b 100644
--- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gdb_settings.gdb
+++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gdb_settings.gdb
@@ -5,4 +5,4 @@
 set breakpoint pending on
 
 ## Example break point:
-#break trajectory_execution_manager.cpp:228
\ No newline at end of file
+#break trajectory_execution_manager.cpp:228
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..7c7a4f6a4b 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 @@
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
   <arg     if="$(arg debug)" name="launch_prefix"
-	   value="gdb -x $(find [GENERATED_PACKAGE_NAME])/launch/gdb_settings.gdb --ex run --args" />
+           value="gdb -x $(find [GENERATED_PACKAGE_NAME])/launch/gdb_settings.gdb --ex run --args" />
 
   <!-- Verbose Mode Option -->
   <arg name="info" default="$(arg debug)" />
@@ -15,6 +15,7 @@
   <arg name="pipeline" default="ompl" />
   <arg name="allow_trajectory_execution" default="true"/>
   <arg name="fake_execution" default="false"/>
+  <arg name="fake_execution_type" default="interpolate" />
   <arg name="max_safe_path_cost" default="1"/>
   <arg name="jiggle_fraction" default="0.05" />
   <arg name="publish_monitored_planning_scene" default="true"/>
@@ -46,6 +47,8 @@
   <!-- Planning Functionality -->
   <include ns="move_group" file="$(find [GENERATED_PACKAGE_NAME])/launch/planning_pipeline.launch.xml">
     <arg name="pipeline" value="$(arg pipeline)" />
+    <arg name="capabilities" value="$(arg capabilities)"/>
+    <arg name="disable_capabilities" value="$(arg disable_capabilities)"/>
   </include>
 
   <!-- Trajectory Execution Functionality -->
@@ -53,6 +56,7 @@
     <arg name="moveit_manage_controllers" value="true" />
     <arg name="moveit_controller_manager" value="[ROBOT_NAME]" unless="$(arg fake_execution)"/>
     <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
+    <arg name="fake_execution_type" value="$(arg fake_execution_type)" />
   </include>
 
   <!-- Sensors Functionality -->
@@ -68,8 +72,6 @@
     <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
     <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
     <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
-    <param name="capabilities" value="$(arg capabilities)"/>
-    <param name="disable_capabilities" value="$(arg disable_capabilities)"/>
 
 
     <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml
index 833414a345..b3959b8630 100644
--- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml
+++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_controller_manager.launch.xml
@@ -1,10 +1,7 @@
 <launch>
+  <!-- Define the controller manager plugin to use for trajectory execution -->
+  <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
 
-  <!-- loads moveit_controller_manager on the parameter server which is taken as argument 
-    if no argument is passed, moveit_simple_controller_manager will be set -->
-  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
-  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
-
-  <!-- loads ros_controllers to the param server -->
+  <!-- loads controller list to the param server -->
   <rosparam file="$(find [GENERATED_PACKAGE_NAME])/config/ros_controllers.yaml"/>
 </launch>
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 615b2f12a5..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,7 +9,7 @@
   <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
 
   <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
-	args="$(arg command_args)" output="screen">
+        args="$(arg command_args)" output="screen">
   </node>
 
 </launch>
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 7dbc6e96b7..4cc5f7b19c 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
@@ -3,6 +3,12 @@
   <!-- OMPL Plugin for MoveIt! -->
   <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
 
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
+
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
   <!-- The request adapters (plugins) used when planning with OMPL.
        ORDER MATTERS -->
   <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
@@ -16,6 +22,8 @@
   <param name="planning_plugin" value="$(arg planning_plugin)" />
   <param name="request_adapters" value="$(arg planning_adapters)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="capabilities" value="$(arg capabilities)" />
+  <param name="disable_capabilities" value="$(arg disable_capabilities)" />
 
   <rosparam command="load" file="$(find [GENERATED_PACKAGE_NAME])/config/ompl_planning.yaml"/>
 
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 @@
+<launch>
+
+  <!-- Pilz Command Planner Plugin for MoveIt -->
+  <arg name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
+
+  <!-- The request adapters (plugins) used when planning.
+       ORDER MATTERS -->
+  <arg name="planning_adapters" value="" />
+
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
+
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+
+  <param name="planning_plugin" value="$(arg planning_plugin)" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+
+  <!-- MoveGroup capabilities to load, append sequence capability -->
+  <param name="capabilities" value="$(arg capabilities)
+                                    pilz_industrial_motion_planner/MoveGroupSequenceAction
+                                    pilz_industrial_motion_planner/MoveGroupSequenceService
+                                   " />
+  <param name="disable_capabilities" value="$(arg disable_capabilities)" />
+</launch>
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 54efbec4e7..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,6 +14,7 @@
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
     <rosparam command="load" file="$(find [GENERATED_PACKAGE_NAME])/config/joint_limits.yaml"/>
+    <rosparam command="load" file="$(find [GENERATED_PACKAGE_NAME])/config/cartesian_limits.yaml"/>
   </group>
 
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
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 @@
 
   <arg name="pipeline" default="ompl" />
 
-  <include file="$(find [GENERATED_PACKAGE_NAME])/launch/$(arg pipeline)_planning_pipeline.launch.xml" />
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
+
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
+  <include file="$(find [GENERATED_PACKAGE_NAME])/launch/$(arg pipeline)_planning_pipeline.launch.xml">
+    <arg name="capabilities" value="$(arg capabilities)"/>
+    <arg name="disable_capabilities" value="$(arg disable_capabilities)"/>
+  </include>
 
 </launch>
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..be577b0c0c 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
@@ -1,6 +1,7 @@
 <launch>
 
   <!-- This file makes it easy to include the settings for trajectory execution  -->
+  <arg name="fake_execution_type" default="interpolate" />
 
   <!-- Flag indicating whether MoveIt! is allowed to load/unload  or switch controllers -->
   <arg name="moveit_manage_controllers" default="true"/>
@@ -15,6 +16,8 @@
 
   <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
   <arg name="moveit_controller_manager" default="[ROBOT_NAME]" />
-  <include file="$(find [GENERATED_PACKAGE_NAME])/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
+  <!-- We use pass_all_args=true here to pass execution_type, which is required by fake controllers, but not by real-robot controllers.
+       As real-robot controller_manager.launch files shouldn't be required to define this argument, we use the trick of passing all args. -->
+  <include file="$(find [GENERATED_PACKAGE_NAME])/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" pass_all_args="true" />
 
 </launch>
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..bd38696c9f 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
@@ -22,8 +22,11 @@
   <run_depend>moveit_planners_ompl</run_depend>
   <run_depend>moveit_ros_visualization</run_depend>
   <run_depend>moveit_setup_assistant</run_depend>
+  <run_depend>moveit_simple_controller_manager</run_depend>
   <run_depend>joint_state_publisher</run_depend>
+  <run_depend>joint_state_publisher_gui</run_depend>
   <run_depend>robot_state_publisher</run_depend>
+  <run_depend>rviz</run_depend>
   <run_depend>tf2_ros</run_depend>
   <run_depend>xacro</run_depend>
   <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
diff --git a/moveit_setup_assistant/test/moveit_config_data_test.cpp b/moveit_setup_assistant/test/moveit_config_data_test.cpp
index a9d1ed7357..5474e01559 100644
--- a/moveit_setup_assistant/test/moveit_config_data_test.cpp
+++ b/moveit_setup_assistant/test/moveit_config_data_test.cpp
@@ -125,7 +125,7 @@ TEST_F(MoveItConfigData, ReadingSensorsConfig)
 
   // Default config for the two available sensor plugins
   // Make sure both are parsed correctly
-  EXPECT_EQ(config_data->getSensorPluginConfig().size(), 2u);
+  ASSERT_EQ(config_data->getSensorPluginConfig().size(), 2u);
 
   EXPECT_EQ(config_data->getSensorPluginConfig()[0]["sensor_plugin"].getValue(),
             std::string("occupancy_map_monitor/PointCloudOctomapUpdater"));
@@ -134,6 +134,48 @@ TEST_F(MoveItConfigData, ReadingSensorsConfig)
             std::string("occupancy_map_monitor/DepthImageOctomapUpdater"));
 }
 
+// This tests writing of sensors_3d.yaml
+TEST_F(MoveItConfigData, WritingSensorsConfig)
+{
+  // Contains all the config data for the setup assistant
+  moveit_setup_assistant::MoveItConfigDataPtr config_data;
+  config_data.reset(new moveit_setup_assistant::MoveItConfigData());
+
+  // Empty Config Should have No Sensors
+  EXPECT_EQ(config_data->getSensorPluginConfig().size(), 0u);
+
+  // Temporary file used during the test and is deleted when the test is finished
+  char test_file[] = "/tmp/msa_unittest_sensors.yaml";
+
+  // sensors yaml written correctly
+  EXPECT_EQ(config_data->output3DSensorPluginYAML(test_file), true);
+
+  // Set default file
+  boost::filesystem::path setup_assistant_path(config_data->setup_assistant_path_);
+  std::string default_file_path =
+      (setup_assistant_path / "templates/moveit_config_pkg_template/config/sensors_3d.yaml").string();
+
+  // Read from the written file
+  config_data.reset(new moveit_setup_assistant::MoveItConfigData());
+  EXPECT_EQ(config_data->input3DSensorsYAML(test_file), true);
+
+  // Should still have No Sensors
+  EXPECT_EQ(config_data->getSensorPluginConfig().size(), 0u);
+
+  // Now load the default file and write it to a file
+  config_data.reset(new moveit_setup_assistant::MoveItConfigData());
+  EXPECT_EQ(config_data->input3DSensorsYAML(default_file_path), true);
+  EXPECT_EQ(config_data->getSensorPluginConfig().size(), 2u);
+  EXPECT_EQ(config_data->output3DSensorPluginYAML(test_file), true);
+
+  // Read from the written file
+  config_data.reset(new moveit_setup_assistant::MoveItConfigData());
+  EXPECT_EQ(config_data->input3DSensorsYAML(test_file), true);
+
+  // Should now have two sensors
+  EXPECT_EQ(config_data->getSensorPluginConfig().size(), 2u);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);