diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/CMakeLists.txt b/moveit_demo_nodes/run_ompl_constrained_planning/CMakeLists.txt new file mode 100644 index 0000000000..e48d3b573b --- /dev/null +++ b/moveit_demo_nodes/run_ompl_constrained_planning/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.5) +project(run_ompl_constrained_planning) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +# This shouldn't be necessary (required by moveit_simple_controller_manager) +find_package(rosidl_default_runtime REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(run_ompl_constrained_planning src/run_ompl_constrained_planning.cpp) + +ament_target_dependencies(run_ompl_constrained_planning + moveit_ros_planning_interface + Boost +) + +install(TARGETS run_ompl_constrained_planning + EXPORT export_${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/README.md b/moveit_demo_nodes/run_ompl_constrained_planning/README.md new file mode 100644 index 0000000000..531d395c48 --- /dev/null +++ b/moveit_demo_nodes/run_ompl_constrained_planning/README.md @@ -0,0 +1,38 @@ +MoveIt 2 Logo + +# MoveIt 2 Beta - OMPL Constrained Planning Demo +## Setup +Before running the demo add the following lines to `ompl_planning.yaml` in the `panda_config` package +bellow `panda_arm:` + +``` +enforce_constrained_state_space: true +projection_evaluator: joints(panda_joint1,panda_joint2) +``` + +## Running +This demo includes a launch configuration for running MoveGroup and a separate demo node. + +- Note: This demo shows how to construct a variety of +constraints. See [util.cpp](https://github.com/ros-planning/moveit2/blob/main/moveit_core/kinematic_constraints/src/utils.cpp) for helper functions to automate constructing the constraint messages. + +The MoveGroup setup can be started: +``` +ros2 launch run_ompl_constrained_planning run_move_group.launch.py +``` + +This allows you to start planning and executing motions: +``` +ros2 launch run_move_group run_move_group_interface.launch.py +``` + +## Details +### State space selection process +There are three options for state space selection. + +1. Set `enforce_constrained_state_space = true` AND there must be path constraints in the planning request. This overrides all other settings and selects a `ConstrainedPlanningStateSpace` factory. If there are no path constraints in the planning request, this option is ignored, the constrained state space is only usefull for paths constraints. At the moment only a single position constraint is supported. + +2. Set `enforce_joint_model_state_space = true` and option 1. is false, then this overrides the remaining settings and selects `JointModelStateSpace` factory. Some planning problems such as orientation path constraints are represented in `PoseModelStateSpace` and sampled via IK. However, consecutive IK solutions are not checked for proximity at the moment and sometimes happen to be flipped, leading to invalid trajectories. This workaround lets the user prevent this problem by forcing rejection sampling in `JointModelStateSpace`. + +3. When options 1. and 2. are both set to false, the factory is selected based on the priority each one returns. See [PoseModelStateSpaceFactory::canRepresentProblem](https://github.com/ros-planning/moveit2/blob/b4ff391133c2809e9f697d44593c89a77d1d4c5c/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp#L45) for details on the selection process. In short, it selects `PoseModelStateSpace` if there is an IK solver and a path constraint. + diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/config/controllers.yaml b/moveit_demo_nodes/run_ompl_constrained_planning/config/controllers.yaml new file mode 100644 index 0000000000..58ea42051d --- /dev/null +++ b/moveit_demo_nodes/run_ompl_constrained_planning/config/controllers.yaml @@ -0,0 +1,15 @@ +controller_names: + - panda_arm_controller + +panda_arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/config/panda_controllers.yaml b/moveit_demo_nodes/run_ompl_constrained_planning/config/panda_controllers.yaml new file mode 100644 index 0000000000..bc6e45d277 --- /dev/null +++ b/moveit_demo_nodes/run_ompl_constrained_planning/config/panda_controllers.yaml @@ -0,0 +1,18 @@ +panda_arm_controller: + ros__parameters: + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + write_op_modes: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 \ No newline at end of file diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/config/run_move_group.rviz b/moveit_demo_nodes/run_ompl_constrained_planning/config/run_move_group.rviz new file mode 100644 index 0000000000..edd5b76028 --- /dev/null +++ b/moveit_demo_nodes/run_ompl_constrained_planning/config/run_move_group.rviz @@ -0,0 +1,405 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PlanningScene1 + - /Trajectory1 + - /MotionPlanning1 + - /MarkerArray1 + Splitter Ratio: 0.4383954107761383 + Tree Height: 807 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: false + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: panda_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: false + Velocity_Scaling_Factor: 0.1 + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + /: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visualization_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /display_contacts + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: panda_link0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.1508772373199463 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3453982472419739 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.1253975629806519 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 958 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1533 + X: 331 + Y: 10 diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/config/start_positions.yaml b/moveit_demo_nodes/run_ompl_constrained_planning/config/start_positions.yaml new file mode 100644 index 0000000000..f4553b3e33 --- /dev/null +++ b/moveit_demo_nodes/run_ompl_constrained_planning/config/start_positions.yaml @@ -0,0 +1,19 @@ +fake_joint_driver_node: + ros__parameters: + start_position: + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + values: + - 0.0 + - -0.785 + - 0.0 + - -2.356 + - 0.0 + - 1.571 + - 0.785 \ No newline at end of file diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_move_group.launch.py b/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_move_group.launch.py new file mode 100644 index 0000000000..5dfff6ec67 --- /dev/null +++ b/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_move_group.launch.py @@ -0,0 +1,120 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + + # planning_context + robot_description_config = load_file('moveit_resources_panda_description', 'urdf/panda.urdf') + robot_description = {'robot_description' : robot_description_config} + + robot_description_semantic_config = load_file('moveit_resources_panda_moveit_config', 'config/panda.srdf') + robot_description_semantic = {'robot_description_semantic' : robot_description_semantic_config} + + kinematics_yaml = load_yaml('moveit_resources_panda_moveit_config', 'config/kinematics.yaml') + robot_description_kinematics = { 'robot_description_kinematics' : kinematics_yaml } + + # Planning Functionality + ompl_planning_pipeline_config = { 'move_group' : { + 'planning_plugin' : 'ompl_interface/OMPLPlanner', + 'request_adapters' : """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""" , + 'start_state_max_bounds_error' : 0.1 } } + + ompl_planning_yaml = load_yaml('moveit_resources_panda_moveit_config', 'config/ompl_planning.yaml') + ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) + + # Trajectory Execution Functionality + controllers_yaml = load_yaml('run_ompl_constrained_planning', 'config/controllers.yaml') + moveit_controllers = { 'moveit_simple_controller_manager' : controllers_yaml, + 'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'} + + trajectory_execution = {'moveit_manage_controllers': True, + 'trajectory_execution.allowed_execution_duration_scaling': 1.2, + 'trajectory_execution.allowed_goal_duration_margin': 0.5, + 'trajectory_execution.allowed_start_tolerance': 0.01} + + planning_scene_monitor_parameters = {"publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True} + + # Start the actual move_group node/action server + run_move_group_node = Node(package='moveit_ros_move_group', + executable='move_group', + output='screen', + # prefix='kitty -e gdb -e run --args', + parameters=[robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters]) + + # RViz + rviz_config_file = get_package_share_directory('run_ompl_constrained_planning') + "/config/run_move_group.rviz" + rviz_node = Node(package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments=['-d', rviz_config_file], + parameters=[robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml]) + + # Static TF + static_tf = Node(package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + output='log', + arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'panda_link0']) + + # Publish TF + robot_state_publisher = Node(package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[robot_description]) + + # Fake joint driver + fake_joint_driver_node = Node(package='fake_joint_driver', + executable='fake_joint_driver_node', + parameters=[{'controller_name': 'panda_arm_controller'}, + os.path.join(get_package_share_directory("run_ompl_constrained_planning"), "config", "panda_controllers.yaml"), + os.path.join(get_package_share_directory("run_ompl_constrained_planning"), "config", "start_positions.yaml"), + robot_description] + ) + + # Warehouse mongodb server + mongodb_server_node = Node(package='warehouse_ros_mongo', + executable='mongo_wrapper_ros.py', + parameters=[{'warehouse_port': 33829}, + {'warehouse_host': 'localhost'}, + {'warehouse_plugin': 'warehouse_ros_mongo::MongoDatabaseConnection'}], + output='screen') + + return LaunchDescription([ rviz_node, static_tf, robot_state_publisher, run_move_group_node, fake_joint_driver_node, mongodb_server_node ]) diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_ompl_demo.launch.py b/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_ompl_demo.launch.py new file mode 100644 index 0000000000..005ea8f390 --- /dev/null +++ b/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_ompl_demo.launch.py @@ -0,0 +1,50 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + # planning_context + robot_description_config = load_file('moveit_resources_panda_description', 'urdf/panda.urdf') + robot_description = {'robot_description': robot_description_config} + + robot_description_semantic_config = load_file('moveit_resources_panda_moveit_config', 'config/panda.srdf') + robot_description_semantic = {'robot_description_semantic': robot_description_semantic_config} + + kinematics_yaml = load_yaml('moveit_resources_panda_moveit_config', 'config/kinematics.yaml') + + # MoveGroupInterface demo executable + run_move_group_demo = Node(name='run_ompl_constrained_planning', + package='run_ompl_constrained_planning', + executable='run_ompl_constrained_planning', + output='screen', + # prefix='kitty -e gdb -e run --args', + parameters=[robot_description, + robot_description_semantic, + kinematics_yaml]) + + return LaunchDescription([run_move_group_demo]) \ No newline at end of file diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/package.xml b/moveit_demo_nodes/run_ompl_constrained_planning/package.xml new file mode 100644 index 0000000000..b39f1aeccf --- /dev/null +++ b/moveit_demo_nodes/run_ompl_constrained_planning/package.xml @@ -0,0 +1,24 @@ + + + + run_ompl_constrained_planning + 2.1.0 + Demo ompl constrained planning capabilities + Boston Cleek + BSD + + ament_cmake + moveit_common + + moveit_ros_planning_interface + + fake_joint_driver + warehouse_ros_mongo + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/src/run_ompl_constrained_planning.cpp b/moveit_demo_nodes/run_ompl_constrained_planning/src/run_ompl_constrained_planning.cpp new file mode 100644 index 0000000000..8967d2d6b1 --- /dev/null +++ b/moveit_demo_nodes/run_ompl_constrained_planning/src/run_ompl_constrained_planning.cpp @@ -0,0 +1,556 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, 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: Boston Cleek + Desc: A simple demo node running ompl constrained planning capabilities for planning and execution +*/ + +#include +#include +#include + +#include +#include +#include +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("ompl_constrained_planning_demo"); +static const std::string PLANNING_GROUP = "panda_arm"; +static const double PLANNING_TIME_S = 30.0; +static const double PLANNING_ATTEMPTS = 5.0; + +class ConstrainedPlanning +{ +public: + ConstrainedPlanning(const rclcpp::Node::SharedPtr& node) : node_(node), marker_count_(0) + { + marker_pub_ = node_->create_publisher("/visualization_marker", 10); + + move_group_ = std::make_shared(node_, PLANNING_GROUP); + move_group_->setPlanningTime(PLANNING_TIME_S); + move_group_->setNumPlanningAttempts(PLANNING_ATTEMPTS); + + ref_link_ = move_group_->getPoseReferenceFrame(); + ee_link_ = move_group_->getEndEffectorLink(); + } + + void moveToStart() + { + // Clear previous goals and constraints + // May have been set previously + move_group_->clearPoseTargets(); + move_group_->clearPathConstraints(); + + RCLCPP_INFO(LOGGER, "moveToStart"); + + const moveit_msgs::msg::RobotState goal_state = createRobotState("ready"); + move_group_->setStartStateToCurrentState(); + move_group_->setJointValueTarget(goal_state.joint_state); + move_group_->move(); + } + + void planBoxConstraints() + { + move_group_->clearPoseTargets(); + move_group_->clearPathConstraints(); + + RCLCPP_INFO(LOGGER, "planBoxConstraints"); + + const moveit_msgs::msg::RobotState start_state = createRobotState("ready"); + const geometry_msgs::msg::PoseStamped pose_goal = createPoseGoal(0.0, 0.3, -0.3); + const moveit_msgs::msg::PositionConstraint pcm = createBoxConstraint(); + + moveit_msgs::msg::Constraints path_constraints; + path_constraints.name = "box constraints"; + path_constraints.position_constraints.emplace_back(pcm); + + move_group_->setStartState(start_state); + move_group_->setPoseTarget(pose_goal); + move_group_->setPathConstraints(path_constraints); + + moveit::planning_interface::MoveGroupInterface::Plan plan1; + const bool plan_success = (move_group_->plan(plan1) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan 1 (box constraint) %s", plan_success ? "SUCCEEDED" : "FAILED"); + + move_group_->move(); + } + + void planPlaneConstraints() + { + move_group_->clearPoseTargets(); + move_group_->clearPathConstraints(); + + RCLCPP_INFO(LOGGER, "planPlaneConstraints"); + const geometry_msgs::msg::PoseStamped pose_goal = createPoseGoal(0.0, 0.0, -0.3); + const moveit_msgs::msg::PositionConstraint pcm = verticlePlaneConstraint(); + + moveit_msgs::msg::Constraints path_constraints; + + // For equality constraints set to: "use_equality_constraints" + path_constraints.name = "use_equality_constraints"; + + path_constraints.position_constraints.emplace_back(pcm); + + move_group_->setStartStateToCurrentState(); + move_group_->setPoseTarget(pose_goal); + move_group_->setPathConstraints(path_constraints); + + moveit::planning_interface::MoveGroupInterface::Plan plan2; + const bool plan_success = (move_group_->plan(plan2) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan 2 (plane equality constraint) %s", plan_success ? "SUCCEEDED" : "FAILED"); + + move_group_->move(); + } + + void planOrientationConstraints() + { + move_group_->clearPoseTargets(); + move_group_->clearPathConstraints(); + + RCLCPP_INFO(LOGGER, "planOrientationConstraints"); + + const std::vector angle_tol = { 1.0, 1.0, 1.0 }; // orientation tolerances (rad) + const geometry_msgs::msg::PoseStamped pose_goal = createPoseGoal(0.0, 0.5, 0.0); + + RCLCPP_INFO(LOGGER, "Pose goal position [x y z] : %f, %f, %f orientation [x y z w] %f, %f, %f, %f:", + pose_goal.pose.position.x, pose_goal.pose.position.y, pose_goal.pose.position.z, + pose_goal.pose.orientation.x, pose_goal.pose.orientation.y, pose_goal.pose.orientation.z, + pose_goal.pose.orientation.w); + + // Create an orientation constraint + moveit_msgs::msg::OrientationConstraint ocm; + ocm.header.frame_id = ref_link_; + ocm.link_name = ee_link_; + ocm.orientation = pose_goal.pose.orientation; + ocm.absolute_x_axis_tolerance = angle_tol.at(0); + ocm.absolute_y_axis_tolerance = angle_tol.at(1); + ocm.absolute_z_axis_tolerance = angle_tol.at(2); + ocm.weight = 1.0; + + moveit_msgs::msg::Constraints cm; + cm.name = "orientation constraints"; + cm.orientation_constraints.emplace_back(ocm); + + move_group_->setStartStateToCurrentState(); + move_group_->setPoseTarget(pose_goal); + move_group_->setPathConstraints(cm); + + // Plan and move + moveit::planning_interface::MoveGroupInterface::Plan plan1; + const bool plan_success = (move_group_->plan(plan1) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan 1 (box constraint) %s", plan_success ? "SUCCEEDED" : "FAILED"); + + move_group_->move(); + } + + void planLineConstraints() + { + move_group_->clearPoseTargets(); + move_group_->clearPathConstraints(); + + const moveit_msgs::msg::RobotState start_state = createRobotState("ready"); + const geometry_msgs::msg::PoseStamped pose_goal = createPoseGoal(0.0, 0.0, -0.3); + const moveit_msgs::msg::PositionConstraint pcm = createLineConstraint(); + + moveit_msgs::msg::Constraints path_constraints; + + // For equality constraints set to: "use_equality_constraints" + path_constraints.name = "use_equality_constraints"; + + path_constraints.position_constraints.emplace_back(pcm); + + move_group_->setStartState(start_state); + move_group_->setPoseTarget(pose_goal); + move_group_->setPathConstraints(path_constraints); + + moveit::planning_interface::MoveGroupInterface::Plan plan3; + const bool plan_success = (move_group_->plan(plan3) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan 3 (line equality constraint) %s", plan_success ? "SUCCEEDED" : "FAILED"); + } + + void planVerticlePlaneConstraints() + { + move_group_->clearPoseTargets(); + move_group_->clearPathConstraints(); + + const moveit_msgs::msg::RobotState start_state = createRobotState("ready"); + const geometry_msgs::msg::PoseStamped pose_goal = createPoseGoal(0.1, 0.0, -0.3); + const moveit_msgs::msg::PositionConstraint pcm = verticlePlaneConstraint(); + + moveit_msgs::msg::Constraints path_constraints; + + // For equality constraints set to: "use_equality_constraints" + path_constraints.name = "use_equality_constraints"; + + path_constraints.position_constraints.emplace_back(pcm); + + addObstacle("box1"); + + move_group_->setStartState(start_state); + move_group_->setPoseTarget(pose_goal); + move_group_->setPathConstraints(path_constraints); + + moveit::planning_interface::MoveGroupInterface::Plan plan4; + const bool plan_success = (move_group_->plan(plan4) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan 4 (verticle plane equality constraint) %s", plan_success ? "SUCCEEDED" : "FAILED"); + } + + void deleteAllMarkers() + { + RCLCPP_INFO(LOGGER, "Delete all markers"); + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = ref_link_; + marker.header.stamp = node_->now(); + marker.ns = "/"; + marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_pub_->publish(marker); + marker_count_ = 0; + } + +private: + moveit_msgs::msg::RobotState createRobotState(const std::string& name) const + { + RCLCPP_INFO(LOGGER, "createRobotState"); + + const std::map joint_map = move_group_->getNamedTargetValues(name); + moveit_msgs::msg::RobotState robot_state; + robot_state.joint_state.header.frame_id = ref_link_; + robot_state.joint_state.header.stamp = node_->now(); + + for (const auto& joint : joint_map) + { + RCLCPP_INFO(LOGGER, "Starting State, joint: %s and angle %f:", joint.first.c_str(), joint.second); + robot_state.joint_state.name.emplace_back(joint.first); + robot_state.joint_state.position.emplace_back(joint.second); + } + + return robot_state; + } + + geometry_msgs::msg::PoseStamped createPoseGoal(double dx, double dy, double dz) + { + geometry_msgs::msg::PoseStamped pose = move_group_->getCurrentPose(); + + displaySphere(pose.pose, "red"); + + pose.pose.position.x += dx; + pose.pose.position.y += dy; + pose.pose.position.z += dz; + + displaySphere(pose.pose, "green"); + + return pose; + } + + moveit_msgs::msg::PositionConstraint createBoxConstraint() + { + moveit_msgs::msg::PositionConstraint pcm; + pcm.header.frame_id = ref_link_; + pcm.link_name = ee_link_; + pcm.weight = 1.0; + + shape_msgs::msg::SolidPrimitive cbox; + cbox.type = shape_msgs::msg::SolidPrimitive::BOX; + cbox.dimensions = { 0.1, 0.4, 0.4 }; + pcm.constraint_region.primitives.emplace_back(cbox); + + geometry_msgs::msg::PoseStamped pose = move_group_->getCurrentPose(); + + geometry_msgs::msg::Pose cbox_pose; + cbox_pose.position.x = pose.pose.position.x; + cbox_pose.position.y = 0.15; + cbox_pose.position.z = 0.45; + pcm.constraint_region.primitive_poses.emplace_back(cbox_pose); + + displayBox(cbox_pose, cbox.dimensions); + + return pcm; + } + + moveit_msgs::msg::PositionConstraint createPlaneConstraint() + { + moveit_msgs::msg::PositionConstraint pcm; + pcm.header.frame_id = ref_link_; + pcm.link_name = ee_link_; + pcm.weight = 1.0; + + shape_msgs::msg::SolidPrimitive cbox; + cbox.type = shape_msgs::msg::SolidPrimitive::BOX; + + // For equality constraint set box dimension to: 1e-3 > 0.0005 > 1e-4 + cbox.dimensions = { 1.0, 0.0005, 1.0 }; + pcm.constraint_region.primitives.emplace_back(cbox); + + geometry_msgs::msg::PoseStamped pose = move_group_->getCurrentPose(); + + geometry_msgs::msg::Pose cbox_pose; + cbox_pose.position = pose.pose.position; + + // turn the constraint region 45 degrees around the x-axis + tf2::Quaternion quat; + quat.setRPY(M_PI / 4.0, 0.0, 0.0); + + cbox_pose.orientation.x = quat.x(); + cbox_pose.orientation.y = quat.y(); + cbox_pose.orientation.z = quat.z(); + cbox_pose.orientation.w = quat.w(); + + pcm.constraint_region.primitive_poses.emplace_back(cbox_pose); + + displayBox(cbox_pose, cbox.dimensions); + + return pcm; + } + + moveit_msgs::msg::PositionConstraint verticlePlaneConstraint() + { + moveit_msgs::msg::PositionConstraint pcm; + pcm.header.frame_id = ref_link_; + pcm.link_name = ee_link_; + pcm.weight = 1.0; + + shape_msgs::msg::SolidPrimitive cbox; + cbox.type = shape_msgs::msg::SolidPrimitive::BOX; + + // For equality constraint set box dimension to: 1e-3 > 0.0005 > 1e-4 + cbox.dimensions = { 1.0, 0.0005, 1.0 }; + pcm.constraint_region.primitives.emplace_back(cbox); + + geometry_msgs::msg::PoseStamped pose = move_group_->getCurrentPose(); + + geometry_msgs::msg::Pose cbox_pose; + cbox_pose.position = pose.pose.position; + + pcm.constraint_region.primitive_poses.emplace_back(cbox_pose); + + displayBox(cbox_pose, cbox.dimensions); + + return pcm; + } + + moveit_msgs::msg::PositionConstraint createLineConstraint() + { + moveit_msgs::msg::PositionConstraint pcm; + pcm.header.frame_id = ref_link_; + pcm.link_name = ee_link_; + pcm.weight = 1.0; + + shape_msgs::msg::SolidPrimitive cbox; + cbox.type = shape_msgs::msg::SolidPrimitive::BOX; + + // For equality constraint set box dimension to: 1e-3 > 0.0005 > 1e-4 + cbox.dimensions = { 0.0005, 0.0005, 1.0 }; + pcm.constraint_region.primitives.emplace_back(cbox); + + geometry_msgs::msg::PoseStamped pose = move_group_->getCurrentPose(); + + geometry_msgs::msg::Pose cbox_pose; + cbox_pose.position = pose.pose.position; + + // turn the constraint region 45 degrees around the x-axis + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, 0.0); + + cbox_pose.orientation.x = quat.x(); + cbox_pose.orientation.y = quat.y(); + cbox_pose.orientation.z = quat.z(); + cbox_pose.orientation.w = quat.w(); + + pcm.constraint_region.primitive_poses.emplace_back(cbox_pose); + + displayBox(cbox_pose, cbox.dimensions); + + return pcm; + } + + void displayBox(const geometry_msgs::msg::Pose& pose, + const rosidl_runtime_cpp::BoundedVector>& dimensions) + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = ref_link_; + marker.header.stamp = node_->now(); + marker.ns = "/"; + marker.id = marker_count_; + + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration(0); + + marker.color.a = 0.5; + marker.pose = pose; + marker.scale.x = dimensions.at(0); + marker.scale.y = dimensions.at(1); + marker.scale.z = dimensions.at(2); + + marker_pub_->publish(marker); + marker_count_++; + } + + void displaySphere(const geometry_msgs::msg::Pose& pose, const std::string& color) + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = ref_link_; + marker.header.stamp = node_->now(); + marker.ns = "/"; + marker.id = marker_count_; + + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration(0); + + marker.pose = pose; + marker.scale.x = 0.05; + marker.scale.y = 0.05; + marker.scale.z = 0.05; + + if (color == "red") + { + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.color.a = 1.0; + } + else if (color == "green") + { + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.color.a = 1.0; + } + else + { + RCLCPP_ERROR(LOGGER, "Sphere color not specified"); + } + + marker_pub_->publish(marker); + marker_count_++; + } + + void addObstacle(const std::string& object_id) const + { + moveit_msgs::msg::CollisionObject collision_object; + collision_object.header.frame_id = ref_link_; + collision_object.id = object_id; + + shape_msgs::msg::SolidPrimitive primitive; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[0] = 0.2; + primitive.dimensions[1] = 0.4; + primitive.dimensions[2] = 0.1; + + geometry_msgs::msg::Pose box_pose; + box_pose.orientation.w = 1.0; + box_pose.position.x = 0.5; + box_pose.position.y = 0.0; + box_pose.position.z = 0.5; + + collision_object.primitives.emplace_back(primitive); + collision_object.primitive_poses.emplace_back(box_pose); + collision_object.operation = collision_object.ADD; + + std::vector collision_objects; + collision_objects.push_back(collision_object); + + // Now, let's add the collision object into the world + planning_scene_interface_.addCollisionObjects(collision_objects); + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr marker_pub_; + + moveit::planning_interface::MoveGroupInterfacePtr move_group_; + moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; + + std::string ref_link_, ee_link_; + unsigned int marker_count_; +}; + +int main(int argc, char** argv) +{ + RCLCPP_INFO(LOGGER, "Initialize node"); + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + // This enables loading undeclared parameters + // best practice would be to declare parameters in the corresponding classes + // and provide descriptions about expected use + node_options.automatically_declare_parameters_from_overrides(true); + + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("run_ompl_constrained_planning", node_options); + ConstrainedPlanning constrained_planning(node); + + std::thread run_demo([&constrained_planning]() { + // Wait for rviz + rclcpp::sleep_for(std::chrono::seconds(5)); + + // Note: see See moveit_core/kinematic_constraints/src/utils.cpp + // contains helper functions to automate constructing constraint messages. + + // 1. Box Constraints + constrained_planning.planBoxConstraints(); + constrained_planning.deleteAllMarkers(); + + constrained_planning.moveToStart(); + + // 2. Equality Constraints + // If you make a box really thin along one dimension, you get something plane like. + // When solving the problem, you can tell the planner to model this really thin box as an equality constraint. + // This is achieved by setting the name of the constraint to :code:`"use_equality_constraints"`. + // In addition, any dimension of the box below a treshold of :code:`0.001` will be considered an equality + // constraint. However, if we make it too small, the box will be thinner that the tolerance used by OMPL to evaluate + // constraints (:code:`1e-4` by default). MoveIt will use the stricter tolerance (the box width) to check the + // constraints, and many states will appear invalid. That's where the number :code:`0.0005` comes from, it is + // between :code:`0.00001` and :code:`0.001`. + constrained_planning.planPlaneConstraints(); + constrained_planning.deleteAllMarkers(); + + constrained_planning.moveToStart(); + + // 3. Orientation Constraints + constrained_planning.planOrientationConstraints(); + constrained_planning.deleteAllMarkers(); + + // Additional examples with line constraints and obstacles + // constrained_planning.planLineConstraints(); + // constrained_planning.planVerticlePlaneConstraints(); + }); + + rclcpp::spin(node); + run_demo.join(); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/moveit_planners/ompl/CMakeLists.txt b/moveit_planners/ompl/CMakeLists.txt index 0ec5bb36ff..0edd9029b8 100644 --- a/moveit_planners/ompl/CMakeLists.txt +++ b/moveit_planners/ompl/CMakeLists.txt @@ -7,9 +7,11 @@ moveit_package() find_package(Boost REQUIRED system filesystem date_time thread serialization) find_package(moveit_core REQUIRED) +find_package(moveit_msgs REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(rclcpp REQUIRED) find_package(pluginlib REQUIRED) +find_package(tf2_eigen REQUIRED) find_package(tf2_ros REQUIRED) find_package(ompl REQUIRED) diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index c41f5cbef4..b0cd24327d 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -6,10 +6,13 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/model_based_planning_context.cpp src/parameterization/model_based_state_space.cpp src/parameterization/model_based_state_space_factory.cpp + src/parameterization/joint_space/constrained_planning_state_space.cpp + src/parameterization/joint_space/constrained_planning_state_space_factory.cpp src/parameterization/joint_space/joint_model_state_space.cpp src/parameterization/joint_space/joint_model_state_space_factory.cpp src/parameterization/work_space/pose_model_state_space.cpp src/parameterization/work_space/pose_model_state_space_factory.cpp + src/detail/ompl_constraints.cpp src/detail/threadsafe_state_storage.cpp src/detail/state_validity_checker.cpp src/detail/projection_evaluators.cpp @@ -25,9 +28,11 @@ find_package(OpenMP REQUIRED) ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_core + moveit_msgs moveit_ros_planning rclcpp pluginlib + tf2_eigen tf2_ros OMPL Boost @@ -66,6 +71,7 @@ install(DIRECTORY include/ DESTINATION include) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(ros_testing REQUIRED) find_package(Eigen3 REQUIRED) ament_add_gtest(test_state_space test/test_state_space.cpp) @@ -73,16 +79,27 @@ if(BUILD_TESTING) target_link_libraries(test_state_space ${MOVEIT_LIB_NAME}) set_target_properties(test_state_space PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") - # TODO(henningkayser): port tests to ROS2 - # find_package(rostest REQUIRED) - # find_package(tf2_eigen REQUIRED) - # - # add_rostest_gtest(test_planning_context_manager - # test/test_planning_context_manager.test - # test/test_planning_context_manager.cpp) - # target_link_libraries(test_planning_context_manager ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES} ${tf2_eigen_LIBRARIES}) - # - # catkin_add_gtest(test_state_validity_checker test/test_state_validity_checker.cpp) - # target_link_libraries(test_state_validity_checker ${MOVEIT_LIB_NAME} ${OMPL_LIBRARIES} ${catkin_LIBRARIES}) - # set_target_properties(test_state_validity_checker PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + ament_add_gtest(test_state_validity_checker test/test_state_validity_checker.cpp) + ament_target_dependencies(test_state_validity_checker moveit_core OMPL Boost Eigen3) + target_link_libraries(test_state_validity_checker ${MOVEIT_LIB_NAME}) + set_target_properties(test_state_validity_checker PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + + ament_add_gtest(test_planning_context_manager test/test_planning_context_manager.cpp) + ament_target_dependencies(test_planning_context_manager moveit_core tf2_eigen OMPL Boost Eigen3) + target_link_libraries(test_planning_context_manager ${MOVEIT_LIB_NAME}) + + ament_add_gtest(test_ompl_constraints test/test_ompl_constraints.cpp) + ament_target_dependencies(test_ompl_constraints moveit_core OMPL Boost Eigen3) + target_link_libraries(test_ompl_constraints ${MOVEIT_LIB_NAME}) + + ament_add_gtest(test_constrained_planning_state_space test/test_constrained_planning_state_space.cpp) + ament_target_dependencies(test_constrained_planning_state_space moveit_core OMPL Boost Eigen3) + target_link_libraries(test_constrained_planning_state_space ${MOVEIT_LIB_NAME}) + set_target_properties(test_constrained_planning_state_space PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + + ament_add_gtest(test_constrained_state_validity_checker test/test_constrained_state_validity_checker.cpp) + ament_target_dependencies(test_constrained_state_validity_checker moveit_core OMPL Boost Eigen3) + target_link_libraries(test_constrained_state_validity_checker ${MOVEIT_LIB_NAME}) + set_target_properties(test_constrained_state_validity_checker PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + endif() diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h new file mode 100644 index 0000000000..4da21a921c --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h @@ -0,0 +1,337 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of KU Leuven nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer, Boston Cleek */ + +#pragma once + +#include + +#include + +#include +#include +#include +#include + +namespace ompl_interface +{ +MOVEIT_CLASS_FORWARD(BaseConstraint); +MOVEIT_CLASS_FORWARD(BoxConstraint); +MOVEIT_CLASS_FORWARD(OrientationConstraint); + +/** \brief Represents upper and lower bound on the elements of a vector. + * + * OMPL ConstrainedStateSpace requires a model of the constraints given as: + * f1(joint_values) = 0 + * f2(joint_values) = 0 + * f3(joint_values) = 0 + * ... + * + * So we use a penalty function to convert bounds to an equality constraint. + * If you do need equality constraint, you can represent them by setting the upper bound and lower bound almost equal. + * Or you can use the EqualityPositionConstraint version by setting the name of the constraint to + * "use_equality_constraints". But the latter will ignore bounds on other dimensions. + * **/ +class Bounds +{ +public: + Bounds(); + Bounds(const std::vector& lower, const std::vector& upper); + /** \brief Distance to region inside bounds + * + * Distance of a given value outside the bounds, zero inside the bounds. + * Creates a penalty function that looks like this: + * + * (penalty) ^ + * | \ / + * | \ / + * | \_____/ + * |----------------> (variable to be constrained) + * */ + Eigen::VectorXd penalty(const Eigen::Ref& x) const; + + /** \brief Derivative of the penalty function + * ^ + * | + * | -1-1-1 0 0 0 +1+1+1 + * |------------------------> + * **/ + Eigen::VectorXd derivative(const Eigen::Ref& x) const; + + std::size_t size() const; + +private: + std::vector lower_, upper_; + std::size_t size_; + + friend std::ostream& operator<<(std::ostream& os, const ompl_interface::Bounds& bounds); +}; + +/** \brief Pretty printing of bounds. **/ +std::ostream& operator<<(std::ostream& os, const ompl_interface::Bounds& bounds); + +/**************************** + * Base class for constraints + * **************************/ +/** \brief Abstract base class for different types of constraints, implementations of ompl::base::Constraint + * + * To create a constrained state space in OMPL, we need a model of the constraints, that can be written in the form of + * equality constraints F(joint_values) = 0. This class uses `Bounds` defined above, to convert: + * + * lower_bound < scalar value < upper bound + * + * into an equation of the form f(x) = 0. + * + * The 'scalar value' can be the difference between the position or orientation of a link and a target position or + * orientation, or any other error metric that can be calculated using the `moveit::core::RobotModel` and + * `moveit::core::JointModelGroup`. + * */ +class BaseConstraint : public ompl::base::Constraint +{ +public: + /** \brief Construct a BaseConstraint using 3 `num_cons` by default because all constraints currently implemented have + * 3 constraint equations. **/ + BaseConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, + const unsigned int num_dofs, const unsigned int num_cons = 3); + + /** \brief Initialize constraint based on message content. + * + * This is necessary because we cannot call the pure virtual + * parseConstraintsMsg method from the constructor of this class. + * */ + void init(const moveit_msgs::msg::Constraints& constraints); + + /** OMPL's main constraint evaluation function. + * + * OMPL requires you to override at least "function" which represents the constraint F(q) = 0 + * */ + void function(const Eigen::Ref& joint_values, Eigen::Ref out) const override; + + /** \brief Jacobian of the constraint function. + * + * Optionally you can also provide dF(q)/dq, the Jacobian of the constraint. + * + * */ + void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const override; + + /** \brief Wrapper for forward kinematics calculated by MoveIt's Robot State. + * + * TODO(jeroendm) Are these actually const, as the robot state is modified? How come it works? + * Also, output arguments could be significantly more performant, + * but MoveIt's robot state does not support passing Eigen::Ref objects at the moment. + * */ + Eigen::Isometry3d forwardKinematics(const Eigen::Ref& joint_values) const; + + /** \brief Calculate the robot's geometric Jacobian using MoveIt's Robot State. + * + * Ideally I would pass the output argument from OMPL's jacobian function directly, + * but I cannot pass an object of type , Eigen::Ref to MoveIt's + * Jacobian method. + * */ + Eigen::MatrixXd robotGeometricJacobian(const Eigen::Ref& joint_values) const; + + /** \brief Parse bounds on position and orientation parameters from MoveIt's constraint message. + * + * This can be non-trivial given the often complex structure of these messages. + * */ + virtual void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) = 0; + + /** \brief For inequality constraints: calculate the value of the parameter that is being constrained by the bounds. + * + * In this Position constraints case, it calculates the x, y and z position + * of the end-effector. This error is then converted in generic equality constraints in the implementation of + * `ompl_interface::BaseConstraint::function`. + * + * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::function directly and ignore + * the bounds calculation. + * */ + virtual Eigen::VectorXd calcError(const Eigen::Ref& /*x*/) const; + + /** \brief For inequality constraints: calculate the Jacobian for the current parameters that are being constrained. + * * + * This error jacobian, as the name suggests, is only the jacobian of the position / orientation / ... error. + * It does not take into account the derivative of the penalty functions defined in the Bounds class. + * This correction is added in the implementation of of BaseConstraint::jacobian. + * + * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::jacobian directly and ignore + * the bounds calculation. + * + * TODO(jeroendm), Maybe also use an output argument as in `ompl::base::Constraint::jacobian(x, out)` for better + * performance? + * */ + virtual Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref& /*x*/) const; + + // the methods below are specifically for debugging and testing + + const std::string& getLinkName() + { + return link_name_; + } + + const Eigen::Vector3d getTargetPosition() + { + return target_position_; + } + + const Eigen::Quaterniond getTargetOrientation() + { + return target_orientation_; + } + +protected: + /** \brief Thread-safe storage of the robot state. + * + * The robot state is modified for kinematic calculations. As an instance of this class is possibly used in multiple + * threads due to OMPL's LazyGoalSampler, we need a separate robot state in every thread. + * */ + TSStateStorage state_storage_; + const moveit::core::JointModelGroup* joint_model_group_; + + // all attributes below can be considered const as soon as the constraint message is parsed + // but I (jeroendm) do not know how to elegantly express this in C++ + // parsing the constraints message and passing all this data members separately to the constructor + // is a solution, but it adds complexity outside this class, which is also not ideal. + + /** \brief Robot link the constraints are applied to. */ + std::string link_name_; + + /** \brief Upper and lower bounds on constrained variables. */ + Bounds bounds_; + + /** \brief target for equality constraints, nominal value for inequality constraints. */ + Eigen::Vector3d target_position_; + + /** \brief target for equality constraints, nominal value for inequality constraints. */ + Eigen::Quaterniond target_orientation_; + +public: + // Macro for classes containing fixed size eigen vectors that are dynamically allocated when used. + // https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/****************************************** + * Box constraint + * ****************************************/ +/** \brief Box shaped position constraints + * + * Reads bounds on x, y and z position from a position constraint + * at constraint_region.primitives[0].dimensions. + * Where the primitive has to be of type `shape_msgs/SolidPrimitive.BOX`. + * + * These bounds are applied around the nominal position and orientation + * of the box. + * */ +class BoxConstraint : public BaseConstraint +{ +public: + BoxConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, + const unsigned int num_dofs); + + void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) override; + + Eigen::VectorXd calcError(const Eigen::Ref& x) const override; + + Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref& x) const override; +}; + +/****************************************** + * Equality Position Constraints + * ****************************************/ +/** \brief Equality constraints on a link's position. + * + * When you set the name of a constraint to 'use_equality_constraints', all constraints with a dimension lower than + * `EQUALITY_CONSTRAINT_THRESHOLD` will be modelled as equality constraints. + * + * The dimension value for the others are ignored. For example, a box with dimensions [1.0, 1e-5, 1.0] + * will result in equality constraints on the y-position, and no constraints on the x or z-position. + * + * TODO(jeroendm) We could make this a base class `EqualityConstraints` with a specialization for position and orientation + * constraints in the future. But the direct overriding of `function` and `jacobian` is probably more performant. + * */ +class EqualityPositionConstraint : public BaseConstraint +{ +public: + EqualityPositionConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, + const unsigned int num_dofs); + + void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) override; + + void function(const Eigen::Ref& joint_values, Eigen::Ref out) const override; + + void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const override; + +private: + /** \brief Position bounds under this threshold are interpreted as equality constraints, the others as unbounded. + * + * WARNING: Below follows the explanation of an ugly hack. Ideally, the user could specify equality constraints by + * setting the constraint dimension to zero. However, this would result in a constraint region primitive with a zero + * dimension in MoveIt, which the planner can (almost) never satisfy. Therefore we use a threshold value, below which + * the constraint is interpreted as an equality constraint. This threshold value is not used in the planning algorithm itself! + * + * This threshold value should be larger than the tolerance of the constraints specified in OMPL + * (ompl::magic::CONSTRAINT_PROJECTION_TOLERANCE = 1e-4). + * + * This is necessary because the constraints are also checked by MoveIt in the StateValidity checker. If this check + * would use a stricter tolerance than was used to satisfy the constraints in OMPL, all states would be invalid. + * Therefore the dimension of an equality constraint specified in the constraint message should be at least equal the + * the tolerance used by OMPL to project onto the constraints. To avoid checking for exact equality on floats, the + * threshold is made a bit larger. + * + * EQUALITY_CONSTRAINT_THRESHOLD > tolerance in constraint message > OMPL projection tolerance + * + * That's why the value is 1e-3 > 1e-4. + * Now the user can specify any value between 1e-3 and 1e-4 to specify an equality constraint. + * **/ + static constexpr double EQUALITY_CONSTRAINT_THRESHOLD{ 0.001 }; + + /** \brief Bool vector indicating which dimensions are constrained. **/ + std::vector is_dim_constrained_; +}; + +/** \brief Extract position constraints from the MoveIt message. + * + * Assumes there is a single primitive of type `shape_msgs/SolidPrimitive.BOX`. + * The dimensions of the box are the bounds on the deviation of the link origin from + * the target pose, given in constraint_regions.primitive_poses[0]. + * */ +Bounds positionConstraintMsgToBoundVector(const moveit_msgs::msg::PositionConstraint& pos_con); + +/** \brief Factory to create constraints based on what is in the MoveIt constraint message. **/ +std::shared_ptr createOMPLConstraint(const moveit::core::RobotModelConstPtr& robot_model, + const std::string& group, + const moveit_msgs::msg::Constraints& constraints); + +} // namespace ompl_interface \ No newline at end of file diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h index 54259e9d8e..193e851b40 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h @@ -32,7 +32,18 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Jeroen De Maeyer */ + +/** A state validity checker checks: + * + * - Bounds (joint limits). + * - Collision. + * - Kinematic path constraints. + * - Generic user-specified feasibility using the `isStateFeasible` of the planning scene. + * + * IMPORTANT: Although the isValid method takes the state as `const ompl::base::State* state`, + * it uses const_cast to modify the validity of the state with `markInvalid` and `markValid` for caching. + * **/ #pragma once @@ -61,8 +72,8 @@ class StateValidityChecker : public ompl::base::StateValidityChecker return isValid(state, dist, verbose_); } - bool isValid(const ompl::base::State* state, bool verbose) const; - bool isValid(const ompl::base::State* state, double& dist, bool verbose) const; + virtual bool isValid(const ompl::base::State* state, bool verbose) const; + virtual bool isValid(const ompl::base::State* state, double& dist, bool verbose) const; virtual double cost(const ompl::base::State* state) const; double clearance(const ompl::base::State* state) const override; @@ -81,4 +92,60 @@ class StateValidityChecker : public ompl::base::StateValidityChecker collision_detection::CollisionRequest collision_request_with_cost_; bool verbose_; }; -} // namespace ompl_interface + +/** \brief A StateValidityChecker that can handle states of type `ompl::base::ConstraintStateSpace::StateType`. + * + * We cannot not just cast the states and pass them to the isValid version of the parent class, because inside the + * state-validity checker, the line: + * + * planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); + * + * requires the state type to be of the constrained state space, while: + * + * state->as()->isValidityKnown() + * + * requires accessing the underlying state by calling `getState()` on the constrained state space state. + * Therefore this class implements specific versions of the isValid methods. + * + * We still check the path constraints, because not all states sampled by the constrained state space + * satisfy the constraints unfortunately. This is a complicated issue. For more details see: + * https://github.com/ros-planning/moveit/issues/2092#issuecomment-669911722. + **/ +class ConstrainedPlanningStateValidityChecker : public StateValidityChecker +{ +public: + using StateValidityChecker::isValid; + +public: + ConstrainedPlanningStateValidityChecker(const ModelBasedPlanningContext* planning_context) + : StateValidityChecker(planning_context) + { + } + + /** \brief Check validity for states of type ompl::base::ConstrainedStateSpace + * + * This state type is special in that it "wraps" around a normal state, + * which can be accessed by the getState() method. In this class we assume that this state, + * is of type `ompl_interface::ConstrainedPlanningStateSpace`, which inherits from + * `ompl_interface::ModelBasedStateSpace`. + * + * (For the actual implementation of this, look at the ompl::base::WrapperStateSpace.) + * + * Code sample that can be used to check all the assumptions: + * + * #include + * #include + * + * // the code below should be pasted at the top of the isValid method + * auto css = dynamic_cast(wrapped_state); + * assert(css != nullptr); + * auto cpss = dynamic_cast(planning_context_->getOMPLStateSpace().get()); + * assert(cpss != nullptr); + * auto cssi = dynamic_cast(si_); + * assert(cssi != nullptr); + * + **/ + bool isValid(const ompl::base::State* wrapped_state, bool verbose) const override; + bool isValid(const ompl::base::State* wrapped_state, double& dist, bool verbose) const override; +}; +} // namespace ompl_interface \ No newline at end of file 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 c94a34fd5d..574d779d21 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 @@ -45,6 +45,7 @@ #include #include #include +#include namespace ompl_interface { @@ -69,6 +70,19 @@ struct ModelBasedPlanningContextSpecification ModelBasedStateSpacePtr state_space_; og::SimpleSetupPtr ompl_simple_setup_; // pass in the correct simple setup type + + /** \brief OMPL constrained state space to handle path constraints. + * + * When the parameter "use_ompl_constrained_planning" is set to true in ompl_planning.yaml, + * the path constraints are handled by this state space. + * + * **Important**: because code often depents on the attribute `state_space_` to copy states from MoveIt to OMPL, we + * must set `state_space_` to have type `ompl_interface::ConstrainedPlanningStateSpace`. The actual planning does + * not happen with this `state_space_`, but it is used to create the `constrained_state_space_` of type + * `ompl::base::ConstrainedStateSpace`. The latter is the one passed to OMPL simple setup (after creating a + * ConstrainedSpaceInformation object from it). + * */ + ob::ConstrainedStateSpacePtr constrained_state_space_; }; class ModelBasedPlanningContext : public planning_interface::PlanningContext diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.h new file mode 100644 index 0000000000..d00b2ec249 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.h @@ -0,0 +1,81 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of KU Leuven nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ + +#pragma once + +#include +#include + +namespace ompl_interface +{ +MOVEIT_CLASS_FORWARD(ConstrainedPlanningStateSpace); + +/** \brief State space to be used in combination with OMPL's constrained planning functionality. + * + * The state space is called ConstrainedPlanningStateSpace, as in "use this state space for constrained planning". + * Not to be confused with the `ompl::base::ConstrainedStateSpace`, which is constrained in the sense that it generates + * states that satisfy the constraints. This state space does not! OMPL's `ompl::base::ConstrainedStateSpace` will wrap + *around ConstrainedPlanningStateSpace. + * + * This class overrides the virtual methods to copy states from OMPL to MoveIt and vice-versa. It is used as state space + * in the `ModelBasedPlanningContextSpecification` and can handle general `ompl::base::State*` pointers that need to be + * casted to a `ompl::Base::ConstrainedStateSpace::StateType`, instead of the 'normal' + * `ompl_interface::ModelBasedStateSpace::StateType`. This casting looks like this: + * + * ompl_state->as()->getState()->as() + * + * where `getState()` is used to acces the underlying state space, which should be an instance of this class. + **/ +class ConstrainedPlanningStateSpace : public ModelBasedStateSpace +{ +public: + static const std::string PARAMETERIZATION_TYPE; + + ConstrainedPlanningStateSpace(const ModelBasedStateSpaceSpecification& spec); + + const std::string& getParameterizationType() const override + { + return PARAMETERIZATION_TYPE; + } + + // override copy operations between OMPL and ROS, because a constrained state has a different internal structure + double* getValueAddressAtIndex(ompl::base::State* ompl_state, const unsigned int index) const override; + void copyToRobotState(moveit::core::RobotState& robot_state, const ompl::base::State* ompl_state) const override; + void copyToOMPLState(ompl::base::State* ompl_state, const moveit::core::RobotState& robot_state) const override; + void copyJointToOMPLState(ompl::base::State* ompl_state, const moveit::core::RobotState& robot_state, + const moveit::core::JointModel* joint_model, int ompl_state_joint_index) const override; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h new file mode 100644 index 0000000000..e7780d763b --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h @@ -0,0 +1,65 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of KU Leuven nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ +/* Mostly copied from Ioan Sucan's code */ + +#pragma once + +#include + +namespace ompl_interface +{ +class ConstrainedPlanningStateSpaceFactory : public ModelBasedStateSpaceFactory +{ +public: + ConstrainedPlanningStateSpaceFactory(); + + /** \brief Return a priority that this planner should be used for this specific planning problem. + * + * This state space factory is currently only used if `use_ompl_constrained_state_space` was set to `true` in + * ompl_planning.yaml. In that case it is the only factory to choose from, so the priority does not matter. + * It returns a low priority so it will never be choosen when others are available. + * (The second lowest priority is -1 in the PoseModelStateSpaceFactory.) + * + * For more details on this state space selection process, see: + * https://github.com/JeroenDM/moveit/pull/2 + * **/ + int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, + const moveit::core::RobotModelConstPtr& robot_model) const override; + +protected: + ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp new file mode 100644 index 0000000000..5fea0752f4 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -0,0 +1,388 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of KU Leuven nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer, Boston Cleek */ + +#include +#include + +#include + +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_planners_ompl.ompl_constraints"); + +namespace ompl_interface +{ +Bounds::Bounds() : size_(0) +{ +} + +Bounds::Bounds(const std::vector& lower, const std::vector& upper) + : lower_(lower), upper_(upper), size_(lower.size()) +{ + // how to report this in release mode?? + assert(lower_.size() == upper_.size()); +} + +Eigen::VectorXd Bounds::penalty(const Eigen::Ref& x) const +{ + assert((long)lower_.size() == x.size()); + Eigen::VectorXd penalty(x.size()); + + for (unsigned int i = 0; i < x.size(); i++) + { + if (x[i] < lower_.at(i)) + { + penalty[i] = lower_.at(i) - x[i]; + } + else if (x[i] > upper_.at(i)) + { + penalty[i] = x[i] - upper_.at(i); + } + else + { + penalty[i] = 0.0; + } + } + return penalty; +} + +Eigen::VectorXd Bounds::derivative(const Eigen::Ref& x) const +{ + assert((long)lower_.size() == x.size()); + Eigen::VectorXd derivative(x.size()); + + for (unsigned int i = 0; i < x.size(); i++) + { + if (x[i] < lower_.at(i)) + { + derivative[i] = -1.0; + } + else if (x[i] > upper_.at(i)) + { + derivative[i] = 1.0; + } + else + { + derivative[i] = 0.0; + } + } + return derivative; +} + +std::size_t Bounds::size() const +{ + return size_; +} + +std::ostream& operator<<(std::ostream& os, const ompl_interface::Bounds& bounds) +{ + os << "Bounds:\n"; + for (std::size_t i{ 0 }; i < bounds.size(); ++i) + { + os << "( " << bounds.lower_[i] << ", " << bounds.upper_[i] << " )\n"; + } + return os; +} + +/**************************** + * Base class for constraints + * **************************/ +BaseConstraint::BaseConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, + const unsigned int num_dofs, const unsigned int num_cons_) + : ompl::base::Constraint(num_dofs, num_cons_) + , state_storage_(robot_model) + , joint_model_group_(robot_model->getJointModelGroup(group)) + +{ +} + +void BaseConstraint::init(const moveit_msgs::msg::Constraints& constraints) +{ + parseConstraintMsg(constraints); +} + +void BaseConstraint::function(const Eigen::Ref& joint_values, + Eigen::Ref out) const +{ + const Eigen::VectorXd current_values = calcError(joint_values); + out = bounds_.penalty(current_values); +} + +void BaseConstraint::jacobian(const Eigen::Ref& joint_values, + Eigen::Ref out) const +{ + const Eigen::VectorXd constraint_error = calcError(joint_values); + const Eigen::VectorXd constraint_derivative = bounds_.derivative(constraint_error); + const Eigen::MatrixXd robot_jacobian = calcErrorJacobian(joint_values); + for (std::size_t i = 0; i < bounds_.size(); i++) + { + out.row(i) = constraint_derivative[i] * robot_jacobian.row(i); + } +} + +Eigen::Isometry3d BaseConstraint::forwardKinematics(const Eigen::Ref& joint_values) const +{ + moveit::core::RobotState* robot_state = state_storage_.getStateStorage(); + robot_state->setJointGroupPositions(joint_model_group_, joint_values); + return robot_state->getGlobalLinkTransform(link_name_); +} + +Eigen::MatrixXd BaseConstraint::robotGeometricJacobian(const Eigen::Ref& joint_values) const +{ + moveit::core::RobotState* robot_state = state_storage_.getStateStorage(); + robot_state->setJointGroupPositions(joint_model_group_, joint_values); + Eigen::MatrixXd jacobian; + // return value (success) not used, could return a garbage jacobian. + robot_state->getJacobian(joint_model_group_, joint_model_group_->getLinkModel(link_name_), + Eigen::Vector3d(0.0, 0.0, 0.0), jacobian); + return jacobian; +} + +Eigen::VectorXd BaseConstraint::calcError(const Eigen::Ref& /*x*/) const +{ + RCLCPP_WARN_STREAM(LOGGER, + "BaseConstraint: Constraint method calcError was not overridden, so it should not be used."); + return Eigen::VectorXd::Zero(getCoDimension()); +} + +Eigen::MatrixXd BaseConstraint::calcErrorJacobian(const Eigen::Ref& /*x*/) const +{ + RCLCPP_WARN_STREAM( + LOGGER, "BaseConstraint: Constraint method calcErrorJacobian was not overridden, so it should not be used."); + return Eigen::MatrixXd::Zero(getCoDimension(), n_); +} + +/****************************************** + * Position constraints + * ****************************************/ +BoxConstraint::BoxConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, + const unsigned int num_dofs) + : BaseConstraint(robot_model, group, num_dofs) +{ +} + +void BoxConstraint::parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) +{ + RCLCPP_DEBUG(LOGGER, "Parsing box position constraint for OMPL constrained state space."); + assert(bounds_.size() == 0); + bounds_ = positionConstraintMsgToBoundVector(constraints.position_constraints.at(0)); + RCLCPP_DEBUG(LOGGER, "Parsed Box constraints"); + RCLCPP_DEBUG_STREAM(LOGGER, "Bounds: " << bounds_); + + // extract target position and orientation + geometry_msgs::msg::Point position = + constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position; + + target_position_ << position.x, position.y, position.z; + + tf2::fromMsg(constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).orientation, + target_orientation_); + + link_name_ = constraints.position_constraints.at(0).link_name; + RCLCPP_DEBUG_STREAM(LOGGER, "Position constraints applied to link: " << link_name_); +} + +Eigen::VectorXd BoxConstraint::calcError(const Eigen::Ref& x) const +{ + return target_orientation_.matrix().transpose() * (forwardKinematics(x).translation() - target_position_); +} + +Eigen::MatrixXd BoxConstraint::calcErrorJacobian(const Eigen::Ref& x) const +{ + return target_orientation_.matrix().transpose() * robotGeometricJacobian(x).topRows(3); +} + +/****************************************** + * Equality constraints + * ****************************************/ +EqualityPositionConstraint::EqualityPositionConstraint(const moveit::core::RobotModelConstPtr& robot_model, + const std::string& group, const unsigned int num_dofs) + : BaseConstraint(robot_model, group, num_dofs) +{ +} + +void EqualityPositionConstraint::parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) +{ + const auto dims = constraints.position_constraints.at(0).constraint_region.primitives.at(0).dimensions; + + is_dim_constrained_ = { false, false, false }; + for (std::size_t i = 0; i < dims.size(); i++) + { + if (dims.at(i) < EQUALITY_CONSTRAINT_THRESHOLD) + { + if (dims.at(i) < getTolerance()) + { + RCLCPP_ERROR_STREAM( + LOGGER, + "Dimension: " << i + << " of position constraint is smaller than the tolerance used to evaluate the constraints. " + "This will make all states invalid and planning will fail. Please use a value between: " + << getTolerance() << " and " << EQUALITY_CONSTRAINT_THRESHOLD); + } + + is_dim_constrained_.at(i) = true; + } + } + + // extract target position and orientation + geometry_msgs::msg::Point position = + constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position; + + target_position_ << position.x, position.y, position.z; + + tf2::fromMsg(constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).orientation, + target_orientation_); + + RCLCPP_DEBUG_STREAM(LOGGER, "Equality constraint on x-position? " << (is_dim_constrained_[0] ? "yes" : "no")); + RCLCPP_DEBUG_STREAM(LOGGER, "Equality constraint on y-position? " << (is_dim_constrained_[1] ? "yes" : "no")); + RCLCPP_DEBUG_STREAM(LOGGER, "Equality constraint on z-position? " << (is_dim_constrained_[2] ? "yes" : "no")); + + link_name_ = constraints.position_constraints.at(0).link_name; + RCLCPP_DEBUG_STREAM(LOGGER, "Position constraints applied to link: " << link_name_); +} + +void EqualityPositionConstraint::function(const Eigen::Ref& joint_values, + Eigen::Ref out) const +{ + Eigen::Vector3d error = + target_orientation_.matrix().transpose() * (forwardKinematics(joint_values).translation() - target_position_); + for (std::size_t dim = 0; dim < 3; dim++) + { + if (is_dim_constrained_.at(dim)) + { + out[dim] = error[dim]; // equality constraint dimension + } + else + { + out[dim] = 0.0; // unbounded dimension + } + } +} + +void EqualityPositionConstraint::jacobian(const Eigen::Ref& joint_values, + Eigen::Ref out) const +{ + out.setZero(); + Eigen::MatrixXd jac = target_orientation_.matrix().transpose() * robotGeometricJacobian(joint_values).topRows(3); + for (std::size_t dim = 0; dim < 3; dim++) + { + if (is_dim_constrained_.at(dim)) + { + out.row(dim) = jac.row(dim); // equality constraint dimension + } + } +} + +/************************************ + * MoveIt constraint message parsing + * **********************************/ +Bounds positionConstraintMsgToBoundVector(const moveit_msgs::msg::PositionConstraint& pos_con) +{ + auto dims = pos_con.constraint_region.primitives.at(0).dimensions; + + // dimension of -1 signifies unconstrained parameter, so set to infinity + for (auto& dim : dims) + { + if (dim == -1) + { + dim = std::numeric_limits::infinity(); + } + } + + return { { -dims.at(0) / 2.0, -dims.at(1) / 2.0, -dims.at(2) / 2.0 }, + { dims.at(0) / 2.0, dims.at(1) / 2.0, dims.at(2) / 2.0 } }; +} + +/****************************************** + * OMPL Constraints Factory + * ****************************************/ +std::shared_ptr createOMPLConstraint(const moveit::core::RobotModelConstPtr& robot_model, + const std::string& group, + const moveit_msgs::msg::Constraints& constraints) +{ + // TODO(bostoncleek): does this reach the end w/o a return ? + + const std::size_t num_dofs = robot_model->getJointModelGroup(group)->getVariableCount(); + const std::size_t num_pos_con = constraints.position_constraints.size(); + const std::size_t num_ori_con = constraints.orientation_constraints.size(); + + // This factory method contains template code to support different constraints, but only position constraints are + // currently supported. The other options return a nullptr for now and should not be used. + + if (num_pos_con > 1) + { + RCLCPP_WARN(LOGGER, "Only a single position constraints supported. Using the first one."); + } + if (num_ori_con > 1) + { + RCLCPP_WARN(LOGGER, "Only a single orientation constraints supported. Using the first one."); + } + + if (num_pos_con > 0 && num_ori_con > 0) + { + RCLCPP_ERROR(LOGGER, "Combining position and orientation constraints not implemented yet for OMPL's constrained " + "state space."); + return nullptr; + } + else if (num_pos_con > 0) + { + RCLCPP_DEBUG_STREAM(LOGGER, "Constraint name: " << constraints.name); + BaseConstraintPtr pos_con; + if (constraints.name == "use_equality_constraints") + { + RCLCPP_DEBUG(LOGGER, "OMPL is using equality position constraints."); + pos_con = std::make_shared(robot_model, group, num_dofs); + } + else + { + RCLCPP_DEBUG(LOGGER, "OMPL is using box position constraints."); + pos_con = std::make_shared(robot_model, group, num_dofs); + } + pos_con->init(constraints); + return pos_con; + } + else if (num_ori_con > 0) + { + RCLCPP_ERROR(LOGGER, "Orientation constraints are not yet supported."); + return nullptr; + } + else + { + RCLCPP_ERROR(LOGGER, "No path constraints found in planning request."); + return nullptr; + } +} +} // namespace ompl_interface \ No newline at end of file diff --git a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp index cc9177f96f..11076c1f05 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp @@ -32,17 +32,18 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Jeroen De Maeyer */ #include #include #include #include +#include + namespace ompl_interface { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.state_validity_checker"); -} // namespace ompl_interface ompl_interface::StateValidityChecker::StateValidityChecker(const ModelBasedPlanningContext* pc) : ompl::base::StateValidityChecker(pc->getOMPLSimpleSetup()->getSpaceInformation()) @@ -73,16 +74,21 @@ void ompl_interface::StateValidityChecker::setVerbose(bool flag) verbose_ = flag; } -bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, bool verbose) const +bool StateValidityChecker::isValid(const ompl::base::State* state, bool verbose) const { + assert(state != nullptr); // Use cached validity if it is available if (state->as()->isValidityKnown()) + { return state->as()->isMarkedValid(); + } if (!si_->satisfiesBounds(state)) { if (verbose) + { RCLCPP_INFO(LOGGER, "State outside bounds"); + } const_cast(state)->as()->markInvalid(); return false; } @@ -120,8 +126,9 @@ bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* stat return !res.collision; } -bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, double& dist, bool verbose) const +bool StateValidityChecker::isValid(const ompl::base::State* state, double& dist, bool verbose) const { + assert(state != nullptr); // Use cached validity and distance if they are available if (state->as()->isValidityKnown() && state->as()->isGoalDistanceKnown()) @@ -133,7 +140,9 @@ bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* stat if (!si_->satisfiesBounds(state)) { if (verbose) + { RCLCPP_INFO(LOGGER, "State outside bounds"); + } const_cast(state)->as()->markInvalid(0.0); return false; } @@ -169,8 +178,9 @@ bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* stat return !res.collision; } -double ompl_interface::StateValidityChecker::cost(const ompl::base::State* state) const +double StateValidityChecker::cost(const ompl::base::State* state) const { + assert(state != nullptr); double cost = 0.0; moveit::core::RobotState* robot_state = tss_.getStateStorage(); @@ -181,13 +191,16 @@ double ompl_interface::StateValidityChecker::cost(const ompl::base::State* state planning_context_->getPlanningScene()->checkCollision(collision_request_with_cost_, res, *robot_state); for (const collision_detection::CostSource& cost_source : res.cost_sources) + { cost += cost_source.cost * cost_source.getVolume(); + } return cost; } -double ompl_interface::StateValidityChecker::clearance(const ompl::base::State* state) const +double StateValidityChecker::clearance(const ompl::base::State* state) const { + assert(state != nullptr); moveit::core::RobotState* robot_state = tss_.getStateStorage(); planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); @@ -195,3 +208,112 @@ double ompl_interface::StateValidityChecker::clearance(const ompl::base::State* planning_context_->getPlanningScene()->checkCollision(collision_request_with_distance_, res, *robot_state); return res.collision ? 0.0 : (res.distance < 0.0 ? std::numeric_limits::infinity() : res.distance); } + +/******************************************* + * Constrained Planning StateValidityChecker + * *****************************************/ +bool ConstrainedPlanningStateValidityChecker::isValid(const ompl::base::State* wrapped_state, bool verbose) const +{ + assert(wrapped_state != nullptr); + // Unwrap the state from a ConstrainedStateSpace::StateType + auto state = wrapped_state->as()->getState(); + + // Use cached validity if it is available + if (state->as()->isValidityKnown()) + { + return state->as()->isMarkedValid(); + } + + // do not use the unwrapped state here, as satisfiesBounds expects a state of type ConstrainedStateSpace::StateType + if (!si_->satisfiesBounds(wrapped_state)) // si_ = ompl::base::SpaceInformation + { + RCLCPP_DEBUG(LOGGER, "State outside bounds"); + const_cast(state)->as()->markInvalid(); + return false; + } + + moveit::core::RobotState* robot_state = tss_.getStateStorage(); + // do not use the unwrapped state here, as copyToRobotState expects a state of type ConstrainedStateSpace::StateType + planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, wrapped_state); + + // check path constraints + const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints(); + if (kset && !kset->decide(*robot_state, verbose).satisfied) + { + const_cast(state)->as()->markInvalid(); + return false; + } + + // check feasibility + if (!planning_context_->getPlanningScene()->isStateFeasible(*robot_state, verbose)) + { + const_cast(state)->as()->markInvalid(); + return false; + } + + // check collision avoidance + collision_detection::CollisionResult res; + planning_context_->getPlanningScene()->checkCollision( + verbose ? collision_request_simple_verbose_ : collision_request_simple_, res, *robot_state); + if (!res.collision) + { + const_cast(state)->as()->markValid(); + } + else + { + const_cast(state)->as()->markInvalid(); + } + return !res.collision; +} + +bool ConstrainedPlanningStateValidityChecker::isValid(const ompl::base::State* wrapped_state, double& dist, + bool verbose) const +{ + assert(wrapped_state != nullptr); + // Unwrap the state from a ConstrainedStateSpace::StateType + auto state = wrapped_state->as()->getState(); + + // Use cached validity and distance if they are available + if (state->as()->isValidityKnown() && + state->as()->isGoalDistanceKnown()) + { + dist = state->as()->distance; + return state->as()->isMarkedValid(); + } + + // do not use the unwrapped state here, as satisfiesBounds expects a state of type ConstrainedStateSpace::StateType + if (!si_->satisfiesBounds(wrapped_state)) // si_ = ompl::base::SpaceInformation + { + RCLCPP_DEBUG(LOGGER, "State outside bounds"); + const_cast(state)->as()->markInvalid(0.0); + return false; + } + + moveit::core::RobotState* robot_state = tss_.getStateStorage(); + + // do not use the unwrapped state here, as copyToRobotState expects a state of type ConstrainedStateSpace::StateType + planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, wrapped_state); + + // check path constraints + const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints(); + if (kset && !kset->decide(*robot_state, verbose).satisfied) + { + const_cast(state)->as()->markInvalid(); + return false; + } + + // check feasibility + if (!planning_context_->getPlanningScene()->isStateFeasible(*robot_state, verbose)) + { + dist = 0.0; + return false; + } + + // check collision avoidance + collision_detection::CollisionResult res; + planning_context_->getPlanningScene()->checkCollision( + verbose ? collision_request_with_distance_verbose_ : collision_request_with_distance_, res, *robot_state); + dist = res.distance; + return !res.collision; +} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp b/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp index e0cfa8245e..655f9e1dff 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp @@ -49,7 +49,9 @@ ompl_interface::TSStateStorage::TSStateStorage(const moveit::core::RobotState& s ompl_interface::TSStateStorage::~TSStateStorage() { for (auto& thread_state : thread_states_) + { delete thread_state.second; + } } moveit::core::RobotState* ompl_interface::TSStateStorage::getStateStorage() const @@ -61,9 +63,12 @@ moveit::core::RobotState* ompl_interface::TSStateStorage::getStateStorage() cons if (it == thread_states_.end()) { st = new moveit::core::RobotState(start_state_); + st->setToDefaultValues(); // avoid uninitialized memory thread_states_[std::this_thread::get_id()] = st; } else + { st = it->second; + } return st; } diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 1014eb41be..ac951cce6c 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -99,6 +99,7 @@ ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std:: , interpolate_(true) , hybridize_(true) { + complete_initial_robot_state_.setToDefaultValues(); // avoid uninitialized memory complete_initial_robot_state_.update(); constraints_library_ = std::make_shared(this); @@ -117,11 +118,23 @@ void ompl_interface::ModelBasedPlanningContext::configure(const rclcpp::Node::Sh ompl_simple_setup_->getStateSpace()->setStateSamplerAllocator( std::bind(&ModelBasedPlanningContext::allocPathConstrainedSampler, this, std::placeholders::_1)); - // convert the input state to the corresponding OMPL state - ompl::base::ScopedState<> ompl_start_state(spec_.state_space_); - spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState()); - ompl_simple_setup_->setStartState(ompl_start_state); - ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(new StateValidityChecker(this))); + if (spec_.constrained_state_space_) + { + // convert the input state to the corresponding OMPL state + ompl::base::ScopedState<> ompl_start_state(spec_.constrained_state_space_); + spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState()); + ompl_simple_setup_->setStartState(ompl_start_state); + ompl_simple_setup_->setStateValidityChecker( + ob::StateValidityCheckerPtr(std::make_shared(this))); + } + else + { + // convert the input state to the corresponding OMPL state + ompl::base::ScopedState<> ompl_start_state(spec_.state_space_); + spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState()); + ompl_simple_setup_->setStartState(ompl_start_state); + ompl_simple_setup_->setStateValidityChecker(std::make_shared(this)); + } if (path_constraints_ && constraints_library_) { @@ -182,24 +195,36 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str { int idx = getJointModelGroup()->getVariableGroupIndex(joint); for (unsigned int q = 0; q < variable_count; ++q) + { j.push_back(idx + q); + } } else + { RCLCPP_WARN(LOGGER, "%s: Ignoring joint '%s' in projection since it has 0 DOF", name_.c_str(), joint.c_str()); + } } else + { RCLCPP_ERROR(LOGGER, "%s: Attempted to set projection evaluator with respect to value of joint " "'%s', but that joint is not known to the group '%s'.", name_.c_str(), joint.c_str(), getGroupName().c_str()); + } } if (j.empty()) + { RCLCPP_ERROR(LOGGER, "%s: No valid joints specified for joint projection", name_.c_str()); + } else + { return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorJointValue(this, j)); + } } else + { RCLCPP_ERROR(LOGGER, "Unable to allocate projection evaluator based on description: '%s'", peval.c_str()); + } return ob::ProjectionEvaluatorPtr(); } @@ -294,7 +319,9 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() } if (cfg.empty()) + { return; + } std::string optimizer; ompl::base::OptimizationObjectivePtr objective; @@ -335,7 +362,9 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() // Don't clear planner data if multi-query planning is enabled it = cfg.find("multi_query_planning_enabled"); if (it != cfg.end()) + { multi_query_planning_enabled_ = boost::lexical_cast(it->second); + } // check whether the path returned by the planner should be interpolated it = cfg.find("interpolate"); @@ -385,7 +414,9 @@ void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_m if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 && wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 && wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0) + { RCLCPP_WARN(LOGGER, "It looks like the planning volume was not specified."); + } RCLCPP_DEBUG(LOGGER, "%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = " @@ -446,14 +477,18 @@ bool ompl_interface::ModelBasedPlanningContext::getSolutionPath(robot_trajectory { traj.clear(); if (ompl_simple_setup_->haveSolutionPath()) + { convertPath(ompl_simple_setup_->getSolutionPath(), traj); + } return ompl_simple_setup_->haveSolutionPath(); } void ompl_interface::ModelBasedPlanningContext::setVerboseStateValidityChecks(bool flag) { if (ompl_simple_setup_->getStateValidityChecker()) + { static_cast(ompl_simple_setup_->getStateValidityChecker().get())->setVerbose(flag); + } } ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal() @@ -465,8 +500,11 @@ ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal() { constraint_samplers::ConstraintSamplerPtr constraint_sampler; if (spec_.constraint_sampler_manager_) + { constraint_sampler = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(), goal_constraint->getAllConstraints()); + } + if (constraint_sampler) { ob::GoalPtr goal = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraint, constraint_sampler)); @@ -475,9 +513,13 @@ ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal() } if (!goals.empty()) + { return goals.size() == 1 ? goals[0] : ompl::base::GoalPtr(new GoalSampleableRegionMux(goals)); + } else + { RCLCPP_ERROR(LOGGER, "Unable to construct goal representation"); + } return ob::GoalPtr(); } @@ -488,24 +530,34 @@ ompl_interface::ModelBasedPlanningContext::constructPlannerTerminationCondition( { auto it = spec_.config_.find("termination_condition"); if (it == spec_.config_.end()) + { return ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)); + } + std::string termination_string = it->second; std::vector termination_and_params; boost::split(termination_and_params, termination_string, boost::is_any_of("[ ,]")); if (termination_and_params.empty()) + { RCLCPP_ERROR(LOGGER, "Termination condition not specified"); + } + // Terminate if a maximum number of iterations is exceeded or a timeout occurs. // The semantics of "iterations" are planner-specific, but typically it corresponds to the number of times // an attempt was made to grow a roadmap/tree. else if (termination_and_params[0] == "Iteration") { if (termination_and_params.size() > 1) + { return ob::plannerOrTerminationCondition( ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)), ob::IterationTerminationCondition(std::stoul(termination_and_params[1]))); + } else + { RCLCPP_ERROR(LOGGER, "Missing argument to Iteration termination condition"); + } } // TODO: remove when ROS Melodic and older are no longer supported #if OMPL_VERSION_VALUE >= 1005000 @@ -519,7 +571,9 @@ ompl_interface::ModelBasedPlanningContext::constructPlannerTerminationCondition( { solutions_window = std::stoul(termination_and_params[1]); if (termination_and_params.size() > 2) + { epsilon = moveit::core::toDouble(termination_and_params[2]); + } } return ob::plannerOrTerminationCondition( ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start)), @@ -536,8 +590,9 @@ ompl_interface::ModelBasedPlanningContext::constructPlannerTerminationCondition( ob::exactSolnPlannerTerminationCondition(ompl_simple_setup_->getProblemDefinition())); } else + { RCLCPP_ERROR(LOGGER, "Unknown planner termination condition"); - + } // return a planner termination condition to suppress compiler warning return ob::plannerAlwaysTerminatingCondition(); } @@ -552,7 +607,9 @@ void ompl_interface::ModelBasedPlanningContext::setCompleteInitialState( void ompl_interface::ModelBasedPlanningContext::clear() { if (!multi_query_planning_enabled_) + { ompl_simple_setup_->clear(); + } // TODO: remove when ROS Melodic and older are no longer supported #if OMPL_VERSION_VALUE >= 1005000 else @@ -563,7 +620,9 @@ void ompl_interface::ModelBasedPlanningContext::clear() // this is not the case, then multi-query planning should not be enabled. auto planner = dynamic_cast(ompl_simple_setup_->getPlanner().get()); if (planner != nullptr) + { planner->clearValidity(); + } } #endif ompl_simple_setup_->clearStartStates(); @@ -598,14 +657,18 @@ bool ompl_interface::ModelBasedPlanningContext::setGoalConstraints( new kinematic_constraints::KinematicConstraintSet(getRobotModel())); kset->add(constr, getPlanningScene()->getTransforms()); if (!kset->empty()) + { goal_constraints_.push_back(kset); + } } if (goal_constraints_.empty()) { RCLCPP_WARN(LOGGER, "%s: No goal constraints specified. There is no problem to solve.", name_.c_str()); if (error) + { error->val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; + } return false; } @@ -636,20 +699,28 @@ void ompl_interface::ModelBasedPlanningContext::startSampling() { bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES); if (gls) + { static_cast(ompl_simple_setup_->getGoal().get())->startSampling(); + } else + { // we know this is a GoalSampleableMux by elimination static_cast(ompl_simple_setup_->getGoal().get())->startSampling(); + } } void ompl_interface::ModelBasedPlanningContext::stopSampling() { bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES); if (gls) + { static_cast(ompl_simple_setup_->getGoal().get())->stopSampling(); + } else + { // we know this is a GoalSampleableMux by elimination static_cast(ompl_simple_setup_->getGoal().get())->stopSampling(); + } } void ompl_interface::ModelBasedPlanningContext::preSolve() @@ -658,7 +729,9 @@ void ompl_interface::ModelBasedPlanningContext::preSolve() ompl_simple_setup_->getProblemDefinition()->clearSolutionPaths(); const ob::PlannerPtr planner = ompl_simple_setup_->getPlanner(); if (planner && !multi_query_planning_enabled_) + { planner->clear(); + } startSampling(); ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter(); } @@ -671,7 +744,9 @@ void ompl_interface::ModelBasedPlanningContext::postSolve() RCLCPP_DEBUG(LOGGER, "There were %d valid motions and %d invalid motions.", v, iv); if (ompl_simple_setup_->getProblemDefinition()->hasApproximateSolution()) + { RCLCPP_WARN(LOGGER, "Computed solution is approximate"); + } } bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res) @@ -686,7 +761,9 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion } if (interpolate_) + { interpolateSolution(); + } // fill the response RCLCPP_DEBUG(LOGGER, "%s: Returning successful solution with %lu states", getName().c_str(), @@ -779,10 +856,16 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i ompl_parallel_plan_.clearPlanners(); if (ompl_simple_setup_->getPlannerAllocator()) for (unsigned int i = 0; i < count; ++i) + { ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator()); + } else + { for (unsigned int i = 0; i < count; ++i) + { ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal())); + } + } ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); registerTerminationCondition(ptc); @@ -800,11 +883,20 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i { ompl_parallel_plan_.clearPlanners(); if (ompl_simple_setup_->getPlannerAllocator()) + { for (unsigned int i = 0; i < max_planning_threads_; ++i) + { ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator()); + } + } else + { for (unsigned int i = 0; i < max_planning_threads_; ++i) + { ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal())); + } + } + bool r = ompl_parallel_plan_.solve(ptc, 1, count, hybridize_) == ompl::base::PlannerStatus::EXACT_SOLUTION; result = result && r; } @@ -813,11 +905,20 @@ bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned i { ompl_parallel_plan_.clearPlanners(); if (ompl_simple_setup_->getPlannerAllocator()) + { for (int i = 0; i < n; ++i) + { ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator()); + } + } else + { for (int i = 0; i < n; ++i) + { ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal())); + } + } + bool r = ompl_parallel_plan_.solve(ptc, 1, count, hybridize_) == ompl::base::PlannerStatus::EXACT_SOLUTION; result = result && r; } @@ -847,7 +948,9 @@ bool ompl_interface::ModelBasedPlanningContext::terminate() { std::unique_lock slock(ptc_lock_); if (ptc_) + { ptc_->terminate(); + } return true; } @@ -875,4 +978,4 @@ bool ompl_interface::ModelBasedPlanningContext::loadConstraintApproximations(con return true; } return false; -} +} \ No newline at end of file diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index 7796f661b9..b783b6453b 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -45,11 +45,9 @@ namespace ompl_interface { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.ompl_interface"); -} // namespace ompl_interface -ompl_interface::OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, - const rclcpp::Node::SharedPtr& node, - const std::string& parameter_namespace) +OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace) : node_(node) , parameter_namespace_(parameter_namespace) , robot_model_(robot_model) @@ -63,10 +61,9 @@ ompl_interface::OMPLInterface::OMPLInterface(const moveit::core::RobotModelConst loadConstraintSamplers(); } -ompl_interface::OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, - const planning_interface::PlannerConfigurationMap& pconfig, - const rclcpp::Node::SharedPtr& node, - const std::string& parameter_namespace) +OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, + const planning_interface::PlannerConfigurationMap& pconfig, + const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) : node_(node) , parameter_namespace_(parameter_namespace) , robot_model_(robot_model) @@ -80,9 +77,9 @@ ompl_interface::OMPLInterface::OMPLInterface(const moveit::core::RobotModelConst loadConstraintSamplers(); } -ompl_interface::OMPLInterface::~OMPLInterface() = default; +OMPLInterface::~OMPLInterface() = default; -void ompl_interface::OMPLInterface::setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig) +void OMPLInterface::setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig) { planning_interface::PlannerConfigurationMap pconfig2 = pconfig; @@ -100,41 +97,42 @@ void ompl_interface::OMPLInterface::setPlannerConfigurations(const planning_inte context_manager_.setPlannerConfigurations(pconfig2); } -ompl_interface::ModelBasedPlanningContextPtr -ompl_interface::OMPLInterface::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req) const +ModelBasedPlanningContextPtr +OMPLInterface::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req) const { moveit_msgs::msg::MoveItErrorCodes dummy; return getPlanningContext(planning_scene, req, dummy); } -ompl_interface::ModelBasedPlanningContextPtr -ompl_interface::OMPLInterface::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - moveit_msgs::msg::MoveItErrorCodes& error_code) const +ModelBasedPlanningContextPtr +OMPLInterface::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::msg::MoveItErrorCodes& error_code) const { ModelBasedPlanningContextPtr ctx = context_manager_.getPlanningContext(planning_scene, req, error_code, node_, use_constraints_approximations_); if (ctx) + { configureContext(ctx); + } return ctx; } -void ompl_interface::OMPLInterface::configureContext(const ModelBasedPlanningContextPtr& context) const +void OMPLInterface::configureContext(const ModelBasedPlanningContextPtr& context) const { context->simplifySolutions(simplify_solutions_); } -void ompl_interface::OMPLInterface::loadConstraintSamplers() +void OMPLInterface::loadConstraintSamplers() { constraint_sampler_manager_loader_.reset( new constraint_sampler_manager_loader::ConstraintSamplerManagerLoader(node_, constraint_sampler_manager_)); } -bool ompl_interface::OMPLInterface::loadPlannerConfiguration( - const std::string& group_name, const std::string& planner_id, - const std::map& group_params, - planning_interface::PlannerConfigurationSettings& planner_config) +bool OMPLInterface::loadPlannerConfiguration(const std::string& group_name, const std::string& planner_id, + const std::map& group_params, + planning_interface::PlannerConfigurationSettings& planner_config) { rcl_interfaces::msg::ListParametersResult planner_params_result = node_->list_parameters({ parameter_namespace_ + ".planner_configs." + planner_id }, 2); @@ -162,7 +160,7 @@ bool ompl_interface::OMPLInterface::loadPlannerConfiguration( return true; } -void ompl_interface::OMPLInterface::loadPlannerConfigurations() +void OMPLInterface::loadPlannerConfigurations() { // read the planning configuration for each group planning_interface::PlannerConfigurationMap pconfig; @@ -172,7 +170,9 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() { // the set of planning parameters that can be specific for the group (inherited by configurations of that group) static const std::string KNOWN_GROUP_PARAMS[] = { "projection_evaluator", "longest_valid_segment_fraction", - "enforce_joint_model_state_space" }; + "enforce_joint_model_state_space", + "enforce_constrained_state_space" }; + const std::string group_name_param = parameter_namespace_ + "." + group_name; // get parameters specific for the robot planning group @@ -222,7 +222,9 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() if (node_->get_parameter(group_name_param + ".default_planner_config", default_planner_id)) { if (!loadPlannerConfiguration(group_name, default_planner_id, specific_group_params, default_pc)) + { default_planner_id = ""; + } } if (default_planner_id.empty()) @@ -243,7 +245,9 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() { planning_interface::PlannerConfigurationSettings pc; if (loadPlannerConfiguration(group_name, planner_id, specific_group_params, pc)) + { pconfig[pc.name] = pc; + } } } } @@ -253,13 +257,16 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() RCLCPP_DEBUG(LOGGER, "Parameters for configuration '%s'", config.first.c_str()); for (const std::pair& parameters : config.second.config) + { RCLCPP_DEBUG(LOGGER, " - %s = %s", parameters.first.c_str(), parameters.second.c_str()); + } } setPlannerConfigurations(pconfig); } -void ompl_interface::OMPLInterface::printStatus() +void OMPLInterface::printStatus() { RCLCPP_INFO(LOGGER, "OMPL ROS interface is running."); } +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space.cpp new file mode 100644 index 0000000000..e9d2775a5e --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space.cpp @@ -0,0 +1,101 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of KU Leuven nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ + +#include + +#include + +namespace ompl_interface +{ +const std::string ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE = "ConstrainedPlanningJointModel"; + +ConstrainedPlanningStateSpace::ConstrainedPlanningStateSpace(const ModelBasedStateSpaceSpecification& spec) + : ModelBasedStateSpace(spec) +{ + setName(getName() + "_" + PARAMETERIZATION_TYPE); +} + +double* ConstrainedPlanningStateSpace::getValueAddressAtIndex(ompl::base::State* ompl_state, + const unsigned int index) const +{ + assert(ompl_state != nullptr); + if (index >= variable_count_) + { + return nullptr; + } + + // Developer tip: replace this with a dynamic_cast for debugging + return ompl_state->as()->values + index; +} + +void ConstrainedPlanningStateSpace::copyToRobotState(moveit::core::RobotState& robot_state, + const ompl::base::State* ompl_state) const +{ + assert(ompl_state != nullptr); + robot_state.setJointGroupPositions( + spec_.joint_model_group_, + ompl_state->as()->getState()->as()->values); + robot_state.update(); +} + +void ConstrainedPlanningStateSpace::copyToOMPLState(ompl::base::State* ompl_state, + const moveit::core::RobotState& robot_state) const +{ + assert(ompl_state != nullptr); + robot_state.copyJointGroupPositions( + spec_.joint_model_group_, + ompl_state->as()->getState()->as()->values); + + // clear any cached info (such as validity known or not) + ompl_state->as()->getState()->as()->clearKnownInformation(); +} + +void ConstrainedPlanningStateSpace::copyJointToOMPLState(ompl::base::State* ompl_state, + const moveit::core::RobotState& robot_state, + const moveit::core::JointModel* joint_model, + int ompl_state_joint_index) const +{ + assert(ompl_state != nullptr); + assert(joint_model != nullptr); + memcpy(getValueAddressAtIndex(ompl_state->as()->getState(), + ompl_state_joint_index), + robot_state.getVariablePositions() + joint_model->getFirstVariableIndex(), + joint_model->getVariableCount() * sizeof(double)); + + // clear any cached info (such as validity known or not) + ompl_state->as()->getState()->as()->clearKnownInformation(); +} +} // namespace ompl_interface \ No newline at end of file diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp new file mode 100644 index 0000000000..c69e6adc78 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp @@ -0,0 +1,63 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of KU Leuven nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ +/* Mostly copied from Ioan Sucan's code */ + +#include +#include + +namespace ompl_interface +{ +ConstrainedPlanningStateSpaceFactory::ConstrainedPlanningStateSpaceFactory() : ModelBasedStateSpaceFactory() +{ + type_ = ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE; +} + +int ConstrainedPlanningStateSpaceFactory::canRepresentProblem( + const std::string& /*group*/, const moveit_msgs::msg::MotionPlanRequest& /*req*/, + const moveit::core::RobotModelConstPtr& /*robot_model*/) const +{ + // Return the lowest priority currently existing in the ompl interface. + // This state space will only be selected if it is the only option to choose from. + // See header file for more info. + return -2; +} + +ModelBasedStateSpacePtr ompl_interface::ConstrainedPlanningStateSpaceFactory::allocStateSpace( + const ModelBasedStateSpaceSpecification& space_spec) const +{ + return std::make_shared(space_spec); +} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 56f8d391b9..32d2483a5f 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -67,9 +67,15 @@ #include #include +#include +#include + #include #include +#include +#include #include +#include using namespace std::placeholders; @@ -83,9 +89,7 @@ struct PlanningContextManager::CachedContexts std::mutex lock_; }; -} // namespace ompl_interface - -ompl_interface::MultiQueryPlannerAllocator::~MultiQueryPlannerAllocator() +MultiQueryPlannerAllocator::~MultiQueryPlannerAllocator() { // Store all planner data for (const auto& entry : planner_data_storage_paths_) @@ -98,8 +102,9 @@ ompl_interface::MultiQueryPlannerAllocator::~MultiQueryPlannerAllocator() } template -ompl::base::PlannerPtr ompl_interface::MultiQueryPlannerAllocator::allocatePlanner( - const ob::SpaceInformationPtr& si, const std::string& new_name, const ModelBasedPlanningContextSpecification& spec) +ompl::base::PlannerPtr MultiQueryPlannerAllocator::allocatePlanner(const ob::SpaceInformationPtr& si, + const std::string& new_name, + const ModelBasedPlanningContextSpecification& spec) { // Store planner instance if multi-query planning is enabled auto cfg = spec.config_; @@ -115,7 +120,9 @@ ompl::base::PlannerPtr ompl_interface::MultiQueryPlannerAllocator::allocatePlann // If we already have an instance, use that one auto planner_map_it = planners_.find(new_name); if (planner_map_it != planners_.end()) + { return planner_map_it->second; + } // Certain multi-query planners allow loading and storing the generated planner data. This feature can be // selectively enabled for loading and storing using the bool parameters 'load_planner_data' and @@ -156,9 +163,11 @@ ompl::base::PlannerPtr ompl_interface::MultiQueryPlannerAllocator::allocatePlann } template -ompl::base::PlannerPtr ompl_interface::MultiQueryPlannerAllocator::allocatePlannerImpl( - const ob::SpaceInformationPtr& si, const std::string& new_name, const ModelBasedPlanningContextSpecification& spec, - bool load_planner_data, bool store_planner_data, const std::string& file_path) +ompl::base::PlannerPtr +MultiQueryPlannerAllocator::allocatePlannerImpl(const ob::SpaceInformationPtr& si, const std::string& new_name, + const ModelBasedPlanningContextSpecification& spec, + bool load_planner_data, bool store_planner_data, + const std::string& file_path) { ob::PlannerPtr planner; // Try to initialize planner with loaded planner data @@ -169,62 +178,42 @@ ompl::base::PlannerPtr ompl_interface::MultiQueryPlannerAllocator::allocatePlann storage_.load(file_path.c_str(), data); planner.reset(allocatePersistentPlanner(data)); if (!planner) + { RCLCPP_ERROR(LOGGER, "Creating a '%s' planner from persistent data is not supported. Going to create a new instance.", new_name.c_str()); + } } + if (!planner) + { planner.reset(new T(si)); + } + if (!new_name.empty()) + { planner->setName(new_name); + } + planner->params().setParams(spec.config_, true); // Remember which planner instances to store when the destructor is called if (store_planner_data) + { planner_data_storage_paths_[new_name] = file_path; + } + return planner; } // default implementation template -inline ompl::base::Planner* -ompl_interface::MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& /*data*/) +inline ompl::base::Planner* MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& /*data*/) { return nullptr; }; -// TODO: remove when ROS Melodic and older are no longer supported -// namespace is scoped instead of global because of GCC bug 56480 -#if OMPL_VERSION_VALUE >= 1005000 -namespace ompl_interface -{ -template <> -inline ompl::base::Planner* -MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) -{ - return new og::PRM(data); -}; -template <> -inline ompl::base::Planner* -MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) -{ - return new og::PRMstar(data); -}; -template <> -inline ompl::base::Planner* -MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) -{ - return new og::LazyPRM(data); -}; -template <> -inline ompl::base::Planner* -MultiQueryPlannerAllocator::allocatePersistentPlanner(const ob::PlannerData& data) -{ - return new og::LazyPRMstar(data); -}; -} // namespace ompl_interface -#endif -ompl_interface::PlanningContextManager::PlanningContextManager(moveit::core::RobotModelConstPtr robot_model, - constraint_samplers::ConstraintSamplerManagerPtr csm) +PlanningContextManager::PlanningContextManager(moveit::core::RobotModelConstPtr robot_model, + constraint_samplers::ConstraintSamplerManagerPtr csm) : robot_model_(std::move(robot_model)) , constraint_sampler_manager_(std::move(csm)) , max_goal_samples_(10) @@ -239,14 +228,15 @@ ompl_interface::PlanningContextManager::PlanningContextManager(moveit::core::Rob registerDefaultStateSpaces(); } -ompl_interface::PlanningContextManager::~PlanningContextManager() = default; +PlanningContextManager::~PlanningContextManager() = default; -ompl_interface::ConfiguredPlannerAllocator -ompl_interface::PlanningContextManager::plannerSelector(const std::string& planner) const +ConfiguredPlannerAllocator PlanningContextManager::plannerSelector(const std::string& planner) const { auto it = known_planners_.find(planner); if (it != known_planners_.end()) + { return it->second; + } else { RCLCPP_ERROR(LOGGER, "Unknown planner: '%s'", planner.c_str()); @@ -255,7 +245,7 @@ ompl_interface::PlanningContextManager::plannerSelector(const std::string& plann } template -void ompl_interface::PlanningContextManager::registerPlannerAllocatorHelper(const std::string& planner_id) +void PlanningContextManager::registerPlannerAllocatorHelper(const std::string& planner_id) { registerPlannerAllocator(planner_id, [&](const ob::SpaceInformationPtr& si, const std::string& new_name, const ModelBasedPlanningContextSpecification& spec) { @@ -263,7 +253,7 @@ void ompl_interface::PlanningContextManager::registerPlannerAllocatorHelper(cons }); } -void ompl_interface::PlanningContextManager::registerDefaultPlanners() +void PlanningContextManager::registerDefaultPlanners() { registerPlannerAllocatorHelper("geometric::AnytimePathShortening"); registerPlannerAllocatorHelper("geometric::BFMT"); @@ -292,28 +282,29 @@ void ompl_interface::PlanningContextManager::registerDefaultPlanners() registerPlannerAllocatorHelper("geometric::TRRT"); } -void ompl_interface::PlanningContextManager::registerDefaultStateSpaces() +void PlanningContextManager::registerDefaultStateSpaces() { registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new JointModelStateSpaceFactory())); registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new PoseModelStateSpaceFactory())); + registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new ConstrainedPlanningStateSpaceFactory())); } -ompl_interface::ConfiguredPlannerSelector ompl_interface::PlanningContextManager::getPlannerSelector() const +ConfiguredPlannerSelector PlanningContextManager::getPlannerSelector() const { return std::bind(&PlanningContextManager::plannerSelector, this, std::placeholders::_1); } -void ompl_interface::PlanningContextManager::setPlannerConfigurations( - const planning_interface::PlannerConfigurationMap& pconfig) +void PlanningContextManager::setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig) { planner_configs_ = pconfig; } -ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext( - const planning_interface::PlannerConfigurationSettings& config, - const StateSpaceFactoryTypeSelector& factory_selector, const moveit_msgs::msg::MotionPlanRequest& /*req*/) const +ModelBasedPlanningContextPtr +PlanningContextManager::getPlanningContext(const planning_interface::PlannerConfigurationSettings& config, + const StateSpaceFactoryTypeSelector& factory_selector, + const moveit_msgs::msg::MotionPlanRequest& req) const { - const ompl_interface::ModelBasedStateSpaceFactoryPtr& factory = factory_selector(config.group); + const ModelBasedStateSpaceFactoryPtr& factory = factory_selector(config.group); // Check for a cached planning context ModelBasedPlanningContextPtr context; @@ -343,14 +334,42 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana context_spec.constraint_sampler_manager_ = constraint_sampler_manager_; context_spec.state_space_ = factory->getNewStateSpace(space_spec); - // Choose the correct simple setup type to load - context_spec.ompl_simple_setup_.reset(new ompl::geometric::SimpleSetup(context_spec.state_space_)); + if (factory->getType() == ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE) + { + RCLCPP_DEBUG_STREAM(LOGGER, "planning_context_manager: Using OMPL's constrained state space for planning."); + + // Select the correct type of constraints based on the path constraints in the planning request. + ompl::base::ConstraintPtr ompl_constraint = + createOMPLConstraint(robot_model_, config.group, req.path_constraints); + + // Create a constrained state space of type "projected state space". + // Other types are available, so we probably should add another setting to ompl_planning.yaml + // to choose between them. + context_spec.constrained_state_space_ = + std::make_shared(context_spec.state_space_, ompl_constraint); + + // Pass the constrained state space to ompl simple setup through the creation of a + // ConstrainedSpaceInformation object. This makes sure the state space is properly initialized. + context_spec.ompl_simple_setup_ = std::make_shared( + std::make_shared(context_spec.constrained_state_space_)); + } + else + { + // Choose the correct simple setup type to load + context_spec.ompl_simple_setup_.reset(new ompl::geometric::SimpleSetup(context_spec.state_space_)); + } RCLCPP_DEBUG(LOGGER, "Creating new planning context"); context.reset(new ModelBasedPlanningContext(config.name, context_spec)); + + // Do not cache a constrained planning context, as the constraints could be changed + // and need to be parsed again. + if (factory->getType() != ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE) { - std::unique_lock slock(cached_contexts_->lock_); - cached_contexts_->contexts_[std::make_pair(config.name, factory->getType())].push_back(context); + { + std::lock_guard slock(cached_contexts_->lock_); + cached_contexts_->contexts_[std::make_pair(config.name, factory->getType())].push_back(context); + } } } @@ -358,22 +377,27 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana context->setMaximumGoalSamples(max_goal_samples_); context->setMaximumStateSamplingAttempts(max_state_sampling_attempts_); context->setMaximumGoalSamplingAttempts(max_goal_sampling_attempts_); + if (max_solution_segment_length_ > std::numeric_limits::epsilon()) + { context->setMaximumSolutionSegmentLength(max_solution_segment_length_); - context->setMinimumWaypointCount(minimum_waypoint_count_); + } + context->setMinimumWaypointCount(minimum_waypoint_count_); context->setSpecificationConfig(config.config); return context; } -const ompl_interface::ModelBasedStateSpaceFactoryPtr& -ompl_interface::PlanningContextManager::getStateSpaceFactory1(const std::string& /* dummy */, - const std::string& factory_type) const +const ModelBasedStateSpaceFactoryPtr& +PlanningContextManager::getStateSpaceFactory1(const std::string& /* dummy */, const std::string& factory_type) const { auto f = factory_type.empty() ? state_space_factories_.begin() : state_space_factories_.find(factory_type); if (f != state_space_factories_.end()) + { + RCLCPP_DEBUG(LOGGER, "Using '%s' parameterization for solving problem", factory_type.c_str()); return f->second; + } else { RCLCPP_ERROR(LOGGER, "Factory of type '%s' was not found", factory_type.c_str()); @@ -382,9 +406,9 @@ ompl_interface::PlanningContextManager::getStateSpaceFactory1(const std::string& } } -const ompl_interface::ModelBasedStateSpaceFactoryPtr& -ompl_interface::PlanningContextManager::getStateSpaceFactory2(const std::string& group, - const moveit_msgs::msg::MotionPlanRequest& req) const +const ModelBasedStateSpaceFactoryPtr& +PlanningContextManager::getStateSpaceFactory2(const std::string& group, + const moveit_msgs::msg::MotionPlanRequest& req) const { // find the problem representation to use auto best = state_space_factories_.end(); @@ -413,7 +437,7 @@ ompl_interface::PlanningContextManager::getStateSpaceFactory2(const std::string& } } -ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext( +ModelBasedPlanningContextPtr PlanningContextManager::getPlanningContext( const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::msg::MotionPlanRequest& req, moveit_msgs::msg::MoveItErrorCodes& error_code, const rclcpp::Node::SharedPtr& node, bool use_constraints_approximation) const @@ -441,9 +465,11 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana req.group_name + "[" + req.planner_id + "]" : req.planner_id); if (pc == planner_configs_.end()) + { RCLCPP_WARN(LOGGER, "Cannot find planning configuration for group '%s' using planner '%s'. Will use defaults instead.", req.group_name.c_str(), req.planner_id.c_str()); + } } if (pc == planner_configs_.end()) @@ -456,6 +482,28 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana } } + // State space selection process + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // There are 3 options for the factory_selector + // 1) enforce_constrained_state_space = true AND there are path constraints in the planning request + // Overrides all other settings and selects a ConstrainedPlanningStateSpace factory + // 2) enforce_joint_model_state_space = true + // If 1) is false, then this one overrides the remaining settings and returns a JointModelStateSpace factory + // 3) Not 1) or 2), then the factory is selected based on the priority that each one returns. + // See PoseModelStateSpaceFactory::canRepresentProblem for details on the selection process. + // In short, it returns a PoseModelStateSpace if there is an IK solver and a path constraint. + // + // enforce_constrained_state_space + // **************************************** + // Check if the user wants to use an OMPL ConstrainedStateSpace for planning. + // This is done by setting 'enforce_constrained_state_space' to 'true' for the desired group in ompl_planing.yaml. + // If there are no path constraints in the planning request, this option is ignored, as the constrained state space is + // only usefull for paths constraints. (And at the moment only a single position constraint is supported, hence: + // req.path_constraints.position_constraints.size() == 1 + // is used in the selection process below.) + // + // enforce_joint_model_state_space + // ******************************* // Check if sampling in JointModelStateSpace is enforced for this group by user. // This is done by setting 'enforce_joint_model_state_space' to 'true' for the desired group in ompl_planning.yaml. // @@ -464,13 +512,26 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana // leading to invalid trajectories. This workaround lets the user prevent this problem by forcing rejection sampling // in JointModelStateSpace. StateSpaceFactoryTypeSelector factory_selector; - auto it = pc->second.config.find("enforce_joint_model_state_space"); + auto constrained_planning_iterator = pc->second.config.find("enforce_constrained_state_space"); + auto joint_space_planning_iterator = pc->second.config.find("enforce_joint_model_state_space"); - if (it != pc->second.config.end() && boost::lexical_cast(it->second)) + if (constrained_planning_iterator != pc->second.config.end() && + boost::lexical_cast(constrained_planning_iterator->second) && + req.path_constraints.position_constraints.size() == 1) + { + factory_selector = std::bind(&PlanningContextManager::getStateSpaceFactory1, this, std::placeholders::_1, + ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE); + } + else if (joint_space_planning_iterator != pc->second.config.end() && + boost::lexical_cast(joint_space_planning_iterator->second)) + { factory_selector = std::bind(&PlanningContextManager::getStateSpaceFactory1, this, std::placeholders::_1, JointModelStateSpace::PARAMETERIZATION_TYPE); + } else + { factory_selector = std::bind(&PlanningContextManager::getStateSpaceFactory2, this, std::placeholders::_1, req); + } ModelBasedPlanningContextPtr context = getPlanningContext(pc->second, factory_selector, req); @@ -487,10 +548,14 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana context->setPlanningVolume(req.workspace_parameters); if (!context->setPathConstraints(req.path_constraints, &error_code)) + { return ModelBasedPlanningContextPtr(); + } if (!context->setGoalConstraints(req.goal_constraints, req.path_constraints, &error_code)) + { return ModelBasedPlanningContextPtr(); + } try { @@ -507,3 +572,4 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana return context; } +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/test/load_test_robot.h b/moveit_planners/ompl/ompl_interface/test/load_test_robot.h index 94c606a14a..e4cb7f5a48 100644 --- a/moveit_planners/ompl/ompl_interface/test/load_test_robot.h +++ b/moveit_planners/ompl/ompl_interface/test/load_test_robot.h @@ -35,10 +35,10 @@ /* Author: Jeroen De Maeyer */ #pragma once - #include #include #include +#include namespace ompl_interface_testing { @@ -103,7 +103,7 @@ class LoadTestRobot { // load robot robot_model_ = moveit::core::loadTestingRobotModel(robot_name_); - robot_state_ = std::make_shared(robot_model_); + robot_state_ = std::make_shared(robot_model_); robot_state_->setToDefaultValues(); joint_model_group_ = robot_state_->getJointModelGroup(group_name_); @@ -112,16 +112,39 @@ class LoadTestRobot ee_link_name_ = joint_model_group_->getLinkModelNames().back(); base_link_name_ = robot_model_->getRootLinkName(); - ROS_INFO_STREAM("Created test robot named: " << robot_name_ << " for planning group " << group_name_); + std::cout << "Loading robot: " << robot_name << " for planning group: " << group_name << std::endl; + } + + Eigen::VectorXd getRandomState() const + { + robot_state_->setToRandomPositions(joint_model_group_); + Eigen::VectorXd q; + robot_state_->copyJointGroupPositions(joint_model_group_, q); + return q; + } + + /** \brief Create a joint position vector with values 0.1, 0.2, 0.3, ... + * where the length depends on the number of joints in the robot. + **/ + Eigen::VectorXd getDeterministicState() const + { + Eigen::VectorXd state(num_dofs_); + double value = 0.1; + for (std::size_t i = 0; i < num_dofs_; i++) + { + state[i] = value; + value += 0.1; + } + return state; } protected: - const std::string group_name_; - const std::string robot_name_; + std::string group_name_; + std::string robot_name_; moveit::core::RobotModelPtr robot_model_; - robot_state::RobotStatePtr robot_state_; - const robot_state::JointModelGroup* joint_model_group_; + moveit::core::RobotStatePtr robot_state_; + const moveit::core::JointModelGroup* joint_model_group_; std::size_t num_dofs_; std::string base_link_name_; diff --git a/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp new file mode 100644 index 0000000000..5def81365a --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp @@ -0,0 +1,341 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of KU Leuven nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ + +/** This file checks if the ConstrainedPlanningStateSpace can properly copy OMPL States of type + * ompl::base::ConstrainedStateSpace::StateType into MoveIt's robot state. + **/ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "load_test_robot.h" + +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit.ompl_planning.test.test_constrained_planning_state_space"); + +/** \brief Dummy constraint for testing, always satisfied. We need this to create and OMPL ConstrainedStateSpace. **/ +class DummyConstraint : public ompl::base::Constraint +{ +public: + DummyConstraint(const unsigned int num_dofs) : ompl::base::Constraint(num_dofs, 1) + { + } + void function(const Eigen::Ref& /*unused*/, Eigen::Ref out) const override + { + out[0] = 0.0; + } +}; + +/** \brief Robot indepentent implementation of the tests. **/ +class TestConstrainedStateSpace : public ompl_interface_testing::LoadTestRobot, public testing::Test +{ +protected: + TestConstrainedStateSpace(const std::string& robot_name, const std::string& group_name) + : LoadTestRobot(robot_name, group_name) + { + } + + void SetUp() override + { + // create a constrained state space for testing + setupMoveItStateSpace(); + setupOMPLStateSpace(); + }; + + void TearDown() override + { + } + + void setupMoveItStateSpace() + { + ompl_interface::ModelBasedStateSpaceSpecification space_spec(robot_model_, group_name_); + moveit_state_space_ = std::make_shared(space_spec); + } + + void setupOMPLStateSpace() + { + // first call setupMoveItStateSpace() + assert(moveit_state_space_ != nullptr); + auto con = std::make_shared(num_dofs_); + constrained_state_space_ = std::make_shared(moveit_state_space_, con); + } + + void testGetValueAddressAtIndex() + { + SCOPED_TRACE("testGetValueAddressAtIndex"); + + Eigen::VectorXd joint_positions = getDeterministicState(); + ompl::base::ScopedState<> ompl_state(constrained_state_space_); + auto state_ptr = ompl_state->as()->getState(); + double* out_joint_positions = dynamic_cast(state_ptr)->values; + EXPECT_FALSE(out_joint_positions == nullptr); + + for (std::size_t i = 0; i < num_dofs_; i++) + { + *(out_joint_positions + i) = joint_positions[i]; + } + + // check getValueAddressAtIndex + // it can only be called with an already unwrapped state, + // this unwrapping is either done in the constrained_state_space_ (see WrapperStateSpace in OMPL), + // or in copyJointToOMPLState in the implementation of ConstrainedPlanningStateSpace in MoveIt. + for (std::size_t i = 0; i < num_dofs_; i++) + { + EXPECT_EQ(joint_positions[i], *(constrained_state_space_->getValueAddressAtIndex(ompl_state.get(), i))); + } + } + + void testCopyToRobotState() + { + SCOPED_TRACE("testCopyToRobotState"); + + // create and OMPL state + // The copy operation of the ConstraintStateSpace::StateType show below is not supported + // ompl_state->as()->copy(joint_positions); + // Because the state spaces implemented in MoveIt do not support casting to Eigen::VectorXd + Eigen::VectorXd joint_positions = getDeterministicState(); + ompl::base::ScopedState<> ompl_state(constrained_state_space_); + auto state_ptr = ompl_state->as()->getState(); + double* out_joint_positions = dynamic_cast(state_ptr)->values; + EXPECT_FALSE(out_joint_positions == nullptr); + + for (std::size_t i = 0; i < num_dofs_; i++) + { + *(out_joint_positions + i) = joint_positions[i]; + } + + // copy into a MoveIt state + moveit::core::RobotState moveit_state(robot_model_); + moveit_state.setToDefaultValues(); + moveit_state_space_->copyToRobotState(moveit_state, ompl_state.get()); + + // check if copy worked out as expected + Eigen::VectorXd out_joint_position(num_dofs_); + moveit_state.copyJointGroupPositions(joint_model_group_, out_joint_position); + + EXPECT_EQ(joint_positions.size(), out_joint_position.size()); + + for (std::size_t i = 0; i < num_dofs_; i++) + { + EXPECT_EQ(joint_positions[i], out_joint_position[i]); + } + } + + void testCopyToOMPLState() + { + SCOPED_TRACE("testCopyToOMPLState"); + + // create a MoveIt state + Eigen::VectorXd joint_positions = getDeterministicState(); + moveit::core::RobotState moveit_state(robot_model_); + moveit_state.setToDefaultValues(); + moveit_state.setJointGroupPositions(joint_model_group_, joint_positions); + + // copy into an OMPL state + ompl::base::ScopedState<> ompl_state(constrained_state_space_); + moveit_state_space_->copyToOMPLState(ompl_state.get(), moveit_state); + + // check if copy worked out as expected + // (Again, support for casting to Eigen::VectorXd would have been nice here.) + auto state_ptr = ompl_state->as()->getState(); + double* out_joint_positions = dynamic_cast(state_ptr)->values; + EXPECT_FALSE(out_joint_positions == nullptr); + + for (std::size_t i = 0; i < num_dofs_; i++) + { + EXPECT_EQ(joint_positions[i], *(out_joint_positions + i)); + } + } + + void testCopyJointToOMPLState() + { + SCOPED_TRACE("testCopyJointToOMPLState"); + + EXPECT_TRUE(true); + // create a MoveIt state + Eigen::VectorXd joint_positions = getDeterministicState(); + moveit::core::RobotState moveit_state(robot_model_); + moveit_state.setToDefaultValues(); + moveit_state.setJointGroupPositions(joint_model_group_, joint_positions); + + // copy into an OMPL state, one index at a time + ompl::base::ScopedState<> ompl_state(constrained_state_space_); + auto joint_model_names = joint_model_group_->getActiveJointModelNames(); + + for (std::size_t joint_index = 0; joint_index < num_dofs_; joint_index++) + { + const moveit::core::JointModel* joint_model = joint_model_group_->getJointModel(joint_model_names[joint_index]); + EXPECT_FALSE(joint_model == nullptr); + + RCLCPP_DEBUG_STREAM(LOGGER, "Joint model: " << joint_model->getName() << " index: " << joint_index); + RCLCPP_DEBUG_STREAM(LOGGER, "first index: " << joint_model->getFirstVariableIndex() * sizeof(double)); + RCLCPP_DEBUG_STREAM(LOGGER, "width: " << joint_model->getVariableCount() * sizeof(double)); + + moveit_state_space_->copyJointToOMPLState(ompl_state.get(), moveit_state, joint_model, joint_index); + } + + // check if copy worked out as expected + auto state_ptr = ompl_state->as()->getState(); + double* out_joint_positions = dynamic_cast(state_ptr)->values; + EXPECT_FALSE(out_joint_positions == nullptr); + + for (std::size_t i = 0; i < num_dofs_; i++) + { + EXPECT_EQ(joint_positions[i], *(out_joint_positions + i)); + } + } + +protected: + ompl::base::ConstrainedStateSpacePtr constrained_state_space_; + ompl_interface::ConstrainedPlanningStateSpacePtr moveit_state_space_; +}; + +// /*************************************************************************** +// * Run all tests on the Panda robot +// * ************************************************************************/ +class PandaCopyStateTest : public TestConstrainedStateSpace +{ +protected: + PandaCopyStateTest() : TestConstrainedStateSpace("panda", "panda_arm") + { + } +}; + +TEST_F(PandaCopyStateTest, testGetValueAddressAtIndex) +{ + testGetValueAddressAtIndex(); +} + +TEST_F(PandaCopyStateTest, testCopyToRobotState) +{ + testCopyToRobotState(); +} + +TEST_F(PandaCopyStateTest, testCopyToOMPLState) +{ + testCopyToOMPLState(); +} + +TEST_F(PandaCopyStateTest, testCopyJointToOMPLState) +{ + testCopyJointToOMPLState(); +} + +/*************************************************************************** + * Run all tests on the Fanuc robot + * ************************************************************************/ +class FanucCopyStateTest : public TestConstrainedStateSpace +{ +protected: + FanucCopyStateTest() : TestConstrainedStateSpace("fanuc", "manipulator") + { + } +}; + +TEST_F(FanucCopyStateTest, testGetValueAddressAtIndex) +{ + testGetValueAddressAtIndex(); +} + +TEST_F(FanucCopyStateTest, testCopyToRobotState) +{ + testCopyToRobotState(); +} + +TEST_F(FanucCopyStateTest, testCopyToOMPLState) +{ + testCopyToOMPLState(); +} + +TEST_F(FanucCopyStateTest, testCopyJointToOMPLState) +{ + testCopyJointToOMPLState(); +} + +// /*************************************************************************** +// * Run all tests on the PR2 robot its left arm +// * ************************************************************************/ +class PR2CopyStateTest : public TestConstrainedStateSpace +{ +protected: + PR2CopyStateTest() : TestConstrainedStateSpace("pr2", "left_arm") + { + } +}; + +TEST_F(PR2CopyStateTest, testGetValueAddressAtIndex) +{ + testGetValueAddressAtIndex(); +} + +TEST_F(PR2CopyStateTest, testCopyToRobotState) +{ + testCopyToRobotState(); +} + +TEST_F(PR2CopyStateTest, testCopyToOMPLState) +{ + testCopyToOMPLState(); +} + +TEST_F(PR2CopyStateTest, testCopyJointToOMPLState) +{ + testCopyJointToOMPLState(); +} + +/*************************************************************************** + * MAIN + * ************************************************************************/ +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp new file mode 100644 index 0000000000..7d00d2d862 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp @@ -0,0 +1,323 @@ + +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of KU Leuven nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ + +#include "load_test_robot.h" + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +/** \brief This flag sets the verbosity level for the state validity checker. **/ +constexpr bool VERBOSE = false; + +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit.ompl_planning.test.test_constrained_state_validity_checker"); + +/** \brief Pretty print std:vectors **/ +std::ostream& operator<<(std::ostream& os, const std::vector& v) +{ + os << "( "; + for (auto value : v) + os << value << ", "; + os << " )"; + return os; +} + +/** \brief Dummy constraint for testing, always satisfied. We need this to create and OMPL ConstrainedStateSpace. **/ +class DummyConstraint : public ompl::base::Constraint +{ +public: + DummyConstraint(const unsigned int num_dofs) : ompl::base::Constraint(num_dofs, 1) + { + } + void function(const Eigen::Ref& /*unused*/, Eigen::Ref out) const override + { + out[0] = 0.0; + } +}; + +/** \brief Generic implementation of the tests that can be executed on different robots. **/ +class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, public testing::Test +{ +public: + TestStateValidityChecker(const std::string& robot_name, const std::string& group_name) + : LoadTestRobot(robot_name, group_name) + { + initial_robot_state_ = std::make_shared(robot_model_); + initial_robot_state_->setToDefaultValues(); + } + + void testConstructor() + { + SCOPED_TRACE("testEqualityPositionConstraints"); + EXPECT_TRUE(planning_context_.get()); + } + + void testJointLimits(const std::vector& position_in_limits) + { + SCOPED_TRACE("testJointLimits"); + + // create a validity checker for this test + auto checker = std::make_shared(planning_context_.get()); + checker->setVerbose(VERBOSE); + + // setup an ompl state with the "position_in_limits" joint values + robot_state_->setJointGroupPositions(joint_model_group_, position_in_limits); + ompl::base::ScopedState<> ompl_state(constrained_state_space_); + state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); + + // cast ompl state to a specific type, useful in the rest of this test + auto state = ompl_state->as() + ->getState() + ->as(); + + RCLCPP_DEBUG_STREAM(LOGGER, + std::vector(state->values, state->values + joint_model_group_->getVariableCount())); + + // assume the default position in not in self-collision + // and there are no collision objects of path constraints so this state should be valid + bool result = checker->isValid(ompl_state.get()); + EXPECT_TRUE(result); + + // do the same for the version with distance + double distance = 0.0; + result = checker->isValid(ompl_state.get(), distance); + + RCLCPP_DEBUG(LOGGER, "Distance from the isValid function '%f': ", distance); + EXPECT_TRUE(result); + EXPECT_GT(distance, 0.0); + + // move first joint obviously outside any joint limits + state->values[0] = std::numeric_limits::max(); + state->clearKnownInformation(); // make sure the validity checker does not use the cached value + + RCLCPP_DEBUG_STREAM(LOGGER, + std::vector(state->values, state->values + joint_model_group_->getVariableCount())); + + bool result_2 = checker->isValid(ompl_state.get()); + EXPECT_FALSE(result_2); + + // do the same for the version with distance + double distance_2 = 0.0; + result_2 = checker->isValid(ompl_state.get(), distance_2); + EXPECT_FALSE(result_2); + // isValid function returned false before any distance calculation is done + EXPECT_EQ(distance_2, 0.0); + } + + void testSelfCollision(const std::vector& position_in_self_collision) + { + SCOPED_TRACE("testSelfCollision"); + + // create a validity checker for this test + auto checker = std::make_shared(planning_context_.get()); + checker->setVerbose(VERBOSE); + + robot_state_->setJointGroupPositions(joint_model_group_, position_in_self_collision); + + // use a scoped OMPL state so we don't have to call allocState and freeState + // (as recommended in the OMPL documantion) + ompl::base::ScopedState<> ompl_state(constrained_state_space_); + state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); + + // The code below is just to print the state to the debug stream + auto state = ompl_state->as() + ->getState() + ->as(); + + // ompl_state.reals() throws a segmentation fault for this state type + // use a more involved conversion to std::vector for logging + RCLCPP_DEBUG_STREAM(LOGGER, + std::vector(state->values, state->values + joint_model_group_->getVariableCount())); + + // the given state is known to be in self-collision, we check it here + bool result = checker->isValid(ompl_state.get()); + EXPECT_FALSE(result); + + // do the same for the version with distance + double distance = 0.0; + result = checker->isValid(ompl_state.get(), distance); + EXPECT_FALSE(result); + + // but it should respect the joint limits + bool result_2 = robot_state_->satisfiesBounds(); + EXPECT_TRUE(result_2); + } + + // /*************************************************************************** + // * END Test implementation + // * ************************************************************************/ + +protected: + void SetUp() override + { + // setup all the input we need to create a StateValidityChecker + setupStateSpace(); + setupPlanningContext(); + }; + + void TearDown() override + { + } + + void setupStateSpace() + { + // note: make_shared throws if the allocations below fail, making the test fail when necessary + ompl_interface::ModelBasedStateSpaceSpecification space_spec(robot_model_, group_name_); + state_space_ = std::make_shared(space_spec); + state_space_->computeLocations(); // this gets called in the state space factory normally + + auto dummy_constraint = std::make_shared(num_dofs_); + constrained_state_space_ = std::make_shared(state_space_, dummy_constraint); + } + + void setupPlanningContext() + { + ASSERT_NE(state_space_, nullptr) << "Initialize state space before creating the planning context."; + planning_context_spec_.state_space_ = state_space_; + + // IMPORTANT, we need to create the simple setup with a ConstrainedSpaceInformation object, + // not with the state space, as this will allocate the wrong type of SpaceInformation + auto csi = std::make_shared(constrained_state_space_); + planning_context_spec_.ompl_simple_setup_ = std::make_shared(csi); + + // check if the we succeeded in the comment above + auto si = planning_context_spec_.ompl_simple_setup_->getSpaceInformation(); + auto si_constrained = dynamic_cast(si.get()); + ASSERT_NE(si_constrained, nullptr); + + planning_context_ = + std::make_shared(group_name_, planning_context_spec_); + + planning_scene_ = std::make_shared(robot_model_); + planning_context_->setPlanningScene(planning_scene_); + planning_context_->setCompleteInitialState(*initial_robot_state_); + + RCLCPP_DEBUG(LOGGER, "Planning context with name '%s' is ready (but not configured).", + planning_context_->getName().c_str()); + } + + moveit::core::RobotStatePtr initial_robot_state_; + + ompl_interface::ModelBasedStateSpacePtr state_space_; + ompl_interface::ModelBasedPlanningContextSpecification planning_context_spec_; + ompl_interface::ModelBasedPlanningContextPtr planning_context_; + planning_scene::PlanningScenePtr planning_scene_; + + ompl::base::ConstrainedStateSpacePtr constrained_state_space_; +}; + +// /*************************************************************************** +// * Run all tests on the Panda robot +// * ************************************************************************/ +class PandaValidityCheckerTests : public TestStateValidityChecker +{ +protected: + PandaValidityCheckerTests() : TestStateValidityChecker("panda", "panda_arm") + { + } +}; + +TEST_F(PandaValidityCheckerTests, testConstructor) +{ + testConstructor(); +} + +TEST_F(PandaValidityCheckerTests, testJointLimits) +{ + // use the panda "ready" state from the srdf config + // we know this state should be within limits and self-collision free + testJointLimits({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }); +} + +TEST_F(PandaValidityCheckerTests, testSelfCollision) +{ + // the given state has self collision between "hand" and "panda_link2" + // (I just tried a couple of random states until I found one that collided.) + testSelfCollision({ 2.31827, -0.169668, 2.5225, -2.98568, -0.36355, 0.808339, 0.0843406 }); +} + +/*************************************************************************** + * Run all tests on the Fanuc robot + * ************************************************************************/ +class FanucTestStateValidityChecker : public TestStateValidityChecker +{ +protected: + FanucTestStateValidityChecker() : TestStateValidityChecker("fanuc", "manipulator") + { + } +}; + +TEST_F(FanucTestStateValidityChecker, createStateValidityChecker) +{ + testConstructor(); +} + +TEST_F(FanucTestStateValidityChecker, testJointLimits) +{ + // I assume the Fanucs's zero state is within limits and self-collision free + testJointLimits({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }); +} + +TEST_F(FanucTestStateValidityChecker, testSelfCollision) +{ + // the given state has self collision between "base_link" and "link_5" + // (I just tried a couple of random states until I found one that collided.) + testSelfCollision({ -2.95993, -0.682185, -2.43873, -0.939784, 3.0544, 0.882294 }); +} + +/* (Note: the PR2 has no collision geometry in the moveit_resources package.) */ + +/*************************************************************************** + * MAIN + * ************************************************************************/ +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp new file mode 100644 index 0000000000..8e785ba0e0 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp @@ -0,0 +1,440 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of KU Leuven nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jeroen De Maeyer */ + +/** This file tests the implementation of constraints inheriting from + * the ompl::base::Constraint class in the file /detail/ompl_constraint.h/cpp. + * These are used to create an ompl::base::ConstrainedStateSpace to plan with path constraints. + * + * NOTE q = joint positions + **/ + +#include "load_test_robot.h" + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.test.test_ompl_constraints"); + +/** \brief Number of times to run a test that uses randomly generated input. **/ +constexpr int NUM_RANDOM_TESTS = 10; + +/** \brief Select a robot link at (num_dofs - different_link_offset) to test another link than the end-effector. **/ +constexpr unsigned int DIFFERENT_LINK_OFFSET = 2; + +/** \brief Allowed error when comparing Jacobian matrix error. + * + * High tolerance because of high finite difference error. + * (And it is the L1-norm over the whole matrix difference.) + **/ +constexpr double JAC_ERROR_TOLERANCE = 1e-4; + +/** \brief Helper function to create a specific position constraint. **/ +moveit_msgs::msg::PositionConstraint createPositionConstraint(std::string& base_link, std::string& ee_link) +{ + shape_msgs::msg::SolidPrimitive box_constraint; + box_constraint.type = shape_msgs::msg::SolidPrimitive::BOX; + box_constraint.dimensions = { 0.05, 0.4, 0.05 }; /* use -1 to indicate no constraints. */ + + geometry_msgs::msg::Pose box_pose; + box_pose.position.x = 0.9; + box_pose.position.y = 0.0; + box_pose.position.z = 0.2; + box_pose.orientation.w = 1.0; + + moveit_msgs::msg::PositionConstraint position_constraint; + position_constraint.header.frame_id = base_link; + position_constraint.link_name = ee_link; + position_constraint.constraint_region.primitives.push_back(box_constraint); + position_constraint.constraint_region.primitive_poses.push_back(box_pose); + + return position_constraint; +} + +class TestOMPLConstraints : public ompl_interface_testing::LoadTestRobot, public testing::Test +{ +protected: + TestOMPLConstraints(const std::string& robot_name, const std::string& group_name) + : LoadTestRobot(robot_name, group_name) + { + } + + void SetUp() override + { + } + + void TearDown() override + { + } + + /** \brief Robot forward kinematics. **/ + const Eigen::Isometry3d fk(const Eigen::VectorXd& q, const std::string& link_name) const + { + robot_state_->setJointGroupPositions(joint_model_group_, q); + return robot_state_->getGlobalLinkTransform(link_name); + } + + Eigen::VectorXd getRandomState() + { + robot_state_->setToRandomPositions(joint_model_group_); + Eigen::VectorXd joint_positions; + robot_state_->copyJointGroupPositions(joint_model_group_, joint_positions); + return joint_positions; + } + + Eigen::MatrixXd numericalJacobianPosition(const Eigen::VectorXd& q, const std::string& link_name) const + { + const double h = 1e-6; /* step size for numerical derivation */ + + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(3, num_dofs_); + + // helper matrix for differentiation. + Eigen::MatrixXd m_helper = h * Eigen::MatrixXd::Identity(num_dofs_, num_dofs_); + + for (std::size_t dim = 0; dim < num_dofs_; dim++) + { + Eigen::Vector3d pos = fk(q, link_name).translation(); + Eigen::Vector3d pos_plus_h = fk(q + m_helper.col(dim), link_name).translation(); + Eigen::Vector3d col = (pos_plus_h - pos) / h; + jacobian.col(dim) = col; + } + return jacobian; + } + + void setPositionConstraints() + { + moveit_msgs::msg::Constraints constraint_msgs; + constraint_msgs.position_constraints.push_back(createPositionConstraint(base_link_name_, ee_link_name_)); + + constraint_ = std::make_shared(robot_model_, group_name_, num_dofs_); + constraint_->init(constraint_msgs); + } + + /** \brief Test position constraints a link that is _not_ the end-effector. **/ + void setPositionConstraintsDifferentLink() + { + std::string different_link = joint_model_group_->getLinkModelNames().at(num_dofs_ - DIFFERENT_LINK_OFFSET); + + RCLCPP_DEBUG_STREAM(LOGGER, different_link); + + moveit_msgs::msg::Constraints constraint_msgs; + constraint_msgs.position_constraints.push_back(createPositionConstraint(base_link_name_, different_link)); + + constraint_ = std::make_shared(robot_model_, group_name_, num_dofs_); + constraint_->init(constraint_msgs); + } + + void setEqualityPositionConstraints() + { + moveit_msgs::msg::PositionConstraint pos_con_msg = createPositionConstraint(base_link_name_, ee_link_name_); + + // Make the tolerance on the x dimension smaller than the treshold used to recognize equality constraints. + // (see docstring EqualityPositionConstraint::equality_constraint_threshold). + pos_con_msg.constraint_region.primitives.at(0).dimensions[0] = 0.0005; + + // The unconstrained dimensions are set to a large (unused) value + pos_con_msg.constraint_region.primitives.at(0).dimensions[1] = 1.0; + pos_con_msg.constraint_region.primitives.at(0).dimensions[2] = 1.0; + + moveit_msgs::msg::Constraints constraint_msgs; + constraint_msgs.position_constraints.push_back(pos_con_msg); + + // Tell the planner to use an Equality constraint model + constraint_msgs.name = "use_equality_constraints"; + + constraint_ = std::make_shared(robot_model_, group_name_, num_dofs_); + constraint_->init(constraint_msgs); + } + + void testJacobian() + { + SCOPED_TRACE("testJacobian"); + + double total_error = 999.9; + + for (int i = 0; i < NUM_RANDOM_TESTS; i++) + { + auto q = getRandomState(); + auto jac_exact = constraint_->calcErrorJacobian(q); + + Eigen::MatrixXd jac_approx = numericalJacobianPosition(q, constraint_->getLinkName()); + + RCLCPP_DEBUG_STREAM(LOGGER, "Analytical jacobian:"); + RCLCPP_DEBUG_STREAM(LOGGER, jac_exact); + RCLCPP_DEBUG_STREAM(LOGGER, "Finite difference jacobian:"); + RCLCPP_DEBUG_STREAM(LOGGER, jac_approx); + + total_error = (jac_exact - jac_approx).lpNorm<1>(); + EXPECT_LT(total_error, JAC_ERROR_TOLERANCE); + } + } + + void testOMPLProjectedStateSpaceConstruction() + { + SCOPED_TRACE("testOMPLProjectedStateSpaceConstruction"); + + auto state_space = std::make_shared(num_dofs_); + ompl::base::RealVectorBounds bounds(num_dofs_); + + // get joint limits from the joint model group + auto joint_limits = joint_model_group_->getActiveJointModelsBounds(); + EXPECT_EQ(joint_limits.size(), num_dofs_); + + for (std::size_t i = 0; i < num_dofs_; i++) + { + EXPECT_EQ(joint_limits[i]->size(), (unsigned int)1); + bounds.setLow(i, joint_limits[i]->at(0).min_position_); + bounds.setHigh(i, joint_limits[i]->at(0).max_position_); + } + + state_space->setBounds(bounds); + + auto constrained_state_space = std::make_shared(state_space, constraint_); + + // constrained_state_space->setStateSamplerAllocator() + + auto constrained_state_space_info = + std::make_shared(constrained_state_space); + + // TODO(jeroendm) There are some issue with the sanity checks. + // But these issues do not prevent us to use the ConstrainedPlanningStateSpace! :) + // The jacobian test is expected to fail because of the discontinuous constraint derivative. + // In addition not all samples returned from the state sampler will be valid. + // For more details: https://github.com/ros-planning/moveit/issues/2092#issuecomment-669911722 + try + { + constrained_state_space->sanityChecks(); + } + catch (ompl::Exception& ex) + { + RCLCPP_ERROR(LOGGER, "Sanity checks did not pass: %s", ex.what()); + } + } + + void testEqualityPositionConstraints() + { + SCOPED_TRACE("testEqualityPositionConstraints"); + + EXPECT_NE(constraint_, nullptr) << "First call setEqualityPositionConstraints before calling this test."; + + // all test below are in the assumption that we have equality constraints on the x-position, dimension 0. + + Eigen::VectorXd joint_values = getDeterministicState(); + + Eigen::Vector3d f; + f << 99, 99, 99; // fill in known but wrong values that should all be overwritten + constraint_->function(joint_values, f); + + // f should always be zero for unconstrained dimensions + EXPECT_DOUBLE_EQ(f[1], 0.0); + EXPECT_DOUBLE_EQ(f[2], 0.0); + + Eigen::MatrixXd jac(3, num_dofs_); + jac.setOnes(); // fill in known but wrong values that should all be overwritten + constraint_->jacobian(joint_values, jac); + + for (std::size_t i = 0; i < num_dofs_; i++) + { + // rows for unconstrained dimensions should always be zero + EXPECT_DOUBLE_EQ(jac(1, i), 0.0); + EXPECT_DOUBLE_EQ(jac(2, i), 0.0); + } + // the constrained x-dimension has some non-zeros + // ( we checked this is true for the given joint values!) + EXPECT_NE(f[0], 0.0); + EXPECT_NE(jac.row(0).squaredNorm(), 0.0); + } + +protected: + std::shared_ptr constraint_; +}; + +/*************************************************************************** + * Run all tests on the Panda robot + * ************************************************************************/ +class PandaConstraintTest : public TestOMPLConstraints +{ +protected: + PandaConstraintTest() : TestOMPLConstraints("panda", "panda_arm") + { + } +}; + +TEST_F(PandaConstraintTest, InitPositionConstraint) +{ + setPositionConstraints(); + setPositionConstraintsDifferentLink(); +} + +TEST_F(PandaConstraintTest, PositionConstraintJacobian) +{ + setPositionConstraints(); + testJacobian(); + + constraint_.reset(); + setPositionConstraintsDifferentLink(); + testJacobian(); +} + +TEST_F(PandaConstraintTest, PositionConstraintOMPLCheck) +{ + setPositionConstraints(); + testOMPLProjectedStateSpaceConstruction(); + // testOMPLStateSampler(); + + constraint_.reset(); + setPositionConstraintsDifferentLink(); + testOMPLProjectedStateSpaceConstruction(); +} + +TEST_F(PandaConstraintTest, EqualityPositionConstraints) +{ + setEqualityPositionConstraints(); + testOMPLProjectedStateSpaceConstruction(); + testEqualityPositionConstraints(); +} +/*************************************************************************** + * Run all tests on the Fanuc robot + * ************************************************************************/ +class FanucConstraintTest : public TestOMPLConstraints +{ +protected: + FanucConstraintTest() : TestOMPLConstraints("fanuc", "manipulator") + { + } +}; + +TEST_F(FanucConstraintTest, InitPositionConstraint) +{ + setPositionConstraints(); + setPositionConstraintsDifferentLink(); +} + +TEST_F(FanucConstraintTest, PositionConstraintJacobian) +{ + setPositionConstraints(); + testJacobian(); + + constraint_.reset(); + setPositionConstraintsDifferentLink(); + testJacobian(); +} + +TEST_F(FanucConstraintTest, PositionConstraintOMPLCheck) +{ + setPositionConstraints(); + testOMPLProjectedStateSpaceConstruction(); + // testOMPLStateSampler(); + + constraint_.reset(); + setPositionConstraintsDifferentLink(); + testOMPLProjectedStateSpaceConstruction(); +} + +TEST_F(FanucConstraintTest, EqualityPositionConstraints) +{ + setEqualityPositionConstraints(); + testOMPLProjectedStateSpaceConstruction(); + testEqualityPositionConstraints(); +} + +/*************************************************************************** + * Run all tests on the PR2's left arm + * ************************************************************************/ +class PR2LeftArmConstraintTest : public TestOMPLConstraints +{ +protected: + PR2LeftArmConstraintTest() : TestOMPLConstraints("pr2", "left_arm") + { + } +}; + +TEST_F(PR2LeftArmConstraintTest, InitPositionConstraint) +{ + setPositionConstraints(); + setPositionConstraintsDifferentLink(); +} + +TEST_F(PR2LeftArmConstraintTest, PositionConstraintJacobian) +{ + setPositionConstraints(); + testJacobian(); + + constraint_.reset(); + setPositionConstraintsDifferentLink(); + testJacobian(); +} + +TEST_F(PR2LeftArmConstraintTest, PositionConstraintOMPLCheck) +{ + setPositionConstraints(); + testOMPLProjectedStateSpaceConstruction(); + + constraint_.reset(); + setPositionConstraintsDifferentLink(); + testOMPLProjectedStateSpaceConstruction(); +} + +TEST_F(PR2LeftArmConstraintTest, EqualityPositionConstraints) +{ + setEqualityPositionConstraints(); + testOMPLProjectedStateSpaceConstruction(); + testEqualityPositionConstraints(); +} + +/*************************************************************************** + * MAIN + * ************************************************************************/ +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp index 2e2aabdf47..d15a661c89 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp @@ -65,21 +65,21 @@ #include +// static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.test.test_planning_context_manager"); + /** \brief Generic implementation of the tests that can be executed on different robots. **/ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public testing::Test { public: TestPlanningContext(const std::string& robot_name, const std::string& group_name) - : LoadTestRobot(robot_name, group_name) + : LoadTestRobot(robot_name, group_name), node_(std::make_shared("planning_context_manager_test")) { } - // /*************************************************************************** - // * START Test implementations - // * ************************************************************************/ - void testSimpleRequest(const std::vector& start, const std::vector& goal) { + SCOPED_TRACE("testSimpleRequest"); + // create all the test specific input necessary to make the getPlanningContext call possible planning_interface::PlannerConfigurationSettings pconfig_settings; pconfig_settings.group = group_name_; @@ -87,7 +87,7 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public pconfig_settings.config = { { "enforce_joint_model_state_space", "0" } }; planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } }; - moveit_msgs::MoveItErrorCodes error_code; + moveit_msgs::msg::MoveItErrorCodes error_code; planning_interface::MotionPlanRequest request = createRequest(start, goal); // setup the planning context manager @@ -95,7 +95,7 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public pcm.setPlannerConfigurations(pconfig_map); // see if it returns the expected planning context - auto pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_handle_, false); + auto pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_, false); // the planning context should have a simple setup created EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); @@ -113,6 +113,8 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public void testPathConstraints(const std::vector& start, const std::vector& goal) { + SCOPED_TRACE("testPathConstraints"); + // create all the test specific input necessary to make the getPlanningContext call possible planning_interface::PlannerConfigurationSettings pconfig_settings; pconfig_settings.group = group_name_; @@ -120,7 +122,7 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public pconfig_settings.config = { { "enforce_joint_model_state_space", "0" } }; planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } }; - moveit_msgs::MoveItErrorCodes error_code; + moveit_msgs::msg::MoveItErrorCodes error_code; planning_interface::MotionPlanRequest request = createRequest(start, goal); // give it some more time, as rejection sampling of the path constraints occasionally takes some time @@ -129,7 +131,7 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public // create path constraints around start state, to make sure they are satisfied robot_state_->setJointGroupPositions(joint_model_group_, start); Eigen::Isometry3d ee_pose = robot_state_->getGlobalLinkTransform(ee_link_name_); - geometry_msgs::Quaternion ee_orientation = tf2::toMsg(Eigen::Quaterniond(ee_pose.rotation()); + geometry_msgs::msg::Quaternion ee_orientation = tf2::toMsg(Eigen::Quaterniond(ee_pose.rotation())); // setup the planning context manager ompl_interface::PlanningContextManager pcm(robot_model_, constraint_sampler_manager_); @@ -140,7 +142,7 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public request.path_constraints.orientation_constraints.push_back(createOrientationConstraint(ee_orientation)); // See if the planning context manager returns the expected planning context - auto pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_handle_, false); + auto pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_, false); EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); @@ -163,7 +165,7 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public // solution. We test all of them here. for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response.trajectory_) { - for (std::size_t pt_index = { 0 }; pt_index < trajectory->getWayPointCount(); ++pt_index) + for (std::size_t pt_index = 0; pt_index < trajectory->getWayPointCount(); pt_index++) { EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied); } @@ -176,7 +178,7 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public { ee_pose.translation().x(), ee_pose.translation().y(), ee_pose.translation().z() }, { 0.1, 0.1, 0.1 })); // See if the planning context manager returns the expected planning context - pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_handle_, false); + pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_, false); EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr); @@ -198,17 +200,13 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public // solution. We test all of them here. for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response2.trajectory_) { - for (std::size_t pt_index = { 0 }; pt_index < trajectory->getWayPointCount(); ++pt_index) + for (std::size_t pt_index = 0; pt_index < trajectory->getWayPointCount(); pt_index++) { EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied); } } } - // /*************************************************************************** - // * END Test implementation - // * ************************************************************************/ - protected: void SetUp() override { @@ -217,10 +215,6 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public planning_scene_ = std::make_shared(robot_model_); } - void TearDown() override - { - } - /** Create a planning request to plan from a given start state to a joint space goal. **/ planning_interface::MotionPlanRequest createRequest(const std::vector& start, const std::vector& goal) const @@ -230,40 +224,42 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public request.allowed_planning_time = 5.0; // fill out start state in request - robot_state::RobotState start_state(robot_model_); + moveit::core::RobotState start_state(robot_model_); start_state.setToDefaultValues(); start_state.setJointGroupPositions(joint_model_group_, start); moveit::core::robotStateToRobotStateMsg(start_state, request.start_state); // fill out goal state in request - robot_state::RobotState goal_state(robot_model_); + moveit::core::RobotState goal_state(robot_model_); goal_state.setToDefaultValues(); goal_state.setJointGroupPositions(joint_model_group_, goal); - moveit_msgs::Constraints joint_goal = + + moveit_msgs::msg::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_, 0.001); + request.goal_constraints.push_back(joint_goal); return request; } /** \brief Helper function to create a position constraint. **/ - moveit_msgs::PositionConstraint createPositionConstraint(std::array position, - std::array dimensions) + moveit_msgs::msg::PositionConstraint createPositionConstraint(std::array position, + std::array dimensions) { - shape_msgs::SolidPrimitive box; - box.type = shape_msgs::SolidPrimitive::BOX; + shape_msgs::msg::SolidPrimitive box; + box.type = shape_msgs::msg::SolidPrimitive::BOX; box.dimensions.resize(3); - box.dimensions[shape_msgs::SolidPrimitive::BOX_X] = dimensions[0]; - box.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = dimensions[1]; - box.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = dimensions[2]; + box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = dimensions[0]; + box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = dimensions[1]; + box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = dimensions[2]; - geometry_msgs::Pose box_pose; + geometry_msgs::msg::Pose box_pose; box_pose.position.x = position[0]; box_pose.position.y = position[1]; box_pose.position.z = position[2]; box_pose.orientation.w = 1.0; - moveit_msgs::PositionConstraint pc; + moveit_msgs::msg::PositionConstraint pc; pc.header.frame_id = base_link_name_; pc.link_name = ee_link_name_; pc.constraint_region.primitives.push_back(box); @@ -273,9 +269,10 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public } /** \brief Helper function to create a orientation constraint. **/ - moveit_msgs::OrientationConstraint createOrientationConstraint(const geometry_msgs::Quaternion& nominal_orientation) + moveit_msgs::msg::OrientationConstraint + createOrientationConstraint(const geometry_msgs::msg::Quaternion& nominal_orientation) { - moveit_msgs::OrientationConstraint oc; + moveit_msgs::msg::OrientationConstraint oc; oc.header.frame_id = base_link_name_; oc.link_name = ee_link_name_; oc.orientation = nominal_orientation; @@ -299,7 +296,7 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public /** we need a node handle to call getPlanningRequest, but it is never used, as we disable the * 'use_constraints_approximation' option. **/ - ros::NodeHandle node_handle_; + rclcpp::Node::SharedPtr node_; }; /*************************************************************************** @@ -317,12 +314,12 @@ TEST_F(PandaTestPlanningContext, testSimpleRequest) { // use the panda "ready" state from the srdf config as start state // we know this state should be within limits and self-collision free - testSimpleRequest({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }, { 0, -0.785, 0, -2.356, 0, 1.571, 0.685 }); + testSimpleRequest({ 0., -0.785, 0., -2.356, 0, 1.571, 0.785 }, { 0., -0.785, 0., -2.356, 0, 1.571, 0.685 }); } TEST_F(PandaTestPlanningContext, testPathConstraints) { - testPathConstraints({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }, { 0, -0.785, 0, -2.356, 0, 1.571, 0.685 }); + testPathConstraints({ 0., -0.785, 0., -2.356, 0., 1.571, 0.785 }, { .0, -0.785, 0., -2.356, 0., 1.571, 0.685 }); } /*************************************************************************** @@ -338,12 +335,12 @@ class FanucTestPlanningContext : public TestPlanningContext TEST_F(FanucTestPlanningContext, testSimpleRequest) { - testSimpleRequest({ 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0.1 }); + testSimpleRequest({ 0., 0., 0., 0., 0., 0. }, { 0., 0., 0., 0., 0., 0.1 }); } TEST_F(FanucTestPlanningContext, testPathConstraints) { - testPathConstraints({ 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0.1 }); + testPathConstraints({ 0., 0., 0., 0., 0., 0. }, { 0., 0., 0., 0., 0., 0.1 }); } /*************************************************************************** @@ -351,7 +348,10 @@ TEST_F(FanucTestPlanningContext, testPathConstraints) * ************************************************************************/ int main(int argc, char** argv) { - ros::init(argc, argv, "planning_context_manager_test"); testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + rclcpp::init(argc, argv); + + const int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; } diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test deleted file mode 100644 index 64a3e6c087..0000000000 --- a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp index 9b937e2a63..5d9b60a5bf 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp @@ -63,9 +63,9 @@ #include /** \brief This flag sets the verbosity level for the state validity checker. **/ -constexpr bool VERBOSE{ false }; +constexpr bool VERBOSE = false; -constexpr char LOGNAME[] = "test_state_validity_checker"; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.test.test_state_validity_checker"); /** \brief Pretty print std:vectors **/ std::ostream& operator<<(std::ostream& os, const std::vector& v) @@ -86,19 +86,17 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p { } - /*************************************************************************** - * START Test implementations - * ************************************************************************/ - void testConstructor() { - ompl::base::StateValidityCheckerPtr checker = - std::make_shared(planning_context_.get()); + SCOPED_TRACE("testConstructor"); + EXPECT_TRUE(planning_context_.get()); } /** This test takes a state that is inside the joint limits and collision free as input. **/ void testJointLimits(const std::vector& position_in_limits) { + SCOPED_TRACE("testJointLimits"); + // create a validity checker for this test auto checker = std::make_shared(planning_context_.get()); checker->setVerbose(VERBOSE); @@ -110,7 +108,7 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p ompl::base::ScopedState<> ompl_state(state_space_); state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); - ROS_DEBUG_STREAM_NAMED(LOGNAME, ompl_state.reals()); + RCLCPP_DEBUG_STREAM(LOGGER, ompl_state.reals()); // assume the given position is not in self-collision // and there are no collision objects or path constraints so this state should be valid @@ -120,7 +118,7 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p ompl_state->as()->values[0] = std::numeric_limits::max(); ompl_state->as()->clearKnownInformation(); - ROS_DEBUG_STREAM_NAMED(LOGNAME, ompl_state.reals()); + RCLCPP_DEBUG_STREAM(LOGGER, ompl_state.reals()); EXPECT_FALSE(checker->isValid(ompl_state.get())); } @@ -128,6 +126,8 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p /** This test takes a state that is known to be in self-collision and inside the joint limits as input. **/ void testSelfCollision(const std::vector& position_in_self_collision) { + SCOPED_TRACE("testSelfCollision"); + // create a validity checker for this test auto checker = std::make_shared(planning_context_.get()); checker->setVerbose(VERBOSE); @@ -139,7 +139,7 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p ompl::base::ScopedState<> ompl_state(state_space_); state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); - ROS_DEBUG_STREAM_NAMED(LOGNAME, ompl_state.reals()); + RCLCPP_DEBUG_STREAM(LOGGER, ompl_state.reals()); // the given state is known to be in self-collision, we check it here EXPECT_FALSE(checker->isValid(ompl_state.get())); @@ -150,19 +150,21 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p void testPathConstraints(const std::vector& position_in_joint_limits) { + SCOPED_TRACE("testPathConstraints"); + ASSERT_NE(planning_context_, nullptr) << "Initialize planning context before adding path constraints."; // set the robot to a known position that is withing the joint limits and collision free robot_state_->setJointGroupPositions(joint_model_group_, position_in_joint_limits); // create position constraints around the given robot state - moveit_msgs::Constraints path_constraints; + moveit_msgs::msg::Constraints path_constraints; Eigen::Isometry3d ee_pose = robot_state_->getGlobalLinkTransform(ee_link_name_); path_constraints.name = "test_position_constraints"; path_constraints.position_constraints.push_back(createPositionConstraint( { ee_pose.translation().x(), ee_pose.translation().y(), ee_pose.translation().z() }, { 0.1, 0.1, 0.1 })); - moveit_msgs::MoveItErrorCodes error_code_not_used; + moveit_msgs::msg::MoveItErrorCodes error_code_not_used; ASSERT_TRUE(planning_context_->setPathConstraints(path_constraints, &error_code_not_used)); auto checker = std::make_shared(planning_context_.get()); @@ -173,12 +175,12 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p ompl::base::ScopedState<> ompl_state(state_space_); state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); - ROS_DEBUG_STREAM_NAMED(LOGNAME, ompl_state.reals()); + RCLCPP_DEBUG_STREAM(LOGGER, ompl_state.reals()); EXPECT_TRUE(checker->isValid(ompl_state.get())); // move the position constraints away from the curren end-effector position to make it fail - moveit_msgs::Constraints path_constraints_2(path_constraints); + moveit_msgs::msg::Constraints path_constraints_2(path_constraints); path_constraints_2.position_constraints.at(0).constraint_region.primitive_poses.at(0).position.z += 0.2; ASSERT_TRUE(planning_context_->setPathConstraints(path_constraints_2, &error_code_not_used)); @@ -190,10 +192,6 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p EXPECT_FALSE(checker->isValid(ompl_state.get())); } - /*************************************************************************** - * END Test implementation - * ************************************************************************/ - protected: void SetUp() override { @@ -226,32 +224,29 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p } /** \brief Helper function to create a position constraint. **/ - moveit_msgs::PositionConstraint createPositionConstraint(std::array position, - std::array dimensions) + moveit_msgs::msg::PositionConstraint createPositionConstraint(std::array position, + std::array dimensions) { - shape_msgs::SolidPrimitive box; - box.type = shape_msgs::SolidPrimitive::BOX; + shape_msgs::msg::SolidPrimitive box; + box.type = shape_msgs::msg::SolidPrimitive::BOX; box.dimensions.resize(3); - box.dimensions[shape_msgs::SolidPrimitive::BOX_X] = dimensions[0]; - box.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = dimensions[1]; - box.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = dimensions[2]; + box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = dimensions[0]; + box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = dimensions[1]; + box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = dimensions[2]; - geometry_msgs::Pose box_pose; + geometry_msgs::msg::Pose box_pose; box_pose.position.x = position[0]; box_pose.position.y = position[1]; box_pose.position.z = position[2]; box_pose.orientation.w = 1.0; - moveit_msgs::PositionConstraint position_constraint; - position_constraint.header.frame_id = base_link_name_; - position_constraint.link_name = ee_link_name_; - position_constraint.constraint_region.primitives.push_back(box); - position_constraint.constraint_region.primitive_poses.push_back(box_pose); - - // set the default weight to avoid warning in test output - position_constraint.weight = 1.0; + moveit_msgs::msg::PositionConstraint pc; + pc.header.frame_id = base_link_name_; + pc.link_name = ee_link_name_; + pc.constraint_region.primitives.push_back(box); + pc.constraint_region.primitive_poses.push_back(box_pose); - return position_constraint; + return pc; } ompl_interface::ModelBasedStateSpacePtr state_space_; @@ -280,7 +275,7 @@ TEST_F(PandaValidity, testJointLimits) { // use the panda "ready" state from the srdf config // we know this state should be within limits and self-collision free - testJointLimits({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }); + testJointLimits({ 0., -0.785, 0., -2.356, 0., 1.571, 0.785 }); } TEST_F(PandaValidity, testSelfCollision) @@ -294,7 +289,7 @@ TEST_F(PandaValidity, testPathConstraints) { // use the panda "ready" state from the srdf config // we know this state should be within limits and self-collision free - testPathConstraints({ 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }); + testPathConstraints({ 0., -0.785, 0., -2.356, 0., 1.571, 0.785 }); } /*************************************************************************** diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index 017969d05a..d39b615c97 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -18,16 +18,18 @@ moveit_common moveit_core - ompl + moveit_msgs moveit_ros_planning + ompl rclcpp + tf2_eigen tf2_ros libomp-dev pluginlib moveit_resources_pr2_description - moveit_resources_fanuc_description - moveit_resources_panda_description + moveit_resources_fanuc_moveit_config + moveit_resources_panda_moveit_config eigen tf2_eigen eigen3_cmake_module diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 1823336df6..b3c7c10f9e 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -86,6 +86,27 @@ add_subdirectory(move_group_interface) # add_subdirectory(test) add_subdirectory(moveit_cpp) +############# +## TESTING ## +############# + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ros_testing REQUIRED) + find_package(Boost REQUIRED COMPONENTS filesystem) + + # Move group interface cpp ompl constrained planning integration test + ament_add_gtest_executable(move_group_ompl_constraints_test + test/move_group_ompl_constraints_test.cpp + ) + + target_link_libraries(move_group_ompl_constraints_test ${THIS_PACKAGE_LIBRARIES}) + ament_target_dependencies(move_group_ompl_constraints_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(test/launch/move_group_ompl_constraints.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + # end move group interface cpp ompl constrained planning integration test + +endif() + ament_export_include_directories(include) ament_export_libraries(${THIS_PACKAGE_LIBRARIES}) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index e10efed42f..89d15926fe 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -41,16 +41,16 @@ eigen - ament_lint_auto - ament_lint_common - ament_cmake_gtest + moveit_resources_fanuc_moveit_config + moveit_resources_panda_moveit_config + moveit_simple_controller_manager + fake_joint_driver + warehouse_ros_mongo + moveit_planners_ompl + rviz2 + ros_testing ament_cmake - - moveit_resources_fanuc_moveit_config - moveit_resources_panda_moveit_config - - diff --git a/moveit_ros/planning_interface/test/config/controllers.yaml b/moveit_ros/planning_interface/test/config/controllers.yaml new file mode 100644 index 0000000000..58ea42051d --- /dev/null +++ b/moveit_ros/planning_interface/test/config/controllers.yaml @@ -0,0 +1,15 @@ +controller_names: + - panda_arm_controller + +panda_arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 diff --git a/moveit_ros/planning_interface/test/config/panda_controllers.yaml b/moveit_ros/planning_interface/test/config/panda_controllers.yaml new file mode 100644 index 0000000000..bc6e45d277 --- /dev/null +++ b/moveit_ros/planning_interface/test/config/panda_controllers.yaml @@ -0,0 +1,18 @@ +panda_arm_controller: + ros__parameters: + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + write_op_modes: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 \ No newline at end of file diff --git a/moveit_ros/planning_interface/test/config/start_positions.yaml b/moveit_ros/planning_interface/test/config/start_positions.yaml new file mode 100644 index 0000000000..f4553b3e33 --- /dev/null +++ b/moveit_ros/planning_interface/test/config/start_positions.yaml @@ -0,0 +1,19 @@ +fake_joint_driver_node: + ros__parameters: + start_position: + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + values: + - 0.0 + - -0.785 + - 0.0 + - -2.356 + - 0.0 + - 1.571 + - 0.785 \ No newline at end of file diff --git a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py new file mode 100644 index 0000000000..033c9b4b6f --- /dev/null +++ b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py @@ -0,0 +1,145 @@ +import os +import yaml +import launch +import launch_ros +import launch_testing +from launch import LaunchDescription +from launch.some_substitutions_type import SomeSubstitutionsType +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode +from ament_index_python.packages import get_package_share_directory + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + +def load_test_yaml(absolute_file_path): + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsType): + + # planning_context + robot_description_config = load_file('moveit_resources_panda_description', 'urdf/panda.urdf') + robot_description = {'robot_description' : robot_description_config} + + robot_description_semantic_config = load_file('moveit_resources_panda_moveit_config', 'config/panda.srdf') + robot_description_semantic = {'robot_description_semantic' : robot_description_semantic_config} + + kinematics_yaml = load_yaml('moveit_resources_panda_moveit_config', 'config/kinematics.yaml') + robot_description_kinematics = { 'robot_description_kinematics' : kinematics_yaml } + + # Planning Functionality + ompl_planning_pipeline_config = { 'move_group' : { + 'planning_plugin' : 'ompl_interface/OMPLPlanner', + 'request_adapters' : """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""" , + 'start_state_max_bounds_error' : 0.1 } } + + ompl_planning_yaml = load_yaml('moveit_resources_panda_moveit_config', 'config/ompl_planning.yaml') + ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) + + # Trajectory Execution Functionality + controllers_yaml = load_test_yaml(os.path.join(os.path.dirname(__file__), '../config/controllers.yaml')) + + moveit_controllers = { 'moveit_simple_controller_manager' : controllers_yaml, + 'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'} + + trajectory_execution = {'moveit_manage_controllers': True, + 'trajectory_execution.allowed_execution_duration_scaling': 1.2, + 'trajectory_execution.allowed_goal_duration_margin': 0.5, + 'trajectory_execution.allowed_start_tolerance': 0.01} + + planning_scene_monitor_parameters = {"publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True} + + # Start the actual move_group node/action server + run_move_group_node = Node(package='moveit_ros_move_group', + executable='move_group', + output='screen', + parameters=[robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters]) + + # Static TF + static_tf = Node(package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + output='log', + arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'panda_link0']) + + # Publish TF + robot_state_publisher = Node(package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[robot_description]) + + # Fake joint driver + fake_joint_driver_node = Node(package='fake_joint_driver', + executable='fake_joint_driver_node', + parameters=[{'controller_name': 'panda_arm_controller'}, + os.path.join(os.path.dirname(__file__), '../config/panda_controllers.yaml'), + os.path.join(os.path.dirname(__file__), '../config/start_positions.yaml'), + robot_description]) + + + # Warehouse mongodb server + mongodb_server_node = Node(package='warehouse_ros_mongo', + executable='mongo_wrapper_ros.py', + parameters=[{'warehouse_port': 33829}, + {'warehouse_host': 'localhost'}, + {'warehouse_plugin': 'warehouse_ros_mongo::MongoDatabaseConnection'}], + output='screen') + + + # test executable + ompl_constraint_test = launch_ros.actions.Node(executable=PathJoinSubstitution([LaunchConfiguration('test_binary_dir'), gtest_name]), + parameters=[robot_description, + robot_description_semantic, + kinematics_yaml], + output='screen',) + + return launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument(name='test_binary_dir', + description='Binary directory of package ' + 'containing test executables'), + run_move_group_node, + static_tf, + robot_state_publisher, + fake_joint_driver_node, + mongodb_server_node, + ompl_constraint_test, + launch_testing.actions.ReadyToTest() + ]), {'run_move_group_node': run_move_group_node, + 'static_tf': static_tf, + 'robot_state_publisher': robot_state_publisher, + 'fake_joint_driver_node': fake_joint_driver_node, + 'mongodb_server_node': mongodb_server_node, + 'ompl_constraint_test': ompl_constraint_test} \ No newline at end of file diff --git a/moveit_ros/planning_interface/test/launch/move_group_ompl_constraints.test.py b/moveit_ros/planning_interface/test/launch/move_group_ompl_constraints.test.py new file mode 100644 index 0000000000..4bf608c942 --- /dev/null +++ b/moveit_ros/planning_interface/test/launch/move_group_ompl_constraints.test.py @@ -0,0 +1,21 @@ +import os +import sys +import unittest + +import launch_testing.asserts +sys.path.append(os.path.dirname(__file__)) +from move_group_launch_test_common import generate_move_group_test_description + +def generate_test_description(): + return generate_move_group_test_description(gtest_name='move_group_ompl_constraints_test') + +class TestGTestProcessActive(unittest.TestCase): + + def test_gtest_run_complete(self, proc_info, ompl_constraint_test, run_move_group_node, static_tf, robot_state_publisher, fake_joint_driver_node, mongodb_server_node): + proc_info.assertWaitForShutdown(ompl_constraint_test, timeout=4000.0) + +@launch_testing.post_shutdown_test() +class TestGTestProcessPostShutdown(unittest.TestCase): + + def test_gtest_pass(self, proc_info, ompl_constraint_test, run_move_group_node, static_tf, robot_state_publisher, fake_joint_driver_node, mongodb_server_node): + launch_testing.asserts.assertExitCodes(proc_info, process=ompl_constraint_test) \ No newline at end of file diff --git a/moveit_ros/planning_interface/test/move_group_ompl_constraints_test.cpp b/moveit_ros/planning_interface/test/move_group_ompl_constraints_test.cpp new file mode 100644 index 0000000000..b808ec894b --- /dev/null +++ b/moveit_ros/planning_interface/test/move_group_ompl_constraints_test.cpp @@ -0,0 +1,183 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, 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: Boston Cleek + Desc: move group interace ompl constrained planning capabilities for planning and execution +*/ + +// Testing +#include + +// ROS +#include +#include +#include + +// MoveIt +#include +#include +#include +#include + +// acuracy tested for position and orientation +static const double EPSILON = 1e-2; + +static const std::string PLANNING_GROUP = "panda_arm"; +static const double PLANNING_TIME_S = 60.0; +static const double MAX_VELOCITY_SCALE = 1.0; +static const double MAX_ACCELERATION_SCALE = 1.0; +static const double PLANNING_ATTEMPTS = 5.0; +static const double GOAL_TOLERANCE = 1e-5; + +class ConstrainedPlanningTestFixture : public ::testing::Test +{ +public: + void SetUp() override + { + move_group_ = std::make_shared(node_, PLANNING_GROUP); + move_group_->setPlanningTime(PLANNING_TIME_S); + move_group_->setNumPlanningAttempts(PLANNING_ATTEMPTS); + move_group_->setGoalTolerance(GOAL_TOLERANCE); + move_group_->setMaxVelocityScalingFactor(MAX_VELOCITY_SCALE); + move_group_->setMaxAccelerationScalingFactor(MAX_ACCELERATION_SCALE); + + ref_link_ = move_group_->getPoseReferenceFrame(); + ee_link_ = move_group_->getEndEffectorLink(); + + executor_->add_node(node_); + executor_thread_ = std::thread([this]() { this->executor_->spin(); }); + } + + ConstrainedPlanningTestFixture() + : node_(std::make_shared("ompl_constrained_planning_testing")) + , executor_(std::make_shared()) + { + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + + void planAndMove(const geometry_msgs::msg::PoseStamped& pose_goal_stamped, + const moveit_msgs::msg::Constraints& path_constraint) + { + SCOPED_TRACE("planAndMove"); + + ASSERT_TRUE(move_group_->setPoseTarget(pose_goal_stamped)); + move_group_->setPathConstraints(path_constraint); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + ASSERT_EQ(move_group_->plan(plan), moveit::planning_interface::MoveItErrorCode::SUCCESS); + ASSERT_EQ(move_group_->move(), moveit::planning_interface::MoveItErrorCode::SUCCESS); + } + + void testPose(const geometry_msgs::msg::PoseStamped& pose_goal_stamped) + { + SCOPED_TRACE("testPose"); + + const geometry_msgs::msg::PoseStamped actual_pose_stamped = move_group_->getCurrentPose(); + Eigen::Isometry3d goal_pose, actual_pose; + + tf2::fromMsg(pose_goal_stamped.pose, goal_pose); + tf2::fromMsg(actual_pose_stamped.pose, actual_pose); + + std::stringstream ss; + ss << "expected: \n" << goal_pose.matrix() << "\nactual: \n" << actual_pose.matrix(); + EXPECT_TRUE(actual_pose.isApprox(goal_pose, EPSILON)) << ss.str(); + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + std::thread executor_thread_; + moveit::planning_interface::MoveGroupInterfacePtr move_group_; + std::string ref_link_, ee_link_; + // mutable std::mutex latest_state_mutex_; +}; + +TEST_F(ConstrainedPlanningTestFixture, PositionConstraint) +{ + SCOPED_TRACE("PositionConstraint"); + + // Start + move_group_->setStartStateToCurrentState(); + const geometry_msgs::msg::PoseStamped eef_pose = move_group_->getCurrentPose(); + + // Create a goal + geometry_msgs::msg::PoseStamped pose_goal = eef_pose; + pose_goal.pose.position.y += 0.3; + pose_goal.pose.position.z -= 0.3; + + // Box position constraint + moveit_msgs::msg::PositionConstraint position_constraint; + position_constraint.header.frame_id = ref_link_; + position_constraint.link_name = ee_link_; + position_constraint.weight = 1.0; + + shape_msgs::msg::SolidPrimitive cbox; + cbox.type = shape_msgs::msg::SolidPrimitive::BOX; + cbox.dimensions = { 0.1, 0.4, 0.4 }; + position_constraint.constraint_region.primitives.emplace_back(cbox); + + geometry_msgs::msg::Pose cbox_pose; + cbox_pose.position.x = eef_pose.pose.position.x; + cbox_pose.position.y = 0.15; + cbox_pose.position.z = 0.45; + cbox_pose.orientation.w = 1.0; + position_constraint.constraint_region.primitive_poses.emplace_back(cbox_pose); + + // Create path constraint + moveit_msgs::msg::Constraints path_constraint; + path_constraint.name = "position constraint"; + path_constraint.position_constraints.emplace_back(position_constraint); + + planAndMove(pose_goal, path_constraint); + + testPose(pose_goal); +} + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} \ No newline at end of file