From 0d40cad7c951be1f57e81cc9fc930777c1e7f0f3 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 28 May 2024 18:27:55 +0200 Subject: [PATCH 01/62] initialize pkg --- .../gazebo/arm_sim_scenario/CMakeLists.txt | 22 + simulation/gazebo/arm_sim_scenario/README.md | 0 .../config/chomp_planning.yaml | 18 + .../config/controllers/wx200_controllers.yaml | 22 + .../joint_limits/wx200_joint_limits.yaml | 43 ++ .../arm_sim_scenario/config/kinematics.yaml | 7 + .../gazebo/arm_sim_scenario/config/modes.yaml | 15 + .../config/ompl_planning.yaml | 82 ++ .../config/srdf/wx200.srdf.xacro | 110 +++ .../wx200_trajectory_controllers.yaml | 49 ++ .../launch/arm_description_launch.py | 94 +++ .../launch/ignition_launch.py | 173 +++++ .../arm_sim_scenario/launch/moveit_launch.py | 0 .../launch/sim_moveit_scenario_launch.py | 0 .../meshes/interbotix_black.png | Bin 0 -> 1994 bytes .../meshes/interbotix_texture.gazebo | 87 +++ .../meshes/wx200_meshes/base.stl | Bin 0 -> 75584 bytes .../meshes/wx200_meshes/forearm.stl | Bin 0 -> 48684 bytes .../meshes/wx200_meshes/gripper.stl | Bin 0 -> 17684 bytes .../meshes/wx200_meshes/gripper_bar.stl | Bin 0 -> 36684 bytes .../meshes/wx200_meshes/gripper_finger.stl | Bin 0 -> 22284 bytes .../meshes/wx200_meshes/gripper_prop.stl | Bin 0 -> 22684 bytes .../meshes/wx200_meshes/shoulder.stl | Bin 0 -> 61884 bytes .../meshes/wx200_meshes/upper_arm.stl | Bin 0 -> 43684 bytes .../meshes/wx200_meshes/wrist.stl | Bin 0 -> 57684 bytes .../gazebo/arm_sim_scenario/package.xml | 25 + .../rviz/xsarm_description.rviz | 146 ++++ .../rviz/xsarm_gz_classic.rviz | 253 +++++++ .../arm_sim_scenario/urdf/ar_tag.urdf.xacro | 55 ++ .../arm_sim_scenario/urdf/control.urdf.xacro | 132 ++++ .../urdf/gazebo_configs.urdf.xacro | 93 +++ .../arm_sim_scenario/urdf/wx200.urdf.xacro | 716 ++++++++++++++++++ 32 files changed, 2142 insertions(+) create mode 100644 simulation/gazebo/arm_sim_scenario/CMakeLists.txt create mode 100644 simulation/gazebo/arm_sim_scenario/README.md create mode 100644 simulation/gazebo/arm_sim_scenario/config/chomp_planning.yaml create mode 100644 simulation/gazebo/arm_sim_scenario/config/controllers/wx200_controllers.yaml create mode 100644 simulation/gazebo/arm_sim_scenario/config/joint_limits/wx200_joint_limits.yaml create mode 100644 simulation/gazebo/arm_sim_scenario/config/kinematics.yaml create mode 100644 simulation/gazebo/arm_sim_scenario/config/modes.yaml create mode 100644 simulation/gazebo/arm_sim_scenario/config/ompl_planning.yaml create mode 100644 simulation/gazebo/arm_sim_scenario/config/srdf/wx200.srdf.xacro create mode 100644 simulation/gazebo/arm_sim_scenario/config/trajectory_controllers/wx200_trajectory_controllers.yaml create mode 100644 simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/meshes/interbotix_black.png create mode 100644 simulation/gazebo/arm_sim_scenario/meshes/interbotix_texture.gazebo create mode 100644 simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/base.stl create mode 100644 simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/forearm.stl create mode 100644 simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/gripper.stl create mode 100644 simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/gripper_bar.stl create mode 100644 simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/gripper_finger.stl create mode 100644 simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/gripper_prop.stl create mode 100644 simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/shoulder.stl create mode 100644 simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/upper_arm.stl create mode 100644 simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/wrist.stl create mode 100644 simulation/gazebo/arm_sim_scenario/package.xml create mode 100644 simulation/gazebo/arm_sim_scenario/rviz/xsarm_description.rviz create mode 100644 simulation/gazebo/arm_sim_scenario/rviz/xsarm_gz_classic.rviz create mode 100644 simulation/gazebo/arm_sim_scenario/urdf/ar_tag.urdf.xacro create mode 100644 simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro create mode 100644 simulation/gazebo/arm_sim_scenario/urdf/gazebo_configs.urdf.xacro create mode 100644 simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro diff --git a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt new file mode 100644 index 00000000..1b830ba6 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) +project(arm_sim_scenario) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + + +install( + DIRECTORY config launch urdf config rviz meshes + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/simulation/gazebo/arm_sim_scenario/README.md b/simulation/gazebo/arm_sim_scenario/README.md new file mode 100644 index 00000000..e69de29b diff --git a/simulation/gazebo/arm_sim_scenario/config/chomp_planning.yaml b/simulation/gazebo/arm_sim_scenario/config/chomp_planning.yaml new file mode 100644 index 00000000..75258e53 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/simulation/gazebo/arm_sim_scenario/config/controllers/wx200_controllers.yaml b/simulation/gazebo/arm_sim_scenario/config/controllers/wx200_controllers.yaml new file mode 100644 index 00000000..e273884c --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/controllers/wx200_controllers.yaml @@ -0,0 +1,22 @@ +controller_names: + - /wx200/arm_controller + - /wx200/gripper_controller + +/wx200/arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - waist + - shoulder + - elbow + - wrist_angle + - wrist_rotate + +/wx200/gripper_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - left_finger + - right_finger diff --git a/simulation/gazebo/arm_sim_scenario/config/joint_limits/wx200_joint_limits.yaml b/simulation/gazebo/arm_sim_scenario/config/joint_limits/wx200_joint_limits.yaml new file mode 100644 index 00000000..bed5ec19 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/joint_limits/wx200_joint_limits.yaml @@ -0,0 +1,43 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] + +default_velocity_scaling_factor: 1.0 +default_acceleration_scaling_factor: 1.0 + +joint_limits: + waist: + has_velocity_limits: false + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 5.0 + shoulder: + has_velocity_limits: false + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 5.0 + elbow: + has_velocity_limits: false + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 5.0 + wrist_angle: + has_velocity_limits: false + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 5.0 + wrist_rotate: + has_velocity_limits: false + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 5.0 + left_finger: + has_velocity_limits: true + max_velocity: 1.0 + has_acceleration_limits: true + max_acceleration: 0.01 + right_finger: + has_velocity_limits: true + max_velocity: 1.0 + has_acceleration_limits: true + max_acceleration: 0.01 diff --git a/simulation/gazebo/arm_sim_scenario/config/kinematics.yaml b/simulation/gazebo/arm_sim_scenario/config/kinematics.yaml new file mode 100644 index 00000000..6499f632 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/kinematics.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + interbotix_arm: + kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + position_only_ik: false diff --git a/simulation/gazebo/arm_sim_scenario/config/modes.yaml b/simulation/gazebo/arm_sim_scenario/config/modes.yaml new file mode 100644 index 00000000..127e54d3 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/modes.yaml @@ -0,0 +1,15 @@ +groups: + arm: + operating_mode: position + profile_type: velocity + profile_velocity: 131 + profile_acceleration: 25 + torque_enable: true + +singles: + gripper: + operating_mode: linear_position + profile_type: velocity + profile_velocity: 131 + profile_acceleration: 15 + torque_enable: true diff --git a/simulation/gazebo/arm_sim_scenario/config/ompl_planning.yaml b/simulation/gazebo/arm_sim_scenario/config/ompl_planning.yaml new file mode 100644 index 00000000..79a32ffd --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/ompl_planning.yaml @@ -0,0 +1,82 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar +interbotix_arm: + default_planner_config: RRTConnect + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar +interbotix_gripper: + default_planner_config: RRTConnect + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar diff --git a/simulation/gazebo/arm_sim_scenario/config/srdf/wx200.srdf.xacro b/simulation/gazebo/arm_sim_scenario/config/srdf/wx200.srdf.xacro new file mode 100644 index 00000000..1e2475fa --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/srdf/wx200.srdf.xacro @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulation/gazebo/arm_sim_scenario/config/trajectory_controllers/wx200_trajectory_controllers.yaml b/simulation/gazebo/arm_sim_scenario/config/trajectory_controllers/wx200_trajectory_controllers.yaml new file mode 100644 index 00000000..02d4716f --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/trajectory_controllers/wx200_trajectory_controllers.yaml @@ -0,0 +1,49 @@ +/wx200: + controller_manager: + ros__parameters: + update_rate: 1000 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + gripper_controller: + type: joint_trajectory_controller/JointTrajectoryController + arm_controller: + ros__parameters: + joints: + - waist + - shoulder + - elbow + - wrist_angle + - wrist_rotate + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + state_publish_rate: 50.0 + action_monitor_rate: 20.0 + constraints: + stopped_velocity_tolerance: 0.02 + goal_time: 1.0 + gripper_controller: + ros__parameters: + joints: + - left_finger + - right_finger + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + joint_state_broadcaster: + ros__parameters: + joints: + - waist + - shoulder + - elbow + - wrist_angle + - wrist_rotate + - gripper + - left_finger + - right_finger diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py new file mode 100644 index 00000000..fe7384fc --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py @@ -0,0 +1,94 @@ +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, PathJoinSubstitution +from launch.substitutions.launch_configuration import LaunchConfiguration +from launch.conditions import IfCondition +from launch_ros.actions import Node +from launch_ros.descriptions import ParameterValue + +ARGUMENTS = [ + DeclareLaunchArgument('robot_model', default_value='wx200', + choices=['wx200'], + description='robot_model type of the Interbotix Arm'), + DeclareLaunchArgument('use_sim_time', default_value='false', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('robot_name', default_value=LaunchConfiguration('robot_model'), + description='Robot name'), + DeclareLaunchArgument('use_rviz', default_value='true', + choices=['true', 'false'], + description='launches RViz if set to `true`.'), + DeclareLaunchArgument('rviz_config', + default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), + 'rviz', + 'xsarm_description.rviz', + ]), + description='file path to the config file RViz should load.',) +] + + +def generate_launch_description(): + pkg_arm_sim_scenario = get_package_share_directory('arm_sim_scenario') + xacro_file = PathJoinSubstitution([pkg_arm_sim_scenario, + 'urdf', + 'wx200.urdf.xacro']) + use_rviz = LaunchConfiguration('use_rviz') + robot_name = LaunchConfiguration('robot_name') + robot_model = LaunchConfiguration('robot_model') + use_sim_time = LaunchConfiguration('use_sim_time') + rviz_config = LaunchConfiguration('rviz_config') + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + {'use_sim_time': use_sim_time}, + {'robot_description': ParameterValue(Command([ + 'xacro', ' ', xacro_file, ' ', + 'gazebo:=ignition', ' ', + 'base_link_frame:=','base_link', ' ', + 'use_gripper:=','true', ' ', + 'show_ar_tag:=','false', ' ', + 'show_gripper_bar:=','true', ' ', + 'show_gripper_fingers:=','true', ' ', + 'use_world_frame:=','true', ' ', + 'robot_model:=',robot_model, ' ', + 'robot_name:=',robot_name, ' ', + 'hardware_type:=','gz_classic']), value_type=str)}, + ], + namespace=robot_name + ) + + joint_state_publisher = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + namespace=robot_name + ) + + rviz2 = Node( + condition=IfCondition(use_rviz), + package='rviz2', + executable='rviz2', + name='rviz2', + namespace=robot_name, + arguments=[ + '-d', rviz_config, + ], + parameters=[{ + 'use_sim_time': use_sim_time, + }], + output={'both': 'log'}, + ) + + ld = LaunchDescription(ARGUMENTS) + ld.add_action(robot_state_publisher) + ld.add_action(joint_state_publisher) + ld.add_action(rviz2) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py new file mode 100644 index 00000000..1466d718 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -0,0 +1,173 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, IncludeLaunchDescription,Shutdown +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch import LaunchDescription +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import (LaunchConfiguration,PathJoinSubstitution) +from launch_ros.substitutions import FindPackageShare + +ARGUMENTS = [ + DeclareLaunchArgument('robot_model', default_value='wx200', + choices=['wx200'], + description='model type of the Interbotix Arm'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('robot_name', default_value='wx200', + description='Robot name'), + DeclareLaunchArgument('use_rviz', default_value='true', + choices=['true', 'false'], + description='launches RViz if set to `true`.'), + + DeclareLaunchArgument('headless', default_value='False', + description='Whether to execute simulation gui'), + + DeclareLaunchArgument('rviz_config', + default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), + 'rviz', + 'xsarm_gz_classic.rviz', + ]), + description='file path to the config file RViz should load.',) +] + + +def generate_launch_description(): + + robot_model = LaunchConfiguration('robot_model') + robot_name = LaunchConfiguration('robot_name') + use_rviz = LaunchConfiguration('use_rviz') + rviz_config = LaunchConfiguration('rviz_config') + use_sim_time = LaunchConfiguration('use_sim_time') + + env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], + 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname( + get_package_share_directory('arm_sim_scenario'))} + + ignition_gazebo = ExecuteProcess( + cmd=['ign', 'gazebo', 'empty.sdf', '-r', '-v', '4'], + output='screen', + additional_env=env, + on_exit=Shutdown(), + sigterm_timeout='5', + sigkill_timeout='10', + log_cmd=True, + emulate_tty=True + ) + + spawn_robot_node = Node( + package='ros_gz_sim', + executable='create', + arguments=['-name', 'spawn_wx200', + '-x', '0.0', + '-y', '0.0', + '-z', '0.0', + '-Y', '0.0', + '-topic', 'wx200/robot_description'], + output='screen' + ) + + clock_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + output='screen' + ) + + arm_description_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('arm_sim_scenario'), + 'launch', + 'arm_description_launch.py' + ]) + ]), + launch_arguments={ + 'robot_model': robot_model, + 'robot_name': robot_name, + 'use_rviz': use_rviz, + 'use_sim_time': use_sim_time, + 'rviz_config': rviz_config + }.items(), + ) + + spawn_joint_state_broadcaster_node = Node( + name='joint_state_broadcaster_spawner', + package='controller_manager', + executable='spawner', + namespace=robot_name, + arguments=[ + '-c', + 'controller_manager', + 'joint_state_broadcaster', + ], + parameters=[{ + 'use_sim_time': use_sim_time, + }], + ) + + spawn_arm_controller_node = Node( + name='arm_controller_spawner', + package='controller_manager', + executable='spawner', + namespace=robot_name, + arguments=[ + '-c', + 'controller_manager', + 'arm_controller', + ], + parameters=[{ + 'use_sim_time': use_sim_time, + }] + ) + + spawn_gripper_controller_node = Node( + name='gripper_controller_spawner', + package='controller_manager', + executable='spawner', + namespace=robot_name, + arguments=[ + '-c', + 'controller_manager', + 'gripper_controller', + ], + parameters=[{ + 'use_sim_time': use_sim_time, + }] + ) + + load_joint_state_broadcaster_event = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_robot_node, + on_exit=[spawn_joint_state_broadcaster_node] + ) + ) + + load_arm_controller_event = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_joint_state_broadcaster_node, + on_exit=[spawn_arm_controller_node] + ) + ) + + load_gripper_controller_event = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_joint_state_broadcaster_node, + on_exit=[spawn_gripper_controller_node] + ) + ) + + ld = LaunchDescription(ARGUMENTS) + ld.add_action(ignition_gazebo) + ld.add_action(spawn_robot_node) + ld.add_action(load_joint_state_broadcaster_event) + ld.add_action(load_arm_controller_event) + ld.add_action(load_gripper_controller_event) + ld.add_action(arm_description_launch) + ld.add_action(clock_bridge) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py new file mode 100644 index 00000000..e69de29b diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py new file mode 100644 index 00000000..e69de29b diff --git a/simulation/gazebo/arm_sim_scenario/meshes/interbotix_black.png b/simulation/gazebo/arm_sim_scenario/meshes/interbotix_black.png new file mode 100644 index 0000000000000000000000000000000000000000..e7443eb557636b895a5399b0659681f0c1063e49 GIT binary patch literal 1994 zcmV;*2Q~PKP)pP;}5cyh=>4yh{zI=wFF>_V!z*)$0Jz_{|m5^ena?I`40jjA^;?5y4&p%r4XBM z-aDKYt|6xSagQ`jx4WHE>NfJ-60D$bmN2~RDERpCW13|s#>skL@C_Leg{|ww-bs@D z`03N9AAV3ta&-Z(51#@-ZRi{A=d1vQ&|2@dTc%**j`s&a1lHOq3c#@d002pn?6zAD z2k)AKbNsVp?UE|!fs;a(rHC+NJ3WN=ycw7R`OD|e!>lmb9e@BLDfMQflp<^Ey0%1% z+3`LzC?ZB&@CX0^bS1$2Jw$|vTI(!Li=z1U{Cq4+01&eYf&yr|J)+uL31Ec~U=bHG zT%!OYwEAeRg_O_Vz8PbzwFDq1Shz#w0YLuz`7;0@G+J?U9&9Jhpk)+kt(8($Rn>LP z)%3hA+z%oSOBO&_%T^FFQ+^jB;_N7;{Pz58jN!Nd08+|}C1b&^Z8h@epMG*5JS*@T zv+(7MqDW_%iWO0#zvbX^kuBUNWXfNE`|Wf(HIMYK5Cu}e-CPQZ<2ch=DMdsTvtVsw zTi1u5WkcwT06=G1UKGGyz@xA}!4caf1(rY{YOT{WEsS}2c>$u+>BN~50OBZGuU9D6 znoI-$U9z=9q5iw0uV~qC&`Vl4E9o+40qgiMQ#4Jq#AkDJU z7;Ej}aIn?_fGNvk*8tV^ePj@dIFk95a2wU!7pvUC8>U+_0HkRO09tEi{DZaswp^5D zDTH7jagTn(ZK&h%$c>RuoM~O;`MJnt>AMt!r-lv?NAG1K_|X8s6_kvkVxH$O zFE6~@D2k#e3INEmj2j`~&PpjMWtL@e9M^S?h?A+fp`cZ_N$;)bj)j^C0WRJe>^Vj6 zOIK59t+OoSOPkH+g83JssqcHs(7941(>l*{@F(0`B=hS6{z%s1t84Wk7rv?cn-rHl zhzro(y$hw7);d2N&V0LCU|H2susm0KvjLA|2QFFwpywdquHM;(-zb7?)u*5bX2B2R=dDg6YwexCJ6yLE z!$B4t{n$;>2w>ql9`SmF#ws>}8TdQgT{_PGa)-9?+NHR!Xs<>brf~pZ3hu`iR^+P9 z!vi-uO13}F0LUeR)*^0H$q*@cD{0v0G6Nw`FoxW2w*XL8)$v#+NfJoGqW+^pjIZjPSl7=2I1sE^5+kB9 z=IhtTqA0F#vZGTJ^0 zOhrMOePjrP5JRUJVwfw|T3aXn%~3Ex^HdbPOn;|#VE-W_V&&T*x!dh#F1Hy`#S;{a zn!p9hv*zpX8yjj33;kCDLm`YYPfuU-{NiQe!d*g$YaES{LQI{+>pY$Ryg3CS{jQ|B zk3|;F_CHU->2%_{NX2n476vYddbs5Q!5NR}2r*AVS(a=CuPh2~FzurlHH7|GQxtHr zDT?Cp@o@zOgI?eFT4yy6$Y(1=hm%a45DH5kk)@V|&7ni1Iwo z^TSjuoHC4iP~O2&uq>0~f*VZ1e!pL}ae7zEGnj(O`e!N-uoZAR@JmCN1%^e;9V*Wd z^Jf)CDR5R`jCp)~EQ(^*3hwTeR zVK>LR`5Da%jTPj1XWaLoJS?vN81r=@08~|FOz9t2eXGUXwGlf1^ML>njVYd + + + + Gazebo/DarkGray + + + + + Gazebo/DarkGray + + + + + Gazebo/DarkGray + + + + + Gazebo/DarkGray + + + + + Gazebo/DarkGray + + + + + Gazebo/DarkGray + + + + + Gazebo/DarkGray + + + + + Gazebo/DarkGray + + + + + Gazebo/DarkGray + + + + + Gazebo/DarkGray + + + + Gazebo/DarkGray + + + + + Gazebo/DarkGray + + + + + Gazebo/DarkGray + + + + true + + + + true + + + + true + + + + true + + + + true + + + diff --git a/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/base.stl b/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/base.stl new file mode 100644 index 0000000000000000000000000000000000000000..ff1b0bcde287a862a6ed269756fb741e190431ca GIT binary patch literal 75584 zcmb5X2bdJa_Wr+s7!VK%0tSMx%Yq6hnPsMT2EzylDrP}3qKE`9f&|%BF$c`)8c@Qn z1VJw%vg|Z7n6q9{0Wpu+>s3@l48QYMovL|z*!%rIf1iiPdG?%7S9ec$ojUc_sTp~~ zuw;kC2}gG5*S&A|p4|`Yb7+UbBZiGQJ~nUOyuUmA|NrlchDAla+YSw1es8~^Nd7CE z)gk-i$4v(p6}3#}KQ~KEIQPKprwRM_T#5;{)J-%?VdU?TsySz_cp zowC)}H8E{R3AWTtpp6sXxIC<`ZfM$IBLBHrqNu2I_R~ybZG#E6)J>p`qN2*MqSuj+ z;X3k!J#M|CDaoRu!ZzwAT8d}qEED!~%VaL~&5_f>|6DaEEe%xWy`lu}LcO91a+E@X zEqU60gOVggI`~9=EWKqN2J8+}$=z=x5wrIjgu{2iowK{AY8olKFcT zCE9sQ-GqK0fqoy-1{3bH`39wJ_nnKkC*(WumORY_+K_7qaEEcHn8<&|b##^^;W^gl z$`fpobeM-=`SLTUkiLcK7J~4k}Tj>W2+aLnbz_-ps{xhDE(zo{uu03EV+9+a6{&Ta$ z+}PKNanGQ=LV_)I6S$72UsxZ%`o{654JPuRnu%&JSZM?f*!}u@% zzTC9IME-NL#Gz#$C0g&buU-ceY^j?-8!IMtTl2@?6{d|m(JayLrjIhOG{kWW+o+pp znPejW8FzPjpIy@{Ubff4mi*^ti5{yz&wTTPyH`xGrEUV(amhZTrbfPdPCw?qjyF5UTF<&OozeFySA(Ks>s z?B*>@$af>O8_7h8{JHa$=ikl3vtkPq`F`d9BG@b6OZ;C1zuSD@c6v(ucfbU$+>H$W zH^E-=OjB0W=!hZ3@9e(8JXiUrJvzSH(s_9D<45-}u_Twe^Q>g?mB+W)3*R|gNeW#+kJ3m+HnV?O_QwQZ|n`M1!u z;f}>#`S%}fq^2%iIC|STRfPn56%`Fn{!q18MzJ6+ZP}-4&TBUsf-Ou&0THljq{%N~EYUU}*AM>_X;eJpa2fGzoV8{dt4Z_viC|60;4_1(F35%NwT6Ka-_ zBW2LOy^iKp3#*3&bIe(>g^w%VS=&C^*kkLdi)NfU)Lpr^L<#q9f{?pg?CvffEB|gc zOUO5yb>CAig>R3(L2qFK&vE`cm#1VQJtdxCFT7Fi88c7f!Y9)&HP-Lk6YPbv%0KN7 z{kE!j;NJIlc7(GqfzK|p$fX(`@v^*8C%R|XAB(;6?;!5gf}cMvF4<*}d3M=?>#*;( zy;rX7T&ml(jY?MB^NxGkeH%>VdkM60<(I`J@^4Y$b+FeND~hs3MF;46&IDVS$oG|K zWAmwvOAh_8uW5q`_R9B(Ag+I6O>wT|EJLs*--|X&@R4$<;qBWNpZdbBJGH??zSl+@ zYxmf+;I8Asoe1{IM^q4xc3IY>-#*<-aId&^_L9HL+vM)mo9Ervc~G*sBfN!){CEXz ztQ=WZ{M`wk7Hb=xV6Xg021LcxHx;)z_-RA1B|lCu_sSm$-}%B~{3~pO3HdfPBSRbi zi(oJG$NVJTP`u)v6HFUyVM3lt&0f*Qf}hWpr+uw^qx`Yhiz87kH9vDs7x`y;AMP_I zHoq_1=b=AM@63HnF7^6;TNf37dUZ9foGo<_(l6qyzVE$j*Aou8=2jw%g$ezv#O|-w zmg3+4AdHusT?UkGe1P8+Yl_?2zwn2H{$FG_KbDJx{FK^Usn2x13|n3Ir^L1kN^kRzH6^uUM|u zzZ34bXv6(IO04|lm_=-1!hO!4)%srbx}5gJdABHGzFS9A8<{A9(YPaEse?#F&ML(B zfoBJvd|Z5Xe~%Ikj#%mmYJ<;E&uYPI2}FW$z%8GF@b82>uC`(RZrfPj>k32QIvfE@ z0b$P8pOu+O)$hZ<2aeWjGJlT}xDL5$Z(+iHwgld!e^qv@djFQankr%514mOEdUqY6 z-v`<-mO2Q%SD43ea~bk9_&0~!bAOLqN3imj*A}vc3HLdF9YZedCw+TS5eWAVIvUsE z{{FuSSPBXE-bCgsi#Tq(Y4TUUQS~b5YR^;$5EHHGa_&{5_X)M7P5}-l`z?zQj)vEB`}RzZ|Knb5!x1O< zS>A;$brH6WONN~jF1-2M_^C@?j1Z27*Z9Vrlija=LECV|g}L2I*-{r_+xTziTKXn3vvp=I*2lYY`R9Pw1~{-tcGi?D6%-KH#jy{3CG>&^ZV!qM>B)VM*i z$?=V|_WN+e!ZU`KvZXGVMN)+i*nFY1fvrr7ps@@!Ekmq{Azw23`Bz5+NK7ua=4Z6F=@K(>5G& zUia0dY^jT|Z5%Oi&-A?yULM@_?Y|;~qv6$X!Exo|ZVt2!NBnX0&82Lqi?D5=FGSQS zd*Y4=;b?7RUA~P1!dvPh{B^`}9S>HRvtq)0hL@aSCV#KwIt(%U^T%=J-Xd-6MA$au z?#A(b9NIrhI2vAZr!)EQ+!4EeyAa=?x70=0HsmRZLP3#^0WtdKYsdVScGsiyyVHxH3g-sZsbB7~#i zC2w#x-FHi456%jB>;)GxsQEOI?I* zL!w%MI5@f962vHfEcY2+65+D>SnCLloZeCwVcU@C93Y-+boPWhF1#dCXJx#iV+KdO zaQY~W0KBCx!nPqJnLx%qu?=tEQNo1#3@;gRWbe<7&yRf!p(7b@DI|)TpPe83fT-2c zk|%In=cTk6cbU^zUv865|a<9^IX@(I6`{SYPQrx*fzAM4yC7#5RQhI_S)KpBP1GBv!yP=wxJOxl!y}{ z91SmxKH7#OB&t=jr7ps@p^-C`$QdCV4KIzN+J+-eoO5?ITk0Zg8#)3A7hMr091SlW z4QLyVxb)M7)oiJYux;o_CVX!A(g@*bZKGVqKH5eB;VpF${yH#*3pXq^XT^m13@dKzz8N8(~!nUEAS2*U`gCm5a;Uzs0@YEp-vL4T%QGozr)B-j2TAAIp7) zmqZ5S=lS?y2+6h)qr9aq!nPq%4RZi#$><}5qv0hH4)YA!h9ef8`V=Ckx70=0Hgsmf z#B(OxXLw1ZPP#asOF2SjKQIFDmbwVrM)BA2u;=)0L8AYAgH9dVB3sj`N%HR=M<$0h zZIwNzYR}|(8wMmFscxTb{M(<2ZMkD)7H8}|gTw7zi-Uu&nNY?SCeCZsAvM5B(@{McGIpURpmQ{BCZmpmmSwWo%*M+rPSH z7e4Y~qQh~+g}5&@DtzVK`oXwQ$_6pPUYCyPolW1KPK+FNk`S{W92E}RTrX<Sw^; z6Ty|ev%AF7iM!4`PKbIBjtw{Lu_@kcw|0Y=V6VxS4a$Ce-~)-*&pBC$#>>Zs{j(e6 zThIKnj4ez&bj6_T^llF%z8rJB5FdRoDLj7cviO-}q6B;W{%tZlec7bMexq?6o8F!j zUUu)Y_>7a<4Ppxub92dT`8AUhXN90taQ5jH@lj{hlrh0x?=C$mdqT%9iK_U*oqaBfZG8Kh7+aWF_s+0vW&gd(Z>=~)h$oMk z9q#+}kk~2bzY}8%6My}EOtxo--sR=JItp>wnAzcg*5}5i?*3woElgZ+!LaOwr+il4 z^T94c-2BkY@RJi-$5$TuNsKK_WKTLeJ8N*?#1DTTB*c#IXNGOtw~D`7{$`9VOpKkG z%AVN%u*BHs`v_6~%+&D0%w_QgPkj<0*z5Ec4$scpK0UGY*6u>Q^4rvK_dln`*L?C` zj4ez&|Iy&=mi?zChRp0Q#J_Hu7=FKCNqmpdA4drG`XU?8F5Ug1#QEp<5F)6a7=AZ& zN&Jx5mKa-@xGEdVrg}e^C>f1!aBchZ!zE{LjNcnX3HBOyV!!OnuWJ(<=i)jx>^Lv% z|Ki5@bN2er zeoIt7fTv_wqhaBVjoSoAxBWQA7ABr*(ka{a!R?7d&HFL=$5`0?5ovSuor*-xzwu0W5aO| zZi-(w``j2u0B$czymQjn@Q#X&@#F3p9U<6@pJRzp%O{1m^jR9;aqH+9M*wawO58nd zQh318%i-hX0_nh9rq784v zS$6K~#wA@X^deC{Rv`FE}N&dR^!$T3~z9h`SS@35jm6J=J4&#Zw3U@3nD(Sf=-uAo<2UsR7Ia=8c?bFJiV_1B_Xw;heGW{u$PQtvvLG8lFg-#IGQm%1F7?$bV{4DAtT4HOjG;mq z|CoCsW1Ub&Hs+3=wqfD-<%X6H((1B*LHm%^X3(Uk~TMp?XoF-&BJ2@R}u-7RTzgFp7$Ck)RR5g<2Ug1zqE_Ge~hb^D~v z4YqaKC*0@w5dm@owlMK-gFiFzRvnTyHz>KXG(2>5d4Sx23HIW*S+djzx`uzA8wAJ= z*uuoZ2exOfZPF=ebAxfK4+#hVSQa2RV1m8)-OQyr{kLm)-_!8`xdB_4;P)|?>b|L4 zc=bzz0^|m4VIun0-u`meaM{*afZTvBOzi#HZ<+d!c23&dVCF%s!+$It79clZ3lkfM zevrBG?(RvO8!UdIb$DOpumHIMTbP(~(+8Pd%MVG~++ftK`eCPurv=Cjm|(9{zpBlg zbYj1x%?$=`uOGG@c3ObkfGtdXGru;|xM$y_%?*Z)-I%_k`S}5I118w(<=r02v|1le z+T5V$Ya7#d?|OcK+<+}ioZaie%#+zz(&h$ltXYyiAU9wO6Bq9{Ez|Gg!AYANjJs%h`t>)bn%sa1_Nt%gn>q8Oqmwo_n7jMb z^dnzPHMs#>n0Rmh!!mo$OeJk@u>Rq;>3ZvCn%sa1_UdxVXX~E2U|7=T21}Z>Nbm8= zOp_b1g^A9mzrSwiO+%A5H<z!oMX zZ(b)^vgQUCJu$PkS5RSc1GX^1ks+5lZ*9Zcm(Hp%xd9XG#qlbaI{esH>8bC{GPwaq z04BJ-Tam=qv4V1m8)InJftcwkfdz7}Hx@+~;Fz%iaswvVi{HmwYUHp6;b)^y50D#h1YkN!eDzVi@bPw|0^|lvuow3$xm5kX zT8Bjs9}^%q;0VBUlvuUBRrvFF!vo|7Ot2UCk1|3Yc|h6pWwT9gfVmy_9<14~cR5xW zBR6>B@fFJ_9eRxB26xTdd%!Q5*(Nt&3lo#ye}-=8Fu`6zuXF9? zQZHV8Va@p)W}DoA-$8y?qQv)=N0oKmGTY<^Y+-`i&ZX{|a@N{`H&vL?D|_*K7bV7i zv1{$%8!F65pDj#`KX2RPm+wAIa|0n_brI|(bl^s^k{j%~bxq6PD$FRHElgl8(T#&; zMtDK$%|K>cG&g`|AliY+4VYjrXa+Lhuekv<1JMplZon2Mpc%-Fi{=K<3`9FH zxd9XG1;=t0=KD1_fMy`tfyoWn!UQw}nQ_tF0Gfel2PQXQg1w*_ z$b7%%2G9&dJ21HcTbO`mATutS8$dG;=t06avi+pc#mEU~&VtFaga#=KeG{fMy`t zfyoV+U@vF}q7Z0q0L?(O1CtxDg$ZZ|GKZRZ(F|lxS#twu2BIC9+<*!8 zf@UBJf#wF#3`9FHxdB_4fMy_bw&4!=6F60duLHd% z?lu(H=-q{8AliY`4g9@gf}hY_3Yvjv2QjQELav1I5AF#vEsSh%N1++We1Eys4WJo_ zb`ZmwBDOFA%|K>c%B^ky%|Nt+IMx&~!Cue|WWHb56hSi(?I4adMQmXLnt{x?=$ayE z2BICrv8IR#_JU?0^ZmM}2%3Rt2XU+^Vha<{3}nVd*AziB5beOMDPn@Xpc%+~zpg2Q zW+2*uSyRLoCZHL}jEk-*f@UDvfmu_;1baa$%|Pb+bxjd81JMplZon2Mpczb&@uB4`Gp z9hlsJElfZ&khwoyQv}UGv;(uIhza(BW+3zZx~2%4foKP2O%Yp|fMy_bn7XD2nt^Bs zW=#;=t0=KFO`5i|qQ4$PV&wlD$BK;~j~O%XH$(GKERQ^W*&K{Jr~eqB=p%|Nt+ zIMx)gg$ZZ|GN-I-il7;Yb`Zy!A|}`int{ysms{Ncnt^BsajYp~3lq=`WNy6N>ITpZ zL_3IKO%W69g}G8>uy#!mGy~BNVpvne7A7$Bg)CXu6p3aa+CdC!irB&gM+RAwDw=_4 z2QjQEVuHOmUdcF7Gy~BN;#gC}5r7G9FPDO5AlgA3Yl@g)FTQWN6f^_T4&qo-#1Vk$ zC;`ntw1YU-6fwbG{Qc)r&HvGZ5|I z8LTN{3lsP(sz9yP4WJo_b}#^IiV6wmrL@$TAvb_#AlgA0))etO$nQ#&fMy`tK@HXv zv4sh4JD0+&wrB@~u%?K;_`Qn~PynC=U`-KQn1E)`rr)pKZEm1MZ6U#4N{2Q#!0fze z2LrLDh%HR$I5@Pq0Tciz0a#PS7ABw>oLjtcv0YQ7#2^rCVS>lnvPYvmt2H<)wlKkW z!Qb7rxVwBEdZ*W+=j3}Rdxgn2DB3|8bOW|9!B42HrV`CSw1ZIV4UjKoaZhwqlf@l{ zW+3zZ8DDP@h;|Tay@9td0nI>WTr$4iAQ0^!)OrI?uopA~neWf|dV@f;gHY=YyoCv9 z1~TK4@%09QXa}Lz8+d}fpc%+~f5z7v1fm^;T5sSjOh7Y`8JCQ&HwZ*KNNc@;C)f*` zfz0=3e7!*++Cf_D4ZMX3Xa+LllJWHhfoKP5tvB!ldqFdh`TmTrHwZ*KNNc@;w=eqq_y6_6YK@e zK<4|``Fevuw1c$P8+Z#7&`!l}YAQ0^!t@Q@p!i21| zP0E~E#@8DJq8+5Q-oO*=1FMh5Quh=)_Ma^uopA~neWf|dV@f;gS6HgcncHI z3}h}gkT}?UYfmTeZ4^-+CixG2AD}T7A7=9 z&iZkTk3W-Lr_WH9vxfoKPz)*E<&y*OT(dV@f;gHY=Yd}LvQ+cWhBfoKPz z)*E<&z4*SFdV@f;gHY=Yd}QH>6D6P-h;|Tay@4m#i@$$UZxD!fkk)zwA6Yo!Lqq_y6_M;4Aa zQ39HQXa{MnH}C{|alc~f4Fb^)(pqoeBMV2IC;`ntw1c$P8+d}fxPLVD27zb?q1GEf zKQm9e&b?2h*OZHwZ*K z2({k8zk~eVMG0sIq8)@UeF9=zCY{h4dS95nB0IZOh7Y`8JDcDH;9XN z5Nf@FKNfpIGm!cItgknSi*^udy@9td0nI>WT(Z92ATHWLsPzV(U@vF}GT)!|^#*a# z4nnOr@D?VZ8OV%F*4G;Zq8)@esPzV(U@vF}GT)!|^#*}x2cgy*cncHI3}n70>+1~y(GEhbH}C{|Mc;F12BIB= zT5sUrb0(k}$lOoX*Bb<)9fVqM;0gBPchl4x1fm^;T5sTcA13&HH1!67Xa}Lz8(=ks zdAFH}zO~Q{L^}wz-oQs3CZHLJLXh?K27zb?q1GFC3lq=`WWGP^>kR_Y4nnOr@D?VZ z8OWSj*4G;Zq8)@#@wcfxJ>;=t0=KHh0-XJd8L8$cx-ogYl z1DT7>`g()7Xa^=Y;A62DGy|FM&-!|UxM&9^H((1B&Ub^Z{BMvkJ(GE;*z!oNSWu4{*&esPzUuvT($S63`4pI|#Mjz!U7n{iCTjs1fbJ>53jQ zldHJ_Gy~BNOm4syCZGn$?7ij&&?Jhn8#Fg~Ni+k|4oq&q?;yV`Q39HQ zXa^=YU<(u6wqFr%Mz8F}?_HG8>H#y-XA2V*<=eW;Y`W$KTA{EL!Cpe6a@OSrq8Z36 zy%~kGg$c|U%ABH(+qHVYjP%*UMA5OEWl#TMnj2_^f+5(#1dp|S6@)n}wlKkW!B=yb zyUW)hcN&#irsw2)XzC5hL_09K0b7{hCp3q8gAjWaK{tSA;Ibka?_?!YF?ST20rnTN zxdAi->{SHafGtcwGmse<%?+R#V6P(R228LQGz07}WOD;(2H2|zx&d35fMy^wE}9!a zGr(R&&<&ViFK7nXU&!VL&J3b8zyy0iGr;~rHaCD~fW3;48?c25Xa+LlqPYPy1MF3V z+<*!8f@XmIg=}sB%>a89Ava(P6VMD~#zk`jXa?A;2)O|h>;=sL`wQ9J0Ga{zDnf3+ z7ABw>$Q-}s2G9(!R}pf92ti)_Hk*2bTC-P?)FQKmj|{W!^fGte$`)KM7YOz-las#$75q)c+8DOs>$XL|Z8>F#U5pn}2*bABgb|kX70W<^bRkTKGk=eoo zGy|FY)7$`>0ro0FZomY4K{LRPL^d~oW`MnlkQ=at31|i~hpD*%Gz08agxr7$_JU@B z9f@pi0L=h<6(KiZ3lq=`WG+^7184@=s|dOQ6YK@e06P-d+yI&Z_9}vIz!oOn5zRp6 zlr=YiW`Mnlpc^p3UeFA%BazJwpc!DVBIpKeVFH?g%#CYq0L=h<6+t&(g1w*&xcO33 zZxCXyBIpKeVFJp4%aBdIL5RJIpc}A-362b=-XO$YMbHhHU@wkWrrsdLUPaIiI07)i z?U{Om5PKCtH(-Lj_`aEXgAjWaK{wzCz;u*=W`Mnlpc^p3Ui|%=dV@6fDnf3+5rF9^ z0nGq=6(KiZg1z`THuVN+>{W!^fFl6YQ39F)_9{Ydzyy2o`)KM7(%7pAxdBH2rlSNj z1MF3V+<*!8;(o={8>F#U5pn~L08B>-Xa?A;2)O|h?8W_~sW%9*R}pjryaRaRwcY^x z2AQ!slmYy+x&br;>{Uej6d8-m<9h;{0d^s>x&br;>{Uej6d8iO&=!v1>kUHeRRrCD z-@(Yc5+R@&V6P(R27D|gqSv82>6p!aDaksVJz~94M3Yr1-Dx!Ug%)MfQpHNe85Mr+)tvA4ak7ktx zGA)d3a7UpT$bA1gUvCg%uOh8C@D?VZ8OV&wI$v)PVy_~tH}C{|K{Jr~eqB=p&A{wc zWa|yQg$ZZ|GUK9ail7-_uOh8C@C187Gr;~rHaCD~fW3;e-oRU!fMy^wF1n@&ngRAI zLT{SHafC=`3W`O;LY;FL}0DBc7 zH((1B&38-&=a2)O}UnBe!()Ek7@s|dLPTbPKxwa^T(R}pdpwlD$B z06P--dV>&q6(KiZ3lq=`u)mPa4WJoduOj3IY+(YL!4w&f`g(&9dlex!V1m7%8DM`Q zn;Sqgz+Oej4cNj2Gy|FY(=|oV46s)baswvV3z`A;7qYnlGz08agxr8FOh7Y`IZRzs z1kC_@6(KiZg1w*_$b7%9DS~Ezy^4?++m zpc!DVBCR*@7ABw>$egmSDS~Ezy^6Hnz!U5R%|Pb+*ZF#b5PKDAy@9td0nI?>#@G3J zgK(PcRiyO>o?tJ`l_G=n^#&pKD$;rbZ(#y6U&xYuy+Me*inQLqTbSU;VCoG*>{X=o z2A*Iqj#sAMAjDopT5sSZ3lrR)sW%9*SCQ5mc!It7zL|Q15PKDAy@8J`9C4xqGz08a zr1b`#U@!juO}#;gy^4? zB#uOj3I zOt2UCkGa%~kIxQoO}8(5Xo9IXz|(H(ZZLn1IUQM$YnF{ZSFN3inx?9r9us z6YK?*;q!4HYlXtN9Tnk@%Do1iGP3?4wkTmLfcZ$dR2w;~nn{gnk|nbuXT^k`p{W_p zr4G5GB0S@*7O|tojfnBF*em*-?{kB+v100=K5H(n>t31T>Rs2$uuDak*uLken*LX= zixBLk--me%a;eE1dIj{%$kj z7mlAD^f~#OWoScM#|mus4x;AAyo2(@BnQghEwTCjLxa{0`i6&f`xd#gZ=KI--@A5A zewf}rdtprnc}hAr?jFpQ{c$fHlZ>;4iRem>5l?mtwsk8D|NCi+2*F+tPiT-lHP=3S zzzOZ8jgv0!5)Az-9!}n`D9#opIwp#e?=|m~z2?vTg*bghr{Jf`WO(7|A7gA`qVK-H zCq6i`B>UIXZG|}V-|d3OryLnxFm`K!abaYrAOyv zpBFBeSQKXq6Va75TXL_(Ux;52KKb82Vob1?`vvm&V+Tna?KiE8SMPFhI7bM!Fwtnq zJ@U)P!}Y8-{I)p$M(M=x?g>S4wlKl#XL6}&=RF*c511TYa^4>?CfMuAXXcnT?7jMI z*0u4cKb#V-IdDgeElg}opJNF74UXP;X?)7Kso~KZi{flyf>+!~zZ;$rpZv|VaL85L zV@$Bum=ST)hJCKS-8LZJZNv2N>3Q2?Y++)_>VphnpWQ$AD2{huJ|k@K{*D-1n8=j1 zHH3XX>d$E&A24WU_OglN#WL7Xj2@H(Jes?XjZ$7<)# z3U4`TYm5o@YSQOxvpT`{B^3i6k6n?vJRBGMF~$}qZr}E-A#8uuKYn%W$;p?8FElEO zvxSMR4IVLs?Gr}~9vExg#<6wZOrcow|^VKMt#F=R9GM{rPxsJ8&I=FQ{E4e;%R`y;!@X3O;TZhavQH?E3 zu+o@I{e9}d+A|toZlW3!>?PlmX~TYlb4OfVyPJIHh-z$MLY@Uf*r(+FV;`@5@uY^I57Y+-`+&s^%$>CMxh z$s2{J#squGyJ_06Z*9}RI;Ve@_Z(4;ElkM!ZwT!fz8;X?_uc6xs0=CGd!H4{ z&PcEMcAAN5Y+*wBF+LQW4g`U^GsA@3lpsT=2BB8e4cJB@f=Z&3HFjW zpUIB^X3hRR-Brd6h-z$MLdFuA{J7-c7wdjJq=V5!vUhT848b4mVMaElkMRE|VYi<-*qC?>7xK zQH?E3$cQtOAB$f3Z0oT8^+QcmV+#|kV9WZeciV+eUvi|0YD}<~jDs`z5%Skhb_&N; zBu!Lf3llQd&gMtqCr|7W4*D~0q8eM6knwyrKhppAzq*CfB{x7+V+#|KWn}aDN6#kR z!;d8YKvZK36A}$bqn8{fbU6+a?1j&f%qw)c8Y`7N0}XFbHV(qu5qw-$7o%+q+i`Gr zqQG|AUYRAUPh=oQch*r;#NUrQrb1K})8u*#83Ay1QhEsdzg1bgA{i1qefAy1Qh zEsdzgmO=vK3HuEqPm_Etja&_cvoH}=OpvEZzLrL=#sque?-&=^=L&h6L7szXzyy2Y?}#BbMj`9^`lXba9fZZr z2rA)b3PI@XAc#}SdUn_OM6X~eZH^mlL*t-{Y8qusRMRMGBALc6jcR%wgWPqv)(g){ z?nt@4SIE;OUz1-~_4mqHnBZTp%%za0Nxl}t3?$Ci9gDs2cln;m?Kg-#P4cxEay7OT z67npR+ouG1n&fLS%s_&0mO=va1NKQoo+kNP3^R}*oTZSEw;*BP59Dc*uf;I~3Bp;J z;9ubsbxGbR$=Bk@)tF!}{9WG7gnes~r%ApRN3O<}LPFmEgzXuSr~Uc$fH-C#K{!hx zfjJ@D`yfw~d@YU{ND$6aNJ!6@uze!(G|AWEn1KZ0EKKlkgXU7m(Zz^0fd_jS2SB zad5)+CCJkxUkeb`*usR4wUf5LLY^l1T7am=7AAB&pR|1<@-)fU0z@^oFrhg^()QHI z(uw_Ot?bo5{292M?Z(;krshF6w?(pR3 zyH3v7>o`I75ngd-W%`M`d)F|*UeR5-dTy8;T>Mj2x_5XNe$(6UvBgBgH_MV|9rdub zvE|)K!EbjgOE)^`;ekxBS9Eu=4f2a5-@ULgy>7uZYuLg>bkDPPGtUiP`TC3W+%fmjscN@qSCZanbOggGK=skOIct`qo_^xDtP445#jR}^sHfmy`nohbP4we-g*Ay@X>=a_&rF!KLZo5w|F+O zA$ORzapY(9f}igj6}J2Kqcu#hS9E8JCZBDLA0w+we?Fu&e%I3PGr>f3zl@XTyce(T zdqLQ6Zbcbem>56*_Qb_!#PzIZJh?31`>jdg)Z&gcOt2U4?vYC^UU5(S^vWsWL!)0W zV+#|U%925sVO7d)yG981;tE$;Iq?0#Yflm4{HuN) zz!oO3;w67pLmJ&!yH2j-v0Lw4!xkpEf=_0h|9Z9dtEXm%B`cpOV+#}4KeJory3(Wd zI_f{$Dcx`7tgvNGpBg6Eiz^Xxsfx2MPJi8Sdf0OMf=G=b6E9^aXO4U?scr1usWLrr z)3k7NzqSMTSnS0WmATY)4=zg|x?xgyc;EA$VG9#n=b200)nIkH=<^BT*{80I)QmFm zO6`)&H+v4yvzoqUV|vAtW5eeM52@i}u@~2?=2A~I`8ECQ>@nf5x37=XzA`cI`pua> zTMyDUR{ghL*yi?8;l8IVSi{F+FRr_lHATO+3g^ri5gsu5#er;Lf~$dLkF8Cu!^Lfe zhigBoj8qmgal}&%vfE$WPtU4FS!vkz*YfbI=KIv}vDi!cV_(&rOI^F|(6G-g{ln8P ztaiUTZQdLv8VzfmZT8;o+QzBny~0bL?Hdlg?#c+kUR>oN`;l$w8~$3-D{OJ>hXdKd z1g{sDsx7&O+JU9vqhp)au!RX;#hy#O`eiCSx~g3`YR2j^wlFcjM`?D?ufEXeJn?}M z;XW_43ZER+sD=sl;??-FciZKo!h2fO51-w9Ze*Q46I>%8m3o6mg>9zR4}ZEOh*S?S z@n!SA+5H-<*0Xv}s-xG-*eAKZsD_WlUR>*tOI`K&Md3alzmV>@;Pprq1QX}4D$m}q z;c9K;PN}rG?5Snx70rVhJ{Eg%%|!Y%z*B_{D zEdOv?SX93<-SzI|KqlBLTF27%nOWiB<2t23t>14ATbPJe*POTN^6;CPyQj~+tQx=O z;cI1>IP1ot+0FG&Gvj&bMbEDYKksl;ZLC{q4HN8zUK)L}-9hDR~K+hOhBW5;6$ zWp81ES0T#2YJXRR4LdGhP(NOU+`z9~WI{j792v3+ZDSDHsDm(G+O}EUm`nZlyo#`1 za8vAqxurF1;p3u`T>AZTTQPFJ+^cVA?jC=!S#=p(n9%!{uxIt~npxqTgF3};-*71M z4}UE7LPfmv`}#{8`+hJj+}5fxK5^O61KGj^>hz`G*WdqmTAr(ZhhH1N>ZFH|O?V3v z`i$vWEthf6Da)6|KV2ErFu`8fZ$SEe{T&m1KjQ5dylrtFrhI@+t3&lP>eDb zCNy5@b@b?88b0(}d9dHSeQNkv?1gG7>G$>59W~YlmtIzh{KH$A(73B@Z2z-W`0@mTWP1Ylu z@D?U?jG||C*1C=9VNZ+=j!F-yVS>F-b0_`2{st(H5g$W&NX&blC zUzXnYok_tL$&;R8g1t~XDE+?vmg#;?E7P5}Obd>D=AZ#=VFDG6((mgptX?5w(Qg}0 z4}O$!J6o90ajBlwE?;*_KmFvaU}=xOHB7J%&80N|2#|lUg$X$l{W`Gb zI05B2_WiJVjX8JCcl0{e9eQKze7TM>iu)izF8x-5RI{2Fa;T1VNVDo?N%Vh84(Y+b76=fSo| zE0BM93lo~FYa7RG*%+@{JT~|&(I!H$7h(tIoos#WE#Jv+{oXk$IQ78~kxh6D6Nnv{ zMY1~rmC9VkyU(8-Y`9<@b_DVkCUjmy&uYV}R>7XtBZ7LD_N-xoy%0Mv?_}f0M46N6 zHG6Q-zVuGyAKt=*&d+EYMN>+HdC!&y-J0w+kO}tE`5=8ibZ#lY+!9-ukSLMFKf4R9 z&R+$XzhVm$5{c*+@^p?YpgA(rIuklyrm^<37t4c=6H9}Wp4%{hkHuby9hi5r??>?s zBZ9>rw+ha@W5Y9SVM4MA`dzZOWj=AtC(VN2_gR5#!dsZoxkWvzpXQAUir%Xi9DBtP zHB7JFT@VaBH8!jA(;!G{_D{A5yPI1{0bivIxnuH@L5mI3ffh7iYFf3hWx`H zi@gv#Fz;mFkCu}vf(BP#8|%=qJo0OTOvvaQDu9mL&lja(^2^u7y5HEx{T_?4FaZ_7 z=?Pvp7&NAQMlm0Yz3@4&8|Yd!qkrgnF?l=9`ev+XGb)L$VN;C->$_x~SD^JcSSe>L zx(dw@QlXNOl~4L>%~;%0Q3$MFPv=~_Hhez3kv)_Db5kAwAl#=?ZI_fvHV>$_x~ zR{%AfEllW|!lYHOu)a&yc?DXJgLR0;!i26{Oj$_x~SD^JcSRH9BOh|=FMpi!Q zuTEoqm#p&&v>wM3?4|23lU5DK`Yu`L6=*#U)@&LJ6S~GTY1MG7?~-+1f!5<-U8u1z zq3cMKRt?AcE?MUlXgvhx5)o`rul678z*5hDBtg$d5RWKP@U8TQ;jrCo! z&MVM*98a*9uDMNGH5}`^WSv)_^*C6kYb;FYy56K!!?C_g)_Db5kAv02#=?YD!DM80 zmHvA67FpjV>%0Q3$MFPv>B{A#ug6KtI4c*yJVf0QN!87 zgjA?xWaX3oLO0fT$vQ8ihBLuly6#<{E3EI5bzVjdXA2Wp#f7u7&o0(?$vQ8ihO>nU zUB9pI2iAAVIxnM!vxNz);zAqtt;PB-S?6WcaJDcZ6)Njw<&*wyIM#Q`IxnM!Gr?Y3 z>!5uJ)_2J|FQbODg$bg%c$XOVFIhT(1umR zvA#>zc^NgFElluAU|+{#q5%`^6iT}t}*Mcw4%nu4SDg32EC9_ByckO4eYRh{F~pqV8Y~lW*un%>Sb?w+2Wzin4VH;GY+(W`tU%aj7i+I%4VH;GY+*vG zM-s9sNh1!{Udb9P6LFYeFRZ;n8#dx#?Uk&-G7*O@Okjl-2z_g1?Uk&-G7*O@Okjl- z2pe&*_Da@ZnTW#{Ca}T^gzc}e_Da@ZnTW#{CZs|oAuFFW;$ZEStidu7hY9w=+AFkS zBM#PH$r>yZaoEBHR#<_s5eI9pWDS;yIBa1;DpV4(@<}5O)?UdPEE92+J8Yp-MtmWeoQVL~cY60-71BM#PH$r>yZahPB)iO#5&)q91t zSF#4nL>#s-A!7#A@ai{+wO6tR%S0TuFd@|=30ak-5eI9pWDS;yI83mYjAT$*tj`tJ zUdb9P6LHwWgp8I@y{ykJ)?UdPEE93q!i0>-P$8}F2i9K68Y~lW*usR26H&LVZ!Okd z$r>yZaoECyRF5QNRgy*=ti6&oSSI2y!Co?YMK!thC1&kaMbbnZwlE>%U{tJY#KGDt zS%YOF4qKRzQ8=pNwNJ#_D_MhOA`V-aklX;Z`WkVt_Da@ZnTW#{CL~Y5{sX%HO7{aW z(SQl|!e?27rSU^n{9<1N6LEBB0%O53;fYF3c;mB;II_koKs#E;;w?lFZ?NwGavZiWfxZg; zjEy+RZ?NwGavZiWf&50Y3;j)RVC)f*Tg)xhLu8`kIo)gzP7H=seFfOvsF7g}fJD_ze zNoOe}F!r+V#|9a{-FI>Gc;_Bw4SmvCm_W87<9Piw^Zf_T47x689)IMBD8XL14vYxx zdyf1D`wnOwi?358~u_T?PkidA`_KC=E zu^3FINj73`h^QvVSk2hsWuPp}uR&(($GQpj(H z^zLn{QeZ*FBqECv>u_8p%-?0zy=}wQ%HUXa5^}a$kAwUMdjcTh;8@PWZ6Lpqk-z?y zDe@ca37|D5o?tH=N$#Om2_e71o&Z{7;w^=Qd{0`5g!~430wChxSk6*N$g`l;PRMVt zCxF(N;3+YdLPDOzjC~T3-(XJwtueuqXe>-1zmbu@?w^SK273Z%jfp4N3ui0uO~$@a z$ZxPGfYz9JOCcfeX2!m?$Zw>^B&{_jcx#QNkia~L?HQ2YU{3(8F+tB@EQN&hN*UW< zA-}<%09s>$-p5#&Kz<|Hh3@Kz{04ghXpM;{*b7&V*&-Wpkl$cW0If0cmO?^$@Qm%- zk>6lX0Ie}WPi-trAit69Lid40euF&$w8q2}?1i(!%$lz;39u&sMI2*cLSvVn74jSG z37|D5h=axwAtJvc_nJh0gFOMX#>5lsrQ;QSu8`kgPXMhk!5GI_n9wniKD)?ouqS}l zm|z@bEKKOQOWzOVH`o(EYfLa!GZrRvY^QH6@*C_4pfx5KK^hAa$ZuriufM8`{6_K| zvmXE-i@kJ=s#VxhV-lD>0VLwEg$W%8XMK%HfIR`U#sp($V_`za+F2WMkl$cW0Ie~> z_}o~S(D8iMMjYff*b_i&OprAgON6lbN7hCh%|Ak|G4X_E6UIw3maNTK_8dQd(TD%M zu)3(IGB~VnJo#10TM|F!CRO<{pNophle@+*(i)Qn&n%r=@7J>o!Imh&TG5;KC+)!)w-5RcxKT6Tx1p-74|v#1pz4bNmg4U<(uLe`uM! z?4+NRc=p*|UDp0_`%VOVscJ3H71j=xHCbA6^{TmsU<(tQ8aGHbIli$HhkyS^$@sR{ z?nJN`S48AeP2TETGWdfl4Z#*Bc1+%qSh;zBZDVu$y%+r6|HhpN_R^IKj%cxP(c_m~ zZ3wn7F=+9M#7Wb;Dbe-)|13Oh^~{|J_Tu`6TxvvdyDrV|wfBlGOw_x#GEx7V-rB}q z|G2K?KgZo~+F*jcxV}Ml#yf99=j$r$H^>$yS|;{S{J5h`+Zc7~+C?opJYd>jg1vOj zj1m(%>|53Cb=wA8m}t1*xbkr~2TF8%zj4W+h4vj}g1vNokrGAn-=EE=o3mmI6Y_ao zzKy3wo>X#Va7kT+_u@(d+5hU5HeH@?aHk>I!pD_s%H;3W&`xI-XP&p;2NUe2E5!6V z7SCL|;HlUhrVX|*A>U0V|DCtIuX)w(9q-+VU@xui`nfp#m3A8u=(M7A&?eOET$Cw}{% zHH+$fUumv`3HH(zt=fxj7_oBExMmfGU<(t{2WRtr`vqSd+hycA*Y8BIm#)m!HugB; z_AYG>I@=IzVM1a_HXozfuN&K?`SGK6BG`-9)8|s-|5{OUNxz#7!4@VYCT8=o_L%ui zia%*;zjG$oOV?KGb^QFwq=hqD-elTf3lkFCvu?bSOAY*C$)W+pb_Bo#d+}QLT&m$m zB_%%%8E4vH3llQJ$=;uH^|kBHKeS899Q%|o!Ct&_J(v3A=PycjX<^?ewlJaXmD|WE z*HPlGg9-NHb?CX2+^gb(dzHYkn9%!X{+>(8_p#7@AABtK;&td!3-Id8CI9}$KJ9E_ zLcf1)WAfRxB|X2l;}Ry=i&vG)T*f|wtHSf`bHx@W^f}fxzWMO}sy7chg`ZtxVJ}`) zE_;PF=~Z>lEh7xU7AEw4)HXgJ_rap4v$ii`g1vafcP=G;$-;uZB#G~X3GG+R-(?T% zq4O5H{)&&qUcBPl^ob<}eWE!Q6WTv&8)w9CEcw?Vi_MkuvDk~(Z|72L#%D_^&$9$u zn9zP++xTte$0g0azQwe`1bgxNZ4;xq6vQZVEG9HwX&Vv;ySO;W$6_yDjV)ECGnaNQ zh_&WeOlaKIHYA=ebn%>z#a_G`JD0lRvNMXecC@1Z*bw%Rl&7?8U2?O^&mu zAjdJsVnW_|vFEFJ576ala;KZ8JzF*o#;H=2BzE+`VY(6?P2A7A7PnVxB?Uka>nKZk~Y&_TtsQW-g;k!CZzp z784TNF;}Bw2AMA@aq}g7EcS}dusmIzF1hCjJ0fEX6Fj?v-lsBXe85pXwmtXF3e2%O zr2(IvhJepbv#`YZ-`(4F%F~xDW=kQF*A39d#p{plnmqmQ#|jDOg}*y>z_zi^aq;Tx z_4mlHKIdC!FMP)DZ`)ttLmO}Z_rsE}PuaJMEro>B6Kor+S3X~IcKW?Vg@p4mfA>lQ z+W2cx!>S*S{-FdGrw{07qrUA-9JQr&)%`#1X3t8GRR@uvERq7OZiy(*?5`H zkt@G{)?U>;UfxgMk^DU=B;=Y@i9;J#Cp%P+{1P)#QNnrQ?@n*C_X=13*-x*Sd*u{G zc=26u+N32GT=SnrHGe*3?v=CDLFlYA&I&}?>kpYWm~iR|=)9H+V(7s477ZOb*R|8z;d%3?C6*ZIJ zH*m!4jvsmgmO6+`o_Ok|hNVrvtu=j#>oY*OCmf&M?t-?B3o_jwJM`9@Tz@4!vejsr zaO$!pc75*g$F@IlgXQ2Yvr4(41as{ znreI(V$DKv!*}667eQDp_4bYbGHopQaeOsf3JB9il(_V(+am;e2zOlcDfny^Ui4M?20vOmubM3d zgrTFvr11I(fu7DC7d;(5TiqEwDB9Tlkq4^TQa~6wO4MKcV1y9bd!dg<8&(bf^Su*I z8$VB|tzt_7VdyB);r6m>CgijCf^LsCY#dy$<0sQbs^f-gwiFPCjuL;j>=_}1_Fj1R z(S{u}bbn~E^lX)3%Z}C6Y+(ZN5QJUBa{rq@8{!}FA0q^Np@#rr6~(t|pO7B9GVEJg z-j&;6FJ1SZwc6bi-Ln#vE5i}@4}6R*OrTFe8&-pywA+Q!PgRC1*Nj-q1bd->0bw=B z;|DZd4r2QgRV&zno(Eny6F!ps`p}eGbLCf!y1P^?>ztRImHCXTu+TPu9zaM12^Y4Bg8zI<>p9T31o?AZaZrV8bniUwiy0emNGM{m^e6}(( z@#QO}ATAmA2JUX1gxojtcYAkdz1rOnZ?0(+A=rzrDVN&1OP>(`{xe}fHP#u(N(9qW z=1bew7*?!j%?e8n`1T0Lr&r#x- z!G~1ynea8qItSWk+qKbj%W=t%_hYKr z!UR@cz-D`DM>JnOw~7h&l64uhl1Cii`XIK>XP*rd{xYrI*3G+*zw$LdleGWLmMymciZo}Hrn5}$P?64 zGa+ZES&_MpPD6^^b@+3~7~5S3MzeNIjW%RlT{7;z<`Q%ByGrYIFp1X4G<_xeiv=b@4mG7 zxLvlGHk_pnB0mRUh$D_T&wPVS$Y+<)`(459Q*yxD>Eh@1nJv5N=JyNNGhre>ml1+^ z>7Lt68yFd*4_DfF;WI|Fw%^5d{Q2I7;#H;dO&iWq2cc{39C6Bo(6qsXd^TR7F`D&r zOQDRbi@VO*XxeZVJrgF(T0DE@AU0le=)rs}{cODOIsf~$IIDyI+2i0lo|wOsEro>p z1zFq1alc)6(B@TlE`gV`6cQK(+HoS<=)ULS2R}aNYtsf3&I>fgcXmt-Vnc^{rC(l# zbrX3DJ~M%FoE^7=XgGiGu4}j6h&cd%EPQre_>57Y9Vdc#uEkYde{FOu<^a3}pD`ZA zSQMY5XZ7_h=k#dTZPzE*Qb@QtS$nTKJk(|B?y38By1V>)!tR^3#IL!VmaO^xUUPSy z1#K`PcOgGJ7;c-ATH5G8|1oViOCcfGVdgk&8v~zh{MfiX#+iG?g!2L|_f1#!<6dq4 zs_SFB9yQ+(&H^tckiq2duFN_=rnAntyT(#TAe*r{4%(2J?d3YNjqku%3JEvc;^Rlq ze{=g%9q*c6R7bC-S`xJDwey6G+a*WHTbR&MxK?LsK7r{%N;2)3J+x5?g?0! zaG#?@`2#~r`AqnlbhND1neMD)++NBSCUiWn@xu|@pF282u$PXOwVu-v2Y=D8lr2o? zcwXa&BTgE8a)e+n9W84;rz83t+@q8&Oz3!C;ggixBLkqh+l!b;Mn3$Ct8& z2_3^}{BT6m*+U`(dwp}(-dPz#ChZK1BZ^z}Eny22I%d?0Lr2{F@(ra-u$PYElD4OI z#I;W~j}Yvoqe0O>@vH2Pka4>kCmIVAI%Z7T-p3J@wCdaSWnHpYh180a;@mM#{ zXPZljE+ON?%0T9+OQpBOs9vIRz8BTc=I?ggj!`}cnG5#>EZo<+&r#ylE9TZoz)R<> zwelXh2ikb?$=gcV!pFt<9cPO%d@kjPZ}y!VA=pdjthH|!Wy!R0@xD__*-}85Hf(O- zh}wPUL+!ZVrB4n+jmRZQa~8m&dE4p#|O(J zgwWm#BXhLp=E66aHlFHwVJTY*2t!AS2hQErg$a!E-ElFl!)G@aeu-&g{-#?>*-}6l zI!gTOt9v7a(B4btcTE+a?c4WUb%FUlexKI6lr2nPY>l(BvNXa{zQ~u zFP%@;zFlUWm!XZ{yIj+QEjj~j&V<{}rEZe1WG_fc=_p18Z-!URUOH}~Dak$#s6oDrxfA+zTZg1uZDeoSrJ7(P1b30RnLpQA*ZT@uJ9{JHZr z$=KJ|@aM05dBm$HQk8IdJm^RwsX zNo+d%?I?$q>Op<<3w{-7=^Qi2^j&>elo5N9a;Kgv6qZ8X+If9wAmKx!4?@C`W<1o?fm=k zdgC&T^pmh6vPf>=cT(Z&%M+Oi!y@;J3CUh)R~2_w{V!VxZNlFx_TpzD-^S`?X7)V4 z0}bsu!)Kc(aIe@RxrpDVMzR;$PsW|q()m$>z4)4Z8{u31N}&(>3Tf@HjOM9*VkXk> zhDDnZbMqD5OlZHWYgqKG!k;cYE<&)EJ5r?I4S#s2ktbkb!hMbsL+&{ldaY4iG1}&9 z(*7~8TH@IagNu4pvxN!m!FBfBUB^X_j*JlOrTt@GwKQ$CeEh&_wlJYRxXzxtHr6#d zGD5JI_K$hh(zNmS3xliK!i4tVI(zQg$aOp@La>+ik9pP7w9#+nY1M3DLVIwXJ$G%i z|LyV!!Cu-w=2c76#^5f=YPK+;J-E)EyEgij9Tg$iOZ&&XYH8Yl&dC-g^sUv|bJxb4 zZ`M>X!CvU4r4P^*DUL|5uc>AW6WU8?jk_b}%&T9;1bgXQo7Xwbbv(B9=f{{}FYPh1 ze!j$B`E3`vzQkCV&|V_z=SvJRs{cXV_*m?vJyF)rml)#1DHr!-3)i*tk@CIIokwqc zO#5(iRvJId)$;Y_3Fw?`VL~HZUPU%%_4s}<%wPF$ioN)o&A0K$idavLGUlu_`k1Tb zv&|D19(r&$wlJX)uHf!M=VXGt_?mL5S2{m*^V3TnFe7od7QCqF2472t&sY~ee%u&Q z32u-%vNPUV>It?8jp{rZ;Ts}b)coxK4+1rD&P&@~XWRH|*>g<`&dOK{2vZGciMuAe z)O5Fn5A$^xLeI*4*7fv~wcID#z&gsC#MZ8UlK$|h4U zs4{IZA)mdM)@f)PzkM{<6Kp9UOtq|SW4F!MH<>@JL-Mg)Ddhc zAWYS-C0>5{)24l2wfBk%`Ru*4ZbsYqc@=Lji!lP&)L9tiqJeF!CbQpn|h>)rHput3Q zrzRRmQ0!GYR|XnJU?w7DZa-*z&RSv;XZybGp><_FcT3nQ57`qXOH{9 z=j?6k>s9J5)gYf&jKB=(At94tL1R7Js?<))@;s2B*sIiC#XtxnfyVc=`UD*c`6nmAr3-)0IW+Fl+K->Fh?&QY#7eXW` z_9}8SyB0Kzz)VEQjH+wUBS}*FTs@C1WG_d(Gu=sYzS>B#BpH#IuMgdA6!|*;*T9ZI zSOy7-X+?qlm+&gmC|ajKglDiX6Si6*Xb*g(SKIyd;?&_+f!C|?HOCz_Qr)?0Ai-Ve vJnOh~C$Ng*>t6)*=R@ul5??$75PSCtuoRT|43d7JwIP(LM* literal 0 HcmV?d00001 diff --git a/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/forearm.stl b/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/forearm.stl new file mode 100644 index 0000000000000000000000000000000000000000..513756f0bb72bca7a7a0663dd2d0c6eea52be466 GIT binary patch literal 48684 zcmb823A~L}_xKN8B%&S-NK(1yqKwxi!#(>PG88ha3`K?pqB$DgZj-#BBneTHd(O>} z5*PKj&pk9ql%l94%FtkF)};Px?Y-CDYd`1e_x^p}`)mK6wZ40;JwE%{d#`)@_UqoE zUZP*;daawZYSOGpi@fIbI`-_|vrqo~`Sbs&_y7NI(+QEt-ZeeuMj{2`lGnaoxp~vH zL?rUZx;e9(C(1?Y$#i-A@9x`E^xXGe+0-5WzXEY75M08EfBrcqjRexZw|h;GI%N@D zYs;o-l0e(Ox9j73S|YJ+)21kwaDvi*gDv0tToV!aR~CU<>^Tsfy499mo&MU_d0fKt zW#t2+cy=vWawNEf6I2VztFGN@%i29HL~t!u>tN&H$M>wjl4CizvBcT2#$@HArLu6gy^W5A!!7J1x0_%z-oM372>VF^- zET`iW&P$o%0 zO^0moyto!ir=8j@I1Z6!&s~Chg>6or0AWh5hyrSJo)_0*H4?JHC7ghCTG#Hs6Gn^m z5+Huc$`kOT`)(2Ve&PBndJhNRO&WTMX^#xTN{&M$d~ayh{A{#=wp3mhuAejeN$80u zLtj#9W&?<66~7M=a^yG!s}UfEWX&%m8(hL|sFEvtzBjqz_hk}Xi`5m_P-77PZ-Few z<|*}LIo5C5luKz|J}7zjFS934%!v{~rR4aEoA3R0e_=KeT*3+N zt!j_52(E=*+4lJ5{=z~cxFk%Fhh_}oJ4((zx=vsW+VumDE|9NZa2>v#aU|i7BCO;% z1hgP(|1hPJxa_ARF;~mh6|<2+7#mK{K%!uLx%EYVx6#CHUoFeuy)-w}9z}_Huh+wO zpF=CJI6-xS`C1}uwfg4#BllTG^K4qX2s${7-tNYuE9ejWjGr_iJEIOl2d%C8`!wzT8`y z6Eg{}#YP&~7~K6Dm=kAIK^t5WCT#7p8BUf%_rx?CwX#e!Hk~<95+vaSo9)1>&8cf> zh6}ia6X-43cIHHf;98g_+s=IDv`4t6t&Mqi7SWvOl$;aXL)QkEaDvym@6EZhh~~tA z6I_dz$(gT8*n9l$W*~h6Xw8v>Z6J-!)&Tu?IdwNdP zgr2XY4eD`d!;Td*Gic(HXLE4A0>VndgzFWIL7cC&4fUOYgtcd8B5lJGRuU%kj0yyd zB(Y`G;ONX69h1C-;o7xcAu;jQcChxrFE3tKyai1EkoVV-Lnv@T&<(ksfB%?!F6oX{gRL=>fMn`1d>m4SKD3V`Q}UeSs}mBSKN5++<5 zv@Ss{J?>;(S$jok>Tk+MyP6Z_N(QZy#P!hHRs_@uwz*I?)VjpAXIfL&l_jhsOptA5 zL)UHzYT0(S@1^6mAP}}~k)~M`pF49RwTJU|MlG5@IPJ8DBzV4-2-~1}mz7rbJ{m>% zoaSI^kBV}Q#U<#KC9H=|dq}Tn9fbsqT5h{6LaldP!rGH_V$k!6g6O9+hr`aDDPcx$ zw|y5TgrLq}1Lb^$1W7P$N0dp!82ldyS#MO+j#o)$LuzRoGHu%&ZGc4nLxR_+tt;cL z_KFi^9cfPJH1zhcjWP&XZ&V|OC_p=`98!z&W!{EtL?W}&k$9i=wX3dJgz2aE_g-K5 zorZCiE;TmL1pG5vr7RuC^sT#l!LH%LG6<3!hlpp|;Dk;q8=~j>D-Ith%Z8RPPdHJQ z4Nj=EBqH^CtZ!ARTBrpvUy?{0nM7H&tMg)X0@L+Vm#!asecdu_kmNW-88$ef)6zyH z(iPgHei=4M!aU(b88$ef(~{VEMW^GGgCw#X$`ekMVS^Jot-RXZ;dt$#C1|4zq6`}m zM(DJ(F|{;)oPI?TSq?>WqKtmU37wYxYU(-SIOBsPXrm0GEE_s6NvO26aeb{Aj^HxJ z2T6`YlwpGtI<3Y>Y5w}0nkyFJsErg96;&wd|Ngx&H%>CW8g1BKIVnxOCzBwFOTb8D zeK!&T8!@s$5+rPT61BhV?JmLll_gA%(ZuOT*2k`TaEvSmCu|zZ!FpmO!U>X~SC#+| z9b)75vqZ~U-J}hY$a0uoIf>dIS`ajooZ$V6)AZc;KsyPPfd5JxBtaWC4K_?q1kD_W z;QdOE2}v6QN*#rI67b&=Y;($sYvFV1J-(NF^92q;5|^08h@1o0XG41n>{~o9BrFZ> z0X^}`jmx1Y{@$gDLy*KJpgm0A1)@;fh@lNmkQTEI<0I$5Hx5zOm|*sR$h~=SQrDHV z!3mo-bqGYSpFD@qwadr0Ow)7UTekjsYIA}9l7!VR5b#{JCfL}x{TY|g5|=PN28fom z-ld+XY;eM)&G?Y>ZX}}nt|WASb!JYqN6+QVKqD%HAT2D19$jGLss|4y^%#@{NqD}P zrsuxb|NUc0J(pwy4h9x+=lGh5nKRJ+WyhhgyAT8#rQ#Ac;#rdpHEnBRD(AI?)nYT9clXzE!V->8pBEJs$`t zbySb248qc8YyyGh;DjEjFx!}MC)4y?eQl38(0HNRCz}LcjZNXab_ndG*)wyj!~AM zP*3I?!YPMCkOVw5gi2F6?*6R2Ir-s|5Wz|wB8()HP#&ssU|ak$_TWOM zm0)SCH!2^@yPz#^Cv01CWFeOvhk#y!#N->x%Uv}jbel^Xn1*{f34E?=MBCV0*AgT_ z8$>93vaU3t+e0ubeWD%ClM_l+=;^Sa2wncNJFj5+6Avdgpp(t%q!ie z@SM~1YAgqh2`q=nN!D(NP}VttX{rT9V4Fh=*5?wIPG{Oc0(wb(E;$YXZ4MKIICSKf z`W;>`hEqk`wwPw?64MtHaq-d1H1g%m2ogghqoAniX)xUrL)V2u?3sFLIiPVC(W%oI_ zmt~$qztZKjAT6ZrTMgUe{3+9+7ph*u`!0TY<@Q`DTadg}HZELn zfA-~*PL#7KNibhd(6^x?yz*CMkNI(V20>a#)7PsabQdW=3AL?E>lG`e?ac6R{fez# z%~&xuAb%WH;Jf)YFtrQMOz+WsH}8Ws+2=o14eHB|PMJ3QKK3o)l)OG_@!GZDhLaNX z{iz6DyBP#&VNO)jiqKxko=COGJS=6l^$e$HA4%{QWIGdZvzYDm2|dGQ5Tu1Rs8=el z@;et4cHCZD_C)G&(s#XX!!-42MSQ%wPxAidFG(AAH909^SFfhcbvZtG`J3eOdUr^I z&pw>M6*`nAro$w86{4#<|(u`F8?)IPNF0x>94%sa|97wG9WFgoBkKPuht@(ux4$M}U9#}%gLnhfw3I9d zY2kC-cQv8kACe#m&ll4)^QpSh?~e?Ew2+|HfFkrbmjp>wgU zltGXd+M{_)5ilj_e^w$(Zgwe*hiaAK^j*q}m7^5XG`l)B3b;h3+4ni+lIu3 z(Ee5W-wC!NqrOlEp?eARRl8GwX?m}yZv*{(E>OZ}2I^z@1%S`-yUl!mMD$mYks!?p z{k5$|6yz=c6~ImbdoRtRcX%m2mwQ0sE4^8? zHk2OHjA{sLgA-;i9BM(&M20XnNP?bFzL?f;QQPLm1}8`h37YvbZIFcLi)os*4Pk6> zg0v*kKZgl+?veN11HX*>IbZE%9LB&}DOzE?72 zCZ1sFeQMJNahQE*UV${&!(C@Nm+X3|8|@*-xWpyk+rX@Mfgl^OBW&|Rg0%2C^%&nP zDtee~NP;9TVLXI>b^4KxY@gcZ#k>mjD|ctx_BfX?UwvT1-@J~_CIa-=_CBj6pR<0& z_o>ZX0-kUJ(hgzwsY3+siKbt{NgSQANUv;PviZt-GX$KHv3+XZm!Lhg7M6p0On7J8 zN_f7QwgmJkJSEfj09=B!&<1_);E3Wk2dsqWEA5%S3k16d;1ZNC+MrP&PvYpbPu3Ml zc)pmX=d$Ei zXTli-X<<2NK9;?m&V&_Vzt2p*(uTPQ03(V{s^zGqyto$HpcRMjUA#9%2VnC;!qU(e%t{7s4!pn#lDGul%K+khO{f`8=Y@o%c!?OZ5Tq&K6r;k+2DjpLwo42Npr4d2$Jx8WxW|fpR3_r9BG3SHf_c^ ztd_!r9;qf5}e zSB5rorCjy7gq}H7ISgUuT}|i{Md=kMY#MB_jM?L8R$4l+mFR ztTENw6kfLawuNX1we+_oodM$qUP0|9i69$cg4uxC2g~u*8^1=EK0hx<&98nsFZWs~ z!>q&u)n87}ojP2mZ>rm@6rU^N-NJ98EB_t~ziDGuf+S&LSzclK`yZ~*M3tW_O^omKT(r1$2U&8Gpw@s1Q_@d8R$9t! ztGfDm#Qx}(q7yReiW43B$4f7TA4Rm^wJtj08J-R{;zFCkfJ=m;w2p33e2*>8W|qTHm*jHb@dCrky+` zJ$1z@?NyCqv!Xi_qhMZ~mEZ(v;qypjO1gFKtD2a#XkT>F;}vDeIYC;Am{fXb`9CX_ zSK^Uv(MzW9mF0lhOiFmZlx~{)YFrUF-nbw-<*~-nMwmb?B+&NJ##@9UZu-1J&io1Q zz$|AaJYP&>nI2xTO53RMRpp#Brq6}_U6&v&e2z7}^a5WKgMR-c+PUxvSq`$!wJ?qC z)V6Vu1opA1kCkeo&VudHJ-^nKC8xZQAT3PeXl|a1zn=kThBjwD z6K(SBR_PTLK;Wc>_|sVblo^AT4V3mk|2rn1V(Mg zw>m)&343+ogqw24A6hO;&TZg0Cj#e^^_O?mHufy9o-_N@8W{v>p$(jg4*qb3CO%m9 ze)Rd9`sHv5rg2W3HFbCyUcFv@M)bXx8#UzqhKWeTPutp^bIrEs(nFc;!OMi>d`h>w zbqluaT(#(jOGn8TBuUs_^Zh-7a$~O1<)~JxeCoUj4`mRfh4d|--yuGp*iRFm9v+!< zUGL%yg0zsR_Er5C|N7?X@O&}7>f~DS$s1$Z#*ZKMPYoLXa0WqI zNJO#+#IJ32z9zcQ-ITn3>%t6zw2=5V6^N7md_sG*{_7VSyxpm@Y;%(Ed@a zZuZdTyze5w^gZVi(T~RrT zTGb}!yqE=Z7N)UwKb_c5zSZ2heN2rra|-e=lk+Y~!UVQvg)!Hp)u?@KU!>@hKOd5# zmJ_6fH1;w3KHodAaccK3t)vaCEAP)Iq*qwGqg$OXy&`XwjRzX_N%fr+kv2$z`EmkV z^YM)_?Nzmp&PyFVWwNxv3DV+aQZ_C+>xl-lk8P8+%k$+PVhc9vo26~|1^z!q%P@ugB_qx#P?Q@5VAR`x4SkQUO|f@jp<0YpLhJkfLWH~IIaS_C_Gy^=Vx zDk>hny<>V5ZGTq#dH+O$gSyXTw3H;9#R;=vzQ zCNA22nfPu+TOb;2$rCT{s1ZAFL^FrrTA0(afmeukFK-1z|Fd(&^h(`gV^%kn1WCd~ zx$aZM{w8@qJpOr(n6-Ff?1u|5+Iw|^Bw=FVg2kfgYq>x~YPS{FwH%Y*?}DaI$zN`` zIPua)3&clX^h%$UTApY;GAZ&7;%aH?(bi)8np!cjwQ;~DoDi*FPD~zMC0^zC4nRD= zrGxnP?PjrqXV!BFu7%!Oul`=vRUCfn@z@i=$+8?I2@^LQsTe=dDP*HY6VZpl=f( zKJa>qD^A%Qdu!y*5-#Dy_8G4x?&#Y-Uh|6PKn#xsv79meL{_hD4#BmqAGb2`<}+8t zSAALsh&p%ViQ8Y_5vz5}DFOGL6WkuO;NixifBDv8PXC5bj^aeY#jvw;&)fBkY0M~s>HD+qvC5+txn?22IH!>3z|4UaSvwWq%45L}Cw$@gZC%@Zf} zZz;zAU3cx-Gl~;K(>KJMB(l;+wihQV&$%xC%avJa?yc`FKed&p@KmmN|MGTDIXJ;R z^u3e2cN7ozsv@fT%QNbV>WB9p=!w_&5El>J66<&W7fvtXHHvBKp}u$h-i~7Wff})u z&(w3q2OsBrZ2I2(9c{%|8)oKL{?i?`oZxd8{9c^Ft;F+3@6VrhxP>!nxfY)*eedwQ z&#$RE>OK+cHbu6_it^*)#Rn@m?Tqs-&BwlX!&`G-U-i-5;+g%eq3>D=Z-3rHeecNr ztNeF2-6gi~>h2I+3v;6PitinYS8IPTYmAr{e@hZ1;RGLDzPGsca=+>Rt3`*ujyeR_ zLL2mk^S$j4oY=n7Z@tAsH+j$=R>BEBntiXvjeY&pNert!|T6{+Jy(b3D_iK)C zCJqd5;}Bd6ZPSXw_ZpsfR{KX!t}6O;86XLgaN_%6$;2B|{MdkT17Soxc~70SMfdNC zEqQLFLvSs$K`Ra*8s6j&YWHC*J>q&vkc1Pf8jncf7X?#h5*0pgB-(ibh@UIwiTPLW z&wqDC3y0uZG{fD0JAT*B{-&|mlhs;$c5}nnneg7;I`gXd*%Qnh$T( zo8gT*ymAE~c8qQ%Hvim0?C)_~K9_K!YL!)qiw=y6U-BY;6%EX4B|a_86|J)`atN;V zz}Di#8ztAp5BEpn_cPjx#nA6P-wKvFb;Sv+g-t`7#?L>cKG?W$Ul;Lb&B~(j%7qTWwJ^Q^oo?}_`89!f zuXQ&uZ{8=do-206c)r}jE-i}_HEygIzwxOiK%9NFgZSu{Rk2R7lLVJ=;-Qu=C-%mx z#Jgr;oA)|+v1s(}OR<@qT!L$%SNZ2vir2ogF%UzSbQN8HeLS{!^~thdkt9susustM z?~VGZgBUcXd2IPF=SYGioOtE-6^YI#Ocy`o=L7N2n$F_Wt-s|T7+BXKxEAmEzISG% zjku*;GQa<8O$C>5g7!6lsF^#D_Q3h$3-T>MkxJkh8)A#G5vMuKJrcSMaDl_!!zT8f(&iU-8IGE&r<6luN<{eTyis4xMvy(+8`Zx-3kP z7Cxu%9YwS(w=cE4_B*hPW36*7O#e1!XYftsht)oV*mLWEq6NcVfqe!m2@}VhD^zp|Z&f)uUh-$^k7`RZ%E1Ym?G*7y^)FI~ zzL}CikQOhKB5vHbwn4p5tHTbxTR)tj*;NtG47sf6hiQG}_~124T9}hvmtbAp^~S)W z|K8D0j#?Z!lhbo#X>mmL`(?FEQ;jGa>vJzEx?w~WNst8d*i-}vb|qZ@PU zn5Ojurpa4H%=lpalF$1VE#;ChL2C&`+}mr;ix&?$@?w}EEqqR^b4BR)$6_wQG_A;a zPQF*+%{;LLz9tUO=;W+k`Fjc1!T6Q2{2;Dg--a)iLGZU-VJ$Q65ApB{t%2I%jS`!1Cy zBbc_Q4t5my-qW?)iE7bWvA-rX0>Vnd1oa-@%lod8h%bkqBLrBfYs$X#%#q4b2 zzRMEg^Z&jVyRS!920FY+ZFosqzz8kH)k=tqv+;9m>>yyWeI$%(F%YOF)RlsNDH5%ZFOfNOppX^ zSVGxT>k=e%T}d09K(A1X>QLPl2@}{JD4}vFLe(3t&YTt$JTGbwwT3HZ_e;=*ZgWZ4 z6GfXZ(%AlLy^DnIyBP#&q3tC(J>>bP(|0975}q%n?RuBZyBP#&p$$6ubLL%1kc8)p zX}jKK^KJ$~T4;mL3Vn~wyOJOY&ll6QzINta+2))eEwo|JAlbYt36k)9F^zMH`kG+# zZa&Y8YvFUdpUCE2NsxpSH1FcMi0|#Zy|q~VTXQihJtCh=IKj_Cd@rad#ATHliTqRA z=5q-r+!K>gi!K!-TGkQ$3XZks5>D_l9pAgC?Ny>et-oUx_H}B{C7i%C&cS&8rEX2; z?z*rs-m|2g2|VFNE&3Lr-2^=IRK%XFIw>T$gcGZ~48m&heQ{;u zss3k5f=hV5d|zA<4K7q(aS12*9=RfJZFHBk5hlvsJ;!p)Z+@O6!ga;J^r+r^Z$_0z zk}W^%75n(vyI_3q8xVFjz~?kJeQ$r;{yCf=3HDbzOYm{tx5>4sNr!8QL5F)u8=T;2 z8fmbfIHGcc;}N8VUQycj#=p8X^>fuhV#PgK(gsP;E1ssY2`9X74{2~bg0#>ErG0Po zqjifqy?(EFBkM#M=T_p5U_NR)eEMXA;}LcQ+Y<=i`|_2}IqTl+5-U0F?f`Ar+dF(E zt8N>rTS%{d+%lUJcFn*E_5M)Ah~?kpeA=d(X#QD0X@e6s&2P7TGc*u=$dQig>5@qxE%N$-c`8n^x_i zRvEj`|2xVFyUMWZ9jZ4)lrK3WrCU(i;Dk-{TdY@qG=th)5_XlL+F7k?__}1|ht5X1W8t$9|a01hGyU{rf&mc&P-obsw4TOCLb{a0Ygy)NC zJo8pJGudf)20>c%4&HqGFtEW+!v&Y{eCY(cb9y8Y>}*?b2`4a3w@u+~u<7QMK0nVO zNQ>4C>K5x;@srx0y|cHtZkFe?2R|i48`L(w_h_Hxeh%E3X!F-mIm3}8OyKPcb^G&B zqiXF}9~&+9T=SNi6SagB?)k^jsaN@1;O@s~ySh6B*FvvoZ2I1sU2|XmY}?&p7^RUfW(7uP=oJHki|mV3pN#(jLE)s8R`H%#6y z_mF9?xGX~L=_8@b0smFyP`la6L+pK-HaOwh=mj1kVZBu)pSkUr{;iI7$COsGc#_X; zD`NlG^4q}iiWBbagXLI%u%-MvaXc@srMyxeDjTIQG}nYJEhqRKqlo1nj>;go7N0Q` zaqWo{8lZRX%%JYfU|nSrANQ**ZEy+t&SwU!bu}~KUijQyZzc!dI1$=>R+`1&q&Mg| zANic$Ll#3;O-`=pG#~c#fzT36bAlp<)UNZKWJ&VevqUOPkQP3tI3`6@Si2-yIP7HD zZ?_UmV&l-0iq75EQhG)AmXKhpj1U14JV}V)$tVXWXdR?%^dB@o*>PMZ zL0V{!B8C((>a?qp_b=_2%_W%T1V!^GV*ZuSCM&(6Y>*^O&6BNy(h+oSsiVixrv`A{%I3LF=oFz=mkC^t@N6GsjatjHP zgl#x6KVtiumC31Jwn?EDNx}rh|0u5@ax&SjRy~aQk+Y1B40613Vtz!|S8h*Lp3y^k z#pe>bbBQ_8ETL=+zi?>I>DyD1APH)5qIA%J1{g`Ah!2-dOzr#6R7sE|OxQcAI8)Cb zc2BCP{#i&8e3`kQNfShQ~+}MdYiMc1SJjFZ?fa&!v_TTo;smYX74h6-ZBrk0TO?$1Vs&6_gwRU{# z6l#$qOwf0mvhmZG-5a#oel&w1Eu?9!tq54d>3>#&TAZLSRzjdK@9`eGVe*2G7NZnNh_&*`;T_Fa;M37p4FWUxqhcBeGy;Tg+ z907~wFcMC{UVqxge?Y`Yz#>Kr!L{H#Ax-fnG9IUd;&I?^ewre6ARZ%4aX#Ez-`o9Y z*P!;%EHQlZOBl^z%E1ZrknWWE9z@p!EV{-JT#J`U#uWuDuE==8J%pH_w2fVYD4~F& zgt&wg5J`}>F<1~E6;ON>C%6`RWpC+03|7Emu#6;3P@k4@UIE2>rR#SmPJ_evSZ5>OlqmvDmjN*TuzP#g=Fa3a)qYqt$pOq}V}oPh7PG{wisn7Dvq z;<$tpycXbgbhjSCpeMG(ZuGw>;q{Jbx&z7E3HB4m_Xu`uct7_0)-Ou9gcH11f{l5J z;Oqygi#1D@NE?(d64YN^0;0A8irV6%laC^Z3rJHmmyC}JSbUU`Z~{&N(-d7LW3U2> z!Qy#wEjVdRQ*4%um1$40GQ1XXuBDX>uXPzK)1G2wxP%j!rnQ#b9rP(yCQOhPe0isx zSebl^mEjVeFQ#cVC}U;XQ>;vwAT9U`Pdl+PKE=v#3C|b4^wTz026hMYS>%h6a01ix z?(#kF-10dTUBmO@TCg9$2#9v_DcXs*2cLbArnZq0Rqa_sm63!AT4lj7!4t3u9`hdLqs#RQqJ#n#C1m^!6J$?DA_XiG z$@m^7Z0*9GyMUsd&^k#t;f`~N#|bDNhh}8lt%KNuG)3*;+=Z(T6Y~?Wm>(nI1jIU| zDJn_E6$LD=$Pio$ZP4vT8M_p)*d-$g6ZY05#76}zK1#0PFb*nA(49*e1re|)2>Hgr zlG99tXC4r(khXCX5CsuX6a<%W0%9G~HjV|NAOebl-~`u#D2TL;F`4#x&)}iOqhnio zR~B5t35az_+vu9fGxLHk^Ph|DIDD={a4oD|Tk;-b+XZ=t=EZ)x6MvVUm4pep11n=Q z0v4N*CP|pE_je&SBcRv}v_TS1Kx{^u;+SOYQb4gwoZwn;hLEQCFBu;dP<#}ZZ~{&? z(iAr(W0wMoUE&f>a1UkdQb4gwoZwo#ZDhP!K=Eog2JH!kT7_f08uwO4r34g}!X;P> z>_%gnZc59jt$;;snL0s&w0IrLn7F{k#PNKshj5!R?Zm_d6cfiKyi9PHH|@m41r!s< z39beAI@3-}TtG2#T*3*sJDRpJaS)Fauy`CJ;RMZacvn?MZ3Qf9%Zvj)Gw_inqqYJQ zwZ%p!C-`h9qqYKy+TwX}Ek3*Y9z<;g6t%?(uEl2z`L!L4J8RC1T~A72%#Zmtz%;G; zF-_lzin#L8-iyvXbRI?`8A+J1U&6365wO@LhafGa>3dh%D40Dty0`glu=}w(P4|;9 zjdxOY9E-_G5l5eXKc`tF4PW%GSDc{Rjfz+>DxGRrd^YTUxCCk8bNa?r#Aj>oO;vdI zO^ll`5=?W#-s{7XzqPV7xpn-5un)I7O_DIt=ACh{JE)_C0v&GvJE<-~TKJr99xAVf zZ7wN#;l-`b6I;vBncB(hbA5;XgvL?XvLfdG6>Q_+OB=r_`ui< zvo66jtwk~k9oq+gcg5xvChXf8ZS?zS-I5OLQy61ntYezAFir1ZRr2x>$Fevg<5-LY zwdlReb5g{;z4MEHxZ`ozcd3M7g5D*H7(8u5gLNbSf}K0pE7HQ}^uAI=?B$uM0k7UH zdm^_%T1en{#7HYe=ur!&WLCoS#k9TIk9F1kP$Z|-N$1IOP+nXM(`_o>4g0%~Ya3Gr zl%(qHnJI0MBuv<74z%&?hWSPDy+>rpIYC-T+ZYohAeJl}V##E`;$HDyNj0KMex&Zn zOgR*f~^r@x6HW>lx5Tu2)-HpPp z35ww1?^OP-rLPPXV**h^0gDon-ybLm6LjN3Mj{0)5=nmJV8l_Fpc@`C+9_CKqn+e; z6h<|L30il-??^eRWk8WHyd2zjEV=DhaLYSj(KSZm62)vU9Bz^aEP}@&XvZ9FP>+$f z)dLnKWF%NUKx{@}&gi(|k3nog)A+TSe#3fYwtKW2sHf&sx%?k;8 zG7f#(tarl%Nn8T@@x5d9`8h<41QaoXbwcZNOw;iwa9O96X-WDxX~f;r(l zuWlzmL@vEQBta6iVF})^*!v@c;C;;YE4XVH46b`t690?&(yEBQopp?n`C5{<^n!2p zsYoOw=sSuNw2NmVk%E>(OXc6(<^%a`)W8o)u&ge?0b(w816(bipmhlRw-;x9(-XqF(LxO1no6yXVo1>1IC= zRuU#`Z(s9BpCT5)BiB2sxAUHFd#LZ7(f`|=;}O)u(Ke-hFYD;pMJy7@NZg*!`)=)1 zPR%(UVf(SYapQXjZrqe&5mja!@G*g2(aj;>>wH7RAxPp9d=?%4RfNZ8n7ojnF@ZK{ z)`s8k_r)Jk7Mo!tE@9_+-^+dZTpnZM5R@<4ptleF#;iT}@)#2%K^vB^?z(5 z7Wv{3lrP%Ace{Gm{`u#YY)+5_ZCFCB1{ASjNTos@S0sBPCu|yS+~~WCzIW@wHynZ_ zXu}eAH2`-%+FciAu}dZ|BrL7Uq1HarmyJwv0`pb37&&2AHNJQGTa|f4m9yTlw7sk7 zdlL%wQ0>B;XeCq*vWNAi?m#|tQ%@dCCTkQWRJ$r|?_&AhUwa!sN8a#H%NO-=O zrf);vJ8xu+W>IN_^5R-(!xH^tm(WaI!X+-D)+_3Zerewc^ft&K@Xdf)m=mStZzQGj zTDkV2FL$&--{dX<5hLwb#E8j@6Z~8RW}m57wP#T&hTvLggKpNr*>D{* z9T{EIo<-Lff@`4-dlL{MczlZB;Sx@`XGIW$)t+LoIKj1O{a|9S=$3`x1lPjnbkotf zWg!WYaDrAGCI*XcSvUmOLK}4d(z#_J36gMvRv#t?i*8vs1lOV!j*7t=_EiDhnE*nq zcT^;by;bTG>|O@OX2^RPm>2qvcQJ@2Z+-8kZ?lv79)MZ5s~)ay4&crd(zdt5ZzQF= z1{halB-nRpmkZN03e*}7u4M-5$F4>BDhyYgwD+D#ki;c$#mVA|0$d}G=|2Q^AgzSb zD*h*I1Bn;Dm@4-yT^kgYqzDsE`KldywGLKsENZ>HC{Vf=hV5yyq*Tcz+{Fa0w?cP5oFAdw*S6CP7+!6yPl+^*aqz?W$g)TF}OJ zaeHuz{CTYu*03e01swv$2*obpUueSp@OtZhgdO^NvX83&DndoL)dNEPy(-_^ys0~! zz!e1LqZO-zrjdXrGOY-eR_8cGJihMsYGo0sOeQBd6Na$|=THT~`?uh4FtZY}p$YO9 zh|QZe$#Tqpc_*9#m~wze))Q4%YOKI}@SbJT26z_@5i;M94Y;>7YSey7+;wL4#~A_I zPTNRYodS?o|Gm35oG2Qv48iM45$m^{lC4`1Mla^8zF$>2Oq$uqB-AVuA`*7?DVcan z^w%x;cZ3SSMk&M)Q8XC0Hw$d2=f3wvp9(pva22FD&#Xr|Ju$`%XIDx;VfXcCB(4vL59GtLPSa0MZ zd_g`m{V`h(D*@k4zL>VexPoNi#vhxCH9xh1IZ^$kFO1Z|(z4hb_BVLyO@7xQcsvdx zOq`DI)&6#3^6-w`v95oQlr}iQ(>8+V)vPwj;#sw0ADklp=46^8b*TO6U2BOEJw9>> zl7tBJSAu-+k)u<%S3F9{Xkm?zhw%6A2G&hkuT-mPPh{;;pA$~Y(X|VG*NK)) zU|r$gRKw?JL;XdzFhLUZgb~s!Ma=7YN{+5wX@e6yjb7p3jPt!FeOl+}av&jnSMil@ zPqf6Fbt*apNvL)eZKF1QugZ`!xL2}9^x_z|M$_x+L}3DyG9ix+Jyo$}K&=^c?S zP**a-3I0+--gXoI3?;alyV``2_x_Z8iCR+sHTk}yH7>3fx*IW?!&Wj)2R z+%ueB!U;&ntyit4S3e$Mw6ILp26)wmd1WL#UnIy|S@PFd$&DmTP#yZ-vkh-}{V`~d zIjTM6UlYT)n=nB&4L37?te$gZ#C2kO$;a3pybVk_fM)Hnq}-a-@BcbpOdc~fhV{;) zGbtt#iKW*~Nk8^n0B7{8f4m`j%d9)Z@PhxwxCCh;&^D!g?~XQk$0LjumdV-xuP!35 zxP<46Y4XMyejnF=_F;ibj2t*d3v`%!rNi;bfjz={mt=Qb9Zc__VAf3Yw`7zxqz*93|7 zy|IN|XfDCpl|2#vb{-PgUv--Og*=!`Uej}lByb$y-_GL%wk(d?ZILdp>Vvt2MwCNv zEwpXpYuTt3ToNX5oU8cS^n@MRdM=R!NjL#%Iii$TXFs&R{&5JSg=M0goEZ*n7zxi8 zpWAYQ;P|hXo^jsndl7tCr8{hlm@R+EcOC&)OPC%Nq2OG5|;FTPm^fx&v z`Va{m=QE!RAX;|#;a_NGDB%*MiNNwv+V}3dMoFW zHhM0}AV>>6#8IpM2I!?%?WDOxmYgI!Urf_;-&!FiFs zoz(i1uS?Yz5zaEh2^~Z zMAQ%@!E)F&yxlMJy>~`8NS$^uMyq2vxF_m+ht||Y!;Sd=T$!?7snx5};)MO}C(^6b R&{1T=C76fut4Njn{{cpaG}Hh9 literal 0 HcmV?d00001 diff --git a/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/gripper.stl b/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/gripper.stl new file mode 100644 index 0000000000000000000000000000000000000000..ee2168717ad0b7666dbe0b491579ee1d28a9b813 GIT binary patch literal 17684 zcmb`Of0SKSb;l1if!Gic9m|hKK;u$V1!6KOX*Dx>(-DcXmS9LQQsNIrbXk;zt;GV7 z+r^JWK`RRp<%c{2qBYXm&cs|4^Kzm70MTWj*rHI3)I})21(VfS7Tx>%J?Eb9efJI1 zKl;`R?=WY7_CDvHbN1PL&z%tde?RZpC+RDCY)w=loOf#oL-su$+r##qem;8)azzM< zP^s|_=>ddciBMfiBu(>LHYTE8A607mNR)O1(T05wl5)L$gp5jiQX7QX^d}NWWrTl< z(GccXPig-s_1tq?>voGMNHp|zGBKxec+Pjc4ox1*R!+&lAN<`7z?Twvi(wP7?x_b5OEzM`8 ztEo@q!@u~I(a`Ps!zbe`F4g!YU_dfwQVcf1aB+JC;U)5)u6 zFLlDl)}DvY+-IBV5N;iM4x_sG!F&EW6JhL-WqfWsc0Fr)m3VUP!642kuCP(fhxKP} zdb~6HJBOqFm2W@RNBn%xE)eUkynIsie(wR=SpRRo>mx3|@Lxe3_Jao}RqCkUz7RHk zw&TD1h|Q-|Al~uOAsba5uM%(j*j&&T+`q@@qi%a*i9f4HUVY7!j(zj>3m|pnUVoxl zQwT4)`6dv%UbbmUrT*t1UIXI6Gy92e&OHgl%6ERXF)AgRD|r7K{t@$d|Hj2rI^Ks) znFkvo?Cu-Y(jQ$5V(O{mr&Mb5^#_3X)W|b^#D{)+DTo{1I?))F5=)MK3FvuW`i;?N z%>5e>_C2LNhra1$kUC_+Q++lL`^Gy#-1)-W8_&mS`uM49meczr50E{P z?dgtt2A-8j6q(?;qf`m0Ch_`v&x4ICjy_^Udz7$K02@3jkti}jy_6~|mueC_k3AbU z7F~GZi1sL9X9YHRx*}0zf_f=cRxZ^f4u9?=u<`rnZ5`1bC2UW_2G3w5icC;1rOL{s zn#A6NCt%~!vv!SWj}mqYV1s8R5=ADcmr}kqyE{$dsL3_3vF^ijN3}-@J1elk(-ny# z6VywovT~^=anRGp!p6`azG75+l(3Tq8$5%NC^A94lqxHiY7!a&Xpa(hHerJ&I1-3U z+EQiZQcc1~;-kqMP5E0<~#o6g+|8%yk*Xb;;Y>|TKl z-ae5iGNDpsSCLO?PCx;ACbV`ZA;bXLkK%ju;C|4b+Ju? z5ob4^;7H&Mwx#NG4jZ*+==plMXK3omk%jpc5cF5(OPqi&(Ri0qwZ9U=JREGehf`fj zn12L8zZ(ho#I{uJcZD!d4IA#URTtYN%!7mA{SgWH_O?`ge+XgYhYk1Gs*7zBc3*+u zJs1hxQEjRE9u&fEPT269Q+2UT!tPxVyw4+nyS6Rm=N$LB5EcQzhDQLZi)|7XKY(Cd z5(&f%ZK*mg5yB!F*zib3b+Jvt;vf)=uOflir!7^-S3+1s1{)rcsV=rjSUd-UabhG8 z!?mUAI8g|TEMdbVOV!0T35!cXFy4&>V$rr#9q$TZ5i)Fegsi&QCc#`Z#oCcTOx>39 z6-?QEA}kZOyu8kYJIwnb72Bv~NmYcIe33Hx*yH*6L?n>i3E^JBZQSzkO_s5!l;2A} zs(!*UddsrwjJ}pKKaVzV%apzN2rYx0biy-8l`^jlBIcYjD$7zqc$S(op?PCUm_G*1 z+rTnm5S|HZk9kx`#f)BTSPj5*7o%PSXiCioL0_x$u2Cd~7Jrr)&;5`<@v z+GD;8QZeTgO`e@%RQ9b>=7~Vi59+8a69y48;aZmx=3_w9!&$Zs!n1AdF)slr`Y5$w z89fNk=vB(jJP3LmC9IYKBGxkMQ7K`k88m%~)k{Ely+nKLJVGi~SZYlnSWO0m*JM=6 zP8Wz+_YuM>MIcxy%HwSkqp<+@Z@%xhDm}NR4ee3lzc!LydEjlmCZRn$S6vGm`|SDP zcGc7%3Y9wkq%VS4^uV{KCk)TyRpPEIE(iU~Blj5151*CxEM9miqz+tjXr-DOM4?g# zyy^lF?_PDx^n~GgR7!lHa~9}R{`4J2^TSawk4jlwrkh!x5ydon*YqWC;JIOH3t>&q zAefmjhBq&PDresLCWH@1rQ_Xj?%Iyi*WU6W)1o63qEIP2sUUb#ry&f^JxbV_2Td;_ zHiGt;UxCy>?_&_=QK*#pt`$Cy=l5_rDkaQUf~H5+QK?_Dr(n-$nrBs{QK^(YX%O5Q z`H9d%BsEok#aj(W0C9SzfH7QF~f>Br(RTai_^{%WnLB2@yXiDk5 zlx@>OB6L)|6T3Ekzxznk_n=|iiTd2_hFjrd_Zv>1NYjQ&o$%xb7dpXz`586{`=8vS zgx;tPqR{7VUIO|D`jR5eT&2ttfuN7dwi`AYqf)}W9ccPT6H7LNo^9sQUDQR>(``op zD#fh0i)w9)a5K0EwMPl&yLJcTcRD&M?P131>#@6|noK^=qfja4pIy}3TZBWcQ7O^f z`LGfRyU+0yJ!Y``3T?m1?AAt`ce{>CrR*Mrl;4~!q8^n>*}V(GZ|4?K#Ca4dWp_IW zzx7*0Z9{vMu=oKqBMuW!M4|h~?p;U?yq$|QDwWcwVCw<8-dD+nedit}EWQHKj>uF> z*JfHG=*`JWA$^zHwt?h*6;z6Kzb@+3EyAI+uj(205gqp_^~{U(tW=71zb@+SW}``{ zjk>2v@kyh~KAbu3G&YJfDwSd$UC+F>9Zlq!*F8$;Ezu;H1M;LYo8&oX<)w?? z{51)cV#Z4UF+HI@N-!%X$93Au+G%Q?+06+ArXa2sl68AX0S`NM+s*7$p_1* z3YF5aw}{$Cv+6Xb;*I8Y7|(0{N%M^q1L_sjp49HIM!9^Oz*`p3%WON@bDE#D6LSx< zZT_yAm9X@CXC+k1o&s!m4d8hRn;ms2q0g{6DwSf*fouM}1Z?<7l+ZmL!qS^ZvEHw| zVWEBNdG>Y*Z@>()tmxsUBUFm*ZX{l`lK3F=zGO5tXvL1oQB_Zy*seL0w9)-K}_YE`RS|z{ZUy-aevI%xlZa@1=o6$OLsM zp;>VV_uckauyNH3K0l&T%!^VJr_1k4`U-L#XP#K{5~H@ zgiKJE5^Q%t+s-)zZ*mkr-?(|XN-^6mD~~b;5+M`Rr3Bku&>`IQIBgvC>F?a4Qg-iR z%{}%RNQ6vKmlAAuE7ky>y>~sH)ymUmZBr>`_hsc##y}!ug1VGoy9+vmx9t2lY%IL{ z%xx;g8bDcjR6LLfnV>Eu*zSV1yyT^4!Nv>5zp+iFSV1T&kJ|?lArsW41lwKEA$;`W z(_!Pxul>h1m151ItUQ|-NQ6vKmlAAuL0ctt?ONE_+1anBQmjmrm1m6uiI55EQiAO+ z=n#67D`8{ZS6<#zDOM25%JbTRM92hnDZzFZbO_(8j-id^i+d`?nnQ`o#6Tjfs|o5- zg6)*6|4F$3HXiu#cu%ERnXq|S)o~yZGC^HRXk89s`Mx z37dzFL!}ngAtkD{1BsA{E=Hn+)=@%u)y{9j#wXW&q^DA> zmz1cm3?xD(ST|Net1Tg1yYB|rc=CzA?x_^(0cGWNn1Mvd1ZzD?Xf+{(x4wA+Y<%(N z^Li@93WELqoR#zy9(}Ox6N!)s_9&tC2Rk1Zz02PgDK>99zo$~HGn9xr1`;6?tavD) zRSMX^+agY=laf#1kCc+~~^NT{TovJN1^KCW{%v`1Hxxt1%pP2~iQiARDbZyOH z!&gwHY@K1lD|a&y)TIR5sqSZc7dCt+s+8?ldjdQkGZECK1l#F%A$A5~!%vh-*|~!a zKj$+M)TIR5Je%+(Jzo#^3@T;*12&?Uu=ex>dz4_CJ|+b7iLl`wPNmEb!iHCbXCkOe z3AU5pHQx>!?x|JE{5j`g_s2{Gbt%C%eLn6e+ORuHrR-jU4ZjCxBB)CVw$nXmcP(uA z&8bp$zru#!=Q9!1r3BmQKDU?wHar4QDT@;oGf$*7dYJFwyL z)l39+DZzG%uPlax4UfoF%HlTI@HlZMg1VGoJH?3>i^7ISmMUfOCv14cIuk)%O0b>c zU5lwv-T*#cFNDK1^^qL(W{i@{jlM+ zg@FWW0NSGj+o{fAl?mAJT82uo7FyRb%w8W6GLd?`)=Q$ll3M@ml5Js^^0z7Q-co$y zg6;g4g>7_<%6GgI`7e=ZBmS-^Y8tZdNO;3OiSV9eFSXMM`)y?F{Wkgtnt!JN`s-4#=F-G{v`Gd`|Npm;>VX zuKBZJ^T;4_X39*sNvxT(+uxJveRt?F``e$a%X}^9oI5jDw+!;^dq8~ptLv7Sw}e#A z;>(!#XHBMk)fN!{{>xd5&G&)GwFTBP)OHA`FZ%+B<=?u~-ayN`%y)s#)elx%LiqaL z^&nn5dwkNo52SJ(hIJp+6vD_WR)H8f{ck7Dmw?FCAy%WB#9jLy4I)flVQ--2QJI$j zo$FU+ta!cVx)+0Z*I}C`?VLj@SJGIyQ%xbf>E6fu?Y7?ge)F|SJGCHkeJ|Azv+WSR z{MuVVZ2!^b#;EKBgJ#t+S0h6>?&ud`m#p9Y&Bngckyu;;n!iO}bKkEq5BnZgsr<&? zaLnjgK?vbLkHZ^Vy^meGWw@zpJnq+FkFQ{Zzvn)3Y@ek*^Ow)N!->jwdyc|bH89)p z{QGXdY|q^`nxg058pHL!v6bKC+IsgNEpg)N;rYXwP3Sx{1F*kGopU0H1CF0eoL>9sCT8k#d_B#u;vTT=`B$y&8c)Rgzzs5&H%CcbHB3hyi1hO z9TUO_##T@HPMqJpbJ%)t4*b0pBhcoXE?E=KIc&spE-S?w$M1~ht!=&pg!>Zh(fC2v zDFpLZc)ISdR7#_oCSg7iMD&UEsFcvSD}?iZ@`(SPaPQ>ej~i!DD+u}wLpb4%r63l4 z{iI=)(z-^I*mKk>e{;F_TYszv-&@L> zLa?0(!gr!d*{%e^ccYZB-44QcyN*hUrcbmx$|Q@P-%LH~l zYN=8lk-!D3fdo;K8(z8RWCQji1W}~XM1e#Ui)bW5ef;#NzxCLU@0`64_xDH7=TkC0 zv%a%t@0nS%)|xrj5dL>RKOUp)B@O-0tA9Ce#>_t-H@H<8PVfB9l8%>$tUNvY{-ZnH z)?Y_ZYA98y3ei>_zx&IVR(^Y{*Yo>F6RTHJqwcH(@R%-kD&ZwRGobb0Gf=(nQ;k{zJF>&#jlV_^*9;4 z>}h-s68rq#>6^BCP2SbeqhB@ROUdV^vF6APZ!sM|yW_92xtovRGb8_D#Ki6IokpoA ze(+W!HvPvQUBsDR{E87D`|kRvS0w)HSCfst@5gWM58>^zUzB;3G{*0{cn0UyrtKOg zrG4M@gqQU#$e52i*k;Lb?;M!H+0c2Vf3R-+LpXe!-Hh0fESN#5%Pw-FIvzirImU>2 zGNLg_VE{OmORnFcm`|aIZR5QgX0v!_3wSsh+8hWX9lIdHL=f#>Uf+p zZz40iceGNYF;^bj{^2UAxD`iDljabXJ ztQ9MZMQwBM3Sq;`yBcx+sS8$8O4f%FvZA_(iKncxUah-kO|%cn+%-aGdq?K3m1XXB zeE76iT7p$;!#=jfjt%NOmetjch=fwG!Bs*q*TJ?(h;Gv%>s)l)@OO92P-5pB*3IBr zlQ(Ko`bN15LKs;7c_UtXeDe%S$#@u1U4u`(u+WHsSGS2)6bYFNM(d0TVZz+GM(jFo zcbz3gjm$AC>xznd_4H#CjQF>g57n7iNXgu_vd;FHIN*u%mUIy}On=gJOr3jAG}}pV zFA3qNyN|KCyJ*Y5sHHX?YQr&iBM&EpC!Y1$zV+aO8I;1z?;_r{-dEIVy9`9VB7v1A zb9~|f-m5#pwJV{cjUINbsPBBhch33K7q6rgp0(>g+}}s= zfW_-pVmvw+&CHW^uQr6chTZzyxMA~3O2PVc5p(|R7Vi43wjHLGApy%Lz54NkSKIFV z_;%+_r*Fvldh<>0CvF@&Wx0&t`HPKs|9KyOE2Ze^$HebfyN*M5I&TGQNI>^U2tRqu z{i{1a`MLYxU$sfVi<0L!qvQV7qdVMtAEn?ebz&me$L*C!*;-R;DSNw>Wp#C^JA`ev zn`rCf56e~$Q-|!9M#wtv$o^<$S+&xu1Fm;Z=8$*GyehpHorxx;c91QF@WO2OmS*jH ztwF#aqFv}4iHQ^2{v11hW`}4+k-+oG z{xM;JdjL z82i$qVb;KN>e8|A&J%6_xMbDpVM@UZ>mp7(uk}{l4URaj74?clJWCGPaI^WMFFpVD z6_kS47scu!9CZ5gn~-Iw!$3AHr{Uaew#J8yDzqFsSQ8SCQ-=R#uOU&lf^tFOQ0zJaa%j zvO-E`pOw|0i-`}t!K3ZXBRi~Q4GEdYM$38>3zyFl{g?5wyf>3lUqs%BmDQ^;`9#ND zOY_h52z%qcQu`sDa7RM3L&jm{Y3h6u}#$8^OB7kynK z&3#T1w8tS_^-1^0UcG#GUA5M(cwrrQXI-=8BNP6~R@B__+p1+SLf&WxUSAjS`)|8v zx%~fZjz*9KJkE|iwmtc;mM_v6$8@N#h8>{G>pS<>{cZnvaQbSEqY481M;Ecr>RrtG zY~1*T$WJ7J5$v;GCE0i+l#*4OzqkGhVb7r~Ye;Zbs+@j&mT-<@=d9{T@?KF2<6I>Q z9qbhe%$R-=wvYFUHJFtRw&db@uPB8mrAi3qI@lHoiI(h5YbM0|loPCxsK}%=!^W24 zUQtR`tr5Dn_avebBteT2>!6ghYdX|ts1U`Iv!;tMS}jrh1^{+NKCMbpX z_WDH8772_^mk!Lt4(9Re*egn5O>_}chiBcVWB$QqmkrbQk>D;7!XaPX`hF$S^^e|9 zDXhURdo`u;H|CMOc;}>5Y>Ncee z#R&C~C9biu5*tC?U%6jZW=TO%3Nvw} zLWm+?wX7k*8EU;U;)!1@Qjg4@Ry}&y`L1{MyB!~}i2mw(=c(6M5Qylzh%v8u)?kNy zrbN*M3GQ|wY+7=aS(LetoN+YcDEBN{meYrqA-lo7w>6Epe(A(MO1WRs%HssA36<*JLLOF6Ew6%gzu+BRpau0l!OHKjnK1OvkKGukNFi`*SVz z=Nh9Cewsl7-k`L;*TEk&9fv*r>pn_(6xK?!yoS9ix9wHJJWI{7i8I`hl^^B>M@P|E$% zR;r%_gltFCNN$}MUuRsVW4V#sDvOdO`xbgy@jxSS@-IeGW?bj_mKMmar#h4GHY$qC>`GM0MzRP|7_3AM;uwl!U)4 z67U~Hhs-O}q4SDT?xXnJttCQ9_?VM`ha)-`{c)Y?_-|PslyYCo*I+FXO2X$Z33zIv zL)Lj)-<7U&O1TH zI_4Q+(%HWQ=BJDR&1FWES29FBQhPw3pu2#?%JV7AK>U=5B?3hOb`=Q-Je&^4G5 zUPA(_R&|h)AQF8z`cm4c4 zBh)5F=j0?{MLUsgkJ^Qu*Apz$p^E` zv?&EYD5K5$C<*m_GQw*}z*`amK3tnp@aH zs4tokUPA&tsu0;H5f`gXDfri2C!;x166RB985Z-xqijnN8>mm+X1jIr)ec=R0- zh$cEkjH5nvn^K5(GCF@(h>9}8Ye*m}5*=Brmgp>@6yml{En)MDH6#$-iHpyrc(yugp5LUH9~3Lj3lI%tZ6s#oJExghx-R#VeL_5gXX|?&*t9ZG=V>loB7; zi0YYFmGIbrgqJ-oiAGRknWA0seXXn~aibC576~usv6{q#_8c1BQA&JY)1i6e(Fkvg zgqJ-|G!NNyXp~AR@qJB)=BY;`ye$%5_INkWI#Wt~T+^X>>d^>qi-eawZjUUS<^YOz z#mBX>=8Z=qye$%5_WVO+sWq2Tv@5=^l{ZP=cr?P>BH?AvqnJ-^_jWQc8SW)1i6l(FkvggqJcEyvmvW|HzVI#;I5?;>79I=|7O%&~l?`vh9yS0SPMAne-az1y( zrW^j%rAF+5JcJcTI=- zyOa_i*L3LZ=vpF_gttY)%bHuhS?)%O-=6DOO)2P8k6iS15vn62yoLmNC6)^BYJ^gF zZ|z#b;t$r4z}uB4z?f?+laI5;PFB`@axGzT6l+LeE{G1yU5(X>b}^4LeV&aj2#W{V z775ID(UGkvi)$%`^{6qYjfbqqlCZdzH6*ZVgBI|7V5=z0&>kTpMRW(n|8WOPiq62%B=47NTJ3q~S zD`V%inv*ddvR{>i<$YL#9WMH%jrr8_j_piXb4#W}b4b{C>j}$WVTX(e?7`A2_zapO zGace5XztC**_~v24*k9$EDy)FNWjYw0=|#t`igep2W9%q`zQ&^C$cRP@QH+g52rax z)1f!8;m>K#(w;-VF9^$1vxWqGJ0akUY9`fm=r3yEmuC7r!&(rQ-(_1Q;G>F;?32i_ zYYy3T=)FGp*O@-gM(2S3n-g9`0$zBhhz%0WQ&S3YLZ&a*Ay1y)ddUc{A%SQ@bYxj) z%TrSd@s8&7Z9McH7KDulYe*m}5`uYZN+E93vk8oOM%cV!4GBbdqC@U}Xv~@DsSKqM zf9e^F>Cm-O5O$8k8WMgdJT;||pV5;>YsqbU zM%ezr8WPCj_^xK}O7ql|LY^u6-n{UVuziqikw7*|2sw8)9eRpPDdfxaVmNIbGAhS*+J34JT;||ztl5&dk*!r3c~y=){sD! zQ-~~+n&|BWN+A!Meec=mg0TAxY>R}K%?}n|G|LDlddq@Ro*kBa^29tlOyX}h^$nGT z*N|Xt*>#-uPjmOv+ai>LzV6>N?32i^g!GgDCVx0RwfK87Lss^?^~34s&{ZzNSJH zI^vp=IRBY1n2rgrJlLg!?S8rM=s|t*oyMe{3Q_1_4T%RPEixVNx#&sPQ4kzkj(P9# zpwz;TU)RxIExdN#>x>}VBEh*J9g@Mw#v|GIgUh9#t@O%QHx8^lb%FK$xk>*saPg!? z_WhD8v*#!XN*(a@7fkA=mfw{XqWaUZypBXq|-Q{ak_g*h; zl7OrP$*J$U)#kxs0GwuI|ZW=wxR$08!3 zRM#AscBzpHsEQ9n@ zdoq2ESRyN{h2v_DpMz3jAB|8e+M8ev3GNcHj?J%lmNSG-|Mw?)Cgh?|J=Bpgm!7uL zV$6%T8^YNG{v6xLoXTzDPFl{MPs{%C@wn-_x$;db{rbb(b+(-R4lA#n{Xm=Z&E%7O zJSg?tS|>I=Gf*W2tFBrSul~X5$=_M9+WV9d3E$P~OP{m8pLNx_@@)BYOqcIeK~7NW z&+RQny!qgHRYD}o>*i;CGOb2U7$O<+cy5?0y`5ctuI9CXT zOb2cIji)V;CvK&C9{N933zuX>LMhG|T0ZTReq2Xf!#Q@gy=nPd88UNJJlDZ?%QvmF zuZvK~o`aT5X1hqXQl0I+XCkF`7^lCsZ>1l4%e}gT#c>d&+I`0A}4q0tYgDMf7*AQG~6#U z#`>;SqBlXQ>+jfNvt-Og^9CwJ(JR)FU~l7IQEKh`pEDijb0gcP2BP6yZUF0JJ!nkJ-E1E zX1Oal@LY#9Mn$Yk@+VYedl{l?vHvE zINLcZ<@pBl=QAS7dqt_|?)A5O%%>*xR|vtpSF9l+y)qruUU5gJ)#mfXy`q#nlRd4j z;0jUniZvuSvZf;^623)w4%4Ae&X(d{QA*yM5o&QNM6M&ShJ@HIqt)8Qy<&~nN0U-3 z%9i3@QA%vE5$Y#Yh}vF}5Rb!X^--c;1zAN_qw7xAqm^~l%4)W9b)B;<@q-{7>T6Yq+UFo4me1&7k1BLfO01npsZFd9g$_PBpO!6&uzd;(XYJ}c5u0db zwW7TV){tNiT|xe?#2O6hdRF@(cE!qSeJX1($q7n{?J`0wT!j!t{v50!!Sx@)i?{zs zvREO!)R^CJudkV^H%SlBJ|kz)A5E+@77=q z3C{;LQ_n4BL{bth)6T3>Onm9vXPA!LR{W)pH6%Qn&`dqIlo3fuv`jm54l%Ln>JOWa zeV_PVA8Sashtte*G8vJSM9Z|(4~mJaU%13{yxh6Hk2NGbW@x6GX9^)Bl9KR@5F?A2 z`1<3QnT~TOp4G=15*|%7#Tse+kr7Erv@-3CIkcP;Y8G^z%Rdc|QnW^+lw(9n^8r?t z{^#GTsQ+J1P>SW8IPGj ztYb{P?^|b>j_R%HL0PA}NWMX=ffbCeGQshw0ekx;YKjknmWonQGoQgp5c^qGj3{ z(Z$5_8@Dzc+y3da25U%oY}iaSnhYT$l9Fhdc1DmfanrgV$=Z|EH*J1{H6%QGmAzJ? z;t(<-DT$V8XN(&YuWmh~tvcjic^+CBS|7D5xn;9i^G}H0+ZJTY@knjv^Gu1q72vx#Qt(b?6iF01QS+k1CtYfZfvxY=z^EIDXC0d#G zd^b?3d<3=ZBbWcH`PO`FwVdw;wD}p4jd{0JKK>-0U+|F4-Jc(Mp^kaK>d^B+=J%P0 zm%seggP@d`n|9I^!ukvUS#>0LZMw8gDQ3)TiBJ;W76~sm?QBwV{Ez*;={SEyvrVaL zhQ5mkCE+zByxg=C0lAkk^>n>ile}r@sy3ynX9!(HD2cqLJezP@NA@fkbJ1bhY9+eb zmH*$*CPpK?E$LxFcsbWC?@HH)JvpW1y>(PadD`2X@EQ_c&fl(#x#`dmq?C-a>CiLC z(FkvggqQPqB{R`<=-j0g?bT?6w?)Fsz6#_e&3V-3)r5~;(4cj$W;}D-PJ&Wi*0JgI z$F_aLfzPUraPLpfZBU9dvsCg|{(BIV@^UlP98d_~d~#daH$w8K+uz%u6eH7GB9w%; zMZ(KX%K?V)o`2lSbTrnV(4bT`67C{GNq7wjFE>+-yhHf-@l#F5*j0x$C{>NsyNFN{ zUPHpm%~bON^7nAJpKUrGx%lrJl&WSBx`(X6jkaw;yZ2uT3V8J9var)wA0!B9w&JknnOd)r_ZDxV`P~Kqj-F z{p<*(B=0wZH2`@KO&HIM% z$SEJNdG*^duZ~cvdP37hgp%+Y5?*emnoka4kIg5Wj>E=}Z&RvzZq!ADlJFW5UT&tI zr=Ga$py}9l%bVMjs-9qV5uqf!hJ=?Tb0@i8`QNno{lTW=`sa^qQ>uE#)E4&mtUTg;g(x_wTYQq_~cE+UkK*O2gXGxhBGPCp%E&+(=)?`u;^vhp@}^<=D; z2qocdk??ZU&Y(l~@xMaL8b7se;%qz5!#QF^LV_8#xK|(o^Sktr7{kTAV!Mp)Z?T(P zc|=bg%)>DU#9WQZC*sK18)i?~!`SN+rXVQAd~$DsH6&v1)%unFRbYuTEmqSTJMw<| zHKr=>S>}0a5*#(ZsnE3hH++XKpFcUJ{Ki%jcf4xGJg_Yi%;Ls6DCKv`ns#rcClOdf zg4yUuN8s2pU&gr*6O>~1t2ewqZ9j<00l6-woiusueA(+1_){x-oS*(Ln%-mFnLPuOv=6ZMvXgUhQ^9*?$u2}<* z?rDkQUNJksxW7UaI#@%Zw}qpBPCuHfAbt)?(JSsvu!cnJ@46NrLDtY?WlK@75=znk z>`6pcltk=V>Z{G?RXH=dPkYXme2D7rg`cvhFQy|T`ejF5jzDY15D4fLjDg^<==2Wv>s#)z^&|poNN}aaIw&RU&U9!Vt|t*#Ln5{quEn36bCgj^&)J?(iZMq| zB3d6L;%I`sVh!Vgo;ngr(Yx+ux;^$}^qbIw-~bUT=amBxqgjO(psIpp?vf zu@1glRtUj-%vnPs&f;)9BsQ>hr%{Sz*sQF%H_7N(S?~L~eBLYW;eKz;;}5P)Ba)n; z6!ldI!K&+=gybx(SDxim@mvR`*!K!i=pZ38!`jt|j&DJPp@UK~uZ++*vqBU)SVKbQ zw9y_3o0ja!nU!U}m80qM`MaW&=o{2oq9fRwU=4{lFUnrAhIvZ16!(f!%v1IzSVN-c z+*Qj^yhX7qR@VFl-@5fG$!7_r#9kR;xAZXcYb z(JM-c4K^vwCshcAY_DYv39LI~#1!5R{LBc`Ju*mu(6KbpSc7b8jClZ)49 zQkti%5QPrbkPwg4XwRg+vS5Rq{5*cme7)s4s4G?Nu9yAy2A&)6Un_*ri=dR3-3EuS z?&)u;j^wG07quzn83g~GPc0Ej!rLO@WslqB9@B6C#dM6l=F&E$JcHoBKC2}{NqAc% zyzFse2>;yZHyuka`9zyi9u@m9YHEp465bXGFMD1?ezE@CZZ;n8>bs&%DUXWv{CxVF zS|U+`QeM{GxtoxC0D6iX^yUDiNHdPMn*(<4ToT?Bm-1T#F(LOE>^bxX1Zzmd@p(oh zCE@M5l;4Jk3AvYHI`k$7Ye>-7wwoL_f+gYYx>Rvnq+eHo=F9AUiRsWADQt@bz4L;w zJ1kWxznu~jGUg*X9@z+r)?%Dx^zvV7m2rv9l7f)gP)K<>pGzcU?kbV!Or#WPnY-4K z|7t5EZ0?evl$Z1MA#2cjr7Mb3vhJ*1{XO4k1amTO8Jy17x!4lZp_YN|iv2Jh;%hn=y?>S+z)EmIoD`}pDvN07iA}LrbExwDCOQ# z%g*aYBm9(%1br<#Q8XQTK1eC|GW^tXG{R5&NYF>IlTp*5=bV&sFQaAWr=t;mYDt1V zN(geoYdZA2mQwD$$Y0NrU%QM(_~|Q&*!Pi>YSW?T&XjV`sb%NcqY-|JOoD!wow%zG zJ8`F!dw?xFm#-!4#GN%H=s$)aHwR3I-UFbNd&MogUrmYsh<$3g0bx4yJ_DuP zzi!z*iCV&LK(K}cy>q+CVLJ3)2Bq91Z`pm1TEcE}u!aQv`4Hqris{h%C6sc%y=C`S zY6-iM!Wt6v`t4?o>Ck&Jl=67MfB#cU*v%T&kcguRxglga^u70~HkI7*Qo?R*u`Lo_ z&hL21%`?-X_sA&ad6<^nPpc*D<{4{9#917<4`=?B-k_tDWDv|R(K~mwgx$Gg4GAxM ihE48l@BFCwB@3Dt=qR8d*k!Wc?S2_ zdf(soe(&Xb*SF4Dd(Jw0#_?l2&z?T^=IhP@-C}v0(AKU^_|de^t#!Bq&h=V_tXSQ};dHzw$S;gCCTr7HsU?_C)2@7l)Z2 zu%QGcN?^?EPQ0o9FU|TuutAAx z!N&T{J1R#^zah1u1SLvf%rPmJ z;X_sDQY~H|d*j;vKtLG;e!^UkD8X??q<(ZXBcO%r@O;wr8)|Mt2}*QcjCtLOG`eM- zOSNzvc{=&RO{onfC{Y4qUUwU5T?HGIs1|IX-nw5LHqb~=q6Egg?nGMc!3HI&1sm8m zTP`1++E9WLB{1f7w~_AJV1p9Xf(`8dpAR0C+E9WLB{1goXaoH!*q}tUU;}+D^CRt7 zfuKYQjCnoAs>6Co13@j=xNypB$%n-%5|rq?81uT@u$-5zmjoM>s1|HIeARTRDm|VkC8`A*k6tid_JW$Ptj4^pC9u(4y~p_SaOjRYl1 zV9e`o!#V?esOns*#p|(OK^X*o!d#Fj!Er{UeuSO@>q=mZnc!dSyCo$k(RnfE^;|w` zI+tqUI`S0nAEh)OfuKYQjCtK{q*c;DPzyFtZ}C1@Qi2kl7h_&`8)>x%8J zyYub|-jPA*DI-r%qVtkq_sJ9N=g?yHVT_t?h;CIS&4}h1r_K|UG$Yt|;~BAPTO^+7ImSN6 zn7cXnfv&@!D6tE1-(k!hoFl4~pl5K5U5hdEpCe3=NigQTyv|cbJh@gWLC@frey~?I z+MuKv5i60NSD~c@YYf+!|3({>kjA_uVkPEmD1kAwKPy%CU zfna}Zv_Xl^OCs*`oDC%~h875P*XGfsMCT=u?enir>RDJS^|Y5tmHJ-gk)6AC{kD>g zmxb{KH+;2>>+XjUtL`3gs!^3XB}D==4Jq}6r-^C}#HL?uzoi)gEnXi|+LjU%Pu)4f zR&|$3XoG~?AYqIjQ6{2 zTSZsfXali=hs_9&?oW4qzYQm*sf*8RG4CtB_SyAGlU^@lTq=#N^SXUntk&&X6o^}I z`b4Wrl-P5|3xPiHd!y?~GWmhC0`cJbXLE#V4GcUMh-HV4Fv4A~majdef0gF|5uIH> z^|aA7tm^ugJ{ebq>q)Y<>;Asr$9ML&3}${P@#jO|*9*FdIl`gf$)5sbIFgt2S0{<{f0J`D1lsi z{;?{{pv9Q6lFc7Fnl_N*5#Fzs z{rf&Eu&NV&eN7}FQG(+Wmv(k0$)<}h2{xWSj!IRRa*bZ`IIEvKRq|t*!jTDzRX6R;4T<*+f=jh)t43p!aj3^YH_E1 z{`rp?@spk34}0)f>FuZ~^CMKB^$%NBw%R1Q;^dnGv9@*{eS@cRb zR^|I6u4*954JsyHw^hyF_?^&2hgug^i4yrLDclF6=c>gFS{Jod9lmH(h%P=gid;J? zgZ`o3lH``7{BH2VF{c}uWl#x50ackK&mLd4%vH;?zAF%~R*vkWho}a6j#|p%20!+iy*?17|GlhNB}(XiOcL#)w1Fsc ze^{?YtYXwdl>{4}mrSAreX`zetMW6C-wkvw)nY{Kj$?$aDrB(G+dG*}tdM!aXZ~KR z5+y7OAvbnEHyd_u3N{KSvWjZy4i3@nLk4%fds(kUu`bcAGwOc6vJ3{|PnJQID8Z;( zX6s2ZH}t7$*+0D_V=mQVq^*vP`2DBn1{<$hPgDt`i&L1|PLgjN<(+!FRl8}iUh77O zG3I3q-H$^TjcSAE_8IpFBuX&S?#Fj8ofe3%NQX=P2sWU_NLwA7A74B9ra=7Cx~NJd z2ZdGr_pc6Ef>n9qoR;Z1t~*h(pNzoi zzB%Es`uX6X#i&}IwD?)O4;JU8y(fY_>lo(wuz&5a-C8XVy=7@@sgy}b!(La{&`zxI zw(gUSIB{9E55&@`U9BonqJID11!D8yV)NtqDNc+y`g5%+X+{hVF18&7uVJG|kcL)Q z*YUxI6T6Gf6)2`w@ZIcl^68 zDp5jWXvC84?oe09EI!Yo9HWaYh8*CNW4{}i+xFw0tA`vTqV&U~3;nLHF7q0{&FQIh zy;WJC&O0FEXfH|raAZfzzx--SB#_%C#6bEqk>$f`%55k?E%zf-XduGx6RTLANpvL~ z58yiKtl9%05}}o_x^m5Gs9&+3)AH+H`AIVJ<*~gd|HrtHMU}utx{^SraWflMPq)~= zrUbPx6UV_1{!XSUHYzfP4H6zJ5+#-FhOkw&Y`wbNUA?{cvlGU4pkG;DI`O2)-i0xG z7q0s=rxOPSLM27QqkyN>lxQ&<{Yq$0q;2n_>8b*uk|F^cP7L|T%<``NFK+$vmtX8x zf~$g7zPG1DpAk9Fp#^X8EKAy-ok7gnzbg{34v7-90Xj)u-(zz5isydNhfkF#fx8dv z`STz&M_)B;Rgq9F-mjd%`kz0nZ539h5+!hdgbja61u~Fz3HzFglfrY$zp}-xN`WLfl!GOX0Mj{0pjaNUeQ3P7RJa^^t``) z>pun0RicE&E!c?B?LT*gM^_2e!WcGU25Zl)*dt_6B}!QSgN;}vwNs{iKU9els)aFZ zIANKqS+$2sREZK+yTL}R-P)Y_zwJftl~65=S<_+H*1D^{&sCy??Z;pvc81yuj~~ol z66aDajA0`q%mzC{lqf-au``&BI~oYp;%Z}OK&;pqf(=NNVBGxFhv+`}%IXF}wHQMu zY_}rkZTM7)63oA!ccJ;@D@U{z3DsgPIANJX?Y3fdDp7)UjMH+GfUul55URzRb|T$_ zt+vl=Dp7*_(SMT)qW%0g8wl0nZjOZY5+_umgxM=&pT||9mjoM1s20YktLS-0`_|`! z=PFUc;udVg=%Ob!5UPbSY{U#&Z=Vn{s1hYC|G`GAlJb^A_6${`glb_78?h40yUz4V zREZK+$H9hIyWRHf$4^Wu(Q3Ed6$syhLX=ceAZp0RdlQ9S8*F&@Q5#$pBn2YaaN=i= z>}h}Tsg}pB{xW_~9p~bzpv5S7PoxbHDk%`bMxFqvgpH#Xv+2E^Ht?yE0ukuD4|;P$#)bqu`2vmB?Thbh(xs+VRY1D6udKFZW1af5P^%D|WbIs20Ys;lHg-@8^L~i4tayHSIR~d2UxiwJ^^7@N>!Y!E==;VG#*7Vs!hv z|Fz6?HrN@WL)|7GvlHa&FJgxf3$A(1S5%#n`>p@&Ur0 zopYz~q897Q3DmCb^ETvOB|0zb*zUDv1B5*fHV~@Cnsx$v@b+oPwCR}|5+%4F{qF`K zc;3~yREv8v5)Ef+NR+Ud%7|NBRmZ1J^Hu3ws)aG?DteBd7(7>r5*D{$BSyEw_IU%L zS{TDd%wU<%Q7TcwswCKmRZ@QJy8CjcaLlDz7{i7W={+h`qDqvoIu16x+QX5daHcL+ zyX~$(_#PBO&(yj7q5CoZ2GZsz)9(hfflrkbh(NzLVRY1DRrWq%o#XzN4UtRpGxXDGAoNTP33G!nqw5{+og4S4jXVC2{d+w+wcNgG^oD=_ zU9GZc=#YBrH^$mqFC#k-T>b9A^0&uqxOCl}l^@=`Pd!PLP>B+kJlGQGY4`4JME9u2 z*U!6SV(sc5PA;D|E~!`Mt?#)0;MV#r`^~P;{PRQQcXo`buYYUIhWg!iR<8LbY$%}; zCEmYtR{gxK50_^?HtIbH)l&cRHdLZMsqG~B_`*@OQ{HW@wROJPp(|0J@V8XHQP%cj-teE6{1WlvntM-T5j zzde_Oci$PaVP*Z!N?X!yt0FP%>;qS+gfz0Iam$FgH~w3z5~`K2659Cj9XGZqp;~x1 z>XLgWbk2MGa9LI5x>3CR7?F$ReSUp%ewyYJ0+GQo`eoPV& zznOY#F9?+=fpPYZFo>saeY`0_EnTgjUU7omxBY|mH^OY^aD2vJwpU;y+qKZfzk5G) zz5V4SlQbjnzHqK5)-=zI^hZ{|ThVOh36&_pNPl}M-g=RJZr`HIzq8FZml^f9c8n?z zDxptUJ3CSJ;MM}65+yK3Cz|~9o&`cBdBUTPNay-)O+8m{S;t=OayO`{LBa>6Z~sdwcAnSZ^}|`!OR<|KO%ooC^{q7{kkVmW4R! zwKZ0|9(PEZ5uOj680@d%>0jKd>CWjH0PXt&V_f(5>HUP9CtQ*zKr3N$)a+j?wX(NA z`P~Dz?eB}8vgQEOtk%v-&cVvUN{R$`t!HrcvY$b-Hev3ET9i;N-7875@X>wk+Yi;+ z#Obqobq{ikvq+vex3YgA*8TnUy((!&;2e}BcWwSdSXGfA4J}=5l05a5`GM%}{z#un zni2jDh3eyn2I7kcJQxW`lwcIRQ{O+lju_S2u3nZSREv4ycQdR^jIjUM+epYQzUQ&c z*3NN{jXh7kD$`7VK)IG@(6zE``gb;X3I{}j-{w#P8cwlYWX)&d{qU)0ePoc z*G|Iy!2RQ$D{lX&-EnHV&k-x!$xwapkKc&#d(^T|9@_#!B}D=;^zVPb#^fE-BSDE0 z%#Bz3?{A&hf5Zb5YD0fKsSF$Fuh{1pj~g zOzS1PBUCGYX85()IMP-%%vObO8BVuV{bSt4$ImR8l19Q<9uN z@Zs8;(|*^#=CuiBU5UQao?n#`Dp5jDYIz$fDH5nef9u|BwmR9~(?1XdB^=|s06G(@ zJ@muLL!Q|f2%J)Jf^0_k>2=S6OV(u3E))_aP?r9s}y2-_=rV=ISx%(mc$PucgQOH$dP3P4WvZnD|k|$K61R?^@ z{XI*m_L>r^g)w&8oSkK}A+;O!IVDQ4j@`!Y2-U(E`{v;_crz0jT)1Fei)Ap_;ChRG zAm?ra#0k%>hy*1{XrBC}f7H^wd+nrO<2?iCPwhB*a&qE@LvCpSp%NvucV#D;joa^v zXCFKRYNUBa!IOSQEIGE)rxGRd`QZ6#bY*;wP%V!A+1d7B`OZh)H^%<0^*8hIyL-OB ziF*e6RrX$I*tNEkdRkXR;*i}E{`Tilv#wm}d%a@!+W3xOc;gcl!}n7$hTOgdG4Jd^ zJo@rsN`!Y^N#I>ojP3p1&LnyG@K?hLz(1o6d!IJb`qWuIMn4XI*xc5E#^W1%@mZ)>t*s~0twa1UbDdXVl&dm|3h4)_Jhrjnab22p17q@{J zU`+DQQSrK+ubv)W!rQpD_rJK-B9KY)MD`44b#>lfKL+CJ>fOjWZ48jW?@01{@RP6K zZEts1Yxf+!aIH#|$k)||cg~mu;_{;>^yLZfAE-VO-b?I2QU!6=ne7^@ie^n#YgKae zycg~R(KGTSyn9YRas+LA&Y#FU&k?FcpZv+|xYYAChc7fDdnU^h?vp=FWS;-P{KzED z2!CQ(e8wZ8N{;^Zm(=sPKlpYuD?gkQ5w02t)(>_O#~|=-byioV-jq-+j{V*9scCeV zjyx%s^TPgM)U6U>!F~r&#_s|2&2Ar$?A$!KF8GA^th2YgN$C6EN_2I7xD%&A(Cj|2 zg!jY!U3q$od9ik-b3|1oq?H&PTxWG}GgKMYfq;sAJLM8cFxefQ@&5N%oA6pxK|LAA-?I+=N74IK)R3cZK zBp?=?`_j1mjZlg9c8pP#{v8t#ht;2JN>D3b?dhr}VO1*8d9m7TM}fHefC&>vJ+p5I zB_6A|5_=y*GYDdA{kfGOR3bJ4!AQI3H=p}bBq+%fF>ZFRO;@#+Hk3d%aSvzIJ%h;g z4SzeR51;w(nArbcqVzdR?fJVn6W{n{uUARtEoc&YFLC10C-(_9bS~Axb*?Z;+Q0DV zim7ua)lT^Kl(K564ej&xH~L4OT}eVEMS{X=}Ns-OpHdKM&R+Q7zn2d7ihwO}ul)s{M{xInr%pk|M!phR}V|&JI5^ z!tMDJgxa?6%KYgU%wSC;%535(MSV(=GrxOlBvhh=uF$@Br-{n$U1du dBY2+gaM>Mzwt06BHc|qo4Bqwg1p8Q${2ww-G=%^F literal 0 HcmV?d00001 diff --git a/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/gripper_prop.stl b/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/gripper_prop.stl new file mode 100644 index 0000000000000000000000000000000000000000..212444301dadf35dc3a215c59c04904660405cf0 GIT binary patch literal 22684 zcmb`PX|xs9mB+Wy7)OHGNJGRCT@Jw~&J*|EXI$cdc8gI_BOvXFL$@dxg@g(|f~b_3 z5W7RV+qe`-Cq!HAj>g~}DudBz9Fw*&wjadR32MvI#v$#{|GjtBt$)?23SV-+yuIGp zzwif#+`Wjrq z#Q1#-q$rULQZ*pxs}Y_YsBDgy$8+y?f9(+w|VSy)~De+pMwAS$FJ4&Nh49m zP;%&Vi@STv_GV98xT?Es$+)ime?xBHM ze$M*z;?}3&fA~aM&)I9uz0Y*DRdcrgs2dOpX{#c>x_lJ^qOZYTN3FWVG&Y<$I&4*@ zAzT*|+&?(y{V04tcpe9{Bue`s?6qR>l9u*-*^JSDVqS4wOz;fVM(M{#kcoepu$hg) z>svq3diAnx%|76v)h%<+h=|AF?GM&>wN=wzTo;ZmMI;E_ri{zc8=XQAo-d_jlKY1h8-fnc&ehjXuF+ zyUoWt4r;4LpS3j9i254jHSE`8j5zWeQ~wk}_dX^LeQZ~whhE*WRcw{Wjrh+{A~FLW zZr^bGw80ZcoYU3+A2nqN|M!oNIeTq$!Qz&Btvh>aNJMErA|9!KeQA%L5~G%`=?^GQp4jIs=)=(1Nx3Kf_-|lAiou7QB z8_bf7h}er;QcDPzFmdyhyIG%B{^eFJZBc&TWiM`fKqzEA;<25!b`R^vd#|l%vbJgT z3HF-sv0sJlewR$FC%A+OzJ^5&oh0sL;`W|<)`czY#}5xXx?UqDUH1Ck&l;xj zotMWC))K-cOw`73(P3*p>}U_~858%Oe`Zuy?U9Gw(!)5J9_~K)p5PN@vmN8nRWHn+ z9{~F$D{9s>t;5_BP1l7SFFwiu3;-3k7BEIJo@z>@b?-^4J9{DIk*=PeGM*Q zg5x#*gmu4=q`%K^QBP{{5KT*5>UCq@~;-;XWFO)`yV_gReD$4?0rNQsEO zZhdKj5z~IWBqS6{@4$IoOdNaN7^C<2@Mh5d35gT-`c4! zMpv&7o@71TSIovUDjf5Sh}aAF6(ex>2?@zGe*deXmTK&<`pUuS2l;bw!ub~u-t^Rz zhW`KYjcKd;1bgunttW~l`E`42)D3Obn6RPL^N8!^>k|)-Mx<`l1P z#9k+zz8^+)D7km}ZG!>P*WeN+c6dKFdaGYAD~&m3hN)vN^)NHc#?0vLv(sZX<^y-% z+C%jijBGr3)NpU_YOC-x2uCm@BKE=)zz95}LP9bPzB4evcO^Jet9Vxb<_nv3wN=Nj zUy9!5qgfy#_WJZ^pR(Tm?^kXO36;Us(@$O(6Z~Cqzl`APjw@V{k2w>#&$sj{;6CqTo@od_4fsmej(NmhxD#2uxL<{{q~9v8iwT}@aJG;2 zBf@;OF~AJ(;kucP`P##kwyS`#DbB=*y;v_GpyC=_!UX0*E1fYV4ff*iM?hp6!Wx(v z)^^-Qd)z9mDQdxF?qS!CtsW*_{)2wO&AE{fIbDxaQJ< zu3n!*K3o!hYon|m>~+^$#~IPNdT~f(8XA$cv|f!rn3%lBRHLWgoyH}V1ZKW@;m+B^ z-PLS7CA{Z-g1-l7VJq!N_>GFFAAK*h%53x_BqSw-zk^IHT0Ph3fmQPd;SBdogT2t( zmbMDL9SjJCFe5~`go){oU1Ib%Z%?DwDAV9;f@fk^HSpvN$D>cM7w&dO;EgpTBx@Cq zAQPFbkpZ0T_44hMMHL`oVzLRHm z{;nQtgkvM<^)taabV%PE{f#C%l9Sw_|++K+oqrc(G~35C3PIz4(o;l8~J6x|nb~wl`Lh_YS$r zH0FQ(2W|EW-ylnbY^Z4F4|gZmel#>{6YJ<49$+|}7vNk~p)b$L{i&-`q* zf4K4_)sO{O?A-CU)Hu<%Uw&idIW3YBQBDjQ!Lz+cEcwxi9o2a9&aq8@^NQ__j^DyS zbVclY(-TRFC@1_K(7o&%)hiN@UNGA%CI z5#_{?sq8A1Gfy0|z!UT_#NJ>OG;qS{lf<@xE%l%qSKIXZGuVuuxYvs1@ zVEN8goM0EIC6W?RPOukt<=|B$esK2}tske~JgmtjZacv{bRu;Gd$}Day7MRUn^$M5 zMzm(vHyi8~?iH2@$q6rE!fo^%JB=b=y6Q^P7&-Nk27B>JVI?6s;dL?LcAWU$-MIa( zHH|->y{N%nycSwXNKSZNOt>8t-OPFvww$*x(*$4Tx zdi%wJHhYEZ`z1nh!b_NNJ5GEb=f3rG>{_jx6t~$c-1#XHk`rFSgxhhVJ53^U9=hB# ze*1RPX0LE>szgXmcnK43$BFKLi9GagGfZRBDI44D74Ddo2+0XAVZ!Y=@twH$x13@c zpPfFU!(QRuU5SvK@De86juYK06#0*}Ez?+j&OROX3U?Pvgye*mFyVG=JD4IjUohS@ zzI5g>9rg8HSF z>85ehp2M5$6|Nf86JElE+p(=uh+Mt;Sksu&+`GwM;krgW;U!GC9VfclB67jJ0n@nb zf}@)3#j7}*BD^jp+>UiE3mSJHKE^a&J7aQ_y~0(adcsSXa63-dUgz(=xoIqT`P?RZ zg{!sogqJYkcC34P=*PhyztGU{;P59eX|fmAFQJXSO{_X^itxIaa2xXtH11c2+-Vx? z{y4kIUg0WtJ>exxxE&|HH}S@2&N7Xko^*Yay~5p$dcsSXa67i$6Oma*JcoNB;`fzb zZL(Lm-%?L_2@`I|>3-DsAN76$iRh(|7c|+6_lGJ8$qBEE3Ab^twEd`wC$C3DD$!3) z`)ZTD!u_ZcAvxhCOt>8HjOJ6oYZ8maEG@3U2hT8#q3>?rWG~Lqs3asOye=l(#(m7PMeaK9Z%m_k^6L%u;)aw%WaFhKp=**ejtWxvln72t5^3`DhZAH zQo`$E!flJYpn(|9G!VnJ*$Xk1Y2^DjB|>t-OPFxm;x1?)hBFPsaBcQNOl2CnZ%|1{ zPIz5RxQ%fFErJ-%G!VnJ*$c6jY3Oc7B_TQCbur<##a+-q3}+gM;X3SvSj#kYXRMNt zobbAsaNFW8Xds3&4a9IA_Cic$8oGB^Nk~q3T}-%baThcY!te!fi@TtK7|t{hk#*P$u~tj374m)3Y>^V7Fc^a2@tStYsRS8&yeY43`pK7ZYw<+yxE9aHfHXtixW2wM-+=#VQdR z!=;3mFyXevUC=-bXBvpeI_!m*$}}{4r;^YZE+xD!Cfv5T3mS;wOal>FhrJL}nTBR{ zR1zA)rG(eTgxeN(K?580o&0gWTfD$1&;U!GC9VdGB0Qba=wq_Z8< zeqJqp_cYU3F?U3hz4%PjrUCF<; z7%}&}CVTO@#7z-i7ZYyBiJrSfe$42nHTIFIyUuE|S9lJxp70VT+>Y(srO3G7uP}{; za!r%H!jqcygqJYkHuAFUgfz0-I@%B9!!te!f z%VC2C^5IMa*=-H>Lf)EbXx4otq4{tr;dL?Lw&k!v1Nm^Kf$X*hdm(SlH1upnC87Cn zDdBZ7;kM*v|5nl+e7Tl(4*|9!GseLhA>h zeG!zP1)c-N-y-OB_5XU(U;Qkr*a-IWlm2m{zZwt_3S}Co!(&n2Ip+QqG$QumOt)G> zI4a^iqGEhS{fO9$W5a;R`cWV_^N8tUKiG@&hyo(hNY8D1WXVJ^Lx}nz{1oQrEBDV@ zm1#sg=A3QDBU>cci!;IM2`*ux_%v`8n3Hf zb>|GKU{_uGM|uHK*(xS@j#;bv1dlCe zn+3BZBO>W`%jXd|u(H*3vt*{ssV^OJ-}%HdaNt`hVRqGyf=?iR_ix zn(q@3z+9tP;u=a|6~{Du@6J?Gg4@g4X0<&}e=h^;4ER+I&K;{I3fGE>VvZNL3XxY! zuZf;rK!jwro{BH_kBEq4HIAu37_Dnr<{xF(iZ!@iM4U$G>GdMPPkLU5;U{X5U@u-( z2?$_5f?UExF^^2$GlIQ%3<9FB!6i%-Gw4VoVlN)Gfaq)Rc<>k$BYn8{30}J|u9)+V zDH9wa`Pun6(ceD`qNOOiR_x^`@8cvtt4|SGzg1ip6If^L>J07w=0IPAz4+8cK=d`Z zgbALNaHcV5FT8oRv?cj(C(yk_A$!(xT}%YCy-)D>4sQ^qk^gE0X+)U@d*OY+i2Ro+ zh>(onRxwe$SHe9bB92)33`K498nG9jh^Qs>_ZeDxMRnbn313^z){wPHX+I+N;$r!W1{vu81bEx-x>prDC;?U@jGBZRJMwV z;`<=n`vl(&iu+upJ42N9oV|*>RwbgZk-aJ6X<69kD(+xun=%dTB6aoZ=BLy>GZzG| zzULabdv>k(8ID|xmU?LhZY`nbgq!N6zZ!u1BUX1jrz{})t>X3;_qj?vkEkEk65V|( z?sJuhz6M__CW`w}aLpv%jEWcK*8`SuVFCsRsV$aT^o$F&t;< zaBfU7*T5vwI|FA36!-q?#+<#1JNb14j{y_>ocA_mJ?HNP`qa|vroS2z%=RcHBKAVh zjnG{1fPj$S6S;&5ABR#|bV$#!gFHrFq(1le=fFVlU+17@=pw1ER0NB}^c* z#%Rx{HkGthT!P#f^U{oHuF3qPtX1rV3>YKywhgDxzbnDFw%;oD;#|If=(mbXnBY4`u~mHk;2b!v$%uZd*o!mT0;0NA zmUG9MZdrDlx65ZDXR&d%S)kEx6?<{+SU^;^iiu(#S@vyG%*VrjJt?#GS(1&DKf9_3 z_F~(KEymBrj1#%|gOzPw>vP+GtCrt=L{cKk3HBPYHRK|(r}vtw;=P@Vty!V}^CO#BC?| RZAm00qMTqaKkr^7{vWXDD{24$ literal 0 HcmV?d00001 diff --git a/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/shoulder.stl b/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/shoulder.stl new file mode 100644 index 0000000000000000000000000000000000000000..83df495fe18042296109c044efd15da6982e7a5d GIT binary patch literal 61884 zcmb822b5IB^8ZH=1eNqa5G2mBvo0!8lI+f%8PX6$1%04^AW1|_JP?qqn2_irDyWa$ zodrR`^b~=)J8MD#1qD&T_!B|U2MQ8I=|nqy|GKfwoThKZQitH>lO_=Uf1)w-ucfx_uRRL|NsAXogWB{uF^gf2uybt%&%HB zVnmnNmUBa+fj}VIs?|eLPTSAB$Kt`klj4#{l|+E(9z(*WiJ<)R?p?Wl=Pf_92n6D> z$J*TNK3A`OjMD$UIhqw()4UYt{CeI5nxyOS>lZkpxT9iI0~JO!fU|H%-(j z_ryyz=G;W!^S*3X)9*jK7Xd0pInx!w{gA}cw2{kE@;N&?G{ z#D+Gl+|yTcJ;c)t)cU(`5C4%a=ecQH(s`;*oxrxUY-|Qn;i-_RO(WoWPJ~8>~H}mGd!ewV0*fvZn z8{NL0y`KKiI;D~DwxF=_>IQ8C>%k=u6%wpPG^K4%q-Rw#Lz{Cuvpwqhs#Fb=vzu?x{fbJ$V*-gwT6b}%S1RJQtxK9e zqWdlqtVK2^H@n%Pk*0|D_pWTdU5`PQkQUa7b5g|r#Q58%#~@4630w27eQwWroyi`o^$Tpv?#YTrB9CzM(5ttAjY4Y zHg~_xt&+3y?B#|aNjh=i^sH#8<>jh9+^+qeFYI1zl#gI7qL2SvH#+%Xb4^rVeoyGf zY9k~;?LlKrYNggi>I^AkN@e=4ySjK>QL8>ZjSZ5d6GNIFi1a&jfwqzRK_c|-n&Ccz zwTMpcuNd9@VP#Ep`>aoB#QmyNJf2DG`r!lJ&$^$I_PDg_>3+9+Y2mzjabG=f*YQ1~}YmvP!9R^3n9sNTS*DjqOS`oil66`ZbYwXP{OTHO&o_tnZ zT2-oo-(L@1d!mmdSdvZ@J{c={YW*2)V`=~QLO(XU!AGzbJ_Tjtw&=D{&fhTs-NV%ol%sVRqR==S*dyI5#mV2F=kEh^Q%8Rb$} zHLs+J?9bB_V&Kh`xfQ32pAh&4ceMnj8!#;uklO zHn@+H7LGw0wRU`L>$fm;-KPU2!DEexypQ*m{IHlLv#j~BVF?2$| z(7{E6r45#(6FeuzgH7KYne+JK;dLnjpf~~JHjXS?wQ|ho@nADSh~RZP5hx)V2~Hau z%h?7;FGO>iXpRPr4KgE3#S$ba&7Z59|8o4M(2TJ(pAU+zYSJ)uk5s-U^x*3=Og)%jEuz2LQzP}+mg<_wtJN!1 zP&r2uEFmo<{_)7gQFmVzO_aT0bg0Jcr9OhSh#oetOsZD%i#1WV@g1QRt)7(xOGpce zV-5ZnIk>%oCW4ncq1yjBE&P?ui_)w`=_~u~D!I9&9M|JrK^!?I36`W2H&lBgvao7d zP1GMhIrRO3EwWTduols86)Y=R^=?2DGbYRr{rU`zS}UP65``a*iTw2OSsgR9JMeg@ zRpqOE1Zxrf@bqRSxo3aZMBcKZP~??D;YF>4(n!Q3yPR1yPHSRR?FFGbUVgMVonS5c zylnN#?)&vQzPfP6EuluE^JS^9hhxvD_8(TOrLOtm`MpD}%9AfqJ%c2aMuK|J&=!&QN zOM)e&g#`7he(gERxa^94q3gfeD*G-HtVJ}n|E!ulw2cbA>W5C;`i8W@64F9~`qk(e z9Q*t}^RiI6V=ws#)*_mE%*Js%pKmTVGSs1Kx5g-;G!oRWo<2%M;p?OG%8tL&*|lX$ zSk6l(Lc$(1Ey_`iBsQ(Oex!5aw@)R(60*U)kJ8*1l#Ts^uXjpU*Ko)N60Aj^b4;Xc zT=!lNXWqd&l3)quOKI*4%En!@uW%Ne$@dYgMK(CrQZ`=vZ??0iR!2#&1oNddx2&>p zeAwCWHFw_XBUp=UaIB?le6sCGczezeNw5U-r8M^iW#fxIOTrV!j`b0&MK){&KU;XeBv^v^Qrh+sYV*Z!ug!m?$}}IrT4aMGKvj=V-yE1ytwBE@!CGX4BUxpmexi!|_LqGm!4k}u((G51jl`&{ z-P(&<`v}$|8yx#88{h11@7`LWxg=PE`BIwwqq33e(ARx@&V@dLwaA8zsi|Kzx~adr z<78P$umtm^H2ZmFW9%RIxQA=LEIk7ftVK3>HL2=x=*cl|gYgBDU2-YGSyaG@*%1@l?F24QCe3W3ml;+q@+1S!zn%nHzXE&!4tVN&m%EDtq5-h=d z*`A6MX$%fHI@bMR{lX;Xgy%XU6hawOW4>>J*zt}aomo*oA?OoR}@WBPQ0d4Hgw;W1WU+yLt>tf+b|b5;huC_2{@`YqG81WU+< zC2TaPY~0bTQ}X-{lY9jED~cw`2Jf&b8`>vIf+b|b5*R1avnsv!t-KjOO!E;OO_B}X zlT$XdZHh874Z0OiW5-h=dDNT`+THTS2^~Z)e^N)Tn$2k(L zMK*W`LD|qToFrI+`BIu!lak0ga`gn~v1=dj5v)Zv>^_4i)ux7LCptG?Hb4?A!F(xg zSKCyN>U*a-r>;7+3G+fN`keRPRXuV~Pji0V`izKot;9>%{Qy;uxw9rYLto1eXClY1@oy2ZLi98Xo`#z+3{!%2^Q+-1NIX<|y)?RlxpEGTVn>uI9@xhWzgxW)fdWd~d zI-R6a^_KZkN$AW2X-_>gK{Zz;VS=B6u_4DsnVuDj?Gx=Dv*~~sBdRZW2{F#)Jpif) zNiq>?42pVizY_o3`yPr=b@JQLEhq`E4MkXcbT;wEfg{qpku8*<4ezs>w6{zSJz*kH z;wO|n`E(sUqC98YNKid!{HyaQ%6r$^duY_kdaxv&;I>gVNPAy{EctZVzf!(lFF~Gx zYEIhpCwii6LFLK1%ljUFLXQt>5BWzW*>a9NLvKl72{|Wa!%HaZSX(Zk>Lu;Fb)u&$ zONKU7TG8~>*>*hGsBBk5pu|r=&0Q{)B9w$@04fRQBEJ1?O{CPaM@17rvv&+^&Ro*sb0?xX&7UjcUC>FES(@#mRK#bx-`+Derc<#{l1}j5I6uLXbb{=u zQYCL0K)pRoTP^M-i{mXHl5u(a`DUbErU+ruu!IOs=OzZXnm3?lsj9~h*FRWP=Aku`KnW66e}1Z@(S1EM zF?CM&M62TUl0XR(i?6AZI=Mcsi5Kq~m8doFB}t$JiSADiPmKt6(nNOqlZjWZd_@u{ zL1OZ_C8;AH4r9>l}Mj5-369!up}od#e5~rP{pb==H_l_ZlV%lpqmLG%x+v zgWEL`sk5PY*qDeUP=dtFCGAU3)LEm6z27B^pO{!j5-34}MproZQB8;&jm53%FI+`^ z7bQrDv02LfI38U1SZ>=kj@)4*d{8*&ieLXa> z;Lx7N6>3(N1WHa94Q}ww1gM6zNG(sHzoT2_@%T#UX&%| zA2a)_;V<`1JUZt^U#XD57)9O-FCMvezVsz1AKeo@*$33&;g z_0e`bnE!FToGV7C`5YxkkSDScK>I+)oaY~0?W+fBk+-uEK<%8{bIP1tDG8L2N99=` zZO4PZt(=+jUTT@o1`_18Z3K|GYGKZ%b8~zIYSGMKBY^XQD{>CKGE)*Lq1lILeQ&8g zs<1Za>~pvJY#>21oQ(jcOnWtF->MEi0<~xswGlwatT%H${!dLwpoC^>p7p(@qA2{^ zGRs$`_bVjGyV?lgvnO88t#IJQ`RSt;wT86r9-I8aPZ2fa5OM9Es?~@qkfnZOc)c5GA5_Gw97Q5#tOKQIUMpLd5MV zZ9k{|D-kVaAmmAdA;irQTyH<<+lPJC*Xz4W)TaE!Ujxaq$kzcj`SB zBS|NCw@ne^U9uJfUm79W-R4DU*5b6izq>)ak271nkK-v7ueQ;ive8()^Yfv4=f_ia zB<$`7Jy~kO+UmU`55Zbw!>$LJ&~Jtr327lgX|vBDZTuBMT5Q8htUQ0Ld{$(gwK!j{ zxxI&+Y%?{~zF?4#pcW)8B4~%r67-&zewR#^oo#SC`)za=?|F$odY4Q-E6x`Q-Xm4* zLGP>Scgf_lVjEl%%87Rn6d~RvTd&_GGZHL2rFpMk5w*lSirYr^k~QbNP>a)QCzV=| zo|S%o(9`DJ&TP-V1tH!gTd&_GlTVQ(q=kgtnIIeDU9$E1T`~{BT14|cimHcrmu$U$ zm&`~=3+u#tq>A`WywABqk3p8C6TH_?1pk}v-geQ6Xz=+7CG?3%r#Kx5oQgJ{dfOZH zxg@4jO7(gxNh4AoLIHAQSwiG%w8-|LRTCnl=(T~8G$Q38j145HRID|!_0j0wmrrD{ zfs!;LBt+V1B^Lb|8s#VBg^kHc*mAq&$SNfrLmKt$p8ai1d5F_w67>NMwurPDX>p4&8Jy+I^m(=@s{CW?r&A|@Vg*R}{HnF;bfn#dP~ z*!!`0*|d!3bbJLi@`op%%cgu)gcONnCdkHB<0m=)_l*;-`}sYx9!yv|5Fk&aq9wAy z#H)J;g;0{2ARG5h_?PoQ+h@X+7r)^lEFB1tCsNT8*;$0nr1oG{`*=H5;h$OkS9{n64_v))xOdYN-`5%p#U$68KmJS5S6RBv4Y%o!HN0%a$ zWG2YQovTiSSKc((d41>3Y$R-2?6H{mifkZ(lFS6z`1pZM;oar#cDjEzJ%of!2Lj}Y zRJ24kn5fnBxe!V+6J%rls;S{>zm0bup8k!8uyi0mo=8PYWP^#<-pNg%Br`!a`u7cl zJDi!~G;Xk_2nm}G1Z)&VHki2L-J&9tWG2YQV>2I;``{0MlRFm)n+^nQL`F7{KuKmo z*CR>w5T(jS!lvc3(lMj1ha{Yhi?T)8jU+Rn+apQsac*{v5E3>m+g!)Kx;-TEQP&;` z(FR76nIIdYmn2oc3U~-h%f72)c(TES=vN8Rmy9GcK{iBhPty1}*RX?!u(TY5dM!dW znCN&=?7umUFG%b08Z` z?0%>qB<4ON$xM(9F+007pLY)J^$?bpF@xSaAsbAz?Q>T`%;83onIIb?%5W(zdH!G( z4`FGwm&NglCF*{Dq)5afdUq%i$xM(95ns6?h7-|JLl0qT8N=zlEUE_+I)*cn%mmoj ztYbJsh=>gL$n;tiY-|>`r&GRaWh!I13c4^Lh_qY2~lfDwS*? zLH>g!nF+F?J)Ar0rXC3-Y+CtUwMr!$O!Ph0I-C3;OEME=LwjoX-?N5%!Ey=xJ0c|$p#a*efpe?GFXzC zAR8hgqe#ZxRO76NpeTb^zZBJ|RVvwFqUBu;6DY|{kPQ)$$rHurhZoL8!lqT6Xl*d@ z>x#89>SIY}Lf3<0QMY)^YQ9RPTgcuWgcPJ9cOppyRYH6+JHa@(FVmL$Cv|10c$FRhJ zS(_V+v8{KxB9Y7l*%01`)|~G0lDEj07{aF2x|ThkB~I*LO&(6~)m?gZRNwht0wva-+F`MK$a;q*d1=#^ zi;%EsweQ1`f!UKu4!`C{iU9O(b|jLSpnB+?qGV!ZRS#ilwSUDCjwLFOXqXVOh~AZ# zF`U^wx1W=Zr&dpM&g_llpFX-`)AD8)EC-}Yx*}iw=(YNR{yZOC71$ijJeDUT;Jb1L< z6sN|k)si#ksd{i;s8wT4JXZbHI%3a~Y!u~Aa%NQToLrV#nTHZ2@IH&U7rtqNGhxtu z$z|J?Z$bjKZn&gVtn;-aw2dEojB~QCdNTRmfv59Of&|{ni3hL#=00c9UB$_{CzfwQ z0<})Qkss@PaEZ3zbh^u_Ri-4_wCeIalpujOhvLB(J|5|GEZ&tIQE$a2Bv9+E!q%~k z-8OPPY7cda>i(3R*m`*$N|3-iO!455)dQWMkDW_4j;`2*1Zs^rd`0Y!Q~R`yo4)_2 zv-F~i+*N&-=XoS>gUWNWD%f)B_0H1qHQaY@Td@fV)S6S!F!pQlAKFHN8+WE|sqb#O ze|a8CkiffN@nGVfs8eo5j@x6}icLtMR^EZ?v4!O;m8yOfm4TD33vIr{PVkhamLS`DZAd)K?YQ_J&Ef&|{)iwD0Pe4*2%cVD;dJ1aIJ zfm$;LY>Pf{rn$DUp~JcGV_yw)Pkypo_!28Y0`C#VgEJQ#3Ll&{)V=Og@+Fo)tp*>g zj(+@odu`)Hg=w&1ZrLO;=|FF4Win{ znx7Nl>rUL~9yzi+4<$(8{mXc8XuJ90qDAA}E03($gam5c^hPAQXIU3*V^+bv;TyM1 zaAT*J=b;1%ya^f)?oCC*ulJec{&Hf)CL~a+_nL!|O7ZU6Mz3hKaQ46{?!|Xg=(|ROgy_fcmXH`9 zNgf}V7ix)dlk$wgB#c2LK|+kjRF-+KNR0C&k8{imwZ!;Od3;F{e2I}DA^Zxwi6s1$ z%l-=ULM`FPQXZe^f=@IOB!qv2cbA0Ub=mJ?UZ^GfajD0*yWrc61PS5i;cX`|f4Dq< zU|y&t=9N;<9Oc3sWh6+5`3l~T67!(T^C0GhT4H`I^~|*{%(X^>gqU~XjVdvpyF8y` zUZ^GJ`BG2J;6lt`BuI#e1KzvTfZHf5G<@gHo zLM;(@m3m@07h*UgK|;iH@Sc~56J3rIF)!2-@nfkc7Ih&OH4-G$?P&VWffC`VMZD{B zyo-6EmWYE(Ju$TlF}0B(A>!*&zVR*Nc9-LJ%nP-|3ZRtZdAT-lVQpX}NQiYpDX%Q# z`ok4#9J&6$yiiN5OG-U!9C;5}uW^h72}<)lbYnxVqcAVjBHO&aGWE#UYaAm%g6d>% z-HSC2J*%)@;}`<9=ozkjrA}1MwPKA!?Ge^%93w%3+Qz=eqT4*I*EohiE$R!qdXCUG z#2SbCRamcaj06elWBgvBT;ovR4eK?IAyA9@bftq!v< z#}KGRK7V}qN-@>1#2Sa@4@a+Yj06dqOYFO3dX94R8pjZ*MRQk!F?HBRvmBZS9lgde z5+rC&v~RfSxz^EZ97CWM&B6a2AJR6&8i(d{N3U^=1PPkk`5ipD#-W(O(Q6z-pccgv zx1MRPZHP4v#U+kj;}{7N6rW)s|9($zLr_dD);JVjIeLv_BuG%K zW#3WMF`T2k+pK?xH0R;#)xE7mv;t#Ob*En53{Y|uJN+CT{s!XCVl ztJi9>9!Q`Tt>HY+iq?bjS)l|8@!a4&VZG**?STYp(OT5g=CrQmHa8L^MEjdJna$c+ z_AAT_wP;Q4>ASQ(=e}zsNQiz6?^NqO069J|FVv#F0nZqueFh$bMuLPGkMO3q-pi2V z9P>gg+T-x}6520eUt%Ok2)|<9MK^mg(qCa-s6~4<9-m12KI{{X1PS3E;jMVRw9&NQn8$e4BvT>yz^! z=7m~fu8n!-TG~(Kxz4_O=UzB48BSC_8RXuSD?Y+vl z#FH0j(Vna)_M!bOre{{Yf0d$t?>wyv^yepOmmTxGi{a9`m)dLCC@~%JTw|k=? zXYf@%0=4MQjJ<(<$I;%`Rsoff6LV>yJgDK7~8R4D=DG z3;-5>ilhwuzp;2Der35>Pl zLE(Lh*!y5!s6}^w>{}_q!!-sEXCz2qtQ`*uFWQ*BDCUJ)boa-;T_ZeoWAM~Qf&|9e zayBS{*}xE}g^_bSC}y0-Jma8s)S}x$o>{Fi%xXr0geP(iikY*3XHLuuwdl5xXLc@x z+1W^t5Hq!nwSyu8$l?e9^Fl4UEo9&G5>ZALL>Wec1V+v>k|}^l#t^85aj-YCMC+(U zHF@Wzu45L!cHtb$eG0 z>wyv^s5R~B64nC=)S}*F@3*JL)JB2?^-6n6mKIYR0=1~;+q1j0nA#AiMI()G2*!hH zF}0B(K_l3nL8isjhCnUy7<|()9!!g=jRXnuMD{E-Ev7aEYLVx&w?ETjY9m2{JgPkt zPK&7xfm-CX?MZc7Ol=6%qFKV;drga}jRXmree8*QT1;&S)S}tW-rr4&sf`2)nnmr+ zfwY*~5U547w!Md(7E>Du5)=*C8xUzRwINW8A`W{mBQ2&j1Zq))V{c%m#neWE1Vv8v zeo0zPZ3xt&NX_0PPm8II1PO`&?LC>anA#AiW$&S<$J8kC5S}|l<0Z{VlL)F?rMqJB@!O-zjhYSFsH^K>z$HWDPnlQ#E&d@;2lP>a@Ao?e16 zwUHnpdW^XT*>DOHJjg;Q%I~t3tHEv`Gbyvkp5Y3-U8}dE+1@o(BtGtGayAkSJ9r<@f?C12W zBz+F_m4x`_NFdZVM>3&oP)BOYzS*cLV%SFw}M(^xti3e}Y^AW5?^zi@HO-(-7ToW(07eo{J z8|x|l=0MWgUAnHM&X6*uROXu_ovnx3COTDJ%5>OuuYBErkSY z(dS$zWux8X#}YY<2bxlGFZulPM$vA!*OR5<(kkM)o%<6P9qDTbZVx0X@2eS|a<~Tf zl98i|$D}&@2-c!fjh<01dR6mEnrJWHlpR(SGWDQubtJ7F>;4f<-PG2U%J%k?M>i%u zd{{jz{@&7b;%K=B(XY!j(>D4yYG1r(?@UuFt~qNFJ>#dWXxVT`6Mww?c%pob^2H?K zZ*`>fKW=*rUlKnQ#!R?o0BLzX0!M;bJZSP4CKcHMMfS>R0L`Sc~YW!IJ*V^>^bsheF*Gw`>Ud2-YHd?T4RnAI;al>Fsp=cuZp5 zp7X>njavz&u|2${TJzegiGLrvTH4^}PQ>+Vwng?;?3|&^7Y`~(%vq8p2^xdk=143) z^JgUYgRp*9t7iY4Xws>Rk6!|07Wt`hA$C{_2utd#x@P!V5ciP$GwkB|K`9!gv}c)lc9l1>oa zY(Ya!6fV0bG4_2LwS!DvOpq3($+r3pX==H=*oZ{4eS=J?NRm!W9d%i1`N3M+M*ai0 zB?j&2<|9~(=)3mRh|;e^tG>H%PXENJM&#SAgwjYn^vK02^=q_Lsk+6`@~; zRyMBA9+s&4scH|FkQNdx|M5nN`gLHk5o$dw@%8V1g0+bLcIC1N{W`R=k@v%>#I18? z$WpO{w2-)c$CwiJE5~GG*B8AKi4V^65v)aY>C|Qs`gLe!W6>9PBvvn8Eq?LVN+^xQ zhaGph>ergd#^!P(6CJvCD@-R?i#|VjQDx`-`Wz?j8#X8L-&&Jpsj!D*&!_euR;#71 z`R+Ee5;wj!TM{gxe377jb$Q!1nmAGG&P0!**Tk%*`V!?uY1X1NwSP408ch`a(mgS* z>OIm1OVSDIJwJ-yeNoS9`OaR6mg8yuuz69MwJ1&Pe`(98CN3-6Gx5riMcj!+;aVBAHiBgQ~QrT(nS;BlpC11tl*TaIZH?j3FQ|+Gk?Ndpx zglu3R>qr+Te`Xi{T`&riwM@D&pEbJHpK63yYmj#kpxS;1ojfD$6d3pa2K4( z_YtvdVH~GNpj-Yu|9&e$cBwY1(7Fy$36LM;r)_e3Fb>_?hC3Oi{DyDw?C_ywlO@3t z%$L&Kk5xSu{P|)yF!x>`!CGX4V{K()-IUGYs$1`p1WPbqO7nPBHh#>l=u~Ua&quHp z+2Dv=*{GkW;=KK3A4#wT^QAQV6=fqa>T0LT%~Kj?=Ms z6CXjngrZ5xiPwh8hVHwPUoF(^mXHlg zV4O%cI_}sS?yzH&kDzf*(Ina6-7i%S?Mo!V60%_l8x5-VxT9I8@cA7k`3Uk?6it#1 zyQ@a^&^}QTEFl|~z&MfWQF`xNn`Znl%|~!FNj7+IQPo5Hc1f^=Y*<3gA1Zn!8%+-U zE4lTl2|j}657HtVylz)E^c*D#mXHlg*l1Zjt3H2xn{2yoxR0)bk)#LXK4cut?xQ}2hst4~5D;qjykOWIGUrO`pMb+bzvt8U@$~5v3tVK3> z$3ofAv5zEJg85RKA}6)Fqk61AHq4!W^m{qZkzg&d!8;+!hK}KsXE1vtHea&At4URl zBUevwAG`JeAHiB=!|s=eQsp%~JJG%IvH_A{3Fb>_yV|CDRNp(zJ$2QoO_&#I(dT@I zpz4u(dYZfBv4_RV%Syb2-49Upm^*8dJM^{ua3(_S8rV2k+0gZngxw3Z`Es4q%8P#Y zT>s|1_)Tv z`wL)XgLiQ)QS*2;u{N+0O9$kr<()7^h+Q0dquf&uEIZPf2r8Azm+Qo~)$ZUOyOZ=g z@h<(Tb2UY%G@Yxd=f=N-77sq!|2IRRbw8nKSt?lvRTujUSxC^Hlc(k$f+a|(v?{H# zAzDzDjPl~zQkq*0X+`{Y_fOfj=Bm8u1n)DNdWao0Y7_OFYV;H-Uv=6S5IbGgo}a*y zFrnyLBZ(2qmGO!Mnql zldMM?p~{N{rMZ5JAUhquXe7r6*VfwWF6VQ$XNi3c)P6flG7;*m21-XQNNPs=X^Sig!fq?ff7HVN~lVu zN0jG`9to-kjZJl~CQ9S#Q7dcCl5~RGM%f^WpDz1X%Gc{9$TLvQNt^y2x~aBoLFLK1 z%O~n5=!~AU)ff+W>Vaja5^_$N2xT1!E}`lrVw@N2l6h=kzVy`jZiDm;hCqp*fSQY^ zD(4T`+qt~d!}+;kPO=_pL?`hS)rew(Xl^G(lyf%9zJ(G>bF2An=-FA9%Gy9ec{sYs z@z67kvQNI(B3x2;vWw>t^hEhIgFd&v^!LiBN*;nGNKl$T7k6=%oR?kwP%zwOv$DYw z)FK=F%YyOX;he7;Bf%0RD9xY8gUQ|RH_k3wBmC}lQ)E3@f?9mWa{B7_qUO;Ro4q+wevk0J4qpquYtL&qP5CmR&V+Y%9xsXi8=WJ5x7eNuWg=9mgbjJ? zrG9*8E@|<(6VaUJ&lS-$1HmO>f@wvF9eNsr@*m}ushzom=4{(T@JSVEacfeVpSs)& z|4V|k*hc13@!1_|aXl!_zwT;mOs9&VB%R>jPW2NkNhipjDwP-?MPhs;X?)OA#8PpZ zKDVWM=dLk@NU(%#FoC6|-hOML=NKl$Tj|ZpCeWXzI z_9TrD%nP;Xb6fK!%bU>nFa%4GpfrCj-X9!&m)l}pGC8>FMVtHI-XM10o{D0AZ5X}j zr3SH+feO*Bi)uv6<~EFt?Nvs89uGE|@h`W^dCw$gEI51(N{|Q@G>A1FQ9(o;?}`|% zL+A1C1rNE(|s@lfweIH3iPTlWrzhhkC}3iK@{)|2`pXG@SeL<}u}`xfjg2qA3!n z#h+I^`kNy1HSr=!rgyo_oosTomcKdog(k(U@hbF5Q!?vmv0YE|-3g2eW1wP1Xt zx{P-|6C>)4yc!*lKrJk-xTimRs*~vXdH%nmpKru`8&$qEcH;iy5gPMI$AfQl8twBM- z%g+i4)FMCTX%Cm%10_g&{YwMW{<6(oZgV71%j>B{zjC=>p#%x^`SIZYJsVCQ`l5wP ze}U=sW4mssCT5nB2>M5QhFi{6m!GE*AW0{z7Zom=|h2bR$Je+#b{mnLr5= zIGSY(y1Mo*5~vjjG?FupX>&uM1PL7f#)cg0NT3$kw&R0pte=-G6-$sHdp5GvHFt@` zyif~$jCs0}KnW7)SK`4QqiYs__so5?!gFyx?+?*3&ZzO=Esakk=CA84SCJ?|!s`=> z=+~gP{KgX`Pzyb3JUIS?^NR0VIEhysL;?r>v9pKnW5!GsJ^U`?V>qKKq2M z2NI}-^R6O7!((EfZ@P;TByfg{2g{6`Id8&!<;6WcSe9!7x5i zY#UrN#Djf@{PZFdC_w_FsCe-CybqiHxpRv9df#$MBv1=i7V+S_sb5}X0wqXb%qV`3 zW!v{nvm4&&R-Exc5((78m4#SG4gUQ_CQyO|MzZ3Y*!H(;x+i#(JN&~+E)uAPD+@t1 z>;EDXC_w@vauLbQfB32!Q$5@_&(v{|KrLL0#Dg23fA|F^P=W-m6XLoth+}1?~sI}37qwD{L6iYoS*X!()e&tf&}^)@k>pUM(0$l zf0ZOqf&|Xmil}@46d!?F=quyF?b~|hl$+6?#-ON&^~hcVBY?jmaBip4+WCX(fu&*z z5;$wCdQ`qTB};{Qp%y(uI}cKOEV?vF{z^VwmUsz_0EpOJ!^ue}FdCqiwexvEsETp! zXwOjk-JXgmTGxniZqi~jm)?F}g%s^;#Dg`4{M3{QlprBSFc4$&KBRH(XwP6Is3k^o z>C+EZOwleyJos|z7xE>NKnW6J1OriL`}Z`?9qk#61hvFyE`4BirIgyss6FU+@+Fc$ z2@+xi1F`d9I~wPX_6$aXT4FSpPTW}~rS>vR~mLNg) z_;Y{FS%O;h4EqlFQrn<<;L~M^m#Cdah_NpINnc_l^mLh~Hoq_MS&6C#>AtZ4QSv1e z!x?WN;s=u!QB>*vUS-77Z74>qh{!_Y2Hh$n8I&L)qO&wY#Bdyu83}5MSgUl;_~W8f zWJAPoIU z5fyYj^r}d%&yhea9FJz@#VdL=m{;Qlf0& zS`7)*!Vzqqm0qjav5f>hx4Z-2X&bzHr21f9sO9wx9zwJ;`;2sA&xo(A4f$lH4In6A z*7ABd-P?6NtnWjD`mwDCt>)=@sd`ufwY;7h36vlq*0|6fB5#Llj|6IYXB;F@f`nMB zd;1mH&^^)4UZ_R3?aZldNCG8DkUcv-bj{@$L;|&N)|UN>o~|TNf&}^%v!@@{r(|Z% zz?c#JygVgy#3@-=pOP8hjs)71r(}*eB@63QGDDyiJ_U0&;pkH`6N_S$iL^W=qw^0( zpOTrUhy=!u@|29uSR8#yW(d^sJ}Z5Wzfqp(76sKfKaZ2XsQ!-O?Brx_Bepj7A%BPEYp%zBu=A2WuAQGsBGlMxx zmAwQdNZ`ss_Cz|bl|2y&)WY?SId_&L3L`R%wY-ElC3D3oSy-Qvncj{B&OY&=I3;t% zDOp&bk{JTEu(a}&j7~E)>r*l_1~Jw`+MJjq^(mS0eMpcEI}eIeGCGxU^(mQ=Ac0Yo zJSC%(A6K7}83MI%zA~pvu0AC*GYb;lh)kT4(FvBTPst2{S~y?DgYyEDbHpi`t53wB)TI3Tw?LnuOvOQ3O1jg6q zWK^~}5~$_%)cW*Q_A8Vi;hjIky$m`<=J65Zy$mrXV!VGLdVb8VSj4>yGvlBH3DFZ{ zcIFiKGR%yF1Zs)i9-|ec@^JbzT#j?hSB&!*?|2~XopDftgzzgdyO%+;4bM18pqA+E zFm>|Tbrmtkfc%nP+d4~=ou@0)Rq1PS4_V|Fh?+{=(NPR-fs zUIyodT5Q8R;|N0D%OHXzVvaKTGLdn{F{MI6i~{pSyfY3;kPx#dJS&`WkU%Xlw|iTV zZq~>d2MN>?;|6-7zP-b7iIE^7VieQIyfY5wg<2xkGGoO%Xsyuc>Ipk@qrq#xW8k#O&j>L3clR#xVqH(fIe& z!^}7+K|;);-e*O3Smca@1Zt5V^R$PVaZrMUhz3miduJRZP|NG7amGOj66o{I?gy_q zW4x1wZA0zBE95i+BN^Fl4L{{ZcQ1WJ(b&IU9tc#I%{T679%_w)o<2&DKp$-GWypFUfm%3=ntK_t9wn;G8H{+jm}>!)K}39{8kj%@7a1a+^JUMF|oZMa6@)!uIqP3Dm-sMLc+8 zi9LNq2@)7H#)F@KXir~}KrLKZ#DirA+tXK+Ac2u=Jb2y#d-{q5YT?Qv9(=5kJ$*$9 z5*U$-r@Pjkz9NBIxE6^Adu_6(uP8wR*9r09_qq1;6$#X0kE+%<;*2P#V%yE;UIs@K zIP2s1SNAgT<^cN)B+$qBZzr%PLjq@QMc@qxCQu8#tN&gGdt@(RXYIcxFixO)*!evD zUIt5$z*$?>18<}-fm%3s`ER}OOz$P^Jg*3GFN01-YTS%am7|o^p_K5Th=EfFEkPssn2;mvb zjV&ZlON{1H`;H0T*g^>sVgv($H@1*KEisx)?K>uqG_p6gP=bUQ!9Y}9Yj12Jfm&iT zm)dts#=m24Y@q}RG3J4oc!|BSg#>DeUQ$Z8v6W{?zn8)HdPKh}<=aA{@0zs#UWV1O zgcwn!e4ojGJAox=L-hPK0&hStfm(DHM(-}ENI(5v21~pI+4wtxPGVV$>cRJ6RXwm& zEJ1?o@#p@Uvjny1>GExS|I=lOm#Cda4Eu8FeC-*Ggq|+b)aHB_Pn^oU+F+izmmzOi zpl1-#QYpW2hGX8`%aD-_N{|qdnxPfJcWY6CgowLJ`7Ic8FGJqsKnW5e)=DGzZY>h1 zMf=J2%@=(yLq>flK|;h@X$0S`MFO=%q*iL*d=YnRtMJ`glprBuI3T=hH9OWsEQ&eQPyCP`<2%<1vjuZvcc?8=4ZD)da6Ojn6T{x!E@-dA*rVpqAG&cnI(r=>)&cV|t=06%dp!Yk57K?(MoB*7qSn{n*w6 zcZx7C)be_2Bv68cSmQ!_h-c(*?U6t&?~H>4N{|q1b#K2S8@eaj*$cJEHqUn6-6)hG zLH6wU&^4E15DC=s&gV#=1PSyjW=~(fy^Qk*Mu6zG<-H8^_A*M4z<%s~dl?DT!qH{U zCgj`8m@meRNShOP`SvnOkiclroa)QBmytj%94n#);q7IVAc4`K_w8jQPz%S3JSD@o zm(93C0;56u_Od=DGX!d}4RcB+-(J?IWF~^l6j_>6GJJd4*gyiuqdX-uZ!e<+359O(!{`Fuuab$xGnd z%P2tty@b4%VcuRw0=2NT-nW-AUyQYoHYX{CnzQ z-d;ut5*THgJ1p{9A%R-t$2{#pr~e#Jox_r DvO=am literal 0 HcmV?d00001 diff --git a/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/upper_arm.stl b/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/upper_arm.stl new file mode 100644 index 0000000000000000000000000000000000000000..b429e1fa8cce5f1d20531f5a4f159c9941438cac GIT binary patch literal 43684 zcmb822bfjW_4Y4Cz|d|41p)}@6wrh=DhP59BgH~hMA2AK6bp6~5eruoL9iPU42~$w zogwyy8ksrF6&uA8l^Zpd*gF~{i4x(z-?jI9&N}DJ^?N>^XJkERt>0R^uD#FRXJpb* zV-N0GdDQ5hgZduOw}0P(`w!?jV#3%7M-E%DV#Us$|Nno^XqHMfTe&#b>EzGWq*7Tg zmFizvQ8B0?l{)_5ojZFp>@YvgjBK7a@OdHKep|^?>4#Sjazvns5VoPX6owRSzi@teP6%uB{N;)dO`_M4-_G;*plt9o+We)lbH2 zuB}|1T#N^PEO{#TkR`NV*K2Gy^ha1K)gRIa&Pw`O%MD?Fpf0vl(g)5e(I4_0t(*1M zEWJJV?w8k>rBYKXf0*2C`13O!UkCY=)?0h0QtL$i<8`0*#@GsoG7{9p3FB!UJXxai zt4(S?c=zL#u+jvr@O;JUVe86%?C)ayc5U05?@LbDOKTu&V(A;R)>U=9&BfUEPOsdS zsnbg11g-GAqTtwdj-Pb~4qqTNc*y?Ngnt%(yh`*d4vi8r@( z$~|*xm5eKD;)H9r;g;C@#)j%_{b31$R-obOx|U;1_QBX)wCf4E&u1SP5YWI%6RtO1 zjOKGQIrEbsXa#!85qp~%HPOZR`?a4{x0`pU5Y)g*6K+(x81p-Ks%|yp=mbG4&>eq& z>S{B)98tSzPVW7Gmn8^Vxl)-~%Tmqnd`C&EAzP#eO{qkq{X#bJFq>VB%~c=g*1l_N zt_fO!Fm)PkW|SjlEd96?KRUX$f^7PuGVG5@cQ-HFBFsLBVSfCT-{)_LS|J`{ ztgS_iBr&hweXF!4q(j1vc2^J7h4nCF7dt&Ep^;G!i6?SU4@I2rtTdr#t*eJOqH~QY zRp`r=(9K6a?`v{930sd)6D4d6TaQ~W+|R>Li<*1u!WeyJ)*?&T8Qj0J^7NA{Z+>Z< z>GLPrAK+bi?m#d5%`-w!!x)_NoSTL8+CdfG+{M55x?FWZJz{8rR(SsUxQUg2UGbuc zQSJ@$I`=O1etvDC5Y!+>NXS|%v$>F7e#t=Z>eIV=hm3hPL4-57f8~t)ew9^aX)~i9 zePfXK*C}niR$u*H2x<@`B=kIQF=(Lo`43I-x&rN?1GyN_EHQF!aaGtv{HO%~-AZUdcBU=C0>|RedF*piIP^102Qmu=Ws!o+k zG*LqDUNI6e3R!cEX!vsVkUcJ9U=&7Lw_dfbXrhGlpxeE=9;{~%(iaHRgJOjk7=@8h zQm;mdXrhGPy}FU;b0pFi2s5r4@Pioe4_z?YeKp!e6D9QS)y*iMGm5@IU`;|@5CiRm zItH`WSF=_$Q9|!tU1so^8R!cH)+93;B?kH^l6`!YeMA!_B*U2&z`g|I3S){Hj=n%( z1z?8b%3!nTg`M}z*&L57iaaQ9@s5b*c}rY>%m7E|d@i(!5fDbkKJnCPpH?$lqZLhb z6GPUF1|Gn`>r_?!&#kgayw>j>?uUp(o#S3d~IzZeg5@dX#z1s16mW53+eCgy1x3D zdoS>h{@rS>AFj_ML<2`pA$|0KgNqWhGV#>d{otJzdwrFk;#XF*l63CBXF(x- zb@}wYixIR!49bP{`b$TZnm+gOqcz$uWX@(Yc27C1C_yWDqFhMdG}|vl9Z)i@(S9K_ zE1|9OMG0EL6XimB-*LN^p)b%UTBH3!W_B&4FKyYgC_yWDqFhMV=0}uaOkr$kjrI%q zo|E_PgAr6nU)iZRK`VITx>87Y|M7Ul7{(aXXumF=TL(9PG_ojR$}U!jLAj8A`p6?u z^90Y~*nK9+LoET`dUl7c$ zNrIXff$VDX@%i=?$G>duPi5tMK6$-a$r{YtmYnH9e&nt`ecb6_!-L0{n)%^Q2SMb{ zs?WE96*X~!?``tV>CbDqI(N^wY1MIpR(Q@mqb1B6k^2*BV5JGZP5wGTE708gCJAa_ zr3tbE}EX2`#! zRMf-?yJz6Iy71W#aw`|VAu*_l6RzEc+ZdC#uFNgCY~KVyE70&1oJRTXbTA;?=^*=K zI0)C9F2?G^nv@?pb6J9*6$qZ4xl%n)T0XvFr4ZC;zmVOibTQ^GYG3{B)ngI_tw40_ zIMD2F-07gng{ka$b0#GSTDel09gaI4^i7OX6+<-IuPYU1vn$oOAKodw@v;nK1cWJ- zSb;FDfb(`ooPBPqnub1$69lb5AS;CZ@x(uemOYp2E(A5&FJ#l3>w?okziR%fQq||; zOItBUn6KX4)~U>#ItMYJi4$hD)PUgqz5D^9khyd-Fa=X_6aecs}}T*t+> zfxCiwPZiD#FcRCD`b594$YvlH3D)G|sq+MW*jcb9ak*}CHrsYfG(855FA$miNA*g|sCfulWG1BwL<<8B|P7t&L z&6TK-{_2Cpc!$Cc*<>>P?u0iBd#~zNPB){H1kVs7grV(odB-uiFzY`z{%i9$-&le- zFXLC~?x5MW> z?dhPrd)z+wRz;xfptRYewA$U?myOiBg7RuwiF|zMO+L+obks2 zWv(9P{XB>cE0$ekS0}SZE%>^3QG!PRCeKWDl%@2t|jrI$f`-ei>%)z1r ztq_CvkunECVAe_uYP4S$Z@ArMv@h#flrSX}E5x8&NZ)hr1~W=hzL|rp2d#oV6=Ik+ zbhnmW`@dC`pcP_JE~I}qyM3wYbKlHC_J{b=ej%IwA8z+0W)2o5XoVP*We!%GvFn>T zC}Woz?H95c?e5lc_xlbiO3(^1D9enh#)@O+pxn_@qx~w`>@v)(6~fHHVuZAa^JIu$ zp6#7;^^jabjrI%K)z7TXFK--Jl%SO%*j9!ZfBLaG*B_qNXuptM4;sSE!J-7M;E8e} zebJ1Q%-SU5N@7r>{X%vl(Zm?rX-ZLoR)|5_+-7`zNUqqbMk~bN3Ky)-rbKevhxNSD zod_G5C%5|j!|ebZ5o+9uY-sE_WZiXdMQ%O!hVCD%&dy5Kb|c%}GGg0(I|;jgaFo5l?sx1Cz{UtqRpsW#$Nh>sZ&xR@n$Wv&bEfa(e#M>XM~HAoE~hv+ z^)fwZZm@jZuef^yb>15KMO$&NEw_C>?pNI1M1-IfTEUzk;nd6CfB3jxarYmAM*D@# z-FYEx?pWl0#oe()2wEWq{3DLsv50?nj}vILU&!IU-Q2NA9o$_}grF5-pq ztqg8tct03u;sno2?MPI2kSLWqZw*W0O6b04z(`d0oDss*T&!H{y0cf;y1Yw8TZu;d zg>3r9eV5^SkoUC_f>wwTo}`+4XN(fw2?rYO7c$SBf|1C3=LkV7#Nc_U9f@WXdTO*+ zVw`Hfkj-4F#5fCP6z-nQIvD!WR(Q_y(qPu&?pX+GqJ%z;a+$$nX3&Jm43!v-h!LKo zVkDYTh`VRGU7`jtLP9c;b7cf>Gj# z3}ZwHXQk_uBv@PR*Tx$mCA7r%1DksI(Y~}5p0gE_1T~s~{J?(0^HmRFl%O7{gK4jf z_NA@xJleXNphgoCFQxm#5%t=awqk5M5>=^$V4rIuh<6^wE(q6FzV@Z9@SGzwNl>E+ ziI*Lbe`z`9AY!1M@VEA*t?-<4K1ooc36U{kCa4$~yAmTpI4i{9oDX6I1U1^PjW<@+ z!?YFJOJYO_Ge1yU#NeC{+A1KZ(S9LM+BhL!74(N|FX=%|&iuJI$M8yySrGiHL4dMm!!?l&CeQ7H^M;ju?MA}LSYBVA7TqZ(JOr^fP z`kMMLM_=M;O+`gbkgw!C zJ+fHK@uPj&dRQ9I!|a0?nxIA#konxkKpjvr?Mqup>>&Ffh9=C5q>LX;NIXrX98s@* zX{#W%jCS;gCZxPZqlqA1K)9Ow+LyMH*g^I|3{9k*MiUY*Lbx837@DAL>&NVa7!d;f zZ1R%!E3$hY8M`)yCY)^R!R+H=V6?kZG2^~xnoskxk>YhSih)(X#=S8S;?L5(IL z^SLEZB2#=P%XR7Ruy1|^FZiXEtw)&KWBuWBc%1lPex^uUA%Ac+pcQDHsiQHd zft4o8#t$jd=bE6E)I(+JXbhd(HL-cqV7F&Twr;o@&s#JTx&u&y+5+grL89U9q#-vbLA6{OMg(K{X%9Y zDx?o?wWEIjwqv}uuOtatA;!F82j`#sE8d+~y*wxu5!!SSVqokp`Av&HX6y<< zjrJ??f?bB19|;2U10_KW%(UzYh@s}75Y%YD5-->-sd=6txc5N}WQ=99zf!qG`hyzn z7jn4kQ~4@E&^`6 zSC`313~IDr$jte_PS6T5C|jcYduMttd9RxH+EAnYLJn60jP_y#t>B5WjbZk2-hS|JAILb~Mb?WJzCOAKnX zU&ze)w&ukMS|J8y_PPC9L~FEP$kFU$IvtroN=0AV3Z9|_Go03F0x~lnN|p7?uNs_t zZSWhbKy;`%JHO?f49Bb=AOI#-$GCW zD@`$hK5nOl2!wGh;3zmNyLTdu~Hq+atM?Mquh{$|SF zYP1(4XaxeR0W)>@wyibVFXYXedL_?fvT=e|d~V-Z)Tx2ggKLrY3mGNE{I~t#>Jh|% zFKq=GZD`-T;xVX+6KLIkoOWn)z`-P0T@>ti#VrYU^ATWa?L>7IbqiZY3$Qbt2AYLkJ z;si2Lq~<{kP0$K7a-}`}h{vD?R+>P5413Uf^_JWZ&s{8YP!qHQjT{_~5|`Vnb#!e7 z8LI?$;aT6UA3{(QC%D?#(eBo$1VJm%;W~KLprL;G>%aF`z4Uw?G_;yNeM!Um8_vn^ z_ic0)Eu>fMyPuDrle$@gtEDC&r&8y+d9H|Ih@lBuk#02rYwhCue(z5`?hk%;>5_VC zSQ1^rYmYoLzxGPZ!HGW)_ggmW;#V~+7J?dGC&*!c^zB^gw|=gZf5*SJB?wv}_VTJz z@-xrFtj(_)<-h#RkM&-cH-(@^`-RN@FQnU6b@zv^{=9zv<`Q4~(pGqW`1i-=Fa8WO z>Mt`6^9R56m-++0ZY~5hnt;qvSxDb<&K~}6*Dk6*qVI?VK`X=3F}(rW0xx zr1g#L`=I@5&*PeuWYNepVV zU&zdMh4hD)H}`+J;ib&xD-KQ&v_g!t=MBiuTI!h?gGL?c@AC0onHL`XQTm)3?H4k$ zb|KyJu$15KfKJ}ApZAg&nxGY8jP?8E&pxBl#OOI`l0RVj+nI|qbrORb?H4lFl0tgd zQ-7{MdPBxLq4nnpf>zR2LrU`dKY(-5vzi{|Z@i|dH}mXsg}B*iv|q^K%DHO!w)(Cu zr+BY+yD&k}3NdyZ-z)DA#d+dmQ;zaqThP|qu;Jc1t*=A z*TxB2@%a@W;XF~r7^XGaFXP#LyQ)VfPH-oK=V+0Z7vhXewUrRmXupur(stjj`XfQm z3Ng^LudW$qVyHeBf*S1?GDeKux2tjGX_?=AaOmgV1QdkTHYpPG8OQ1VJmr!2I8I7~Tz#OP>Aqgx)5X2tkeZ z3z_c;g|y0734&ILfy{SiSHw^`Q3z_ZU&zR)wyjj&O%Sv~3}o;Z2BED~ZWn?Y?H4lN z!XoQOf}j;*U?qC-A-vtGHA?!N8toS{-wg|CwGJi-S|J8j)a(kprK+`72x_!n$XKcE z98~Lhf}j;*V8>y10BT<%^PC#(7c%z}h4k~ETwk;Ft}Fae=e4eTE2qE>LMq4qqt%r&sL|Bi|FXM&8@h_j{581vkL1>z-UPxD8IIqSL zk(#3<;XWv=ldT5{Mn_x43DQDjo8La#-+p5+9|Sd;fXqFSS!=gG*1H%%D|n(@NN+v) z^WI0^)4~6)wpVg#)agR-s1F&E!g-)!&&5`!A;7c%!m=2UI=(3)Zd ztq_B9A-%`pU2_lS7uO9weXhiyM*D@#9geL>F@jczLD>?|A3ZTsSv^N$P^0}q=AP)+ z30ffrWlL1w@Kxr*vC||5HQFy^?r?scpcP_Jw#3WN^!0w*d8ovoM*D@#J<+cdv_cHZ zmKgfXao$DEc99s=XupuTCn}`7jqOldjG&dYRj|Kma{00v|EgKu=Dik~^LEpUPNV%o z4tGn)48;gq!4qX$5A&w#O?e&X?V(2drEGUgi1BL#tmr8uT&c|Ixs3sh_6wPNBJ=-r z+Oe#>7(pv|qHKPfV?&4Du5~@F(S9MLpX_c4wJ1i=3Z5uOMv26rM*D?~@n&~Rh~Y+J zf}j;*P_{AL{1Ac~?H4lUjomFFMlpg`(jpPU&2!sUPNV%o#{9RtCB!I3&zZ!QqVMAbt?-=h2{s0-@keVQYl3fK0g-KtpcQDo?*s$@i9rpl zG{LtpOJH2FKlE7AV+^ejoN{`)Kd3R_5yel~5o#=K#f@=h2kdlA&B)tsRt?DKQX^8*_t ztOog!1mt6mK0Duh4TzsF?Hl}`Jar81H)s(PJ4$Fxl2E1cycr8?Q6IEuD%D$}LuN^= zr`};byjQv|5JGE`gpR?sVy~fnbO|+W`&<*zlGv8Ew2px~JTd#)aFm!fX*6DY^trK? zarMLuMHr1HC|3y~Q%U}l^|U4&B_qQ+C5WK55`!brj!HY)?MOs`tbfo!YlC_W>J$2H zzU}Z4e_nR1j9ol0q__V(=eH!ZhVe83Ijs424U2<02#qFKKO2K>HRN~m10r53(w1nf ztymI__ISKP`o6#HWBz@b73Tj!nf2%^POyG>F84T|xtZ~Bk8}F;C6xzVxR>1aKu)FX z9W62uSLz5cOlvejPr)gUXKt4~bGwuv5D!-H6z%}bNs(u6emryYlhGRO7xJ73%EHqq z&)ocY=H@3s&VM{3Hlk!4p?%x%u(T%}>VM{0Kpf_6zyZ^ziVM z(=#_e8FTZKAZUdc;SRu@q@0pt)-`xC23~IDr$d3=79^L|Y=H{o~-26xkP0$K4 zxJQ+npNzTr@y*ST5Y%YDkk5Z^MtGaynVTQq-25a6S|LWbZ#Oq48FTaFo0}gYsL_5Q zH(hyd-kutmTN%&X{P^bPN9MUEXoVQz`-i#t$(WlTe{RP=nVcAEv|q}>?Gj_uHAcXS z^|0T5sCv|EjrI!}tq|Ojv8@sWt>B5f%t(I-L5=nc8NC_Y_OZ_s1g#K*yG*%J z#Nd0Pe96J}TnK8kU&!2j6w>DAhkFK@=bE4uVubq=Z~Y;4%{yJ`Kl|>-Ci^U`Z1&5! z`9((_G6H_X?6c4i=a`)neq+QC!*U1A@}K(ebr}%UXaX|xqY!o5|8=&1V8yxwK`VIr z{N*$A-^{=*8DcD*eVo7bZLH^^M*D@#Ja1!^9(1_><*aTAf>!Xf=Y(nb8|LG-4>4}N zwy(e2`$L4FM*D@#^~%Qh^Zd^Kf&<4T2wEY=8!sN0Z{8KRmWc7;sITfPT22#!8toS{ z*IgT9_Kr36_dk1Xf}j;*eEjajd}TY_z9Po>P7~`toiaxVYP4U-;V#1vb;n*&S9jym zD-#5*5aZ8>kIoMs#~8m`x_H=GUFHfwjrI$f`ygA7nb!`=+_Ctw1VJmrc(yP!Km2vJ zRoBYW^$EUCK#ciQMtjemfSpXJ(S9Lwe{Ew_l^^4c z?em)iK`VG#SyhtvSK|x-G2XcLEN_pxjWVOC(S9NGJ;BCUIrAd#<)3d)5VS&!!B_Rl zf6{~VeDcIAyp1DU3qg(c3z_dbHpZD%bG^}nUOY(q(pGr>aHd!OkF)XB3+mxw)N73< z7!P->HpW-4U*i2Ie_4VEGu*<;pWfL!f9)sudPlXD5Y%YDkeP#RjNNNb@J|13M}nXg zN_#m94GYn zll4E^H%e%$KvpNIabis9*W4ilgtV1wS^rPVuj{RA7|6y-pS{Z{L0|YEuUpVtYZ@ak zVk}`Qf?EK&OXXOK6X7UI(^S-#4gzuL1``qH#*-OpW&rz*cGo$&SgfJmjRCP~=&SV&h?+%l*! z!uk#8IXvAZvlhgWE{4qvR+A)p=Pj{j=4U0p^z)@ux+IWUTTK_zXXoy_appJcR6T4- zk_6idv-Ka-YJ=><{(xV$8%rDJtL~Rf4G7c_8co2Ab}XX1;~02?BbPQdg%O|_C*3#khv1s80TDE zZvGVk-0+5)IAL1>*#~WP$pLN3XSbbK));}i8$9RwXo)ttWo6xO`_|l_hZ@Epff%@_ zO4gjdNWcm*cLqsLjX1VJm%+|^p*e;fW>I{KNmLQn%MO>nPmi9P@GW9eh(`bSMPI8BZRdA%{Lias%%}GveSNQCj>P% zMo5HjUMN*z_d{;l|LUGnb86xQ@=92$YyNjf=`-)$CNVTYE6~hJwp2Eg*coMKj=gb@ z=M}rE#m5y0Xy8{9JgaFu1_@e$=E+MyWI4}u3|fJ}sI*x$UMgy|U&uUp31Ud)wJ&W2 zndd0KPS6Sj_ozvN8toS{&rvKfcjonFAAEa<^tmQz1%js{mguwi!|De%*s)6uta#!9 znVu|x(LUpnc8M5K!mdOi@zEn4ZqmN2Y^^BUwbu1V(1Y49PeMpE-h*5RVWkP4aoAEx z_nH4_Uyx}9x%g6XRKlF_|WZZ`HIZDMpSzl}H&f;RBd4C?P&iKxt{T)k| zl#!26YPyUN@MQmYsQTEDotO-2*yA?R+xV;Dy#=~FD#Ya$?$6kys_9?kO(z# zBKn0;+5D-s*jvL2ea>EkjJ~n7?M?eWXm(@)fsq&|*qb)*{(ZD}v)Linq8>Vi_8aUt z<$Is*e>ttXBrx*^akDdY|39<%TYLvIt?Vf_V$DA*hKH zT!}2fIi-D(zzPRB@)axI?&Ocw#0mYStp0xvgx}D{34W!UEEP3zg5M4&32Nd5SJYo8 zXa$wLT7l+w=$4rB zRIBPu-&`#OHL%hIzk9dDA@}snz0j=A{8oJUt$du|*Z7v$=Y_p;b^RU|f|@uH{`LUI z?$oMNN@s1q!u+3G!rv>16ZpLe{!T;IA6H%2+wA&e>{1ganC)z-&c5^d>J6{F-VRoA z0`rc~?OC5IRW(b+-`Zdd5~zp$)&NA0ue|D|XMGXW1DZI27O`IrfY@_RNp*F%{!$NW z;spA`eg^=;^`H>c#0m7Qz10WdMu`yA#0iW>Yiapaxc&z}&Pq?jW9iX<2UnehXz>Q4=Sa?QEadJbznm)o|Y`mCam#x3m>CaRNEf-W6>v+>-lreae5l@&td= zrkl;b$1tFw>ZYD%mASc+GM+D*aB0KTefKsyfbZ}9Awp223CMhIPB|~Q?51f=-ZTI1 zS=OAr-rTTx)0BqSnjU9XoSV%&Un4RIJ~#gs-B;HJgx16f(uH*WE`JODol$G`vhjP2 z)C2PlvZf2^<}LmaBf^r9K`1ZePbGnlC1})vIT*nCILnHADX-#OqjT7>3bKJJQ?E4-kdYu;T$iQlS z|G||_JB|~-lr>#Q|L2P?<(jZ%w%~?&n=ifSB`kYqqyY+<~>(04y zN+ez(ea_S+0Rdmd2-1c0gB!Y(Rh3+E;;hM1WfM@Y9>VWuXy1Q4h{W0lt_8^3njZX?D4p}`B6k|12wuNh!>5q>B(W49gp;t80;w!>}L=|D^8L* zZ|e~fp(ai+wk00Avwsa*7*@;xki&Sj4WkdfU&XlmmwK7&h)zwMh?WXt3L_;NBT6$9 z*-}LbW;IRJN@*Wk&(=l1pMU++Ig|?Z*f%@a<-wO5W^{wx7;*6@e+vlsvLz&e7O}By zsm3+y=P&4(O`CFO&O4}goX}QHjDLD6q?gP+vD#4P zU)IGrL#T-pENvm(W9FfyszS@(H3#%zqY zPU%(N@9cdOL^u+47DbGeH#aH&YS9})Fu%q}CFhNeG3}X_DfbAZP_S%%X^~`hgG1mVf=45Y)s8&^9N6__9sQ za`RK8eUY%)F3hO5R4wkmvux)-yH&$Vx2Vn%tWzPq@=xt+&cEsszx}3pCSQeF2IGnu z2hWj3ZH8OeYR?EkjV2)Tc_ICAk0v$kmz?PD`sM#3G2oXuO4Eh(z+buqgx16f(vp3! zi;oeT)h8(R4#?XW=RY-&-v_$72>t*i)Uzj;~S3?Z$YIJN%n7t3)rObCS(Ncv(SVBvfaV2}7 zXbd~T?R#4oFHVFSOY=>yR!SS}edIlDaLcc9C_D29Wmu68b{Rtaw6sl))-*<-gtk=N z`@o7BfCTnC@WeSECqhk}29_4D5o+Q@v{c;tL}Ns0Wgwk$p8Mw z>^4GQkdblh3=U&UZue>rU6Xs^gNZUq!cs+vaGoQ^e%t%j%;{GyF*HFd#N$}7Fa%FC|9UgF0)6Ep3VH_jlGD zzv@KtS z@A|0LXaX|#kCuS{#t2$DPv%C0G4@OKJOGr|Xup(ETExH~`(2UVpX*Zbd2u3MbJ7t4 zc{hdoA)F!VuYz#fr*9gAQVF3oq#L^*l&`f6v95QN2=5B@Z)6wJx$5WSzRUbwglPOO z)t>P(2JSPp->`)CDZnr{zi!BhJO*PQAr~F(#`w} zN0TQ@>rS5hX1Vr-yF&Qo_q(imvn%lDl zR}XV*$yTr@^d09sA>X}TxwKzUa}e5!vb}NF{lQY9=ICdX9e(-TuIIWxv?fY~_tAy) z{6W8y)}8*`@St_!D`+cI58Vodw5|vJqP&!KscaA4*|XI>nCIyx-_^p3Us(iqx52jt z{LXT7_KBWo-8HEd?yF*RlN(ssud%HserNgQ+`%glW7W>BwSiw#cE&@gDBFAIyLbNB z12LZ6dAIpSIQ*{5()Q+<&n+>RIlK`Ror G9{&f|XAJNF literal 0 HcmV?d00001 diff --git a/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/wrist.stl b/simulation/gazebo/arm_sim_scenario/meshes/wx200_meshes/wrist.stl new file mode 100644 index 0000000000000000000000000000000000000000..32fdf2b47b7c5984d4b3d99367a6080b82029171 GIT binary patch literal 57684 zcmb822Y6IP_wXZ4ks@4??lqL`CP4@tA>}4i=_tJkNE49Wiy;OnDjku~i)1$ibfpRj z*=$4*umVy<6a|qYP&?>#ef(z#2!rlAR4 zT81{N)v#9WT6G)L32oM~UB{PWH*DDODD?mTzobGTA;+gigoT78S>a>nr1t8W;0p=4 zes{X{yUKruYNBB*9wW5_t!@pe5s+RmGn^tYCyG;1lG20W^=!f%25TJ7hmlS z{X(^p1XF5~J$BnEuw29n*PPvL@0B<&cn2xY9{}A~Pc+?A=5U^u5wZxs$L$nXZWR zadUL3`Nh9F2;)m>?8J~E(}1R{J8#c>X|I0xemGi@#!j@~*_pMYXsst4?KaJJ*a@=2 z-?x_NF0Pxb)tVli9IN3U*Pz^fXHU@wyd+61Dzwz`@fT(O#fKtNhxN{@7wGd{+g;&B zj>e?%vQQ=~ltV)L;B^a(#5rN7er+R0qTX_8rAngFUg_|N&Cn;CxrUdk>vtA_vK_mLBx?tK1H7`s-oR%9jpbl{L*ng??X;7N~5fj;!&oOq>skKCOcn%e^hsTV?i0)*XGta!Ux@*XvN-r0(Y|{uACd_hJS+K zGaM)Ay-O06N;h?Hy)iwAAS?7o?{ktk*x-2jg)!sBxFU_LJHEZjt4qF-2nmVy-WKoG zAtB8Zq^3kuko|QRa)P`fCH(KyoFI*j@b`xzCc)cS67)TlTOURR`4h6AsPCg%DMI=P zBK)-?;*UZNyztM$a(S&tmfs~a^5{)JB!mdA!5T6GYDol^jl|+Ux5MPO%#1wzVu7pc z_g4nLUJ{U4@cj^Y4id5J)5ik@YB-@}e3Kyp`-PXqt>}08Pun zyEZTq1B70#BEUEE0d4TAtE)8J2NE=nMSlQ6J*YG~q0hS^SSzm_S4tz>G$T*8NtG06 zxt~xPPRRDLJ4*OiXH{}=E36fb82UYMM+w4zN5R#V_ZlaZj86k>gTEa~yew`-zstx3 zw3rBJ3$>#@#!G}hjD&DnYdH{9_c9ux`|#JwD$wo;{7Uurt$aq&-6aVb6$!M%s`pCK zRIX8U;imL^;QqjNSlAvKSF-LjV(JH?En3S3L1oDqP7%r`G&sKHMs;il`rz#VIe}tXoYwAI(S!1eiXP-d0{t%MEA-*Ny21)u4p%|Q z{yW%D_}hUsq8{PPO7+}XJxytO8IZ!hQ_n0;`TX>~hZ_*mL~&-(u$xD{RLz*7yblDt-2!wHbBfA?V{0#@*Q0un(!lDLMK zOJnR`dr*{x8cx7fh({m)daVKkw}LAz2<#iZ&$$MAQ_$GY|F>Fk0!ECW|8-miYQ?Q! zREpnm9P72>8mbA7e3Iq0T2L((z9N#~+N;s-xANij!5Qc6%qc>yGqg4MD4U%IUn)sf z^>(wITm$6>h~C%c3fcKpI1t8?CvC53?)Ja~^Fw%;%5HS`{uDme?H%gi8cw|ITQ7)Z zOBO(!x_P}a5KT+6V$ROn%r%^Nb&VvJjNaxojrJ4w3m^A7RCIC;CvM$YBYaFaHaXWc zQWn(`K8_t}vo8{d8#LOy!xL&ZRgA?59uk4A!N54Dmz{j*r z2gS8&{!6x-YcwLRmmvNsG8~BXX)ZzZfA_HaFQ(zdrS8#!_;A;oK%97dh9KtcI_Tya zPK=mUK@bmKoe#vfZ~i2Rm_M$%xrP(zORnVNy|V4>dqDJ0TQ5dp`-_v@T*HZKmW{|iFS3^11n0_PI0;RMVvf{?Qh%vXZI`HE{e0rQw3Y6+Up@l~5&uDOt6w5li*_8V zxW-0s4JY25f_Z#gHpRnyz^ zWjh9zvk_dwiC>?XCwz3Ch;L*SzLJN2LA`at=x4x2_NGNjJFY7!-?Z3@trz3 zZ)soqJ2fY`)jnTs;iGr|*)|_s!wFac5meI@D0E_ zWZjfBuHnS&Nq6J19c_9n`wxO!72J6|7m3xsy>Dv=*Kp!i^^b&)0T=9FMV#PPUtF0d zeC%wy#^!@-I5D7mbK#?DrFH*7aI1&)e}~yAIj`kQ_U~G*;l%dLXW<(tId9qTNSy20 zQQRN1I~{ei8OO_JqFgph%>A6&x;Hpkd}aDrR0S;FRnYdFE?7@H4Ha4RH_Rok z?ta7js&`GyvXYPIa1AGT#KJTjE2n!WHd>LrAv8uKxD}tfOv5*OvbSp8)iGr|Cg*Ss zCwPp*G*(|8@BQVi<=G8tuhs}|#pf>5xHoa6w^^^1F}WqYIb6dD9u+Z-Idfn0el&Mk zcA?wfX#}_8bC+qf@$~b4)@Vgchi~rWa1AGTR0LvV>w9{Cu$E+>$SLCG1h?XImuY-9 zqJ!6c_`R4j-OG8oh7&xFVj5+JxAG3(x-k3nF*IeC{%hK69&j7nE8Q^Dw)Gmuq;rJXQlOkFV(6oHj4} zy=Lt+f?M&q%QOnDc*48r(|2QzPwC<18cy)&j%jrJ`%zBEhO@J~ZRx8K+=|a#rt!)X zr*r;SV_wXWqeH!1!wDV_GL22|9?a=E_08;SpN!B5ZpG&=h$kkl&Dqxat(eIR#(TMj z6Fh=s8oesa&zXIGTK2bfCusz?;&Yd2{CPhv=V;}mn9EbAd%1=aJmzE?9ZEi%^RzWC zyX*gEXau+7vmLC;+@HB`Lz~Gl!(kTU8cy(-lW7dJ9%R2#cvyDZ?{$J(>9gqW`!i#E z9vB&O4}5S9CwP>~GDPwjo?=N4F}FNyj#^OKCVH`8ea=9*Kk6Q z+y;&5NRkZ4seJR!Q{$(ylU!wDY! zG>sBZC0a*UMP_}l<-SI6E7rbv(YdFE~8z4M02UxqVbZFw~cPox-IKiX1u%dXYlQngJ+r4)$J#BG=PAh4RhVu8$0=Vjo?;n9s?hrO^va3CAN%S9b41l z8cygj-YZ6vZ}wzf(=jSawVmGp)qk> z!wL3X0wXbE^c3&w?)5QU+E3C5ZUv)K_>k)c-&`8+eYf1|?441opbp z2yO*qO!$y%6Gzwf^#0gtb@o#^MJ%r21p6)lA2*kE@?N^JKBjMvG8)0HV2lYLay8@Q z*IIci++3BNH7V5M8cwk967cc;m>BPOyA@^lOS{1$SZmY8=B($@*h7;_&1bnnH zN_wkzT@ds0T(-`yTqntv&tjs>1-q+$9PO$G1@G-E~r5xABcVgy0H%KG6 z72lm?8WZ0>m~-UD^6cKbMp#_K3HDtAJ_;82I460`?3gVz$7lq%;(M6jiBo=l&h43t zvfr*f$>JJLuV;a{_)Cg|HcRiWL`nU(#xg92Dzjb=1#WkE@-zDH9^Mju5Vkg?i9KWX% z+=}mEf_(t+#S->Luk1~~z}p7fXT=HjT>?I~jJp}za!ypt-wkzwTk%~_K$Pem7v*W3 zy}sl$Y zDo;-KZhBoK*tsG`$g{7KSbelyntv~+@WBZsV>{&8PSfa-R81pDgSAqGIt^+XoxW3pKNMvz7)us4bLrt#1;xk?rfJ37>8P;wH? zdVE~9jlfgON#4wicYsiue<$#y_xw5sAUgpPk?6xematrs&m}-ff!|G|WTPJ#p)~(a z;AsD$NJLuOX>WM1416j_)k^q)vT=mVcFdjIJ1;!14b-al_N#lJggdGg%yLMOMsJ5Y zy>1$33V+iiGi8{!>7;Li2pqe35*|xh=YXcj%(0|BGf5- zcxPx@|; z0~nh*D0;}*65jshF@B{JICkZ!Mo+>!QC;)S?nnh6y+##v-YHwqyZ?M|L68Q0C?bUs zD>mQB${Kws=jC#PfKVEpkn_CE&(N!1-6q{y@6BmEVN?)-_XJvDNjqkD&Z~4~G1O|q z?Y?_Q|N3gq)lE|cK^oa|MO-}iQr^P&w}5C+>0wrARA^4!lQRTC8l9l$73P3wy=1m) zG$NNV9ZW=|ayyYtX zd~F=@Kq!r*>A8!)(_Q;Sl|t^!j8){rM;ahNCVzZp&wcS;g9vq++>mXdGUghOG9BI&p zBCt(#{=DM-iqVOkcg20sXteOb39`c4ZhZFHyz_1U1|JK%^v()}_nyyB$#+rG$actb zQKpe^8lC5tjM#GP@7U|lCJG;%AS-^S!3x$(w_S{2caeTaQArRN?b17%)yhX2sEI~X zn_&Ipe#I=-a*ZG>sFjbdh-pmuc~vyKx&aOPP=q}7E#G$=<<`$)J*W|?KRiAf3u2T+ zvr!Vzpbtf;j0Lb7-h0(vHWD?0ja?s&Q2S{21T?Z8GIIyqX@TEiZM$}nXm&>hGm!X$$P?=_*9*)HS!`y|2z8&Q__Wz4n$11|4f;?7_P?BoOk>iR6MTl#2(rSK zlMHc&%QYSNmXB}ZIQz))8a(e}|H~)mxbMEx2+}~UG@5>g=-|a_PW)QIH*xeq8YGmQ zBJN;(8!(M(ml|jUjtt7mqzIL*$u#~K+S>_l;dq8n_~3++(Kls*8grqaMvw+;r3g7c z%Dhnv{u=4TuSa|nm+g>c$##&OB4%xwo9d%I12h7A4K+F;=Xps?daJ(^zk>1G7JYC+ z$=FZW=lH(+`k`MPoFEPQPz3f35%A;`yS!n?ixaI#qvUh&8~33TO7rgoWxptXd01wh z0)J*=S?V25y;I@ua~pQbYxPrI=yQCI56xxD64m^lmvQw#^$Ebu?D`z&u3-XC}e5QW{>al<6%4 zeLVZ2k&*b*4>f`b^;AG;HCBX&L3IT8ud@N!W;nOrCDyY%|GlBjcT zYsA%zzJef)oha0ya^Aab*7%8(D{Io@wl@<5Y3u~uUD8L|n1kt&yMhU_LYnRsN%ZVp zCj(SgX;Z!l$|C*9O8auJ@ z?Vo*3FBkAhA0?9qJARy0O%SB96FGf0`%bTa(<_O@wl6vgmslJ`kQLHrD(>_(TvXQY zS7o70Rw%D(GSAmBHO%khu~FwX zdvAOpYDF45u_Y|UcRg{a-^Tz~OUIrDWrYt;kQLI&B^vtPs8ZYSBP8&j(x4S5N;X>N z%PID~|NdBVc|%y#%C4eTR2C9sh4Svgi*k32f6Cv3T^g-&RjJiT%nYQl6Nk2~&fPI| znZF%{2Muuj=-mQQgOmi)*onmF7W;lI{$`x)!JpRu=6Wi1fbhWyvO*eW+1%kB8#I>0 zo97R>PShGGdXO||#ff_J=K11Vw)Btoi7ThL+l_rr_}~OtAzc;Pku`3aC4F2dHPM~? zTLnRo2CX zbL+*&C&-qkf8`FFcjt^4iKIa*PD~gT>1+N}TR(Bs$aF?#WA3(;1k%`v0!-n{E`ebw4eTrJ)* z&~w9=-K?O$2TvBRnX&rnUNNpnV<+lcH+`Rt_{dLuz3pO{ zC$&rvK~_kY%DdugfAvW}F}->PXXDTDtclW~%!#oVPx&@Ym1i^LIRH7y=>%CJy(asD zuUn&%ejfwNR&?Hb%q@J72CX>Jx__>3)QgM!MD2bjtCt+xRg6SVkQLGo-}%wk(_P$8 z)c?Mc^VC;`gb&i76(C>I(`^Nn7x}R9M?GlXi zP;p1eks;qE!wWx|_to29+ukhkdEUc0DgF9~31aN>vBZhyVFmM6Jf7$G@&4ao=?nV4 z97K>6){6Q@j@_EQzsekaf4g`W<>m4kj_p}0?}qWC-^advn=-?DUJ*V>gXMBUeXC(R zIxPJn^KPks#C^^QvO=1^6J@O;8r9hvy|Lg{u0ff;Wl^S*Byr{)pQGBo*r5KPK0rdB zecpEMc04oI5(H_`iW3hbUiBqE=;?3y*^%X(hcn(1&st8971A{G$y)V#BGg&oWjr;T zk}z!lfZS^98jF7Zs_8@T$>U4KTOZBGl4!E_7xzEUWs13-G)Qw|b8jnmTC6{Nt2cRT zLR!VMn4?k6`Y4kX%Jh_wJ}Nwa%)RdA=R~b|xx9v%P5XPt&ROf9eLRbbI>x5869j3n zTu!vEwko!E-A4Xv)p~iO@DH8!f(Wugy65NJ;y*e;_n;>x#C_nLYq45<;IQ&HUn;qP}zNnpsR-~~LC9eHy4X+mECju{I zK#;~xY%4Rx%6d@WPZV069lotWNKiWtdi%uRKmS1VIbB29A9sg7?(SE$gm@Z|27Pd% zXXxU%qTcoX{-}87QTT%ymtAN@8at8x(o6Ame)!Q(tXtGHB7E^F(Q;0Zm1xuZ_2Mf# z_WOx#g%(9@+q_!b=dJx3v>LOdNPNY~c&<5bYohmdlBq3+u(VbME(F-Jx$aceN?#4!XKzg>;A3 zFU7xh_D4VQ&xK~r0p*?%a|vnCieFvwEo=4pzSd2;3J>Ln`wlNY~z}9 zW}Nrx&5f`s<6&QP9_`DEzVu9^YuB!f;NO5cLEmj~3L?jJkEk9JcjCbtu+qT4`#t%@ z0PKYN?&pLj5D$P|#V=QMSE)BQ=k(=C!UrdmtiIKQ3H24P%Ec=}zr#t3uOGP9{jah& z%W5NhkcL}f`{?UFm>`W#cobopjW&T-~R{tLzRVuqSaN0l~Ef(J*5+>XHBF2 zp>l5QZ|HYuhlfR=_|;XBDbpxjx-=t{25BC}AsP0U%v>IZ{SEytBEsrB$`kn7!uHWO zTrfcz^ucNR-85WT&$_U`q2I-|A`KEsR$rE;arsfWMyRh;PIv-euP}CN?RQ~+L%)kY z=Y*0~+z(DcH2qd1)VH%L7i&nr!w!gMW!%``(C@+rX}A?$D~c4F#&eIJ)Ckh(gz8z? zZFsf21N$5L9s0wQpuST1)lCWf4l9c1UtomNAg%g?e>f_(04n`B{qUI=tB`6M%*o#?!f+rei!!#CzPzNI_&YX5xfT#p?Vfh*Zfw+iT#c9 zJJv*f74fUfzr1c-zwS_k(jcvRQ}ryYLDt-miv5lAJN7xv`ba2Q#mj;T6-ncSClFy1 zcdY~a8|QcQ!3ia+c$t_PG(ttvRJm9~`rS0nSJ>^q{>J$oeUOG*;qMeL6Zg4BkVYpw zjDY*xiT#c9J1+|fMXReWW(JMmJ*WuPv#{egBh8iEp>kY-5Pa5p*!P(yfxd*$H+{F6 z#;Gqkp)^Qyf@IV9pyC?Wsas#g{pB1EK2n$!Cp>{KGPqX1T+Nr@-)$tD#+AkgU6Vcy zwXQU7C47(uYsF>yPBe{}q|fsu$O>yqvT4*@^S&#lKu4?QnzF(NY0w9k>FX7JyqGUR zR_KFd(^xa;&(twbjJ77W-7W49(x49|^RMlRZ5z2bp}w{`;bGtBbt-pGJ-;c@s#E!x z*dfFTF4Gq)thnzh&Iph8ElXC|4x;f5IV9vkl`w1fx@KZrshE`~5VJypem9NAi)*+Q zp)}ZXigF=AvT4lg7~xLpU)EY#v!mz_(&&UI5L<+7h);FQm*5ddl3`VULr-_=H$^P? zf8m2P=!44?TQrSk7oX0TAS-MK$&f{`MLGAoPhE>Eb?H^%gEZ(v$#9Ij#Bkw`dAi-sdm%-`&Ce89C$O_v*GMoUN*4mxaCM|B&fw7_; zq(L8CrU)CH6TaIeUxKXA2g#;!yi!m1dlTP^JMrRV;e#~jgUc#%hkc$OL00I4WY}x4 z@FlnRJ1cJIY#J>lyy@pzYG$uE^ z=*o{EE36gCa4KcmAFh{%*R$@eZYX?^27Pc@%~v?XG4 zlBJLS|GT%oDs5k zWJEeSp(2o+fH*@SW(nt4oW;DKz#cNWznwJbL&Tkzh zY6of1hmzHd5okw%pjk#I)T{(~?hY*s`yeIJO09VUa%_26q}ijz+bIHV8s(35X9SN; z>jXvXO=Izh#*Q)TM_G$U>=EC!q(OgNrf9usWTiF9mmn+bbCO|af7KMnzVq#^OI6E@ zaYY*R!DWh(!=CN%cKH%yg+54zlSo%*JIZe}t>x!l6h26UKDd0qAA5(dyVyhd5@dxw zNH&ep$yXg!c6}Ztxe>fI9c!S(^PpcN_gRi&H@1~Kv zJc1D^38b+TM4LwUluGWbSB}Od4;p5nchcyDC$K^T-#DEs=S%Q)9FpNIK_E0W6yzx{pkCCCbWkPI1tro0!nu|=X)cHuR6Mkx*YAS;w< z|0VXvvU~}$!jedad+^owQo~y&TE_5e{?li2UnnmZ3ED>*Oprz=Jd6n4*vpl?a*Wmf zw-3cV$O$gf9t3fJK1hajoQTg|sVzgSNAL6%K1jpM zMVa z7}c<*bs#Cm%QfhOtWc(%Jfa=>5@dxXk!%_{GxHofx*v@zKSX|ikcO9wvf3f^*w9b& zCCCbWkPLtAruK2iZ+ni#p4=e6ok_#XMOi%u9oHkA`4MD=K1jyzs3dDg#dfYKkG8@7 zE55Fx$NFd`MH0;}Kjl7n2>)h^T1!EhM+YeyDT(_z@28$x(nI*5vXCGvlqp6kiT7XF z;EGD05>zX`(nS6wF|EGger8HB*!QZ}gcB57lthJ*Wt=VVe*rsd^r#~z=m{qY*q;~< z`xAo*vci(+xhsj*dxtwu)Hw|MOVs)q%4#J}Jw+uE8$Zk$GB-;Qyj)JuGf@(sb#LHY zaetR+Ice-ZXmwE%MZX>6j4sqQ9IZG(R`@$T6D84e<@3&wwU3Khkp`_ep`L>{uGU=m z(=p-k!Ga)-ouJ(p(#M)Pk2^cP@T#a4C&&tEdcsL!Ys9mz)(!Fehtivo5Z37rl`!?kG->71H$Vl0@5* zm)t+@drpii(x4S5=t(4r<&|2vc2(IRYDF45LD2+BtlVFZt+dz+-D zRh`jR^dM=_iWBs$E{WF%?M>UY_iaIt#!k?ewb}{1b6rYhkUYlW-gZ*$p1X&?L zQ6ot_eX2ls?{j#%MjzpPFBXkTNepluaP4XEz8EE>L7Ed3Rh7hJqxy$=Z?qExY3u~; zDU-zAFDs|M-e_15K~_jpR8UKRn!1cq*=pcfukfx}r z^zm+$Ew1);@$WS#4a%ILokNm<^MfupKNv)i719({l?0qobio} zT&o{W7wsU8ouD01l6dZoo-X6F@F0S$kfyy@lGxvOvTMb-VnGC1Awf}9NgS)u!EKye zAjTDGc)2Lko-#>{a&2&ZIJ}yJm&L76RYxT_OZ5`)L76fUK<^=7< zl7yL=vn|z{^D$a+f~@d&wW5gacyCN2SAnaqh;c<4wBiKqjF3L2*WBZb?K4ymq_GpU z3r!L)-ka>YdZ0uQK~_l9ZU{+CYxUh`r!P;8UDBWxCuldFB=&iVyW1Ct6h1gXR!Gxs z2ualL*L`!zvEPZ=hcsx#3EG7wiIDV{+}4cY!UreF3TfI6AqhAc=7N)9F0MhD6Z(5Q zoLzFk*(E`c#!k=|fb@~|_mLf|Can--m#=5@)o{wNB8k%dsx;`i=a{%ZNP|8&p|X+S zD9NZ=GJI$bp83IXh3E6{!2k37H9P|i5-I?ug7oGhv&S*w@2{kiSd z&hlbr;O*f4l3C)F%1W@^&d3Cdt6i3uf6y5XN7NMk1`ADtv-b(#gaqcTLFlg3V{9CUbf|89PE zn>!;z+#mct=XWr*Px_d#sPOh$Hy;3EdBR1l=G6BK)wKAxHJg44C-iy(rmkfs#@N#Je{`Jd9D6;$c(NTSlXs^R}s87r<8eIp~mW8wPu`71-Wy3*Q2r=b;T>;&!Vkv@i0=-`a4 zUqcY2u@e;Qmqc!Xg097vn+6eNg|yo5gZ*(Ksdn0y0g-|r4O(%6?*vS;;548UP6LYm z-~?GAO}iUqttww#kv9M0eQ`%|EAB(Z;jmU^qINqE_9zD@c=X5}C-k__wKadFEp4!0 z%q3J7(qx4)#pGnIek|J|ed*c4VooHDouCMtB(@b=ln&?a1VI`*L9sJQ4BUA?y@l&t z@f}4PJ3)~!Nx;cTH=LXl*NUzp5`0f8U3Eze>p#q0ZS`U?5=mnxXxFSHCcWrz-dH*( z9IZG(R`|Qx@q2hgxI<;FqugC0-;$4IpaGfk$(-tBL}W$|hVsQefPe;a@7M^+38-i5 zL90NfI=`G053#<$@9-B~w2VMXBYnucL5jc=xmT;5=2?eWx#Um!plkxc1Zi|1GP9M; zh4g!;FB~l2l8+NgPVrGz1XyVpbyOorgFX}?vx~``PJa~I>0o)I0%akg=oFEgOy;Jt z6Dk7@CuBA=N&NK7YzNDK6(|dRC>q;AIku|#e%1)m=mh#BqTA@!JS$f~BikXZWDnA~ z+I(b~M$m}U2{l6HXou_01QMQ$Yl`DC1sPe%CQ1X&?XabHQKZeQSh@gc^Ll?G)_P!v}Z zv%XyKxF7e1XgO)@1VwHo5w~o&V@>}E;e!)og)~KRB?0+;9FX5fw45|(#R-b_N&<2S zIUt9S@WBbPLYktul7Ku#sgS1#zPywMWlm7+OcHQ5JPpo(`tv*Wq z`?s$kzMJABjh&!auk_I@x@@_@&wmp{kQLJOB_j!X&qoa(uPVN)Mx`Vm^A|*f%Zgc) zk5^7m+*cBiU937}7Zc-(H0X~L6uFfIWH56<1~cJ<6J&)n#eF5ABEm|8R-B;7tt3=L zSSQE|>HH$XN`qFMpvbNCp(4UMK~_lX5#gNmFS;rn6xn(L8nogB#eJoZ8(W@@{Nij| z(dVSG6ZFO*iL7Tok9e$q^B{t(kXCOQ_%-otOi1MVrQIU!<*FDme_KLg$g;YT@K5;Q z<#K}FwPdY6IbS{U?=eM$57MANPEZ6|61VrXjC9UA4Bt5FeGp}`LYdwGC2?<4^T?{* zyNkP)GqWlm=x^Q1ny!;PVw}>;y$T zB>_3M+>m2S_}~OtAx+UwNkEP*h@^HEvkz&|iW3y^lmz71azl-(ARHZ>H zPU!EuDw3++gzW^qYs*>{Y*x4C&@qK-qLqq;rm!r;DLy^omDb|R8Yi#+Bkn=cAWa#p zP^QdQ(#NBP#WKrQ3fY1hPLLJ;PT!)E=vii0TJGX?X?B9F@OO&INg{FhevBuIt4kVQ zu9E$EqpV9un}zrL;+*KgU_ySS>SMRu^|hJq&CSGoMRT7lH;}#6N3*M}RjnSkQ&-oC z5O*zU>;$caN#gFdBdI@@ttJT4*a=$MlEeYWgvjTneF`gBDL&HJ3EK)*NQiqxt&hZf zMH)LnF$d|RM1|jKMBE?(^8HHUrWJwd_id%6X6k(MFWJz;E zmNfAuLz0z4$K^i+jFo9hn(V!f24EWSgNv>e1ZnI9y~9fa_Dncn z&x9aIV<+hCToSNnqB`uEfG4MieGkAG^XJ^f-|3B964$eOI*;Ua7CuOWR-B->b4jRd zx~eQB$O>h8BbS8ArmHk|LVMHqs%*MCK~_j>Z~9)9?^P$r3JL8^-wXSx!(czPgKK!X zC{rX|wqw$*b&i;|+u)sAmBp=4rWm;-x-V<%{N>m?;_9mBxrfEV?Zmk*V;mO}n}}Lb zSy--$o_hl83DU>oWhyvUS#yF2wd$a1D+yQ=cEFmj813o}z{B24Jb}o)B<>gLl$N>w zqVU1nq25b8dW7CPVP?^^{_$e}ZlEk|hk6h4=n;A^tUJ45-C5L%m&fFm@Dt7;f~=6Hw+~5l%KF4T{+}jkT!S(v=ou^tm4z0bIR5;! zc7mR}l7O{TC#-6|GD>47=*>wIH-0`H{_sp!=_Bxlr2C+^C`n{K{c1$(@apgm2iKhu zWQD)eTa+Z={VD?9uLMCFv{G+Qsw7Fk`&A^oUx_Ct_rVG6{mKic2i$OaKs-OxI*x~} z71_rwoKSGX2?asWx)at)tuCoMSk?;8O}OFQgt%6m;P*M*X_9~w3T`-|AZ8z4F0Uc2 z8%Y9AD7fK-f*2*F!CG-bTaWTa4ejWzml_&GkQLIjh9rGF-RuL`2OpIeE$2SS3JF>_ zk_4yYvO)56Ih#) zwSqGZ;c%uw^toEKQstsQTBni(oM{M$GYvrmSs_8|RFd#ji3)GqE-xH4^sSGwiUD{6 z5erH54mk+9icg7l@N2~hEi&QF8^6VIz2}=jSC_2NAH_AKk9}8{I*kwdi5?^kTJft( zz9j)?Ga}$@hL}aEg@RD0E^JqK{MC$zu0OpZW>J21)vBB)u-+zpz)D;gti-{0t?DV1 z)v6qP$pkWN;1?vU#I1mpI6;ucPG~D}UWi{k5AiF=RqP3@(b)-#WyxA47i{J@G-JN# z58e)buh0mU#M7se-M!DX5dFbt27W)%s+=U$-}&*d^*S|wU=3-7P7>;G3F!n`AwjEh zlDPZjn(D7Nx-8}^((rOo)>iwxrBg;#Pn+<25J6T*(5jsDq5kfY((rOo)>iwx>Tf&g z1X&?Lt8&tZ`ukBz!^=fkTkZ40YM%pE`$RjaEN+D|#g?UyrD-2I&i%1H6*Ve`>|twO zcH)_br5v9Y`C0g&vXEA*WS+npoAl9YkvGhEsLa#I^bAlq?(#PibmYFM$%?TpN3TZmo zA&Jbl9U0kUc8FS$Mp*fCkfBU*aY>wgt3k%SgL{JrvO((rOo z*3N%;H$?Ub>wH<99}L8)(2Dv1Wp%CeOzHam8~(0E??m=Y>FUi3qjj7h*`6t#-?e;= zp3gpdrgWu2A2ffUOtX?b@3~Hp6}DW>qIzz1rQzkGtOz|*x=xT4`k)!yo+({vc)2Jm zLeG1y6J&)xRDakB((rOorr+(E()B$I>gw{jWYx+FEK|DDV7Z(i*)*Pb^0-4~O6Q|5 zMH`7UV~7mOEVnwJeK?_3`0aVmb%M&pYeh4kX*}P&KFgG@G}sPBsGSNTA2Q2(t`k%) z`k*=3o?BgM(1#+_{t0{DbDf}a(FZ+=f(g=~4@Ib+wP#9K`xcPkGlPn3kE{H+ljT-t zbBRt64S687OzAvE#@EkPL=Up*YMIiN25Uv}J(MYGXwQ4D6J&+giej0;1ZjA=DAVuu zyyvPcZiPN5!WvAFMkmxh5PRNpRTdJ8<|_?)rgWvz3DvXqOzCPbFcRwO^2nW@DP3vo z1kv`q=N|S1(4PhrO%Q((f#p``@e59IRd#=htLec7~2NM)s)(O?K_Dtz&jSdNl)?qs+wrJ0k zt~7RnXnUq~4=b1ZNbynB5Z)PXTw}Lx6b;kquOR!AoIzd+0 z4w6k{>@%fV-gBkFT9FmXw9+KAf2-W;Izd+GgJjcy4CyTIxzg}*k)V|()6g@eD~(R5 z9cfT2EmOKqP`OCc+LS$0x=xT4){10%rgWv@<)W-su=GsnIzd+GgJgTAbfw|tqD<>l zrjeZ*#xkYr1X-aEl1-y}s*~kCR~lX}$|@HA+x71G5@dxwNQS?8f3ye7d#*IRT$E|u z2(oOx(kEYntk4I^rZKZs50)uiX?VFPtGQjzl&%wGg+55OXG-V$0Qg=-xC;V*pR>ix zBQBL&U1_i;w7&plMYM`M$MT-*1X&?LE93S|={iAH*bb8InbMVpmy0s3tDDB6SqG3eBl|Qslr6 zXz-kuA_j@Q2HlS=g}))a;db99qknxh=jtYr(OM&LM*>=5J7_PSY4qw>cW<}Wdvh93 z5P6*g8uY;liaD6Zip_VTvqoRadAZym%)}OmS?I(dAMS>br|5c(D(bvbwxDd%(KE`AY${KREgtveBu0SXa`rw3$Wps?38@;Gd1MfokhXiSK0^8IxPo5ua*sOlm z=0VN8?T3T~5oCoWQ5*&4-G?2ccaO4}$*KeznPyr)PLu)eaQ|X?VFPtC-CD<=kOyr@i65GVrM&f~?R7 z$@ZN6Jd(!ur>f|eJ!iktVE?N)oZ5K`@4H&gex3ODsM>?$7g?5nxF&(Pm?{_j(eLn= zPrkpnS!LkY39`bLt8u00>{l9IE+5mTq37(^Pap9JJw@&@4hR3KEdPO33oMrtBuheN z;8*KsI-#H7f!++{?AHmt3P7^-p>p=~$iFHV{ZV}XzX`I!t4p#ZR0e*f;pL)C@%{fM z$O?UsED4pfUuk%`C{uj@zX`HJA0$gcjYV$50c^C=k#HgfnRBOxhT_GvCOBhvi$1=S)mV-F=u~ar^?x{G`w7tX+_zd zvtK933Vo1l&)Kguyj&z`4H|RyuVq>Ol}0D{DTrF7(|HE|Kv_soxhT`fHw{~s|9}Q- zLRNaM^ql?bBm$Pp_Z_HJ{WH$vE|%p#unWOX5N#SG6Z2duXFuQ1zd%J8KMS%PhlDOm4=s#GVP4OzY8!u zUxKXA2gwjM80uvi_?3p2i?X`U^_=}WL00I4WP8qjhz#a7L)r;y&)KgNWQEs?WP8qjrQzkGOgrmL zBX#i=ma|_c$O?UsY|q)RG`w7tX*Zq7-_COO>jYV$50c@pzC{*f8Tgfkmy0s(zcY@{TrqFZttFn;b zH0{{4=j_)Bvcg)CY|q)RH0Xn@P^KMw=p%#W?AHmhLLVf?m; z%h|6JWQ9IRHjTJ?kt_qh((rOoR?oq<<9N<~oggdpL9#t(KaaPo(_lzY>>c7)zinhW z`;`W3!gnr{4BzKk&VCQG;sito->(;6*|8sD8IzWDaHuT*Izd)gE0Rs){xfS?27aZ% zT9FmXwC~xTvtK933Vo1l&)Kguyj+yke(FJMa#_xPoggdpL9#t(ztZq>QKsi3{0$y0 OXTMI675X6AH2xn;1*P@? literal 0 HcmV?d00001 diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml new file mode 100644 index 00000000..5f2205f7 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -0,0 +1,25 @@ + + + + arm_sim_scenario + 1.1.0 + Robotic Arm wx200 Simulation Scenario Execution + Intel Labs + Intel Labs + Apache-2.0 + + ament_cmake + + controller_manager + effort_controllers + gazebo_ros + gazebo_ros2_control + joint_trajectory_controller + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/simulation/gazebo/arm_sim_scenario/rviz/xsarm_description.rviz b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_description.rviz new file mode 100644 index 00000000..ac0a8870 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_description.rviz @@ -0,0 +1,146 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.6176470518112183 + Tree Height: 503 + - Class: interbotix_xs_rviz/Interbotix Control Panel + Name: Interbotix Control Panel + RobotNameSpace: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.7 + Cell Size: 0.25 + Class: rviz_default_plugins/Grid + Color: 255; 255; 255 + Enabled: true + Line Style: + Line Width: 0.03 + 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 + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 0.15 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 0; 176; 240 + Fixed Frame: world + 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 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + 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: 1.26 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.7853981852531433 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 923 + Hide Left Dock: false + Hide Right Dock: true + Interbotix Control Panel: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000018b00000368fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000001600000226000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000300049006e0074006500720062006f00740069007800200043006f006e00740072006f006c002000500061006e0065006c01000002420000013c0000012e000001510000000100000110000002a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a20000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006080000004cfc0100000002fb0000000800540069006d00650100000000000006080000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004770000036800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + Width: 1544 + X: 72 + Y: 27 diff --git a/simulation/gazebo/arm_sim_scenario/rviz/xsarm_gz_classic.rviz b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_gz_classic.rviz new file mode 100644 index 00000000..5bba6c18 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_gz_classic.rviz @@ -0,0 +1,253 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.6176470518112183 + Tree Height: 811 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.699999988079071 + Cell Size: 0.25 + Class: rviz_default_plugins/Grid + Color: 255; 255; 255 + 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 + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wx200/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/ee_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx200/ee_gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx200/fingers_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx200/forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/gripper_bar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/gripper_prop_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + world: + Value: true + wx200/base_link: + Value: true + wx200/ee_arm_link: + Value: true + wx200/ee_gripper_link: + Value: true + wx200/fingers_link: + Value: true + wx200/forearm_link: + Value: true + wx200/gripper_bar_link: + Value: true + wx200/gripper_link: + Value: true + wx200/gripper_prop_link: + Value: true + wx200/left_finger_link: + Value: true + wx200/right_finger_link: + Value: true + wx200/shoulder_link: + Value: true + wx200/upper_arm_link: + Value: true + wx200/wrist_link: + Value: true + Marker Scale: 0.15000000596046448 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + wx200/base_link: + wx200/shoulder_link: + wx200/upper_arm_link: + wx200/forearm_link: + wx200/wrist_link: + wx200/gripper_link: + wx200/ee_arm_link: + wx200/gripper_bar_link: + wx200/fingers_link: + wx200/ee_gripper_link: + {} + wx200/left_finger_link: + {} + wx200/right_finger_link: + {} + wx200/gripper_prop_link: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 0; 176; 240 + Fixed Frame: world + 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 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + 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: 1.2599999904632568 + 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: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 923 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000018b00000368fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000001600000368000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000300049006e0074006500720062006f00740069007800200043006f006e00740072006f006c002000500061006e0065006c00000002420000013c00000000000000000000000100000110000002a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a20000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006080000004cfc0100000002fb0000000800540069006d00650100000000000006080000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004770000036800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + Width: 1544 + X: 72 + Y: 27 diff --git a/simulation/gazebo/arm_sim_scenario/urdf/ar_tag.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/ar_tag.urdf.xacro new file mode 100644 index 00000000..9e63b914 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/urdf/ar_tag.urdf.xacro @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro new file mode 100644 index 00000000..169314aa --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro @@ -0,0 +1,132 @@ + + + + + + + + + + interbotix_xs_ros_control/XSHardwareInterface + 10 + arm + gripper + joint_states + + + + + fake_components/GenericSystem + + + + + + ign_ros2_control/IgnitionSystem + + + + + + + "${waist_limit_lower}" + "${waist_limit_upper}" + + + + + + + "${shoulder_limit_lower}" + "${shoulder_limit_upper}" + + + + + + + "${elbow_limit_lower}" + "${elbow_limit_upper}" + + + + + + + + "${forearm_roll_limit_lower}" + "${forearm_roll_limit_upper}" + + + + + + + + "${wrist_angle_limit_lower}" + "${wrist_angle_limit_upper}" + + + + + + + + "${wrist_rotate_limit_lower}" + "${wrist_rotate_limit_upper}" + + + + + + + + + + + + + + + + + "${finger_limit_lower}" + "${-finger_limit_lower}" + + + + + + + + + "${-finger_limit_lower}" + "${finger_limit_lower}" + + + + + + + + + + + + + + + + /$(arg robot_name) + + robot_description + robot_state_publisher + $(find arm_sim_scenario)/config/trajectory_controllers/${robot_model}_trajectory_controllers.yaml + + + + + + + diff --git a/simulation/gazebo/arm_sim_scenario/urdf/gazebo_configs.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/gazebo_configs.urdf.xacro new file mode 100644 index 00000000..3c4e87bf --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/urdf/gazebo_configs.urdf.xacro @@ -0,0 +1,93 @@ + + + + + + + Gazebo/DarkGray + 0.01 + + + + Gazebo/DarkGray + 0.01 + true + + + + Gazebo/DarkGray + 0.01 + true + + + + Gazebo/DarkGray + 0.01 + true + + + + Gazebo/DarkGray + 0.01 + true + + + + Gazebo/DarkGray + 0.01 + true + + + + Gazebo/DarkGray + 0.01 + true + + + + Gazebo/DarkGray + 0.01 + + + + 0.01 + Gazebo/DarkGray + + + + Gazebo/DarkGray + + + + Gazebo/DarkGray + 0.01 + + + + Gazebo/DarkGray + 0.01 + + + + true + + + + true + + + + true + + + + true + + + + true + + + + + diff --git a/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro new file mode 100644 index 00000000..4c1b6058 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro @@ -0,0 +1,716 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From c22360ea3662a2574a383f124f0efa0f76acf459 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 29 May 2024 11:52:12 +0200 Subject: [PATCH 02/62] add moveit launch file --- .../arm_sim_scenario/launch/moveit_launch.py | 243 ++++++++++++ .../arm_sim_scenario/rviz/xsarm_moveit.rviz | 351 ++++++++++++++++++ 2 files changed, 594 insertions(+) create mode 100644 simulation/gazebo/arm_sim_scenario/rviz/xsarm_moveit.rviz diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py index e69de29b..b3ff8455 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -0,0 +1,243 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, +) +from launch.conditions import IfCondition, LaunchConfigurationEquals +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, + TextSubstitution, + Command +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import yaml +from launch_ros.descriptions import ParameterValue + + +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: + return None + +ARGUMENTS = [ + DeclareLaunchArgument('robot_model', default_value='wx200', + choices=['wx200'], + description='robot_model type of the Interbotix Arm'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('robot_name', default_value=LaunchConfiguration('robot_model'), + description='Robot name'), + DeclareLaunchArgument('use_rviz', default_value='true', + choices=['true', 'false'], + description='launches RViz if set to `true`.'), + DeclareLaunchArgument('rviz_config', + default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), + 'rviz', + 'xsarm_moveit.rviz', + ]), + description='file path to the config file RViz should load.',), + DeclareLaunchArgument( + 'rviz_frame', + default_value='world', + description=( + 'defines the fixed frame parameter in RViz. Note that if `use_world_frame` is ' + '`false`, this parameter should be changed to a frame that exists.' + ), + ) +] + + +def generate_launch_description(): + + pkg_arm_sim_scenario = get_package_share_directory('arm_sim_scenario') + + use_sim_time = LaunchConfiguration('use_sim_time') + rviz_config = LaunchConfiguration('rviz_config') + use_rviz = LaunchConfiguration('use_rviz') + rviz_frame = LaunchConfiguration('rviz_frame') + robot_name = LaunchConfiguration('robot_name') + robot_model = LaunchConfiguration('robot_model') + + xacro_file = PathJoinSubstitution([pkg_arm_sim_scenario, + 'urdf', + 'wx200.urdf.xacro']) + srdf_xacro_file = PathJoinSubstitution([pkg_arm_sim_scenario, + 'config','srdf', + 'wx200.srdf.xacro']) + + kinematics_config = PathJoinSubstitution([ + pkg_arm_sim_scenario, + 'config', + 'kinematics.yaml', + ]) + + robot_description = {'robot_description': ParameterValue(Command([ + 'xacro', ' ', xacro_file, ' ', + 'gazebo:=ignition', ' ', + 'base_link_frame:=','base_link', ' ', + 'use_gripper:=','true', ' ', + 'show_ar_tag:=','false', ' ', + 'show_gripper_bar:=','true', ' ', + 'show_gripper_fingers:=','true', ' ', + 'use_world_frame:=','true', ' ', + 'robot_model:=',robot_model, ' ', + 'robot_name:=',robot_name, ' ', + 'hardware_type:=','gz_classic']), value_type=str)} + + + + robot_description_semantic = {'robot_description_semantic': ParameterValue(Command([ + 'xacro', ' ', srdf_xacro_file, ' ', + 'robot_name:=',robot_name, ' ', + 'base_link_frame:=','base_link', ' ', + 'use_gripper:=','true', ' ', + 'show_ar_tag:=','false', ' ', + 'show_gripper_bar:=','true', ' ', + 'show_gripper_fingers:=','true', ' ', + 'use_world_frame:=','true', ' ', + 'external_urdf_loc:=','', ' ', + 'external_srdf_loc:=', '', ' ', + 'hardware_type:=','gz_classic']), value_type=str)} + + 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_pipeline_yaml_file = load_yaml( + 'arm_sim_scenario', 'config/ompl_planning.yaml' + ) + + ompl_planning_pipeline_config['move_group'].update(ompl_planning_pipeline_yaml_file) + + controllers_config = load_yaml( + 'arm_sim_scenario', + 'config/controllers/wx200_controllers.yaml' + ) + + config_joint_limits = load_yaml( + 'arm_sim_scenario', + f'config/joint_limits/wx200_joint_limits.yaml' + ) + + joint_limits = { + 'robot_description_planning': config_joint_limits, + } + + moveit_controllers = { + 'moveit_simple_controller_manager': + controllers_config, + 'moveit_controller_manager': + 'moveit_simple_controller_manager/MoveItSimpleControllerManager', + } + + trajectory_execution_parameters = { + '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, + } + + sensor_parameters = { + 'sensors': [''], + } + + remappings = [ + ( + 'wx200/get_planning_scene', + 'wx200/get_planning_scene' + ), + ( + '/arm_controller/follow_joint_trajectory', + 'wx200/arm_controller/follow_joint_trajectory' + ), + ( + '/gripper_controller/follow_joint_trajectory', + 'wx200/gripper_controller/follow_joint_trajectory' + ), + ] + + move_group_node = Node( + package='moveit_ros_move_group', + executable='move_group', + # namespace=robot_name_launch_arg, + parameters=[ + { + 'planning_scene_monitor_options': { + 'robot_description': + 'robot_description', + 'joint_state_topic': + 'wx200/joint_states', + }, + 'use_sim_time': use_sim_time, + }, + robot_description, + robot_description_semantic, + kinematics_config, + ompl_planning_pipeline_config, + trajectory_execution_parameters, + moveit_controllers, + planning_scene_monitor_parameters, + joint_limits, + sensor_parameters, + ], + remappings=remappings, + output={'both': 'screen'}, + ) + + moveit_rviz_node = Node( + condition=IfCondition(use_rviz), + package='rviz2', + executable='rviz2', + name='rviz2', + # namespace=robot_name_launch_arg, + arguments=[ + '-d', rviz_config, + '-f', rviz_frame, + ], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_config, + {'use_sim_time': use_sim_time}, + ], + remappings=remappings, + output={'both': 'log'}, + ) + + ld = LaunchDescription(ARGUMENTS) + ld.add_action(move_group_node) + ld.add_action(moveit_rviz_node) + return ld \ No newline at end of file diff --git a/simulation/gazebo/arm_sim_scenario/rviz/xsarm_moveit.rviz b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_moveit.rviz new file mode 100644 index 00000000..c9239ef3 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_moveit.rviz @@ -0,0 +1,351 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 323 + - 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 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 255; 255; 255 + 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 + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + 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 + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wx200/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/ee_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx200/ee_gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx200/fingers_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx200/forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/gripper_bar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/gripper_prop_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 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: 3x + Trail Step Size: 1 + Trajectory Topic: /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: interbotix_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 + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wx200/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/ee_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx200/ee_gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx200/fingers_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx200/forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/gripper_bar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/gripper_prop_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx200/wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 0; 176; 240 + Fixed Frame: world + 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 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + 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: 1.469738483428955 + 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: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3903985321521759 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.4853975772857666 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 908 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000023000000332fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000180000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001c3000001ac0000017d00ffffff000000010000010f000002eefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002ee000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006430000003efc0100000002fb0000000800540069006d0065000000000000000643000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000040d0000033200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1544 + X: 72 + Y: 27 From 0fc188a689f8690376258a84b5218db68c1391a1 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 29 May 2024 13:00:21 +0200 Subject: [PATCH 03/62] add moveit dependencies --- deb_requirements.txt | 3 +- dependencies/py_moveit2/CMakeLists.txt | 16 + dependencies/py_moveit2/LICENSE | 29 + dependencies/py_moveit2/README.md | 105 + dependencies/py_moveit2/package.xml | 26 + .../py_moveit2/py_moveit2/__init__.py | 6 + .../py_moveit2/py_moveit2/gripper_command.py | 352 +++ .../py_moveit2/gripper_interface.py | 202 ++ dependencies/py_moveit2/py_moveit2/moveit2.py | 2439 +++++++++++++++++ .../py_moveit2/py_moveit2/moveit2_gripper.py | 251 ++ .../py_moveit2/py_moveit2/moveit2_servo.py | 247 ++ 11 files changed, 3675 insertions(+), 1 deletion(-) create mode 100644 dependencies/py_moveit2/CMakeLists.txt create mode 100644 dependencies/py_moveit2/LICENSE create mode 100644 dependencies/py_moveit2/README.md create mode 100644 dependencies/py_moveit2/package.xml create mode 100644 dependencies/py_moveit2/py_moveit2/__init__.py create mode 100644 dependencies/py_moveit2/py_moveit2/gripper_command.py create mode 100644 dependencies/py_moveit2/py_moveit2/gripper_interface.py create mode 100644 dependencies/py_moveit2/py_moveit2/moveit2.py create mode 100644 dependencies/py_moveit2/py_moveit2/moveit2_gripper.py create mode 100644 dependencies/py_moveit2/py_moveit2/moveit2_servo.py diff --git a/deb_requirements.txt b/deb_requirements.txt index f07fc8c3..8c2c53e7 100644 --- a/deb_requirements.txt +++ b/deb_requirements.txt @@ -2,4 +2,5 @@ ros-humble-turtlebot4-simulator ros-humble-py-trees-ros-interfaces python3-autopep8 clang-format -pylint \ No newline at end of file +pylint +ros-humble-moveit \ No newline at end of file diff --git a/dependencies/py_moveit2/CMakeLists.txt b/dependencies/py_moveit2/CMakeLists.txt new file mode 100644 index 00000000..eaa3b214 --- /dev/null +++ b/dependencies/py_moveit2/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.5) +project(py_moveit2) + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +# Install Python package +ament_python_install_package(py_moveit2) + +install(DIRECTORY + DESTINATION lib/${PROJECT_NAME} +) + +# Setup the project +ament_package() diff --git a/dependencies/py_moveit2/LICENSE b/dependencies/py_moveit2/LICENSE new file mode 100644 index 00000000..70069740 --- /dev/null +++ b/dependencies/py_moveit2/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2022, Andrej Orsula +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/dependencies/py_moveit2/README.md b/dependencies/py_moveit2/README.md new file mode 100644 index 00000000..87a05e90 --- /dev/null +++ b/dependencies/py_moveit2/README.md @@ -0,0 +1,105 @@ +# pymoveit2 + +Basic Python interface for MoveIt 2 built on top of ROS 2 actions and services. + +> Note: The official Python library for MoveIt 2 `moveit_py` is now available. Check the announcement [here](https://picknik.ai/moveit/ros/python/google/2023/04/28/GSOC-MoveIt-2-Python-Bindings.html)! + +
+ + + + + + + + + + + + + + + +
Animation of ex_joint_goal.pyAnimation of ex_pose_goal.pyAnimation of ex_gripper.pyAnimation of ex_servo.py
Joint Goal
Pose Goal
Gripper Action
MoveIt 2 Servo
+
+ +## Instructions + +### Dependencies + +These are the primary dependencies required to use this project. + +- ROS 2 [Galactic](https://docs.ros.org/en/galactic/Installation.html), [Humble](https://docs.ros.org/en/humble/Installation.html) or [Iron](https://docs.ros.org/en/iron/Installation.html) +- [MoveIt 2](https://moveit.ros.org/install-moveit2/binary) corresponding to the selected ROS 2 distribution + +All additional dependencies are installed via [rosdep](https://wiki.ros.org/rosdep) during the building process below. + +### Building + +Clone this repository, install dependencies and build with [colcon](https://colcon.readthedocs.io). + +```bash +# Clone this repository into your favourite ROS 2 workspace +git clone https://github.com/AndrejOrsula/pymoveit2.git +# Install dependencies +rosdep install -y -r -i --rosdistro ${ROS_DISTRO} --from-paths . +# Build +colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" +``` + +### Sourcing + +Before utilising this package, remember to source the ROS 2 workspace. + +```bash +source install/local_setup.bash +``` + +This enables importing of `pymoveit2` module from external workspaces. + +## Examples + +To demonstrate `pymoveit2` usage, [examples](./examples) directory contains scripts that demonstrate the basic functionality. Additional examples can be found under [ign_moveit2_examples](https://github.com/AndrejOrsula/ign_moveit2_examples) repository. + +Prior to running the examples, configure an environment for control of a robot with MoveIt 2. For instance, one of the following launch scripts from [panda_ign_moveit2](https://github.com/AndrejOrsula/panda_ign_moveit2) repository can be used. + +```bash +# RViz (fake) ROS 2 control +ros2 launch panda_moveit_config ex_fake_control.launch.py +# Gazebo (simulated) ROS 2 control +ros2 launch panda_moveit_config ex_ign_control.launch.py +``` + +After that, the individual scripts can be run. + +```bash +# Move to joint configuration +ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]" +# Move to Cartesian pose (motion in either joint or Cartesian space) +ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False +# Repeatadly toggle the gripper (or use "open"/"close" actions) +ros2 run pymoveit2 ex_gripper.py --ros-args -p action:="toggle" +# Example of using MoveIt 2 Servo to move the end-effector in a circular motion +ros2 run pymoveit2 ex_servo.py +# Example of adding a collision object with primitive geometry to the planning scene of MoveIt 2 +ros2 run pymoveit2 ex_collision_primitive.py --ros-args -p shape:="sphere" -p position:="[0.5, 0.0, 0.5]" -p dimensions:="[0.04]" +# Example of adding a collision object with mesh geometry to the planning scene of MoveIt 2 +ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p action:="add" -p position:="[0.5, 0.0, 0.5]" -p quat_xyzw:="[0.0, 0.0, -0.707, 0.707]" +``` + +## Directory Structure + +The following directory structure is utilised for this package. + +```bash +. +├── examples/ # [dir] Examples demonstrating the use of `pymoveit2` +├── pymoveit2/ # [dir] ROS 2 launch scripts + ├── robots/ # [dir] Presets for robots (data that can be extracted from URDF/SRDF) + ├── gripper_command.py # Interface for Gripper that is controlled by GripperCommand + ├── moveit2_gripper.py # Interface for MoveIt 2 Gripper that is controlled by JointTrajectoryController + ├── moveit2_servo.py # Interface for MoveIt 2 Servo that enables real-time control in Cartesian Space + └── moveit2.py # Interface for MoveIt 2 that enables planning and execution of trajectories +├── CMakeLists.txt # Colcon-enabled CMake recipe +└── package.xml # ROS 2 package metadata +``` diff --git a/dependencies/py_moveit2/package.xml b/dependencies/py_moveit2/package.xml new file mode 100644 index 00000000..2bac2b77 --- /dev/null +++ b/dependencies/py_moveit2/package.xml @@ -0,0 +1,26 @@ + + + py_moveit2 + 4.0.0 + Basic Python interface for MoveIt 2 built on top of ROS 2 actions and services + Andrej Orsula + Andrej Orsula + BSD + + ament_cmake + ament_cmake_python + + action_msgs + control_msgs + geometry_msgs + moveit_msgs + rclpy + sensor_msgs + shape_msgs + std_srvs + trajectory_msgs + + + ament_cmake + + diff --git a/dependencies/py_moveit2/py_moveit2/__init__.py b/dependencies/py_moveit2/py_moveit2/__init__.py new file mode 100644 index 00000000..6c54a9d1 --- /dev/null +++ b/dependencies/py_moveit2/py_moveit2/__init__.py @@ -0,0 +1,6 @@ +from . import robots +from .gripper_command import GripperCommand +from .gripper_interface import GripperInterface +from .moveit2 import MoveIt2, MoveIt2State +from .moveit2_gripper import MoveIt2Gripper +from .moveit2_servo import MoveIt2Servo diff --git a/dependencies/py_moveit2/py_moveit2/gripper_command.py b/dependencies/py_moveit2/py_moveit2/gripper_command.py new file mode 100644 index 00000000..0ee92ab4 --- /dev/null +++ b/dependencies/py_moveit2/py_moveit2/gripper_command.py @@ -0,0 +1,352 @@ +import threading +from typing import List, Optional, Union + +from action_msgs.msg import GoalStatus +from control_msgs.action import GripperCommand as GripperCommandAction +from rclpy.action import ActionClient +from rclpy.callback_groups import CallbackGroup +from rclpy.node import Node +from rclpy.qos import ( + QoSDurabilityPolicy, + QoSHistoryPolicy, + QoSProfile, + QoSReliabilityPolicy, +) +from sensor_msgs.msg import JointState + + +class GripperCommand: + """ + Python interface for Gripper that is controlled by GripperCommand. + """ + + def __init__( + self, + node: Node, + gripper_joint_names: List[str], + open_gripper_joint_positions: Union[float, List[float]], + closed_gripper_joint_positions: Union[float, List[float]], + max_effort: float = 0.0, + ignore_new_calls_while_executing: bool = True, + callback_group: Optional[CallbackGroup] = None, + gripper_command_action_name: str = "gripper_action_controller/gripper_command", + ): + """ + Construct an instance of `GripperCommand` interface. + - `node` - ROS 2 node that this interface is attached to + - `gripper_joint_names` - List of gripper joint names (can be extracted from URDF) + - `open_gripper_joint_positions` - Configuration of gripper joints when open + - `closed_gripper_joint_positions` - Configuration of gripper joints when fully closed + - `max_effort` - Max effort applied when closing + - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories + while previous is still being executed + - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) + - `gripper_command_action_name` - Name of the action server for the controller + """ + + self._node = node + self._callback_group = callback_group + + # Create subscriber for current joint states + self._node.create_subscription( + msg_type=JointState, + topic="joint_states", + callback=self.__joint_state_callback, + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=self._callback_group, + ) + + # Create action client for move action + self.__gripper_command_action_client = ActionClient( + node=self._node, + action_type=GripperCommandAction, + action_name=gripper_command_action_name, + goal_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + result_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + cancel_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + feedback_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + status_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=self._callback_group, + ) + + # Initialise command goals for opening/closing + self.__open_gripper_joint_positions = open_gripper_joint_positions + self.__open_gripper_command_goal = self.__init_gripper_command_goal( + position=open_gripper_joint_positions, max_effort=max_effort + ) + self.__close_gripper_command_goal = self.__init_gripper_command_goal( + position=closed_gripper_joint_positions, max_effort=max_effort + ) + + # Initialise internals for determining whether the gripper is open or closed + self.__joint_state_mutex = threading.Lock() + self.__joint_state = None + self.__new_joint_state_available = False + # Tolerance used for checking whether the gripper is open or closed + self.__open_tolerance = [ + 0.1 + * abs(open_gripper_joint_positions[i] - closed_gripper_joint_positions[i]) + for i in range(len(open_gripper_joint_positions)) + ] + # Indices of gripper joint within the joint state message topic. + # It is assumed that the order of these does not change during execution. + self.__gripper_joint_indices: Optional[List[int]] = None + + # Flag that determines whether a new goal can be send while the previous one is being executed + self.__ignore_new_calls_while_executing = ignore_new_calls_while_executing + + # Store additional variables for later use + self.__joint_names = gripper_joint_names + + # Internal states that monitor the current motion requests and execution + self.__is_motion_requested = False + self.__is_executing = False + self.__wait_until_executed_rate = self._node.create_rate(1000.0) + + def __call__(self): + """ + Callable that is identical to `GripperCommand.toggle()`. + """ + + self.toggle() + + def toggle(self): + """ + Toggles the gripper between open and closed state. + """ + + if self.is_open: + self.close(skip_if_noop=False) + else: + self.open(skip_if_noop=False) + + def open(self, skip_if_noop: bool = False): + """ + Open the gripper. + - `skip_if_noop` - No action will be performed if the gripper is already open. + """ + + if skip_if_noop and self.is_open: + return + + if self.__ignore_new_calls_while_executing and self.__is_executing: + return + self.__is_motion_requested = True + + self.__send_goal_async_gripper_command(self.__open_gripper_command_goal) + + def close(self, skip_if_noop: bool = False): + """ + Close the gripper. + - `skip_if_noop` - No action will be performed if the gripper is not open. + """ + + if skip_if_noop and self.is_closed: + return + + if self.__ignore_new_calls_while_executing and self.__is_executing: + return + self.__is_motion_requested = True + + self.__send_goal_async_gripper_command(self.__close_gripper_command_goal) + + def reset_open(self, **kwargs): + """ + Reset into open configuration by sending a dummy joint trajectory. + This is useful for simulated robots that allow instantaneous reset of joints. + """ + + self.force_reset_executing_state() + self.__send_goal_async_gripper_command(self.__open_gripper_command_goal) + + def reset_closed(self, **kwargs): + """ + Reset into closed configuration by sending a dummy joint trajectory. + This is useful for simulated robots that allow instantaneous reset of joints. + """ + + self.force_reset_executing_state() + self.__send_goal_async_gripper_command(self.__close_gripper_command_goal) + + def force_reset_executing_state(self): + """ + Force reset of internal states that block execution while `ignore_new_calls_while_executing` is being + used. This function is applicable only in a very few edge-cases, so it should almost never be used. + """ + + self.__is_motion_requested = False + self.__is_executing = False + + def wait_until_executed(self) -> bool: + """ + Wait until the previously requested motion is finalised through either a success or failure. + """ + + if not self.__is_motion_requested: + self._node.get_logger().warn( + "Cannot wait until motion is executed (no motion is in progress)." + ) + return False + + while self.__is_motion_requested or self.__is_executing: + self.__wait_until_executed_rate.sleep() + return True + + def __joint_state_callback(self, msg: JointState): + # Update only if all relevant joints are included in the message + for joint_name in self.joint_names: + if not joint_name in msg.name: + return + + self.__joint_state_mutex.acquire() + self.__joint_state = msg + self.__new_joint_state_available = True + self.__joint_state_mutex.release() + + def __send_goal_async_gripper_command( + self, + goal: GripperCommandAction.Goal, + wait_for_server_timeout_sec: Optional[float] = 1.0, + ): + if not self.__gripper_command_action_client.wait_for_server( + timeout_sec=wait_for_server_timeout_sec + ): + self._node.get_logger().warn( + f"Action server {self.__gripper_command_action_client._action_name} is not yet ready. Better luck next time!" + ) + return + + action_result = self.__gripper_command_action_client.send_goal_async( + goal=goal, + feedback_callback=None, + ) + + action_result.add_done_callback(self.__response_callback_gripper_command) + + def __response_callback_gripper_command(self, response): + goal_handle = response.result() + if not goal_handle.accepted: + self._node.get_logger().warn( + f"Action '{self.__gripper_command_action_client._action_name}' was rejected." + ) + self.__is_motion_requested = False + return + + self.__is_executing = True + self.__is_motion_requested = False + + self.__get_result_future_gripper_command = goal_handle.get_result_async() + self.__get_result_future_gripper_command.add_done_callback( + self.__result_callback_gripper_command + ) + + def __result_callback_gripper_command(self, res): + if res.result().status != GoalStatus.STATUS_SUCCEEDED: + self._node.get_logger().error( + f"Action '{self.__gripper_command_action_client._action_name}' was unsuccessful: {res.result().status}" + ) + + self.__is_executing = False + + @classmethod + def __init_gripper_command_goal( + cls, position: Union[float, List[float]], max_effort: float + ) -> GripperCommandAction.Goal: + if hasattr(position, "__getitem__"): + position = position[0] + + gripper_cmd_goal = GripperCommandAction.Goal() + gripper_cmd_goal.command.position = position + gripper_cmd_goal.command.max_effort = max_effort + + return gripper_cmd_goal + + @property + def gripper_command_action_client(self) -> str: + return self.__gripper_command_action_client + + @property + def joint_names(self) -> List[str]: + return self.__joint_names + + @property + def joint_state(self) -> Optional[JointState]: + self.__joint_state_mutex.acquire() + joint_state = self.__joint_state + self.__joint_state_mutex.release() + return joint_state + + @property + def new_joint_state_available(self): + return self.__new_joint_state_available + + @property + def is_open(self) -> bool: + """ + Gripper is considered to be open if all of the joints are at their open position. + """ + + joint_state = self.joint_state + + # Assume the gripper is open if there are no joint state readings yet + if joint_state is None: + return True + + # For the sake of performance, find the indices of joints only once. + # This is especially useful for robots with many joints. + if self.__gripper_joint_indices is None: + self.__gripper_joint_indices: List[int] = [] + for joint_name in self.joint_names: + self.__gripper_joint_indices.append(joint_state.name.index(joint_name)) + + for local_joint_index, joint_state_index in enumerate( + self.__gripper_joint_indices + ): + if ( + abs( + joint_state.position[joint_state_index] + - self.__open_gripper_joint_positions[local_joint_index] + ) + > self.__open_tolerance[local_joint_index] + ): + return False + + return True + + @property + def is_closed(self) -> bool: + """ + Gripper is considered to be closed if any of the joints is outside of their open position. + """ + + return not self.is_open diff --git a/dependencies/py_moveit2/py_moveit2/gripper_interface.py b/dependencies/py_moveit2/py_moveit2/gripper_interface.py new file mode 100644 index 00000000..05038056 --- /dev/null +++ b/dependencies/py_moveit2/py_moveit2/gripper_interface.py @@ -0,0 +1,202 @@ +from typing import List, Optional + +from rclpy.callback_groups import CallbackGroup +from rclpy.node import Node + +from .gripper_command import GripperCommand +from .moveit2_gripper import MoveIt2Gripper + + +class GripperInterface(MoveIt2Gripper, GripperCommand): + """ + Python interface for MoveIt 2 Gripper that is controlled either by GripperCommand or JointTrajectoryController. + The appropriate interface is automatically selected based on the available action. + """ + + def __init__( + self, + node: Node, + gripper_joint_names: List[str], + open_gripper_joint_positions: List[float], + closed_gripper_joint_positions: List[float], + gripper_group_name: str = "gripper", + execute_via_moveit: bool = False, + ignore_new_calls_while_executing: bool = False, + skip_planning: bool = False, + skip_planning_fixed_motion_duration: float = 0.5, + max_effort: float = 0.0, + callback_group: Optional[CallbackGroup] = None, + follow_joint_trajectory_action_name: str = "DEPRECATED", + gripper_command_action_name: str = "gripper_action_controller/gripper_cmd", + use_move_group_action: bool = False, + ): + """ + Combination of `MoveIt2Gripper` and `GripperCommand` interfaces that automatically + selects the appropriate interface based on the available actions. + """ + + # Check for deprecated parameters + if execute_via_moveit: + node.get_logger().warn( + "Parameter `execute_via_moveit` is deprecated. Please use `use_move_group_action` instead." + ) + use_move_group_action = True + if follow_joint_trajectory_action_name != "DEPRECATED": + node.get_logger().warn( + "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead." + ) + + MoveIt2Gripper.__init__( + self=self, + node=node, + gripper_joint_names=gripper_joint_names, + open_gripper_joint_positions=open_gripper_joint_positions, + closed_gripper_joint_positions=closed_gripper_joint_positions, + gripper_group_name=gripper_group_name, + ignore_new_calls_while_executing=ignore_new_calls_while_executing, + skip_planning=skip_planning, + skip_planning_fixed_motion_duration=skip_planning_fixed_motion_duration, + callback_group=callback_group, + use_move_group_action=use_move_group_action, + ) + + GripperCommand.__init__( + self=self, + node=node, + gripper_joint_names=gripper_joint_names, + open_gripper_joint_positions=open_gripper_joint_positions, + closed_gripper_joint_positions=closed_gripper_joint_positions, + max_effort=max_effort, + ignore_new_calls_while_executing=ignore_new_calls_while_executing, + callback_group=callback_group, + gripper_command_action_name=gripper_command_action_name, + ) + + self.__determine_interface() + + def __determine_interface(self, timeout_sec=1.0): + if self.gripper_command_action_client.wait_for_server(timeout_sec=timeout_sec): + self._interface = GripperCommand + elif self._execute_trajectory_action_client.wait_for_server( + timeout_sec=timeout_sec + ): + self._interface = MoveIt2Gripper + else: + self._interface = None + + if self._interface is None: + self._node.get_logger().warn( + f"Unable to determine the appropriate interface for gripper." + ) + + def __call__(self): + """ + Callable that is identical to `MoveIt2Gripper.toggle()`. + """ + + self.toggle() + + def toggle(self): + """ + Toggles the gripper between open and closed state. + """ + + if self.is_open: + self.close(skip_if_noop=False) + else: + self.open(skip_if_noop=False) + + def open(self, skip_if_noop: bool = False): + """ + Open the gripper. + - `skip_if_noop` - No action will be performed if the gripper is already open. + """ + + if self._interface is None: + self.__determine_interface() + if self._interface is None: + self._node.get_logger().error( + f"Unable to close the gripper because the appropriate interface cannot be determined." + ) + return + + self._interface.open(self=self, skip_if_noop=skip_if_noop) + + def close(self, skip_if_noop: bool = False): + """ + Close the gripper. + - `skip_if_noop` - No action will be performed if the gripper is not open. + """ + + if self._interface is None: + self.__determine_interface() + if self._interface is None: + self._node.get_logger().error( + f"Unable to close the gripper because the appropriate interface cannot be determined." + ) + return + + self._interface.close(self=self, skip_if_noop=skip_if_noop) + + def reset_open(self, sync: bool = True): + """ + Reset into open configuration by sending a dummy joint trajectory. + This is useful for simulated robots that allow instantaneous reset of joints. + """ + + if self._interface is None: + self.__determine_interface() + if self._interface is None: + self._node.get_logger().error( + f"Unable to reset the gripper as open because the appropriate interface cannot be determined." + ) + return + + self._interface.reset_open(self=self, sync=sync) + + def reset_closed(self, sync: bool = True): + """ + Reset into closed configuration by sending a dummy joint trajectory. + This is useful for simulated robots that allow instantaneous reset of joints. + """ + + if self._interface is None: + self.__determine_interface() + if self._interface is None: + self._node.get_logger().error( + f"Unable to reset the gripper as closed because the appropriate interface cannot be determined." + ) + return + + self._interface.reset_closed(self=self, sync=sync) + + def force_reset_executing_state(self): + """ + Force reset of internal states that block execution while `ignore_new_calls_while_executing` is being + used. This function is applicable only in a very few edge-cases, so it should almost never be used. + """ + + if self._interface is None: + self.__determine_interface() + if self._interface is None: + self._node.get_logger().error( + f"Unable to reset the executing state because the appropriate interface cannot be determined." + ) + return + + self._interface.force_reset_executing_state(self=self) + + def wait_until_executed(self) -> bool: + """ + Wait until the previously requested motion is finalised through either a success or failure. + """ + + if self._interface is None: + self.__determine_interface() + if self._interface is None: + self._node.get_logger().error( + f"Unable to wait until a motion is executed because the appropriate interface cannot be determined." + ) + return False + + return self._interface.wait_until_executed(self=self) diff --git a/dependencies/py_moveit2/py_moveit2/moveit2.py b/dependencies/py_moveit2/py_moveit2/moveit2.py new file mode 100644 index 00000000..4aca121f --- /dev/null +++ b/dependencies/py_moveit2/py_moveit2/moveit2.py @@ -0,0 +1,2439 @@ +import copy +import threading +from enum import Enum +from typing import Any, List, Optional, Tuple, Union + +import numpy as np +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from moveit_msgs.action import ExecuteTrajectory, MoveGroup +from moveit_msgs.msg import ( + AllowedCollisionEntry, + AttachedCollisionObject, + CollisionObject, + Constraints, + JointConstraint, + MoveItErrorCodes, + OrientationConstraint, + PlanningScene, + PositionConstraint, +) +from moveit_msgs.srv import ( + ApplyPlanningScene, + GetCartesianPath, + GetMotionPlan, + GetPlanningScene, + GetPositionFK, + GetPositionIK, +) +from rclpy.action import ActionClient +from rclpy.callback_groups import CallbackGroup +from rclpy.node import Node +from rclpy.qos import ( + QoSDurabilityPolicy, + QoSHistoryPolicy, + QoSProfile, + QoSReliabilityPolicy, +) +from rclpy.task import Future +from sensor_msgs.msg import JointState +from shape_msgs.msg import Mesh, MeshTriangle, SolidPrimitive +from std_msgs.msg import Header, String +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + + +class MoveIt2State(Enum): + """ + An enum the represents the current execution state of the MoveIt2 interface. + - IDLE: No motion is being requested or executed + - REQUESTING: Execution has been requested, but the request has not yet been + accepted. + - EXECUTING: Execution has been requested and accepted, and has not yet been + completed. + """ + + IDLE = 0 + REQUESTING = 1 + EXECUTING = 2 + + +class MoveIt2: + """ + Python interface for MoveIt 2 that enables planning and execution of trajectories. + For execution, this interface requires that robot utilises JointTrajectoryController. + """ + + def __init__( + self, + node: Node, + joint_names: List[str], + base_link_name: str, + end_effector_name: str, + group_name: str = "arm", + execute_via_moveit: bool = False, + ignore_new_calls_while_executing: bool = False, + callback_group: Optional[CallbackGroup] = None, + follow_joint_trajectory_action_name: str = "DEPRECATED", + use_move_group_action: bool = False, + ): + """ + Construct an instance of `MoveIt2` interface. + - `node` - ROS 2 node that this interface is attached to + - `joint_names` - List of joint names of the robot (can be extracted from URDF) + - `base_link_name` - Name of the robot base link + - `end_effector_name` - Name of the robot end effector + - `group_name` - Name of the planning group for robot arm + - [DEPRECATED] `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) + FollowJointTrajectory action (controller) is employed otherwise + together with a separate planning service client + - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories + while previous is still being executed + - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) + - [DEPRECATED] `follow_joint_trajectory_action_name` - Name of the action server for the controller + - `use_move_group_action` - Flag that enables execution via MoveGroup action (MoveIt 2) + ExecuteTrajectory action is employed otherwise + together with a separate planning service client + """ + + self._node = node + self._callback_group = callback_group + + # Check for deprecated parameters + if execute_via_moveit: + self._node.get_logger().warn( + "Parameter `execute_via_moveit` is deprecated. Please use `use_move_group_action` instead." + ) + use_move_group_action = True + if follow_joint_trajectory_action_name != "DEPRECATED": + self._node.get_logger().warn( + "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead." + ) + + # Create subscriber for current joint states + self._node.create_subscription( + msg_type=JointState, + topic="joint_states", + callback=self.__joint_state_callback, + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=self._callback_group, + ) + + # Create action client for move action + self.__move_action_client = ActionClient( + node=self._node, + action_type=MoveGroup, + action_name="move_action", + goal_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + result_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + cancel_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + feedback_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + status_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=self._callback_group, + ) + + # Also create a separate service client for planning + self._plan_kinematic_path_service = self._node.create_client( + srv_type=GetMotionPlan, + srv_name="plan_kinematic_path", + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=callback_group, + ) + self.__kinematic_path_request = GetMotionPlan.Request() + + # Create a separate service client for Cartesian planning + self._plan_cartesian_path_service = self._node.create_client( + srv_type=GetCartesianPath, + srv_name="compute_cartesian_path", + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=callback_group, + ) + self.__cartesian_path_request = GetCartesianPath.Request() + + # Create action client for trajectory execution + self._execute_trajectory_action_client = ActionClient( + node=self._node, + action_type=ExecuteTrajectory, + action_name="execute_trajectory", + goal_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + result_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + cancel_service_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + ), + feedback_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + status_sub_qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=self._callback_group, + ) + + # Create a service for getting the planning scene + self._get_planning_scene_service = self._node.create_client( + srv_type=GetPlanningScene, + srv_name="get_planning_scene", + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=callback_group, + ) + self.__planning_scene = None + self.__old_planning_scene = None + self.__old_allowed_collision_matrix = None + + # Create a service for applying the planning scene + self._apply_planning_scene_service = self._node.create_client( + srv_type=ApplyPlanningScene, + srv_name="apply_planning_scene", + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=callback_group, + ) + + self.__collision_object_publisher = self._node.create_publisher( + CollisionObject, "/collision_object", 10 + ) + self.__attached_collision_object_publisher = self._node.create_publisher( + AttachedCollisionObject, "/attached_collision_object", 10 + ) + + self.__cancellation_pub = self._node.create_publisher( + String, "/trajectory_execution_event", 1 + ) + + self.__joint_state_mutex = threading.Lock() + self.__joint_state = None + self.__new_joint_state_available = False + self.__move_action_goal = self.__init_move_action_goal( + frame_id=base_link_name, + group_name=group_name, + end_effector=end_effector_name, + ) + + # Flag to determine whether to execute trajectories via Move Group Action, or rather by calling + # the separate ExecuteTrajectory action + # Applies to `move_to_pose()` and `move_to_configuration()` + self.__use_move_group_action = use_move_group_action + + # Flag that determines whether a new goal can be sent while the previous one is being executed + self.__ignore_new_calls_while_executing = ignore_new_calls_while_executing + + # Store additional variables for later use + self.__joint_names = joint_names + self.__base_link_name = base_link_name + self.__end_effector_name = end_effector_name + self.__group_name = group_name + + # Internal states that monitor the current motion requests and execution + self.__is_motion_requested = False + self.__is_executing = False + self.motion_suceeded = False + self.__execution_goal_handle = None + self.__last_error_code = None + self.__wait_until_executed_rate = self._node.create_rate(1000.0) + self.__execution_mutex = threading.Lock() + + # Event that enables waiting until async future is done + self.__future_done_event = threading.Event() + + #### Execution Polling Functions + def query_state(self) -> MoveIt2State: + with self.__execution_mutex: + if self.__is_motion_requested: + return MoveIt2State.REQUESTING + elif self.__is_executing: + return MoveIt2State.EXECUTING + else: + return MoveIt2State.IDLE + + def cancel_execution(self): + if self.query_state() != MoveIt2State.EXECUTING: + self._node.get_logger().warn("Attempted to cancel without active goal.") + return None + + cancel_string = String() + cancel_string.data = "stop" + self.__cancellation_pub.publish(cancel_string) + + def get_execution_future(self) -> Optional[Future]: + if self.query_state() != MoveIt2State.EXECUTING: + self._node.get_logger().warn("Need active goal for future.") + return None + + return self.__execution_goal_handle.get_result_async() + + def get_last_execution_error_code(self) -> Optional[MoveItErrorCodes]: + return self.__last_error_code + + #### + + def move_to_pose( + self, + pose: Optional[Union[PoseStamped, Pose]] = None, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + target_link: Optional[str] = None, + frame_id: Optional[str] = None, + tolerance_position: float = 0.001, + tolerance_orientation: float = 0.001, + weight_position: float = 1.0, + cartesian: bool = False, + weight_orientation: float = 1.0, + cartesian_max_step: float = 0.0025, + cartesian_fraction_threshold: float = 0.0, + ): + """ + Plan and execute motion based on previously set goals. Optional arguments can be + passed in to internally use `set_pose_goal()` to define a goal during the call. + """ + + if isinstance(pose, PoseStamped): + pose_stamped = pose + elif isinstance(pose, Pose): + pose_stamped = PoseStamped( + header=Header( + stamp=self._node.get_clock().now().to_msg(), + frame_id=( + frame_id if frame_id is not None else self.__base_link_name + ), + ), + pose=pose, + ) + else: + if not isinstance(position, Point): + position = Point( + x=float(position[0]), y=float(position[1]), z=float(position[2]) + ) + if not isinstance(quat_xyzw, Quaternion): + quat_xyzw = Quaternion( + x=float(quat_xyzw[0]), + y=float(quat_xyzw[1]), + z=float(quat_xyzw[2]), + w=float(quat_xyzw[3]), + ) + pose_stamped = PoseStamped( + header=Header( + stamp=self._node.get_clock().now().to_msg(), + frame_id=( + frame_id if frame_id is not None else self.__base_link_name + ), + ), + pose=Pose(position=position, orientation=quat_xyzw), + ) + + if self.__use_move_group_action and not cartesian: + if self.__ignore_new_calls_while_executing and ( + self.__is_motion_requested or self.__is_executing + ): + self._node.get_logger().warn( + "Controller is already following a trajectory. Skipping motion." + ) + return + + # Set goal + self.set_pose_goal( + position=pose_stamped.pose.position, + quat_xyzw=pose_stamped.pose.orientation, + frame_id=pose_stamped.header.frame_id, + target_link=target_link, + tolerance_position=tolerance_position, + tolerance_orientation=tolerance_orientation, + weight_position=weight_position, + weight_orientation=weight_orientation, + ) + # Define starting state as the current state + if self.joint_state is not None: + self.__move_action_goal.request.start_state.joint_state = ( + self.joint_state + ) + # Send to goal to the server (async) - both planning and execution + self._send_goal_async_move_action() + # Clear all previous goal constrains + self.clear_goal_constraints() + self.clear_path_constraints() + + else: + # Plan via MoveIt 2 and then execute directly with the controller + self.execute( + self.plan( + position=pose_stamped.pose.position, + quat_xyzw=pose_stamped.pose.orientation, + frame_id=pose_stamped.header.frame_id, + target_link=target_link, + tolerance_position=tolerance_position, + tolerance_orientation=tolerance_orientation, + weight_position=weight_position, + weight_orientation=weight_orientation, + cartesian=cartesian, + max_step=cartesian_max_step, + cartesian_fraction_threshold=cartesian_fraction_threshold, + ) + ) + + def move_to_configuration( + self, + joint_positions: List[float], + joint_names: Optional[List[str]] = None, + tolerance: float = 0.001, + weight: float = 1.0, + ): + """ + Plan and execute motion based on previously set goals. Optional arguments can be + passed in to internally use `set_joint_goal()` to define a goal during the call. + """ + + if self.__use_move_group_action: + if self.__ignore_new_calls_while_executing and ( + self.__is_motion_requested or self.__is_executing + ): + self._node.get_logger().warn( + "Controller is already following a trajectory. Skipping motion." + ) + return + + # Set goal + self.set_joint_goal( + joint_positions=joint_positions, + joint_names=joint_names, + tolerance=tolerance, + weight=weight, + ) + # Define starting state as the current state + if self.joint_state is not None: + self.__move_action_goal.request.start_state.joint_state = ( + self.joint_state + ) + # Send to goal to the server (async) - both planning and execution + self._send_goal_async_move_action() + # Clear all previous goal constrains + self.clear_goal_constraints() + self.clear_path_constraints() + + else: + # Plan via MoveIt 2 and then execute directly with the controller + self.execute( + self.plan( + joint_positions=joint_positions, + joint_names=joint_names, + tolerance_joint_position=tolerance, + weight_joint_position=weight, + ) + ) + + def plan( + self, + pose: Optional[Union[PoseStamped, Pose]] = None, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + joint_positions: Optional[List[float]] = None, + joint_names: Optional[List[str]] = None, + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance_position: float = 0.001, + tolerance_orientation: Union[float, Tuple[float, float, float]] = 0.001, + tolerance_joint_position: float = 0.001, + weight_position: float = 1.0, + weight_orientation: float = 1.0, + weight_joint_position: float = 1.0, + start_joint_state: Optional[Union[JointState, List[float]]] = None, + cartesian: bool = False, + max_step: float = 0.0025, + cartesian_fraction_threshold: float = 0.0, + ) -> Optional[JointTrajectory]: + """ + Call plan_async and wait on future + """ + future = self.plan_async( + **{ + key: value + for key, value in locals().items() + if key not in ["self", "cartesian_fraction_threshold"] + } + ) + + if future is None: + return None + + # 100ms sleep + rate = self._node.create_rate(10) + while not future.done(): + rate.sleep() + + return self.get_trajectory( + future, + cartesian=cartesian, + cartesian_fraction_threshold=cartesian_fraction_threshold, + ) + + def plan_async( + self, + pose: Optional[Union[PoseStamped, Pose]] = None, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + joint_positions: Optional[List[float]] = None, + joint_names: Optional[List[str]] = None, + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance_position: float = 0.001, + tolerance_orientation: Union[float, Tuple[float, float, float]] = 0.001, + tolerance_joint_position: float = 0.001, + weight_position: float = 1.0, + weight_orientation: float = 1.0, + weight_joint_position: float = 1.0, + start_joint_state: Optional[Union[JointState, List[float]]] = None, + cartesian: bool = False, + max_step: float = 0.0025, + ) -> Optional[Future]: + """ + Plan motion based on previously set goals. Optional arguments can be passed in to + internally use `set_position_goal()`, `set_orientation_goal()` or `set_joint_goal()` + to define a goal during the call. If no trajectory is found within the timeout + duration, `None` is returned. To plan from the different position than the current + one, optional argument `start_` can be defined. + """ + + pose_stamped = None + if pose is not None: + if isinstance(pose, PoseStamped): + pose_stamped = pose + elif isinstance(pose, Pose): + pose_stamped = PoseStamped( + header=Header( + stamp=self._node.get_clock().now().to_msg(), + frame_id=( + frame_id if frame_id is not None else self.__base_link_name + ), + ), + pose=pose, + ) + + self.set_position_goal( + position=pose_stamped.pose.position, + frame_id=pose_stamped.header.frame_id, + target_link=target_link, + tolerance=tolerance_position, + weight=weight_position, + ) + self.set_orientation_goal( + quat_xyzw=pose_stamped.pose.orientation, + frame_id=pose_stamped.header.frame_id, + target_link=target_link, + tolerance=tolerance_orientation, + weight=weight_orientation, + ) + else: + if position is not None: + if not isinstance(position, Point): + position = Point( + x=float(position[0]), y=float(position[1]), z=float(position[2]) + ) + + self.set_position_goal( + position=position, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance_position, + weight=weight_position, + ) + + if quat_xyzw is not None: + if not isinstance(quat_xyzw, Quaternion): + quat_xyzw = Quaternion( + x=float(quat_xyzw[0]), + y=float(quat_xyzw[1]), + z=float(quat_xyzw[2]), + w=float(quat_xyzw[3]), + ) + + self.set_orientation_goal( + quat_xyzw=quat_xyzw, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance_orientation, + weight=weight_orientation, + ) + + if joint_positions is not None: + self.set_joint_goal( + joint_positions=joint_positions, + joint_names=joint_names, + tolerance=tolerance_joint_position, + weight=weight_joint_position, + ) + + # Define starting state for the plan (default to the current state) + if start_joint_state is not None: + if isinstance(start_joint_state, JointState): + self.__move_action_goal.request.start_state.joint_state = ( + start_joint_state + ) + else: + self.__move_action_goal.request.start_state.joint_state = ( + init_joint_state( + joint_names=self.__joint_names, + joint_positions=start_joint_state, + ) + ) + elif self.joint_state is not None: + self.__move_action_goal.request.start_state.joint_state = self.joint_state + + # Plan trajectory asynchronously by service call + if cartesian: + future = self._plan_cartesian_path( + max_step=max_step, + frame_id=( + pose_stamped.header.frame_id + if pose_stamped is not None + else frame_id + ), + ) + else: + # Use service + future = self._plan_kinematic_path() + + # Clear all previous goal constrains + self.clear_goal_constraints() + self.clear_path_constraints() + + return future + + def get_trajectory( + self, + future: Future, + cartesian: bool = False, + cartesian_fraction_threshold: float = 0.0, + ) -> Optional[JointTrajectory]: + """ + Takes in a future returned by plan_async and returns the trajectory if the future is done + and planning was successful, else None. + + For cartesian plans, the plan is rejected if the fraction of the path that was completed is + less than `cartesian_fraction_threshold`. + """ + if not future.done(): + self._node.get_logger().warn( + "Cannot get trajectory because future is not done." + ) + return None + + res = future.result() + + # Cartesian + if cartesian: + if MoveItErrorCodes.SUCCESS == res.error_code.val: + if res.fraction >= cartesian_fraction_threshold: + return res.solution.joint_trajectory + else: + self._node.get_logger().warn( + f"Planning failed! Cartesian planner completed {res.fraction} " + f"of the trajectory, less than the threshold {cartesian_fraction_threshold}." + ) + return None + else: + self._node.get_logger().warn( + f"Planning failed! Error code: {res.error_code.val}." + ) + return None + + # Else Kinematic + res = res.motion_plan_response + if MoveItErrorCodes.SUCCESS == res.error_code.val: + return res.trajectory.joint_trajectory + else: + self._node.get_logger().warn( + f"Planning failed! Error code: {res.error_code.val}." + ) + return None + + def execute(self, joint_trajectory: JointTrajectory): + """ + Execute joint_trajectory by communicating directly with the controller. + """ + + if self.__ignore_new_calls_while_executing and ( + self.__is_motion_requested or self.__is_executing + ): + self._node.get_logger().warn( + "Controller is already following a trajectory. Skipping motion." + ) + return + + execute_trajectory_goal = init_execute_trajectory_goal( + joint_trajectory=joint_trajectory + ) + + if execute_trajectory_goal is None: + self._node.get_logger().warn( + "Cannot execute motion because the provided/planned trajectory is invalid." + ) + return + + self._send_goal_async_execute_trajectory(goal=execute_trajectory_goal) + + def wait_until_executed(self) -> bool: + """ + Wait until the previously requested motion is finalised through either a success or failure. + """ + + if not self.__is_motion_requested: + self._node.get_logger().warn( + "Cannot wait until motion is executed (no motion is in progress)." + ) + return False + + while self.__is_motion_requested or self.__is_executing: + self.__wait_until_executed_rate.sleep() + + return self.motion_suceeded + + def reset_controller( + self, joint_state: Union[JointState, List[float]], sync: bool = True + ): + """ + Reset controller to a given `joint_state` by sending a dummy joint trajectory. + This is useful for simulated robots that allow instantaneous reset of joints. + """ + + if not isinstance(joint_state, JointState): + joint_state = init_joint_state( + joint_names=self.__joint_names, + joint_positions=joint_state, + ) + joint_trajectory = init_dummy_joint_trajectory_from_state(joint_state) + execute_trajectory_goal = init_execute_trajectory_goal( + joint_trajectory=joint_trajectory + ) + + self._send_goal_async_execute_trajectory( + goal=execute_trajectory_goal, + wait_until_response=sync, + ) + + def set_pose_goal( + self, + pose: Optional[Union[PoseStamped, Pose]] = None, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance_position: float = 0.001, + tolerance_orientation: Union[float, Tuple[float, float, float]] = 0.001, + weight_position: float = 1.0, + weight_orientation: float = 1.0, + ): + """ + This is direct combination of `set_position_goal()` and `set_orientation_goal()`. + """ + + if (pose is None) and (position is None or quat_xyzw is None): + raise ValueError( + "Either `pose` or `position` and `quat_xyzw` must be specified!" + ) + + if isinstance(pose, PoseStamped): + pose_stamped = pose + elif isinstance(pose, Pose): + pose_stamped = PoseStamped( + header=Header( + stamp=self._node.get_clock().now().to_msg(), + frame_id=( + frame_id if frame_id is not None else self.__base_link_name + ), + ), + pose=pose, + ) + else: + if not isinstance(position, Point): + position = Point( + x=float(position[0]), y=float(position[1]), z=float(position[2]) + ) + if not isinstance(quat_xyzw, Quaternion): + quat_xyzw = Quaternion( + x=float(quat_xyzw[0]), + y=float(quat_xyzw[1]), + z=float(quat_xyzw[2]), + w=float(quat_xyzw[3]), + ) + pose_stamped = PoseStamped( + header=Header( + stamp=self._node.get_clock().now().to_msg(), + frame_id=( + frame_id if frame_id is not None else self.__base_link_name + ), + ), + pose=Pose(position=position, orientation=quat_xyzw), + ) + + self.set_position_goal( + position=pose_stamped.pose.position, + frame_id=pose_stamped.header.frame_id, + target_link=target_link, + tolerance=tolerance_position, + weight=weight_position, + ) + self.set_orientation_goal( + quat_xyzw=pose_stamped.pose.orientation, + frame_id=pose_stamped.header.frame_id, + target_link=target_link, + tolerance=tolerance_orientation, + weight=weight_orientation, + ) + + def create_position_constraint( + self, + position: Union[Point, Tuple[float, float, float]], + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance: float = 0.001, + weight: float = 1.0, + ) -> PositionConstraint: + """ + Create Cartesian position constraint of `target_link` with respect to `frame_id`. + - `frame_id` defaults to the base link + - `target_link` defaults to end effector + """ + + # Create new position constraint + constraint = PositionConstraint() + + # Define reference frame and target link + constraint.header.frame_id = ( + frame_id if frame_id is not None else self.__base_link_name + ) + constraint.link_name = ( + target_link if target_link is not None else self.__end_effector_name + ) + + # Define target position + constraint.constraint_region.primitive_poses.append(Pose()) + if isinstance(position, Point): + constraint.constraint_region.primitive_poses[0].position = position + else: + constraint.constraint_region.primitive_poses[0].position.x = float( + position[0] + ) + constraint.constraint_region.primitive_poses[0].position.y = float( + position[1] + ) + constraint.constraint_region.primitive_poses[0].position.z = float( + position[2] + ) + + # Define goal region as a sphere with radius equal to the tolerance + constraint.constraint_region.primitives.append(SolidPrimitive()) + constraint.constraint_region.primitives[0].type = 2 # Sphere + constraint.constraint_region.primitives[0].dimensions = [tolerance] + + # Set weight of the constraint + constraint.weight = weight + + return constraint + + def set_position_goal( + self, + position: Union[Point, Tuple[float, float, float]], + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance: float = 0.001, + weight: float = 1.0, + ): + """ + Set Cartesian position goal of `target_link` with respect to `frame_id`. + - `frame_id` defaults to the base link + - `target_link` defaults to end effector + """ + + constraint = self.create_position_constraint( + position=position, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance, + weight=weight, + ) + + # Append to other constraints + self.__move_action_goal.request.goal_constraints[ + -1 + ].position_constraints.append(constraint) + + def create_orientation_constraint( + self, + quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance: Union[float, Tuple[float, float, float]] = 0.001, + weight: float = 1.0, + parameterization: int = 0, # 0: Euler, 1: Rotation Vector + ) -> OrientationConstraint: + """ + Create a Cartesian orientation constraint of `target_link` with respect to `frame_id`. + - `frame_id` defaults to the base link + - `target_link` defaults to end effector + """ + + # Create new position constraint + constraint = OrientationConstraint() + + # Define reference frame and target link + constraint.header.frame_id = ( + frame_id if frame_id is not None else self.__base_link_name + ) + constraint.link_name = ( + target_link if target_link is not None else self.__end_effector_name + ) + + # Define target orientation + if isinstance(quat_xyzw, Quaternion): + constraint.orientation = quat_xyzw + else: + constraint.orientation.x = float(quat_xyzw[0]) + constraint.orientation.y = float(quat_xyzw[1]) + constraint.orientation.z = float(quat_xyzw[2]) + constraint.orientation.w = float(quat_xyzw[3]) + + # Define tolerances + if type(tolerance) == float: + tolerance_xyz = (tolerance, tolerance, tolerance) + else: + tolerance_xyz = tolerance + constraint.absolute_x_axis_tolerance = tolerance_xyz[0] + constraint.absolute_y_axis_tolerance = tolerance_xyz[1] + constraint.absolute_z_axis_tolerance = tolerance_xyz[2] + + # Define parameterization (how to interpret the tolerance) + constraint.parameterization = parameterization + + # Set weight of the constraint + constraint.weight = weight + + return constraint + + def set_orientation_goal( + self, + quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance: Union[float, Tuple[float, float, float]] = 0.001, + weight: float = 1.0, + parameterization: int = 0, # 0: Euler, 1: Rotation Vector + ): + """ + Set Cartesian orientation goal of `target_link` with respect to `frame_id`. + - `frame_id` defaults to the base link + - `target_link` defaults to end effector + """ + + constraint = self.create_orientation_constraint( + quat_xyzw=quat_xyzw, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance, + weight=weight, + parameterization=parameterization, + ) + + # Append to other constraints + self.__move_action_goal.request.goal_constraints[ + -1 + ].orientation_constraints.append(constraint) + + def create_joint_constraints( + self, + joint_positions: List[float], + joint_names: Optional[List[str]] = None, + tolerance: float = 0.001, + weight: float = 1.0, + ) -> List[JointConstraint]: + """ + Creates joint space constraints. With `joint_names` specified, `joint_positions` can be + defined for specific joints in an arbitrary order. Otherwise, first **n** joints + passed into the constructor is used, where **n** is the length of `joint_positions`. + """ + + constraints = [] + + # Use default joint names if not specified + if joint_names == None: + joint_names = self.__joint_names + + for i in range(len(joint_positions)): + # Create a new constraint for each joint + constraint = JointConstraint() + + # Define joint name + constraint.joint_name = joint_names[i] + + # Define the target joint position + constraint.position = joint_positions[i] + + # Define telerances + constraint.tolerance_above = tolerance + constraint.tolerance_below = tolerance + + # Set weight of the constraint + constraint.weight = weight + + constraints.append(constraint) + + return constraints + + def set_joint_goal( + self, + joint_positions: List[float], + joint_names: Optional[List[str]] = None, + tolerance: float = 0.001, + weight: float = 1.0, + ): + """ + Set joint space goal. With `joint_names` specified, `joint_positions` can be + defined for specific joints in an arbitrary order. Otherwise, first **n** joints + passed into the constructor is used, where **n** is the length of `joint_positions`. + """ + + constraints = self.create_joint_constraints( + joint_positions=joint_positions, + joint_names=joint_names, + tolerance=tolerance, + weight=weight, + ) + + # Append to other constraints + self.__move_action_goal.request.goal_constraints[-1].joint_constraints.extend( + constraints + ) + + def clear_goal_constraints(self): + """ + Clear all goal constraints that were previously set. + Note that this function is called automatically after each `plan_kinematic_path()`. + """ + + self.__move_action_goal.request.goal_constraints = [Constraints()] + + def create_new_goal_constraint(self): + """ + Create a new set of goal constraints that will be set together with the request. Each + subsequent setting of goals with `set_joint_goal()`, `set_pose_goal()` and others will be + added under this newly created set of constraints. + """ + + self.__move_action_goal.request.goal_constraints.append(Constraints()) + + def set_path_joint_constraint( + self, + joint_positions: List[float], + joint_names: Optional[List[str]] = None, + tolerance: float = 0.001, + weight: float = 1.0, + ): + """ + Set joint space path constraints. With `joint_names` specified, `joint_positions` can be + defined for specific joints in an arbitrary order. Otherwise, first **n** joints + passed into the constructor is used, where **n** is the length of `joint_positions`. + """ + + constraints = self.create_joint_constraints( + joint_positions=joint_positions, + joint_names=joint_names, + tolerance=tolerance, + weight=weight, + ) + + # Append to other constraints + self.__move_action_goal.request.path_constraints.joint_constraints.extend( + constraints + ) + + def set_path_position_constraint( + self, + position: Union[Point, Tuple[float, float, float]], + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance: float = 0.001, + weight: float = 1.0, + ): + """ + Set Cartesian position path constraint of `target_link` with respect to `frame_id`. + - `frame_id` defaults to the base link + - `target_link` defaults to end effector + """ + + constraint = self.create_position_constraint( + position=position, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance, + weight=weight, + ) + + # Append to other constraints + self.__move_action_goal.request.path_constraints.position_constraints.append( + constraint + ) + + def set_path_orientation_constraint( + self, + quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance: Union[float, Tuple[float, float, float]] = 0.001, + weight: float = 1.0, + parameterization: int = 0, # 0: Euler Angles, 1: Rotation Vector + ): + """ + Set Cartesian orientation path constraint of `target_link` with respect to `frame_id`. + - `frame_id` defaults to the base link + - `target_link` defaults to end effector + """ + + constraint = self.create_orientation_constraint( + quat_xyzw=quat_xyzw, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance, + weight=weight, + parameterization=parameterization, + ) + + # Append to other constraints + self.__move_action_goal.request.path_constraints.orientation_constraints.append( + constraint + ) + + def clear_path_constraints(self): + """ + Clear all path constraints that were previously set. + Note that this function is called automatically after each `plan_kinematic_path()`. + """ + + self.__move_action_goal.request.path_constraints = Constraints() + + def compute_fk( + self, + joint_state: Optional[Union[JointState, List[float]]] = None, + fk_link_names: Optional[List[str]] = None, + ) -> Optional[Union[PoseStamped, List[PoseStamped]]]: + """ + Call compute_fk_async and wait on future + """ + future = self.compute_fk_async( + **{key: value for key, value in locals().items() if key != "self"} + ) + + if future is None: + return None + + # 100ms sleep + rate = self._node.create_rate(10) + while not future.done(): + rate.sleep() + + return self.get_compute_fk_result(future, fk_link_names=fk_link_names) + + def get_compute_fk_result( + self, + future: Future, + fk_link_names: Optional[List[str]] = None, + ) -> Optional[Union[PoseStamped, List[PoseStamped]]]: + """ + Takes in a future returned by compute_fk_async and returns the poses + if the future is done and successful, else None. + """ + if not future.done(): + self._node.get_logger().warn( + "Cannot get FK result because future is not done." + ) + return None + + res = future.result() + + if MoveItErrorCodes.SUCCESS == res.error_code.val: + if fk_link_names is None: + return res.pose_stamped[0] + else: + return res.pose_stamped + else: + self._node.get_logger().warn( + f"FK computation failed! Error code: {res.error_code.val}." + ) + return None + + def compute_fk_async( + self, + joint_state: Optional[Union[JointState, List[float]]] = None, + fk_link_names: Optional[List[str]] = None, + ) -> Optional[Future]: + """ + Compute forward kinematics for all `fk_link_names` in a given `joint_state`. + - `fk_link_names` defaults to end-effector + - `joint_state` defaults to the current joint state + """ + + if not hasattr(self, "__compute_fk_client"): + self.__init_compute_fk() + + if fk_link_names is None: + self.__compute_fk_req.fk_link_names = [self.__end_effector_name] + else: + self.__compute_fk_req.fk_link_names = fk_link_names + + if joint_state is not None: + if isinstance(joint_state, JointState): + self.__compute_fk_req.robot_state.joint_state = joint_state + else: + self.__compute_fk_req.robot_state.joint_state = init_joint_state( + joint_names=self.__joint_names, + joint_positions=joint_state, + ) + elif self.joint_state is not None: + self.__compute_fk_req.robot_state.joint_state = self.joint_state + + stamp = self._node.get_clock().now().to_msg() + self.__compute_fk_req.header.stamp = stamp + + if not self.__compute_fk_client.service_is_ready(): + self._node.get_logger().warn( + f"Service '{self.__compute_fk_client.srv_name}' is not yet available. Better luck next time!" + ) + return None + + return self.__compute_fk_client.call_async(self.__compute_fk_req) + + def compute_ik( + self, + position: Union[Point, Tuple[float, float, float]], + quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], + start_joint_state: Optional[Union[JointState, List[float]]] = None, + constraints: Optional[Constraints] = None, + wait_for_server_timeout_sec: Optional[float] = 1.0, + ) -> Optional[JointState]: + """ + Call compute_ik_async and wait on future + """ + future = self.compute_ik_async( + **{key: value for key, value in locals().items() if key != "self"} + ) + + if future is None: + return None + + # 10ms sleep + rate = self._node.create_rate(10) + while not future.done(): + rate.sleep() + + return self.get_compute_ik_result(future) + + def get_compute_ik_result( + self, + future: Future, + ) -> Optional[JointState]: + """ + Takes in a future returned by compute_ik_async and returns the joint states + if the future is done and successful, else None. + """ + if not future.done(): + self._node.get_logger().warn( + "Cannot get IK result because future is not done." + ) + return None + + res = future.result() + + if MoveItErrorCodes.SUCCESS == res.error_code.val: + return res.solution.joint_state + else: + self._node.get_logger().warn( + f"IK computation failed! Error code: {res.error_code.val}." + ) + return None + + def compute_ik_async( + self, + position: Union[Point, Tuple[float, float, float]], + quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], + start_joint_state: Optional[Union[JointState, List[float]]] = None, + constraints: Optional[Constraints] = None, + wait_for_server_timeout_sec: Optional[float] = 1.0, + ) -> Optional[Future]: + """ + Compute inverse kinematics for the given pose. To indicate beginning of the search space, + `start_joint_state` can be specified. Furthermore, `constraints` can be imposed on the + computed IK. + - `start_joint_state` defaults to current joint state. + - `constraints` defaults to None. + """ + + if not hasattr(self, "__compute_ik_client"): + self.__init_compute_ik() + + if isinstance(position, Point): + self.__compute_ik_req.ik_request.pose_stamped.pose.position = position + else: + self.__compute_ik_req.ik_request.pose_stamped.pose.position.x = float( + position[0] + ) + self.__compute_ik_req.ik_request.pose_stamped.pose.position.y = float( + position[1] + ) + self.__compute_ik_req.ik_request.pose_stamped.pose.position.z = float( + position[2] + ) + if isinstance(quat_xyzw, Quaternion): + self.__compute_ik_req.ik_request.pose_stamped.pose.orientation = quat_xyzw + else: + self.__compute_ik_req.ik_request.pose_stamped.pose.orientation.x = float( + quat_xyzw[0] + ) + self.__compute_ik_req.ik_request.pose_stamped.pose.orientation.y = float( + quat_xyzw[1] + ) + self.__compute_ik_req.ik_request.pose_stamped.pose.orientation.z = float( + quat_xyzw[2] + ) + self.__compute_ik_req.ik_request.pose_stamped.pose.orientation.w = float( + quat_xyzw[3] + ) + + if start_joint_state is not None: + if isinstance(start_joint_state, JointState): + self.__compute_ik_req.ik_request.robot_state.joint_state = ( + start_joint_state + ) + else: + self.__compute_ik_req.ik_request.robot_state.joint_state = ( + init_joint_state( + joint_names=self.__joint_names, + joint_positions=start_joint_state, + ) + ) + elif self.joint_state is not None: + self.__compute_ik_req.ik_request.robot_state.joint_state = self.joint_state + + if constraints is not None: + self.__compute_ik_req.ik_request.constraints = constraints + + stamp = self._node.get_clock().now().to_msg() + self.__compute_ik_req.ik_request.pose_stamped.header.stamp = stamp + + if not self.__compute_ik_client.wait_for_service( + timeout_sec=wait_for_server_timeout_sec + ): + self._node.get_logger().warn( + f"Service '{self.__compute_ik_client.srv_name}' is not yet available. Better luck next time!" + ) + return None + + return self.__compute_ik_client.call_async(self.__compute_ik_req) + + def reset_new_joint_state_checker(self): + """ + Reset checker of the new joint state. + """ + + self.__joint_state_mutex.acquire() + self.__new_joint_state_available = False + self.__joint_state_mutex.release() + + def force_reset_executing_state(self): + """ + Force reset of internal states that block execution while `ignore_new_calls_while_executing` is being + used. This function is applicable only in a very few edge-cases, so it should almost never be used. + """ + + self.__execution_mutex.acquire() + self.__is_motion_requested = False + self.__is_executing = False + self.__execution_mutex.release() + + def add_collision_primitive( + self, + id: str, + primitive_type: int, + dimensions: Tuple[float, float, float], + pose: Optional[Union[PoseStamped, Pose]] = None, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + frame_id: Optional[str] = None, + operation: int = CollisionObject.ADD, + ): + """ + Add collision object with a primitive geometry specified by its dimensions. + + `primitive_type` can be one of the following: + - `SolidPrimitive.BOX` + - `SolidPrimitive.SPHERE` + - `SolidPrimitive.CYLINDER` + - `SolidPrimitive.CONE` + """ + + if (pose is None) and (position is None or quat_xyzw is None): + raise ValueError( + "Either `pose` or `position` and `quat_xyzw` must be specified!" + ) + + if isinstance(pose, PoseStamped): + pose_stamped = pose + elif isinstance(pose, Pose): + pose_stamped = PoseStamped( + header=Header( + stamp=self._node.get_clock().now().to_msg(), + frame_id=( + frame_id if frame_id is not None else self.__base_link_name + ), + ), + pose=pose, + ) + else: + if not isinstance(position, Point): + position = Point( + x=float(position[0]), y=float(position[1]), z=float(position[2]) + ) + if not isinstance(quat_xyzw, Quaternion): + quat_xyzw = Quaternion( + x=float(quat_xyzw[0]), + y=float(quat_xyzw[1]), + z=float(quat_xyzw[2]), + w=float(quat_xyzw[3]), + ) + pose_stamped = PoseStamped( + header=Header( + stamp=self._node.get_clock().now().to_msg(), + frame_id=( + frame_id if frame_id is not None else self.__base_link_name + ), + ), + pose=Pose(position=position, orientation=quat_xyzw), + ) + + msg = CollisionObject( + header=pose_stamped.header, + id=id, + operation=operation, + pose=pose_stamped.pose, + ) + + msg.primitives.append( + SolidPrimitive(type=primitive_type, dimensions=dimensions) + ) + + self.__collision_object_publisher.publish(msg) + + def add_collision_box( + self, + id: str, + size: Tuple[float, float, float], + pose: Optional[Union[PoseStamped, Pose]] = None, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + frame_id: Optional[str] = None, + operation: int = CollisionObject.ADD, + ): + """ + Add collision object with a box geometry specified by its size. + """ + + assert len(size) == 3, "Invalid size of the box!" + + self.add_collision_primitive( + id=id, + primitive_type=SolidPrimitive.BOX, + dimensions=size, + pose=pose, + position=position, + quat_xyzw=quat_xyzw, + frame_id=frame_id, + operation=operation, + ) + + def add_collision_sphere( + self, + id: str, + radius: float, + pose: Optional[Union[PoseStamped, Pose]] = None, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + frame_id: Optional[str] = None, + operation: int = CollisionObject.ADD, + ): + """ + Add collision object with a sphere geometry specified by its radius. + """ + + if quat_xyzw is None: + quat_xyzw = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + + self.add_collision_primitive( + id=id, + primitive_type=SolidPrimitive.SPHERE, + dimensions=[ + radius, + ], + pose=pose, + position=position, + quat_xyzw=quat_xyzw, + frame_id=frame_id, + operation=operation, + ) + + def add_collision_cylinder( + self, + id: str, + height: float, + radius: float, + pose: Optional[Union[PoseStamped, Pose]] = None, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + frame_id: Optional[str] = None, + operation: int = CollisionObject.ADD, + ): + """ + Add collision object with a cylinder geometry specified by its height and radius. + """ + + self.add_collision_primitive( + id=id, + primitive_type=SolidPrimitive.CYLINDER, + dimensions=[height, radius], + pose=pose, + position=position, + quat_xyzw=quat_xyzw, + frame_id=frame_id, + operation=operation, + ) + + def add_collision_cone( + self, + id: str, + height: float, + radius: float, + pose: Optional[Union[PoseStamped, Pose]] = None, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + frame_id: Optional[str] = None, + operation: int = CollisionObject.ADD, + ): + """ + Add collision object with a cone geometry specified by its height and radius. + """ + + self.add_collision_primitive( + id=id, + primitive_type=SolidPrimitive.CONE, + dimensions=[height, radius], + pose=pose, + position=position, + quat_xyzw=quat_xyzw, + frame_id=frame_id, + operation=operation, + ) + + def add_collision_mesh( + self, + filepath: Optional[str], + id: str, + pose: Optional[Union[PoseStamped, Pose]] = None, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + frame_id: Optional[str] = None, + operation: int = CollisionObject.ADD, + scale: Union[float, Tuple[float, float, float]] = 1.0, + mesh: Optional[Any] = None, + ): + """ + Add collision object with a mesh geometry. Either `filepath` must be + specified or `mesh` must be provided. + Note: This function required 'trimesh' Python module to be installed. + """ + + # Load the mesh + try: + import trimesh + except ImportError as err: + raise ImportError( + "Python module 'trimesh' not found! Please install it manually in order " + "to add collision objects into the MoveIt 2 planning scene." + ) from err + + # Check the parameters + if (pose is None) and (position is None or quat_xyzw is None): + raise ValueError( + "Either `pose` or `position` and `quat_xyzw` must be specified!" + ) + if (filepath is None and mesh is None) or ( + filepath is not None and mesh is not None + ): + raise ValueError("Exactly one of `filepath` or `mesh` must be specified!") + if mesh is not None and not isinstance(mesh, trimesh.Trimesh): + raise ValueError("`mesh` must be an instance of `trimesh.Trimesh`!") + + if isinstance(pose, PoseStamped): + pose_stamped = pose + elif isinstance(pose, Pose): + pose_stamped = PoseStamped( + header=Header( + stamp=self._node.get_clock().now().to_msg(), + frame_id=( + frame_id if frame_id is not None else self.__base_link_name + ), + ), + pose=pose, + ) + else: + if not isinstance(position, Point): + position = Point( + x=float(position[0]), y=float(position[1]), z=float(position[2]) + ) + if not isinstance(quat_xyzw, Quaternion): + quat_xyzw = Quaternion( + x=float(quat_xyzw[0]), + y=float(quat_xyzw[1]), + z=float(quat_xyzw[2]), + w=float(quat_xyzw[3]), + ) + pose_stamped = PoseStamped( + header=Header( + stamp=self._node.get_clock().now().to_msg(), + frame_id=( + frame_id if frame_id is not None else self.__base_link_name + ), + ), + pose=Pose(position=position, orientation=quat_xyzw), + ) + + msg = CollisionObject( + header=pose_stamped.header, + id=id, + operation=operation, + pose=pose_stamped.pose, + ) + + if filepath is not None: + mesh = trimesh.load(filepath) + + # Scale the mesh + if isinstance(scale, float): + scale = (scale, scale, scale) + if not (scale[0] == scale[1] == scale[2] == 1.0): + # If the mesh was passed in as a parameter, make a copy of it to + # avoid transforming the original. + if filepath is not None: + mesh = mesh.copy() + # Transform the mesh + transform = np.eye(4) + np.fill_diagonal(transform, scale) + mesh.apply_transform(transform) + + msg.meshes.append( + Mesh( + triangles=[MeshTriangle(vertex_indices=face) for face in mesh.faces], + vertices=[ + Point(x=vert[0], y=vert[1], z=vert[2]) for vert in mesh.vertices + ], + ) + ) + + self.__collision_object_publisher.publish(msg) + + def remove_collision_object(self, id: str): + """ + Remove collision object specified by its `id`. + """ + + msg = CollisionObject() + msg.id = id + msg.operation = CollisionObject.REMOVE + msg.header.stamp = self._node.get_clock().now().to_msg() + self.__collision_object_publisher.publish(msg) + + def remove_collision_mesh(self, id: str): + """ + Remove collision mesh specified by its `id`. + Identical to `remove_collision_object()`. + """ + + self.remove_collision_object(id) + + def attach_collision_object( + self, + id: str, + link_name: Optional[str] = None, + touch_links: List[str] = [], + weight: float = 0.0, + ): + """ + Attach collision object to the robot. + """ + + if link_name is None: + link_name = self.__end_effector_name + + msg = AttachedCollisionObject( + object=CollisionObject(id=id, operation=CollisionObject.ADD) + ) + msg.link_name = link_name + msg.touch_links = touch_links + msg.weight = weight + + self.__attached_collision_object_publisher.publish(msg) + + def detach_collision_object(self, id: int): + """ + Detach collision object from the robot. + """ + + msg = AttachedCollisionObject( + object=CollisionObject(id=id, operation=CollisionObject.REMOVE) + ) + self.__attached_collision_object_publisher.publish(msg) + + def detach_all_collision_objects(self): + """ + Detach collision object from the robot. + """ + + msg = AttachedCollisionObject( + object=CollisionObject(operation=CollisionObject.REMOVE) + ) + self.__attached_collision_object_publisher.publish(msg) + + def move_collision( + self, + id: str, + position: Union[Point, Tuple[float, float, float]], + quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], + frame_id: Optional[str] = None, + ): + """ + Move collision object specified by its `id`. + """ + + msg = CollisionObject() + + if not isinstance(position, Point): + position = Point( + x=float(position[0]), y=float(position[1]), z=float(position[2]) + ) + if not isinstance(quat_xyzw, Quaternion): + quat_xyzw = Quaternion( + x=float(quat_xyzw[0]), + y=float(quat_xyzw[1]), + z=float(quat_xyzw[2]), + w=float(quat_xyzw[3]), + ) + + pose = Pose() + pose.position = position + pose.orientation = quat_xyzw + msg.pose = pose + msg.id = id + msg.operation = CollisionObject.MOVE + msg.header.frame_id = ( + frame_id if frame_id is not None else self.__base_link_name + ) + msg.header.stamp = self._node.get_clock().now().to_msg() + + self.__collision_object_publisher.publish(msg) + + def update_planning_scene(self) -> bool: + """ + Gets the current planning scene. Returns whether the service call was + successful. + """ + + if not self._get_planning_scene_service.service_is_ready(): + self._node.get_logger().warn( + f"Service '{self._get_planning_scene_service.srv_name}' is not yet available. Better luck next time!" + ) + return False + self.__planning_scene = self._get_planning_scene_service.call( + GetPlanningScene.Request() + ).scene + return True + + def allow_collisions(self, id: str, allow: bool) -> Optional[Future]: + """ + Takes in the ID of an element in the planning scene. Modifies the allowed + collision matrix to (dis)allow collisions between that object and all other + object. + + If `allow` is True, a plan will succeed even if the robot collides with that object. + If `allow` is False, a plan will fail if the robot collides with that object. + Returns whether it successfully updated the allowed collision matrix. + + Returns the future of the service call. + """ + # Update the planning scene + if not self.update_planning_scene(): + return None + allowed_collision_matrix = self.__planning_scene.allowed_collision_matrix + self.__old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix) + + # Get the location in the allowed collision matrix of the object + j = None + if id not in allowed_collision_matrix.entry_names: + allowed_collision_matrix.entry_names.append(id) + else: + j = allowed_collision_matrix.entry_names.index(id) + # For all other objects, (dis)allow collisions with the object with `id` + for i in range(len(allowed_collision_matrix.entry_values)): + if j is None: + allowed_collision_matrix.entry_values[i].enabled.append(allow) + elif i != j: + allowed_collision_matrix.entry_values[i].enabled[j] = allow + # For the object with `id`, (dis)allow collisions with all other objects + allowed_collision_entry = AllowedCollisionEntry( + enabled=[allow for _ in range(len(allowed_collision_matrix.entry_names))] + ) + if j is None: + allowed_collision_matrix.entry_values.append(allowed_collision_entry) + else: + allowed_collision_matrix.entry_values[j] = allowed_collision_entry + + # Apply the new planning scene + if not self._apply_planning_scene_service.service_is_ready(): + self._node.get_logger().warn( + f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!" + ) + return None + return self._apply_planning_scene_service.call_async( + ApplyPlanningScene.Request(scene=self.__planning_scene) + ) + + def process_allow_collision_future(self, future: Future) -> bool: + """ + Return whether the allow collision service call is done and has succeeded + or not. If it failed, reset the allowed collision matrix to the old one. + """ + if not future.done(): + return False + + # Get response + resp = future.result() + + # If it failed, restore the old planning scene + if not resp.success: + self.__planning_scene.allowed_collision_matrix = ( + self.__old_allowed_collision_matrix + ) + + return resp.success + + def clear_all_collision_objects(self) -> Optional[Future]: + """ + Removes all attached and un-attached collision objects from the planning scene. + + Returns a future for the ApplyPlanningScene service call. + """ + # Update the planning scene + if not self.update_planning_scene(): + return None + self.__old_planning_scene = copy.deepcopy(self.__planning_scene) + + # Remove all collision objects from the planning scene + self.__planning_scene.world.collision_objects = [] + self.__planning_scene.robot_state.attached_collision_objects = [] + + # Apply the new planning scene + if not self._apply_planning_scene_service.service_is_ready(): + self._node.get_logger().warn( + f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!" + ) + return None + return self._apply_planning_scene_service.call_async( + ApplyPlanningScene.Request(scene=self.__planning_scene) + ) + + def cancel_clear_all_collision_objects_future(self, future: Future): + """ + Cancel the clear all collision objects service call. + """ + self._apply_planning_scene_service.remove_pending_request(future) + + def process_clear_all_collision_objects_future(self, future: Future) -> bool: + """ + Return whether the clear all collision objects service call is done and has succeeded + or not. If it failed, restore the old planning scene. + """ + if not future.done(): + return False + + # Get response + resp = future.result() + + # If it failed, restore the old planning scene + if not resp.success: + self.__planning_scene = self.__old_planning_scene + + return resp.success + + def __joint_state_callback(self, msg: JointState): + # Update only if all relevant joints are included in the message + for joint_name in self.joint_names: + if not joint_name in msg.name: + return + + self.__joint_state_mutex.acquire() + self.__joint_state = msg + self.__new_joint_state_available = True + self.__joint_state_mutex.release() + + def _plan_kinematic_path(self) -> Optional[Future]: + # Reuse request from move action goal + self.__kinematic_path_request.motion_plan_request = ( + self.__move_action_goal.request + ) + + stamp = self._node.get_clock().now().to_msg() + self.__kinematic_path_request.motion_plan_request.workspace_parameters.header.stamp = ( + stamp + ) + for ( + constraints + ) in self.__kinematic_path_request.motion_plan_request.goal_constraints: + for position_constraint in constraints.position_constraints: + position_constraint.header.stamp = stamp + for orientation_constraint in constraints.orientation_constraints: + orientation_constraint.header.stamp = stamp + + if not self._plan_kinematic_path_service.service_is_ready(): + self._node.get_logger().warn( + f"Service '{self._plan_kinematic_path_service.srv_name}' is not yet available. Better luck next time!" + ) + return None + + return self._plan_kinematic_path_service.call_async( + self.__kinematic_path_request + ) + + def _plan_cartesian_path( + self, + max_step: float = 0.0025, + frame_id: Optional[str] = None, + ) -> Optional[Future]: + # Reuse request from move action goal + self.__cartesian_path_request.start_state = ( + self.__move_action_goal.request.start_state + ) + + # The below attributes were introduced in Iron and do not exist in Humble. + if hasattr(self.__cartesian_path_request, "max_velocity_scaling_factor"): + self.__cartesian_path_request.max_velocity_scaling_factor = ( + self.__move_action_goal.request.max_velocity_scaling_factor + ) + if hasattr(self.__cartesian_path_request, "max_acceleration_scaling_factor"): + self.__cartesian_path_request.max_acceleration_scaling_factor = ( + self.__move_action_goal.request.max_acceleration_scaling_factor + ) + + self.__cartesian_path_request.group_name = ( + self.__move_action_goal.request.group_name + ) + self.__cartesian_path_request.link_name = self.__end_effector_name + self.__cartesian_path_request.max_step = max_step + + self.__cartesian_path_request.header.frame_id = ( + frame_id if frame_id is not None else self.__base_link_name + ) + + stamp = self._node.get_clock().now().to_msg() + self.__cartesian_path_request.header.stamp = stamp + + self.__cartesian_path_request.path_constraints = ( + self.__move_action_goal.request.path_constraints + ) + for ( + position_constraint + ) in self.__cartesian_path_request.path_constraints.position_constraints: + position_constraint.header.stamp = stamp + for ( + orientation_constraint + ) in self.__cartesian_path_request.path_constraints.orientation_constraints: + orientation_constraint.header.stamp = stamp + # no header in joint_constraint message type + + target_pose = Pose() + target_pose.position = ( + self.__move_action_goal.request.goal_constraints[-1] + .position_constraints[-1] + .constraint_region.primitive_poses[0] + .position + ) + target_pose.orientation = ( + self.__move_action_goal.request.goal_constraints[-1] + .orientation_constraints[-1] + .orientation + ) + + self.__cartesian_path_request.waypoints = [target_pose] + + if not self._plan_cartesian_path_service.service_is_ready(): + self._node.get_logger().warn( + f"Service '{self._plan_cartesian_path_service.srv_name}' is not yet available. Better luck next time!" + ) + return None + + return self._plan_cartesian_path_service.call_async( + self.__cartesian_path_request + ) + + def _send_goal_async_move_action(self): + self.__execution_mutex.acquire() + stamp = self._node.get_clock().now().to_msg() + self.__move_action_goal.request.workspace_parameters.header.stamp = stamp + if not self.__move_action_client.server_is_ready(): + self._node.get_logger().warn( + f"Action server '{self.__move_action_client._action_name}' is not yet available. Better luck next time!" + ) + return + + self.__last_error_code = None + self.__is_motion_requested = True + self.__send_goal_future_move_action = self.__move_action_client.send_goal_async( + goal=self.__move_action_goal, + feedback_callback=None, + ) + + self.__send_goal_future_move_action.add_done_callback( + self.__response_callback_move_action + ) + + self.__execution_mutex.release() + + def __response_callback_move_action(self, response): + self.__execution_mutex.acquire() + goal_handle = response.result() + if not goal_handle.accepted: + self._node.get_logger().warn( + f"Action '{self.__move_action_client._action_name}' was rejected." + ) + self.__is_motion_requested = False + return + + self.__execution_goal_handle = goal_handle + self.__is_executing = True + self.__is_motion_requested = False + + self.__get_result_future_move_action = goal_handle.get_result_async() + self.__get_result_future_move_action.add_done_callback( + self.__result_callback_move_action + ) + self.__execution_mutex.release() + + def __result_callback_move_action(self, res): + self.__execution_mutex.acquire() + if res.result().status != GoalStatus.STATUS_SUCCEEDED: + self._node.get_logger().warn( + f"Action '{self.__move_action_client._action_name}' was unsuccessful: {res.result().status}." + ) + self.motion_suceeded = False + else: + self.motion_suceeded = True + + self.__last_error_code = res.result().result.error_code + + self.__execution_goal_handle = None + self.__is_executing = False + self.__execution_mutex.release() + + def _send_goal_async_execute_trajectory( + self, + goal: ExecuteTrajectory, + wait_until_response: bool = False, + ): + self.__execution_mutex.acquire() + + if not self._execute_trajectory_action_client.server_is_ready(): + self._node.get_logger().warn( + f"Action server '{self._execute_trajectory_action_client._action_name}' is not yet available. Better luck next time!" + ) + return + + self.__last_error_code = None + self.__is_motion_requested = True + self.__send_goal_future_execute_trajectory = ( + self._execute_trajectory_action_client.send_goal_async( + goal=goal, + feedback_callback=None, + ) + ) + + self.__send_goal_future_execute_trajectory.add_done_callback( + self.__response_callback_execute_trajectory + ) + self.__execution_mutex.release() + + def __response_callback_execute_trajectory(self, response): + self.__execution_mutex.acquire() + goal_handle = response.result() + if not goal_handle.accepted: + self._node.get_logger().warn( + f"Action '{self._execute_trajectory_action_client._action_name}' was rejected." + ) + self.__is_motion_requested = False + return + + self.__execution_goal_handle = goal_handle + self.__is_executing = True + self.__is_motion_requested = False + + self.__get_result_future_execute_trajectory = goal_handle.get_result_async() + self.__get_result_future_execute_trajectory.add_done_callback( + self.__result_callback_execute_trajectory + ) + self.__execution_mutex.release() + + def __response_callback_with_event_set_execute_trajectory(self, response): + self.__future_done_event.set() + + def __result_callback_execute_trajectory(self, res): + self.__execution_mutex.acquire() + if res.result().status != GoalStatus.STATUS_SUCCEEDED: + self._node.get_logger().warn( + f"Action '{self._execute_trajectory_action_client._action_name}' was unsuccessful: {res.result().status}." + ) + self.motion_suceeded = False + else: + self.motion_suceeded = True + + self.__last_error_code = res.result().result.error_code + + self.__execution_goal_handle = None + self.__is_executing = False + self.__execution_mutex.release() + + @classmethod + def __init_move_action_goal( + cls, frame_id: str, group_name: str, end_effector: str + ) -> MoveGroup.Goal: + move_action_goal = MoveGroup.Goal() + move_action_goal.request.workspace_parameters.header.frame_id = frame_id + # move_action_goal.request.workspace_parameters.header.stamp = "Set during request" + move_action_goal.request.workspace_parameters.min_corner.x = -1.0 + move_action_goal.request.workspace_parameters.min_corner.y = -1.0 + move_action_goal.request.workspace_parameters.min_corner.z = -1.0 + move_action_goal.request.workspace_parameters.max_corner.x = 1.0 + move_action_goal.request.workspace_parameters.max_corner.y = 1.0 + move_action_goal.request.workspace_parameters.max_corner.z = 1.0 + # move_action_goal.request.start_state = "Set during request" + move_action_goal.request.goal_constraints = [Constraints()] + move_action_goal.request.path_constraints = Constraints() + # move_action_goal.request.trajectory_constraints = "Ignored" + # move_action_goal.request.reference_trajectories = "Ignored" + move_action_goal.request.pipeline_id = "" + move_action_goal.request.planner_id = "" + move_action_goal.request.group_name = group_name + move_action_goal.request.num_planning_attempts = 5 + move_action_goal.request.allowed_planning_time = 0.5 + move_action_goal.request.max_velocity_scaling_factor = 0.0 + move_action_goal.request.max_acceleration_scaling_factor = 0.0 + # Note: Attribute was renamed in Iron (https://github.com/ros-planning/moveit_msgs/pull/130) + if hasattr(move_action_goal.request, "cartesian_speed_limited_link"): + move_action_goal.request.cartesian_speed_limited_link = end_effector + else: + move_action_goal.request.cartesian_speed_end_effector_link = end_effector + move_action_goal.request.max_cartesian_speed = 0.0 + + # move_action_goal.planning_options.planning_scene_diff = "Ignored" + move_action_goal.planning_options.plan_only = False + # move_action_goal.planning_options.look_around = "Ignored" + # move_action_goal.planning_options.look_around_attempts = "Ignored" + # move_action_goal.planning_options.max_safe_execution_cost = "Ignored" + # move_action_goal.planning_options.replan = "Ignored" + # move_action_goal.planning_options.replan_attempts = "Ignored" + # move_action_goal.planning_options.replan_delay = "Ignored" + + return move_action_goal + + def __init_compute_fk(self): + self.__compute_fk_client = self._node.create_client( + srv_type=GetPositionFK, + srv_name="compute_fk", + callback_group=self._callback_group, + ) + + self.__compute_fk_req = GetPositionFK.Request() + self.__compute_fk_req.header.frame_id = self.__base_link_name + # self.__compute_fk_req.header.stamp = "Set during request" + # self.__compute_fk_req.fk_link_names = "Set during request" + # self.__compute_fk_req.robot_state.joint_state = "Set during request" + # self.__compute_fk_req.robot_state.multi_dof_ = "Ignored" + # self.__compute_fk_req.robot_state.attached_collision_objects = "Ignored" + self.__compute_fk_req.robot_state.is_diff = False + + def __init_compute_ik(self): + # Service client for IK + self.__compute_ik_client = self._node.create_client( + srv_type=GetPositionIK, + srv_name="compute_ik", + callback_group=self._callback_group, + ) + + self.__compute_ik_req = GetPositionIK.Request() + self.__compute_ik_req.ik_request.group_name = self.__group_name + # self.__compute_ik_req.ik_request.robot_state.joint_state = "Set during request" + # self.__compute_ik_req.ik_request.robot_state.multi_dof_ = "Ignored" + # self.__compute_ik_req.ik_request.robot_state.attached_collision_objects = "Ignored" + self.__compute_ik_req.ik_request.robot_state.is_diff = False + # self.__compute_ik_req.ik_request.constraints = "Set during request OR Ignored" + self.__compute_ik_req.ik_request.avoid_collisions = True + # self.__compute_ik_req.ik_request.ik_link_name = "Ignored" + self.__compute_ik_req.ik_request.pose_stamped.header.frame_id = ( + self.__base_link_name + ) + # self.__compute_ik_req.ik_request.pose_stamped.header.stamp = "Set during request" + # self.__compute_ik_req.ik_request.pose_stamped.pose = "Set during request" + # self.__compute_ik_req.ik_request.ik_link_names = "Ignored" + # self.__compute_ik_req.ik_request.pose_stamped_vector = "Ignored" + # self.__compute_ik_req.ik_request.timeout.sec = "Ignored" + # self.__compute_ik_req.ik_request.timeout.nanosec = "Ignored" + + @property + def planning_scene(self) -> Optional[PlanningScene]: + return self.__planning_scene + + @property + def follow_joint_trajectory_action_client(self) -> str: + return self.__follow_joint_trajectory_action_client + + @property + def end_effector_name(self) -> str: + return self.__end_effector_name + + @property + def base_link_name(self) -> str: + return self.__base_link_name + + @property + def joint_names(self) -> List[str]: + return self.__joint_names + + @property + def joint_state(self) -> Optional[JointState]: + self.__joint_state_mutex.acquire() + joint_state = self.__joint_state + self.__joint_state_mutex.release() + return joint_state + + @property + def new_joint_state_available(self): + return self.__new_joint_state_available + + @property + def max_velocity(self) -> float: + return self.__move_action_goal.request.max_velocity_scaling_factor + + @max_velocity.setter + def max_velocity(self, value: float): + self.__move_action_goal.request.max_velocity_scaling_factor = value + + @property + def max_acceleration(self) -> float: + return self.__move_action_goal.request.max_acceleration_scaling_factor + + @max_acceleration.setter + def max_acceleration(self, value: float): + self.__move_action_goal.request.max_acceleration_scaling_factor = value + + @property + def num_planning_attempts(self) -> int: + return self.__move_action_goal.request.num_planning_attempts + + @num_planning_attempts.setter + def num_planning_attempts(self, value: int): + self.__move_action_goal.request.num_planning_attempts = value + + @property + def allowed_planning_time(self) -> float: + return self.__move_action_goal.request.allowed_planning_time + + @allowed_planning_time.setter + def allowed_planning_time(self, value: float): + self.__move_action_goal.request.allowed_planning_time = value + + @property + def cartesian_avoid_collisions(self) -> bool: + return self.__cartesian_path_request.request.avoid_collisions + + @cartesian_avoid_collisions.setter + def cartesian_avoid_collisions(self, value: bool): + self.__cartesian_path_request.avoid_collisions = value + + @property + def cartesian_jump_threshold(self) -> float: + return self.__cartesian_path_request.request.jump_threshold + + @cartesian_jump_threshold.setter + def cartesian_jump_threshold(self, value: float): + self.__cartesian_path_request.jump_threshold = value + + @property + def cartesian_prismatic_jump_threshold(self) -> float: + return self.__cartesian_path_request.request.prismatic_jump_threshold + + @cartesian_prismatic_jump_threshold.setter + def cartesian_prismatic_jump_threshold(self, value: float): + self.__cartesian_path_request.prismatic_jump_threshold = value + + @property + def cartesian_revolute_jump_threshold(self) -> float: + return self.__cartesian_path_request.request.revolute_jump_threshold + + @cartesian_revolute_jump_threshold.setter + def cartesian_revolute_jump_threshold(self, value: float): + self.__cartesian_path_request.revolute_jump_threshold = value + + @property + def pipeline_id(self) -> int: + return self.__move_action_goal.request.pipeline_id + + @pipeline_id.setter + def pipeline_id(self, value: str): + self.__move_action_goal.request.pipeline_id = value + + @property + def planner_id(self) -> int: + return self.__move_action_goal.request.planner_id + + @planner_id.setter + def planner_id(self, value: str): + self.__move_action_goal.request.planner_id = value + + +def init_joint_state( + joint_names: List[str], + joint_positions: Optional[List[str]] = None, + joint_velocities: Optional[List[str]] = None, + joint_effort: Optional[List[str]] = None, +) -> JointState: + joint_state = JointState() + + joint_state.name = joint_names + joint_state.position = ( + joint_positions if joint_positions is not None else [0.0] * len(joint_names) + ) + joint_state.velocity = ( + joint_velocities if joint_velocities is not None else [0.0] * len(joint_names) + ) + joint_state.effort = ( + joint_effort if joint_effort is not None else [0.0] * len(joint_names) + ) + + return joint_state + + +def init_execute_trajectory_goal( + joint_trajectory: JointTrajectory, +) -> Optional[ExecuteTrajectory.Goal]: + if joint_trajectory is None: + return None + + execute_trajectory_goal = ExecuteTrajectory.Goal() + + execute_trajectory_goal.trajectory.joint_trajectory = joint_trajectory + + return execute_trajectory_goal + + +def init_dummy_joint_trajectory_from_state( + joint_state: JointState, duration_sec: int = 0, duration_nanosec: int = 0 +) -> JointTrajectory: + joint_trajectory = JointTrajectory() + joint_trajectory.joint_names = joint_state.name + + point = JointTrajectoryPoint() + point.positions = joint_state.position + point.velocities = joint_state.velocity + point.accelerations = [0.0] * len(joint_trajectory.joint_names) + point.effort = joint_state.effort + point.time_from_start.sec = duration_sec + point.time_from_start.nanosec = duration_nanosec + joint_trajectory.points.append(point) + + return joint_trajectory diff --git a/dependencies/py_moveit2/py_moveit2/moveit2_gripper.py b/dependencies/py_moveit2/py_moveit2/moveit2_gripper.py new file mode 100644 index 00000000..35fcdc1c --- /dev/null +++ b/dependencies/py_moveit2/py_moveit2/moveit2_gripper.py @@ -0,0 +1,251 @@ +import math +from typing import List, Optional + +from rclpy.callback_groups import CallbackGroup +from rclpy.node import Node + +from .moveit2 import * + + +class MoveIt2Gripper(MoveIt2): + """ + Python interface for MoveIt 2 Gripper that is controlled by JointTrajectoryController. + This implementation builds on MoveIt2 to reuse code (while keeping MoveIt2 standalone). + """ + + def __init__( + self, + node: Node, + gripper_joint_names: List[str], + open_gripper_joint_positions: List[float], + closed_gripper_joint_positions: List[float], + gripper_group_name: str = "gripper", + execute_via_moveit: bool = False, + ignore_new_calls_while_executing: bool = False, + skip_planning: bool = False, + skip_planning_fixed_motion_duration: float = 0.5, + callback_group: Optional[CallbackGroup] = None, + follow_joint_trajectory_action_name: str = "DEPRECATED", + use_move_group_action: bool = False, + ): + """ + Construct an instance of `MoveIt2Gripper` interface. + - `node` - ROS 2 node that this interface is attached to + - `gripper_joint_names` - List of gripper joint names (can be extracted from URDF) + - `open_gripper_joint_positions` - Configuration of gripper joints when open + - `closed_gripper_joint_positions` - Configuration of gripper joints when fully closed + - `gripper_group_name` - Name of the planning group for robot gripper + - [DEPRECATED] `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) + FollowJointTrajectory action (controller) is employed otherwise + together with a separate planning service client + - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories + while previous is still being executed + - `skip_planning` - If enabled, planning is skipped and a single joint trajectory point is published + for closing or opening. This enables much faster operation, but the collision + checking is disabled and the motion smoothness will depend on the controller. + - `skip_planning_fixed_motion_duration` - Desired duration for the closing and opening motions when + `skip_planning` mode is enabled. + - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) + - [DEPRECATED] `follow_joint_trajectory_action_name` - Name of the action server for the controller + - `use_move_group_action` - Flag that enables execution via MoveGroup action (MoveIt 2) + ExecuteTrajectory action is employed otherwise + together with a separate planning service client + """ + + # Check for deprecated parameters + if execute_via_moveit: + node.get_logger().warn( + "Parameter `execute_via_moveit` is deprecated. Please use `use_move_group_action` instead." + ) + use_move_group_action = True + if follow_joint_trajectory_action_name != "DEPRECATED": + node.get_logger().warn( + "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead." + ) + + super().__init__( + node=node, + joint_names=gripper_joint_names, + base_link_name="", + end_effector_name="", + group_name=gripper_group_name, + ignore_new_calls_while_executing=ignore_new_calls_while_executing, + callback_group=callback_group, + use_move_group_action=use_move_group_action, + ) + self.__del_redundant_attributes() + + assert ( + len(gripper_joint_names) + == len(open_gripper_joint_positions) + == len(closed_gripper_joint_positions) + ) + self.__open_gripper_joint_positions = open_gripper_joint_positions + self.__closed_gripper_joint_positions = closed_gripper_joint_positions + + self.__skip_planning = skip_planning + if skip_planning: + duration_sec = math.floor(skip_planning_fixed_motion_duration) + duration_nanosec = int( + 10e8 * (skip_planning_fixed_motion_duration - duration_sec) + ) + self.__open_dummy_trajectory_goal = init_follow_joint_trajectory_goal( + init_dummy_joint_trajectory_from_state( + init_joint_state( + joint_names=gripper_joint_names, + joint_positions=open_gripper_joint_positions, + ), + duration_sec=duration_sec, + duration_nanosec=duration_nanosec, + ) + ) + self.__close_dummy_trajectory_goal = init_follow_joint_trajectory_goal( + init_dummy_joint_trajectory_from_state( + init_joint_state( + joint_names=gripper_joint_names, + joint_positions=closed_gripper_joint_positions, + ), + duration_sec=duration_sec, + duration_nanosec=duration_nanosec, + ) + ) + + # Tolerance used for checking whether the gripper is open or closed + self.__open_tolerance = [ + 0.1 + * abs(open_gripper_joint_positions[i] - closed_gripper_joint_positions[i]) + for i in range(len(gripper_joint_names)) + ] + # Indices of gripper joint within the joint state message topic. + # It is assumed that the order of these does not change during execution. + self.__gripper_joint_indices: Optional[List[int]] = None + + def __call__(self): + """ + Callable that is identical to `MoveIt2Gripper.toggle()`. + """ + + self.toggle() + + def toggle(self): + """ + Toggles the gripper between open and closed state. + """ + + if self.is_open: + self.close(skip_if_noop=False) + else: + self.open(skip_if_noop=False) + + def open(self, skip_if_noop: bool = False): + """ + Open the gripper. + - `skip_if_noop` - No action will be performed if the gripper is already open. + """ + + if skip_if_noop and self.is_open: + return + + if self.__skip_planning: + self.__open_without_planning() + else: + self.move_to_configuration( + joint_positions=self.__open_gripper_joint_positions + ) + + def close(self, skip_if_noop: bool = False): + """ + Close the gripper. + - `skip_if_noop` - No action will be performed if the gripper is not open. + """ + + if skip_if_noop and self.is_closed: + return + + if self.__skip_planning: + self.__close_without_planning() + else: + self.move_to_configuration( + joint_positions=self.__closed_gripper_joint_positions + ) + + def reset_open(self, sync: bool = True): + """ + Reset into open configuration by sending a dummy joint trajectory. + This is useful for simulated robots that allow instantaneous reset of joints. + """ + + self.reset_controller( + joint_state=self.__open_gripper_joint_positions, sync=sync + ) + + def reset_closed(self, sync: bool = True): + """ + Reset into closed configuration by sending a dummy joint trajectory. + This is useful for simulated robots that allow instantaneous reset of joints. + """ + + self.reset_controller( + joint_state=self.__closed_gripper_joint_positions, sync=sync + ) + + def __open_without_planning(self): + self._send_goal_async_follow_joint_trajectory( + goal=self.__open_dummy_trajectory_goal, + wait_until_response=False, + ) + + def __close_without_planning(self): + self._send_goal_async_follow_joint_trajectory( + goal=self.__close_dummy_trajectory_goal, + wait_until_response=False, + ) + + def __del_redundant_attributes(self): + self.move_to_pose = None + self.set_pose_goal = None + self.set_position_goal = None + self.set_orientation_goal = None + self.compute_fk = None + self.compute_ik = None + + @property + def is_open(self) -> bool: + """ + Gripper is considered to be open if all of the joints are at their open position. + """ + + joint_state = self.joint_state + + # Assume the gripper is open if there are no joint state readings yet + if joint_state is None: + return True + + # For the sake of performance, find the indices of joints only once. + # This is especially useful for robots with many joints. + if self.__gripper_joint_indices is None: + self.__gripper_joint_indices: List[int] = [] + for joint_name in self.joint_names: + self.__gripper_joint_indices.append(joint_state.name.index(joint_name)) + + for local_joint_index, joint_state_index in enumerate( + self.__gripper_joint_indices + ): + if ( + abs( + joint_state.position[joint_state_index] + - self.__open_gripper_joint_positions[local_joint_index] + ) + > self.__open_tolerance[local_joint_index] + ): + return False + + return True + + @property + def is_closed(self) -> bool: + """ + Gripper is considered to be closed if any of the joints is outside of their open position. + """ + + return not self.is_open diff --git a/dependencies/py_moveit2/py_moveit2/moveit2_servo.py b/dependencies/py_moveit2/py_moveit2/moveit2_servo.py new file mode 100644 index 00000000..340db14d --- /dev/null +++ b/dependencies/py_moveit2/py_moveit2/moveit2_servo.py @@ -0,0 +1,247 @@ +from copy import deepcopy +from typing import Optional, Tuple + +from geometry_msgs.msg import TwistStamped +from rclpy.callback_groups import CallbackGroup +from rclpy.node import Node +from rclpy.qos import ( + QoSDurabilityPolicy, + QoSHistoryPolicy, + QoSProfile, + QoSReliabilityPolicy, +) +from rclpy.task import Future +from std_srvs.srv import Trigger + + +class MoveIt2Servo: + """ + Python interface for MoveIt 2 Servo that enables real-time control in Cartesian Space. + This implementation is just a thin wrapper around TwistStamped message publisher. + """ + + def __init__( + self, + node: Node, + frame_id: str, + linear_speed: float = 1.0, + angular_speed: float = 1.0, + enable_at_init: bool = True, + callback_group: Optional[CallbackGroup] = None, + ): + """ + Construct an instance of `MoveIt2Servo` interface. + - `node` - ROS 2 node that this interface is attached to + - `frame_id` - Reference frame in which to publish command messages + - `linear_speed` - Factor that can be used to scale all input linear twist commands + - `angular_speed` - Factor that can be used to scale all input angular twist commands + - `enable_at_init` - Flag that enables initialisation of MoveIt 2 Servo during initialisation + Otherwise, `MoveIt2Servo.enable()` must be called explicitly + - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) + """ + + self._node = node + + # Create publisher + self.__twist_pub = self._node.create_publisher( + msg_type=TwistStamped, + topic="delta_twist_cmds", + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_ALL, + ), + callback_group=callback_group, + ) + + # Create service clients + self.__start_service = self._node.create_client( + srv_type=Trigger, + srv_name="/servo_node/start_servo", + callback_group=callback_group, + ) + self.__stop_service = self._node.create_client( + srv_type=Trigger, + srv_name="/servo_node/stop_servo", + callback_group=callback_group, + ) + self.__trigger_req = Trigger.Request() + self.__is_enabled = False + + # Initialize message based on passed arguments + self.__twist_msg = TwistStamped() + self.__twist_msg.header.frame_id = frame_id + self.__twist_msg.twist.linear.x = linear_speed + self.__twist_msg.twist.linear.y = linear_speed + self.__twist_msg.twist.linear.z = linear_speed + self.__twist_msg.twist.angular.x = angular_speed + self.__twist_msg.twist.angular.y = angular_speed + self.__twist_msg.twist.angular.z = angular_speed + + # Enable servo immediately, if desired + if enable_at_init: + self.enable() + + def __del__(self): + """ + Try to stop MoveIt 2 Servo during destruction. + """ + + try: + if self.is_enabled: + self.__stop_service.call_async(self.__trigger_req) + except: + pass + + def __call__( + self, + linear: Tuple[float, float, float] = (0.0, 0.0, 0.0), + angular: Tuple[float, float, float] = (0.0, 0.0, 0.0), + ): + """ + Callable that is identical to `MoveIt2Servo.servo()`. + """ + + self.servo(linear=linear, angular=angular) + + def servo( + self, + linear: Tuple[float, float, float] = (0.0, 0.0, 0.0), + angular: Tuple[float, float, float] = (0.0, 0.0, 0.0), + enable_if_disabled: bool = True, + ): + """ + Apply linear and angular twist using MoveIt 2 Servo. + Input is scaled by `linear_speed` and `angular_speed`, respectively. + """ + + if not self.is_enabled: + self._node.get_logger().warn( + "Command failed because MoveIt Servo is not yet enabled." + ) + if enable_if_disabled: + self._node.get_logger().warn( + f"Calling '{self.__start_service.srv_name}' service to enable MoveIt Servo..." + ) + if not self.enable(): + return + else: + return + + twist_msg = deepcopy(self.__twist_msg) + twist_msg.header.stamp = self._node.get_clock().now().to_msg() + twist_msg.twist.linear.x *= linear[0] + twist_msg.twist.linear.y *= linear[1] + twist_msg.twist.linear.z *= linear[2] + twist_msg.twist.angular.x *= angular[0] + twist_msg.twist.angular.y *= angular[1] + twist_msg.twist.angular.z *= angular[2] + self.__twist_pub.publish(twist_msg) + + def enable( + self, wait_for_server_timeout_sec: Optional[float] = 1.0, sync: bool = False + ) -> bool: + """ + Enable MoveIt 2 Servo server via async service call. + """ + + while not self.__start_service.wait_for_service( + timeout_sec=wait_for_server_timeout_sec + ): + self._node.get_logger().warn( + f"Service '{self.__start_service.srv_name}' is not yet available..." + ) + return False + + if sync: + result = self.__start_service.call(self.__trigger_req) + if not result.success: + self._node.get_logger().error( + f"MoveIt Servo could not be enabled. ({result.message})" + ) + self.__is_enabled = result.success + return result.success + else: + start_service_future = self.__start_service.call_async(self.__trigger_req) + start_service_future.add_done_callback(self.__enable_done_callback) + return True + + def disable( + self, wait_for_server_timeout_sec: Optional[float] = 1.0, sync: bool = False + ) -> bool: + """ + Disable MoveIt 2 Servo server via async service call. + """ + + while not self.__stop_service.wait_for_service( + timeout_sec=wait_for_server_timeout_sec + ): + self._node.get_logger().warn( + f"Service '{self.__stop_service.srv_name}' is not yet available..." + ) + return False + + if sync: + result = self.__stop_service.call(self.__trigger_req) + if not result.success: + self._node.get_logger().error( + f"MoveIt Servo could not be disabled. ({result.message})" + ) + self.__is_enabled = not result.success + return result.success + else: + stop_service_future = self.__stop_service.call_async(self.__trigger_req) + stop_service_future.add_done_callback(self.__disable_done_callback) + return True + + def __enable_done_callback(self, future: Future): + result: Trigger.Response = future.result() + + if not result.success: + self._node.get_logger().error( + f"MoveIt Servo could not be enabled. ({result.message})" + ) + + self.__is_enabled = result.success + + def __disable_done_callback(self, future: Future): + result: Trigger.Response = future.result() + + if not result.success: + self._node.get_logger().error( + f"MoveIt Servo could not be disabled. ({result.message})" + ) + + self.__is_enabled = not result.success + + @property + def is_enabled(self) -> bool: + return self.__is_enabled + + @property + def frame_id(self) -> str: + return self.__twist_msg.header.frame_id + + @frame_id.setter + def frame_id(self, value: str): + self.__twist_msg.header.frame_id = value + + @property + def linear_speed(self) -> float: + return self.__twist_msg.twist.linear.x + + @linear_speed.setter + def linear_speed(self, value: float): + self.__twist_msg.twist.linear.x = value + self.__twist_msg.twist.linear.y = value + self.__twist_msg.twist.linear.z = value + + @property + def angular_speed(self) -> float: + return self.__twist_msg.twist.angular.x + + @angular_speed.setter + def angular_speed(self, value: float): + self.__twist_msg.twist.angular.x = value + self.__twist_msg.twist.angular.y = value + self.__twist_msg.twist.angular.z = value From d661965f882ba3428100e11912f6d8fbaeba51eb Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 29 May 2024 14:01:00 +0200 Subject: [PATCH 04/62] update dependencies --- .../py_moveit2/py_moveit2/__init__.py | 1 - .../gazebo/arm_sim_scenario/CMakeLists.txt | 1 + .../arm_sim_scenario/__init__.py | 1 + .../arm_sim_scenario/robots/__init__.py | 1 + .../arm_sim_scenario/robots/wx200.py | 35 +++++++++++++++++++ 5 files changed, 38 insertions(+), 1 deletion(-) create mode 100644 simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py create mode 100644 simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/__init__.py create mode 100644 simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py diff --git a/dependencies/py_moveit2/py_moveit2/__init__.py b/dependencies/py_moveit2/py_moveit2/__init__.py index 6c54a9d1..7af250e7 100644 --- a/dependencies/py_moveit2/py_moveit2/__init__.py +++ b/dependencies/py_moveit2/py_moveit2/__init__.py @@ -1,4 +1,3 @@ -from . import robots from .gripper_command import GripperCommand from .gripper_interface import GripperInterface from .moveit2 import MoveIt2, MoveIt2State diff --git a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt index 1b830ba6..c7797401 100644 --- a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt +++ b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt @@ -8,6 +8,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +ament_python_install_package(arm_sim_scenario) install( DIRECTORY config launch urdf config rviz meshes diff --git a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py new file mode 100644 index 00000000..1cef6684 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py @@ -0,0 +1 @@ +from . import robots \ No newline at end of file diff --git a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/__init__.py b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/__init__.py new file mode 100644 index 00000000..53c4742e --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/__init__.py @@ -0,0 +1 @@ +from . import wx200 \ No newline at end of file diff --git a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py new file mode 100644 index 00000000..499ffe5d --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py @@ -0,0 +1,35 @@ +from typing import List + +MOVE_GROUP_ARM: str = "interbotix_arm" +MOVE_GROUP_GRIPPER: str = "interbotix_gripper" + +OPEN_GRIPPER_JOINT_POSITIONS: List[float] = [0.037, -0.037] +CLOSED_GRIPPER_JOINT_POSITIONS: List[float] = [0.0195, -0.0195] + + +def joint_names() -> List[str]: + return [ + "waist", + "shoulder", + "elbow", + "wrist_angle", + "wrist_rotate" + "left_finger", + "right_finger", + ] + + +def base_link_name(prefix: str = "wx200") -> str: + return prefix + "/base_link" + + +def end_effector_name(prefix: str = "wx200") -> str: + return prefix + "hand_tcp" + + +def gripper_joint_names() -> List[str]: + return [ + "right_finger", + "left_finger" + + ] From e9b308bd1089f79931f4c3d7aa8d8c5b3ce9feac Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 29 May 2024 14:13:31 +0200 Subject: [PATCH 05/62] make format --- .../py_moveit2/py_moveit2/__init__.py | 2 + .../py_moveit2/py_moveit2/gripper_command.py | 1 + .../py_moveit2/gripper_interface.py | 2 + dependencies/py_moveit2/py_moveit2/moveit2.py | 2 + .../py_moveit2/py_moveit2/moveit2_gripper.py | 1 + .../py_moveit2/py_moveit2/moveit2_servo.py | 1 + .../arm_sim_scenario/__init__.py | 2 +- .../arm_sim_scenario/robots/__init__.py | 2 +- .../launch/arm_description_launch.py | 42 +++++-- .../launch/ignition_launch.py | 65 ++++++---- .../arm_sim_scenario/launch/moveit_launch.py | 111 ++++++++++-------- .../launch/sim_moveit_scenario_launch.py | 15 +++ 12 files changed, 154 insertions(+), 92 deletions(-) diff --git a/dependencies/py_moveit2/py_moveit2/__init__.py b/dependencies/py_moveit2/py_moveit2/__init__.py index 7af250e7..2933c2ba 100644 --- a/dependencies/py_moveit2/py_moveit2/__init__.py +++ b/dependencies/py_moveit2/py_moveit2/__init__.py @@ -1,3 +1,5 @@ +# License: BSD + from .gripper_command import GripperCommand from .gripper_interface import GripperInterface from .moveit2 import MoveIt2, MoveIt2State diff --git a/dependencies/py_moveit2/py_moveit2/gripper_command.py b/dependencies/py_moveit2/py_moveit2/gripper_command.py index 0ee92ab4..ee22516b 100644 --- a/dependencies/py_moveit2/py_moveit2/gripper_command.py +++ b/dependencies/py_moveit2/py_moveit2/gripper_command.py @@ -1,3 +1,4 @@ +# License: BSD import threading from typing import List, Optional, Union diff --git a/dependencies/py_moveit2/py_moveit2/gripper_interface.py b/dependencies/py_moveit2/py_moveit2/gripper_interface.py index 05038056..7a54108b 100644 --- a/dependencies/py_moveit2/py_moveit2/gripper_interface.py +++ b/dependencies/py_moveit2/py_moveit2/gripper_interface.py @@ -1,3 +1,5 @@ +# License: BSD + from typing import List, Optional from rclpy.callback_groups import CallbackGroup diff --git a/dependencies/py_moveit2/py_moveit2/moveit2.py b/dependencies/py_moveit2/py_moveit2/moveit2.py index 4aca121f..e22d075c 100644 --- a/dependencies/py_moveit2/py_moveit2/moveit2.py +++ b/dependencies/py_moveit2/py_moveit2/moveit2.py @@ -1,3 +1,5 @@ +# License: BSD + import copy import threading from enum import Enum diff --git a/dependencies/py_moveit2/py_moveit2/moveit2_gripper.py b/dependencies/py_moveit2/py_moveit2/moveit2_gripper.py index 35fcdc1c..622395ce 100644 --- a/dependencies/py_moveit2/py_moveit2/moveit2_gripper.py +++ b/dependencies/py_moveit2/py_moveit2/moveit2_gripper.py @@ -1,3 +1,4 @@ +# License: BSD import math from typing import List, Optional diff --git a/dependencies/py_moveit2/py_moveit2/moveit2_servo.py b/dependencies/py_moveit2/py_moveit2/moveit2_servo.py index 340db14d..afadd046 100644 --- a/dependencies/py_moveit2/py_moveit2/moveit2_servo.py +++ b/dependencies/py_moveit2/py_moveit2/moveit2_servo.py @@ -1,3 +1,4 @@ +# License: BSD from copy import deepcopy from typing import Optional, Tuple diff --git a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py index 1cef6684..dfdb5a49 100644 --- a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py +++ b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py @@ -1 +1 @@ -from . import robots \ No newline at end of file +from . import robots diff --git a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/__init__.py b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/__init__.py index 53c4742e..b5f41325 100644 --- a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/__init__.py +++ b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/__init__.py @@ -1 +1 @@ -from . import wx200 \ No newline at end of file +from . import wx200 diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py index fe7384fc..12c8d541 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py @@ -1,3 +1,19 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -22,10 +38,10 @@ description='launches RViz if set to `true`.'), DeclareLaunchArgument('rviz_config', default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), - 'rviz', - 'xsarm_description.rviz', - ]), - description='file path to the config file RViz should load.',) + 'rviz', + 'xsarm_description.rviz', + ]), + description='file path to the config file RViz should load.',) ] @@ -50,15 +66,15 @@ def generate_launch_description(): {'robot_description': ParameterValue(Command([ 'xacro', ' ', xacro_file, ' ', 'gazebo:=ignition', ' ', - 'base_link_frame:=','base_link', ' ', - 'use_gripper:=','true', ' ', - 'show_ar_tag:=','false', ' ', - 'show_gripper_bar:=','true', ' ', - 'show_gripper_fingers:=','true', ' ', - 'use_world_frame:=','true', ' ', - 'robot_model:=',robot_model, ' ', - 'robot_name:=',robot_name, ' ', - 'hardware_type:=','gz_classic']), value_type=str)}, + 'base_link_frame:=', 'base_link', ' ', + 'use_gripper:=', 'true', ' ', + 'show_ar_tag:=', 'false', ' ', + 'show_gripper_bar:=', 'true', ' ', + 'show_gripper_fingers:=', 'true', ' ', + 'use_world_frame:=', 'true', ' ', + 'robot_model:=', robot_model, ' ', + 'robot_name:=', robot_name, ' ', + 'hardware_type:=', 'gz_classic']), value_type=str)}, ], namespace=robot_name ) diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index 1466d718..702d65a4 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -1,17 +1,30 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, IncludeLaunchDescription,Shutdown +from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, IncludeLaunchDescription, Shutdown from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch import LaunchDescription from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import (LaunchConfiguration,PathJoinSubstitution) +from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + ARGUMENTS = [ DeclareLaunchArgument('robot_model', default_value='wx200', choices=['wx200'], @@ -30,15 +43,15 @@ DeclareLaunchArgument('rviz_config', default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), - 'rviz', - 'xsarm_gz_classic.rviz', - ]), - description='file path to the config file RViz should load.',) + 'rviz', + 'xsarm_gz_classic.rviz', + ]), + description='file path to the config file RViz should load.',) ] def generate_launch_description(): - + robot_model = LaunchConfiguration('robot_model') robot_name = LaunchConfiguration('robot_name') use_rviz = LaunchConfiguration('use_rviz') @@ -46,8 +59,8 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], - 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname( - get_package_share_directory('arm_sim_scenario'))} + 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname( + get_package_share_directory('arm_sim_scenario'))} ignition_gazebo = ExecuteProcess( cmd=['ign', 'gazebo', 'empty.sdf', '-r', '-v', '4'], @@ -61,22 +74,22 @@ def generate_launch_description(): ) spawn_robot_node = Node( - package='ros_gz_sim', - executable='create', - arguments=['-name', 'spawn_wx200', - '-x', '0.0', - '-y', '0.0', - '-z', '0.0', - '-Y', '0.0', - '-topic', 'wx200/robot_description'], - output='screen' + package='ros_gz_sim', + executable='create', + arguments=['-name', 'spawn_wx200', + '-x', '0.0', + '-y', '0.0', + '-z', '0.0', + '-Y', '0.0', + '-topic', 'wx200/robot_description'], + output='screen' ) clock_bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], - output='screen' + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + output='screen' ) arm_description_launch = IncludeLaunchDescription( diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py index b3ff8455..04c13ea1 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -1,25 +1,35 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, - IncludeLaunchDescription, - OpaqueFunction, ) -from launch.conditions import IfCondition, LaunchConfigurationEquals -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( LaunchConfiguration, PathJoinSubstitution, - PythonExpression, - TextSubstitution, Command ) +from launch.conditions import IfCondition from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -import yaml from launch_ros.descriptions import ParameterValue +import yaml def load_yaml(package_name, file_path): @@ -29,9 +39,10 @@ def load_yaml(package_name, file_path): try: with open(absolute_file_path, 'r') as file: return yaml.safe_load(file) - except EnvironmentError: + except EnvironmentError: return None - + + ARGUMENTS = [ DeclareLaunchArgument('robot_model', default_value='wx200', choices=['wx200'], @@ -46,17 +57,17 @@ def load_yaml(package_name, file_path): description='launches RViz if set to `true`.'), DeclareLaunchArgument('rviz_config', default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), - 'rviz', - 'xsarm_moveit.rviz', - ]), - description='file path to the config file RViz should load.',), + 'rviz', + 'xsarm_moveit.rviz', + ]), + description='file path to the config file RViz should load.',), DeclareLaunchArgument( - 'rviz_frame', - default_value='world', - description=( - 'defines the fixed frame parameter in RViz. Note that if `use_world_frame` is ' - '`false`, this parameter should be changed to a frame that exists.' - ), + 'rviz_frame', + default_value='world', + description=( + 'defines the fixed frame parameter in RViz. Note that if `use_world_frame` is ' + '`false`, this parameter should be changed to a frame that exists.' + ), ) ] @@ -76,42 +87,40 @@ def generate_launch_description(): 'urdf', 'wx200.urdf.xacro']) srdf_xacro_file = PathJoinSubstitution([pkg_arm_sim_scenario, - 'config','srdf', - 'wx200.srdf.xacro']) - + 'config', 'srdf', + 'wx200.srdf.xacro']) + kinematics_config = PathJoinSubstitution([ pkg_arm_sim_scenario, 'config', 'kinematics.yaml', ]) - robot_description = {'robot_description': ParameterValue(Command([ - 'xacro', ' ', xacro_file, ' ', - 'gazebo:=ignition', ' ', - 'base_link_frame:=','base_link', ' ', - 'use_gripper:=','true', ' ', - 'show_ar_tag:=','false', ' ', - 'show_gripper_bar:=','true', ' ', - 'show_gripper_fingers:=','true', ' ', - 'use_world_frame:=','true', ' ', - 'robot_model:=',robot_model, ' ', - 'robot_name:=',robot_name, ' ', - 'hardware_type:=','gz_classic']), value_type=str)} - - - - robot_description_semantic = {'robot_description_semantic': ParameterValue(Command([ - 'xacro', ' ', srdf_xacro_file, ' ', - 'robot_name:=',robot_name, ' ', - 'base_link_frame:=','base_link', ' ', - 'use_gripper:=','true', ' ', - 'show_ar_tag:=','false', ' ', - 'show_gripper_bar:=','true', ' ', - 'show_gripper_fingers:=','true', ' ', - 'use_world_frame:=','true', ' ', - 'external_urdf_loc:=','', ' ', - 'external_srdf_loc:=', '', ' ', - 'hardware_type:=','gz_classic']), value_type=str)} + robot_description = {'robot_description': ParameterValue(Command([ + 'xacro', ' ', xacro_file, ' ', + 'gazebo:=ignition', ' ', + 'base_link_frame:=', 'base_link', ' ', + 'use_gripper:=', 'true', ' ', + 'show_ar_tag:=', 'false', ' ', + 'show_gripper_bar:=', 'true', ' ', + 'show_gripper_fingers:=', 'true', ' ', + 'use_world_frame:=', 'true', ' ', + 'robot_model:=', robot_model, ' ', + 'robot_name:=', robot_name, ' ', + 'hardware_type:=', 'gz_classic']), value_type=str)} + + robot_description_semantic = {'robot_description_semantic': ParameterValue(Command([ + 'xacro', ' ', srdf_xacro_file, ' ', + 'robot_name:=', robot_name, ' ', + 'base_link_frame:=', 'base_link', ' ', + 'use_gripper:=', 'true', ' ', + 'show_ar_tag:=', 'false', ' ', + 'show_gripper_bar:=', 'true', ' ', + 'show_gripper_fingers:=', 'true', ' ', + 'use_world_frame:=', 'true', ' ', + 'external_urdf_loc:=', '', ' ', + 'external_srdf_loc:=', '', ' ', + 'hardware_type:=', 'gz_classic']), value_type=str)} ompl_planning_pipeline_config = { 'move_group': { @@ -240,4 +249,4 @@ def generate_launch_description(): ld = LaunchDescription(ARGUMENTS) ld.add_action(move_group_node) ld.add_action(moveit_rviz_node) - return ld \ No newline at end of file + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py index e69de29b..3ba13780 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -0,0 +1,15 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 From 8e918556b8c62ed59bbb98934ec88750d04b4485 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 29 May 2024 17:23:32 +0200 Subject: [PATCH 06/62] add scenario --- examples/example_moveit/example_moveit.osc | 12 +++ .../actions/move_to_joint_pose.py | 80 +++++++++++++++++++ .../scenario_execution_ros/lib_osc/ros.osc | 3 + scenario_execution_ros/setup.py | 1 + .../arm_sim_scenario/robots/wx200.py | 2 +- .../launch/ignition_launch.py | 2 +- .../arm_sim_scenario/launch/moveit_launch.py | 2 +- .../launch/sim_moveit_scenario_launch.py | 61 ++++++++++++++ 8 files changed, 160 insertions(+), 3 deletions(-) create mode 100644 examples/example_moveit/example_moveit.osc create mode 100644 scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc new file mode 100644 index 00000000..cf0eb5e9 --- /dev/null +++ b/examples/example_moveit/example_moveit.osc @@ -0,0 +1,12 @@ +import osc.ros + +scenario example_moveit: + do parallel: + serial: + move_to_joint_pose() with: + keep(it.joint_pose == '[-1.5708, 0.0, 0.0, 0.0, 0.0]') + wait elapsed(3s) + emit end + time_out: serial: + wait elapsed(60s) + emit fail \ No newline at end of file diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py new file mode 100644 index 00000000..06a3561c --- /dev/null +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py @@ -0,0 +1,80 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node + +from py_moveit2 import MoveIt2, MoveIt2State +from arm_sim_scenario.robots import wx200 + +import py_trees +import ast + + +class MoveToJointPose(py_trees.behaviour.Behaviour): + + def __init__(self, name: str, joint_pose: str): + super().__init__(name) + self.joint_pose = ast.literal_eval(joint_pose) + self.execute = False + + def setup(self, **kwargs): + try: + self.node: Node = kwargs['node'] + except KeyError as e: + error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format( + self.name, self.__class__.__name__) + raise KeyError(error_message) from e + + self.synchronous = True + # If non-positive, don't cancel. Only used if synchronous is False + self.cancel_after_secs = 0.0 + + # Create MoveIt 2 interface + self.moveit2 = MoveIt2( + node=self.node, + joint_names=wx200.joint_names(), + base_link_name=wx200.base_link_name(), + end_effector_name=wx200.end_effector_name(), + group_name=wx200.MOVE_GROUP_ARM, + callback_group=ReentrantCallbackGroup() + ) + + self.moveit2.planner_id = "RRTConnectkConfigDefault" + + # Scale down velocity and acceleration of joints (percentage of maximum) + self.moveit2.max_velocity = 0.5 + self.moveit2.max_acceleration = 0.5 + + def update(self) -> py_trees.common.Status: + self.current_state = self.moveit2.query_state() + self.logger.info(f"Current State: {self.current_state}") + if (self.current_state == MoveIt2State.IDLE): + if (self.execute == False): + self.moveit2.move_to_configuration(self.joint_pose) + result = py_trees.common.Status.RUNNING + else: + result = py_trees.common.Status.SUCCESS + elif (self.current_state == MoveIt2State.EXECUTING): + self.logger.info(f"Executing joint pose....") + result = py_trees.common.Status.RUNNING + self.execute = True + else: + self.logger.info(f"Requesting joint pose....") + result = py_trees.common.Status.RUNNING + + return result + diff --git a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc index aeae9566..bd073939 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc @@ -141,3 +141,6 @@ action wait_for_data: action wait_for_topics: # wait for topics to get available topics: list of topics + +action move_to_joint_pose: + joint_pose: string \ No newline at end of file diff --git a/scenario_execution_ros/setup.py b/scenario_execution_ros/setup.py index c33ff6eb..80de9789 100644 --- a/scenario_execution_ros/setup.py +++ b/scenario_execution_ros/setup.py @@ -63,6 +63,7 @@ 'assert_topic_latency = scenario_execution_ros.actions.assert_topic_latency:AssertTopicLatency', 'assert_tf_moving = scenario_execution_ros.actions.assert_tf_moving:AssertTfMoving', 'assert_lifecycle_state = scenario_execution_ros.actions.assert_lifecycle_state:AssertLifecycleState', + 'move_to_joint_pose = scenario_execution_ros.actions.move_to_joint_pose:MoveToJointPose', ], 'scenario_execution.osc_libraries': [ 'ros = ' diff --git a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py index 499ffe5d..1c79aee8 100644 --- a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py +++ b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py @@ -24,7 +24,7 @@ def base_link_name(prefix: str = "wx200") -> str: def end_effector_name(prefix: str = "wx200") -> str: - return prefix + "hand_tcp" + return prefix + "/gripper_link" def gripper_joint_names() -> List[str]: diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index 702d65a4..02ec9117 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -34,7 +34,7 @@ description='use_sim_time'), DeclareLaunchArgument('robot_name', default_value='wx200', description='Robot name'), - DeclareLaunchArgument('use_rviz', default_value='true', + DeclareLaunchArgument('use_rviz', default_value='false', choices=['true', 'false'], description='launches RViz if set to `true`.'), diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py index 04c13ea1..e766e593 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -52,7 +52,7 @@ def load_yaml(package_name, file_path): description='use_sim_time'), DeclareLaunchArgument('robot_name', default_value=LaunchConfiguration('robot_model'), description='Robot name'), - DeclareLaunchArgument('use_rviz', default_value='true', + DeclareLaunchArgument('use_rviz', default_value='false', choices=['true', 'false'], description='launches RViz if set to `true`.'), DeclareLaunchArgument('rviz_config', diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py index 3ba13780..0bb224d6 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -13,3 +13,64 @@ # and limitations under the License. # # SPDX-License-Identifier: Apache-2.0 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetLaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +def generate_launch_description(): + + arm_sim_scenario_dir = get_package_share_directory('arm_sim_scenario') + scenario_execution_dir = get_package_share_directory('scenario_execution_ros') + # gazebo_tf_publisher_dir = get_package_share_directory('gazebo_tf_publisher') + tf_to_pose_publisher_dir = get_package_share_directory('tf_to_pose_publisher') + + scenario = LaunchConfiguration('scenario') + scenario_execution = LaunchConfiguration('scenario_execution') + arg_scenario = DeclareLaunchArgument('scenario', + description='Scenario file to execute') + arg_scenario_execution = DeclareLaunchArgument( + 'scenario_execution', default_value='True', + description='Wether to execute scenario execution') + + ignition = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'ignition_launch.py'])]), + ) + + moveit_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'moveit_launch.py'])]), + ) + + # groundtruth_publisher = IncludeLaunchDescription( + # PythonLaunchDescriptionSource(os.path.join(gazebo_tf_publisher_dir, 'launch', 'gazebo_tf_publisher_launch.py')), + # launch_arguments=[ + # ('ign_pose_topic', ['/world/', world, '/dynamic_pose/info']), + # ] + # ) + + scenario_exec = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([scenario_execution_dir, 'launch', 'scenario_launch.py'])]), + condition=IfCondition(scenario_execution), + launch_arguments=[ + ('scenario', scenario), + ] + ) + + tf_to_pose = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([tf_to_pose_publisher_dir, 'launch', 'tf_to_pose_launch.py'])]), + ) + + ld = LaunchDescription([ + arg_scenario, + arg_scenario_execution + ]) + ld.add_action(ignition) + ld.add_action(moveit_bringup) + ld.add_action(scenario_exec) + ld.add_action(tf_to_pose) + return ld From 48859ec84d86d26e15f8ab9ea75ae80f745e46a2 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 29 May 2024 17:46:57 +0200 Subject: [PATCH 07/62] update submodule --- .gitmodules | 3 + dependencies/py_moveit2/CMakeLists.txt | 16 - dependencies/py_moveit2/LICENSE | 29 - dependencies/py_moveit2/README.md | 105 - dependencies/py_moveit2/package.xml | 26 - .../py_moveit2/py_moveit2/__init__.py | 7 - .../py_moveit2/py_moveit2/gripper_command.py | 353 --- .../py_moveit2/gripper_interface.py | 204 -- dependencies/py_moveit2/py_moveit2/moveit2.py | 2441 ----------------- .../py_moveit2/py_moveit2/moveit2_gripper.py | 252 -- .../py_moveit2/py_moveit2/moveit2_servo.py | 248 -- dependencies/pymoveit2 | 1 + .../actions/move_to_joint_pose.py | 2 +- 13 files changed, 5 insertions(+), 3682 deletions(-) delete mode 100644 dependencies/py_moveit2/CMakeLists.txt delete mode 100644 dependencies/py_moveit2/LICENSE delete mode 100644 dependencies/py_moveit2/README.md delete mode 100644 dependencies/py_moveit2/package.xml delete mode 100644 dependencies/py_moveit2/py_moveit2/__init__.py delete mode 100644 dependencies/py_moveit2/py_moveit2/gripper_command.py delete mode 100644 dependencies/py_moveit2/py_moveit2/gripper_interface.py delete mode 100644 dependencies/py_moveit2/py_moveit2/moveit2.py delete mode 100644 dependencies/py_moveit2/py_moveit2/moveit2_gripper.py delete mode 100644 dependencies/py_moveit2/py_moveit2/moveit2_servo.py create mode 160000 dependencies/pymoveit2 diff --git a/.gitmodules b/.gitmodules index 52691286..216655d9 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "dependencies/py_trees"] path = dependencies/py_trees url = https://github.com/splintered-reality/py_trees.git +[submodule "dependencies/pymoveit2"] + path = dependencies/pymoveit2 + url = https://github.com/AndrejOrsula/pymoveit2.git diff --git a/dependencies/py_moveit2/CMakeLists.txt b/dependencies/py_moveit2/CMakeLists.txt deleted file mode 100644 index eaa3b214..00000000 --- a/dependencies/py_moveit2/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(py_moveit2) - -# Find dependencies -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) - -# Install Python package -ament_python_install_package(py_moveit2) - -install(DIRECTORY - DESTINATION lib/${PROJECT_NAME} -) - -# Setup the project -ament_package() diff --git a/dependencies/py_moveit2/LICENSE b/dependencies/py_moveit2/LICENSE deleted file mode 100644 index 70069740..00000000 --- a/dependencies/py_moveit2/LICENSE +++ /dev/null @@ -1,29 +0,0 @@ -BSD 3-Clause License - -Copyright (c) 2022, Andrej Orsula -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/dependencies/py_moveit2/README.md b/dependencies/py_moveit2/README.md deleted file mode 100644 index 87a05e90..00000000 --- a/dependencies/py_moveit2/README.md +++ /dev/null @@ -1,105 +0,0 @@ -# pymoveit2 - -Basic Python interface for MoveIt 2 built on top of ROS 2 actions and services. - -> Note: The official Python library for MoveIt 2 `moveit_py` is now available. Check the announcement [here](https://picknik.ai/moveit/ros/python/google/2023/04/28/GSOC-MoveIt-2-Python-Bindings.html)! - -
- - - - - - - - - - - - - - - -
Animation of ex_joint_goal.pyAnimation of ex_pose_goal.pyAnimation of ex_gripper.pyAnimation of ex_servo.py
Joint Goal
Pose Goal
Gripper Action
MoveIt 2 Servo
-
- -## Instructions - -### Dependencies - -These are the primary dependencies required to use this project. - -- ROS 2 [Galactic](https://docs.ros.org/en/galactic/Installation.html), [Humble](https://docs.ros.org/en/humble/Installation.html) or [Iron](https://docs.ros.org/en/iron/Installation.html) -- [MoveIt 2](https://moveit.ros.org/install-moveit2/binary) corresponding to the selected ROS 2 distribution - -All additional dependencies are installed via [rosdep](https://wiki.ros.org/rosdep) during the building process below. - -### Building - -Clone this repository, install dependencies and build with [colcon](https://colcon.readthedocs.io). - -```bash -# Clone this repository into your favourite ROS 2 workspace -git clone https://github.com/AndrejOrsula/pymoveit2.git -# Install dependencies -rosdep install -y -r -i --rosdistro ${ROS_DISTRO} --from-paths . -# Build -colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" -``` - -### Sourcing - -Before utilising this package, remember to source the ROS 2 workspace. - -```bash -source install/local_setup.bash -``` - -This enables importing of `pymoveit2` module from external workspaces. - -## Examples - -To demonstrate `pymoveit2` usage, [examples](./examples) directory contains scripts that demonstrate the basic functionality. Additional examples can be found under [ign_moveit2_examples](https://github.com/AndrejOrsula/ign_moveit2_examples) repository. - -Prior to running the examples, configure an environment for control of a robot with MoveIt 2. For instance, one of the following launch scripts from [panda_ign_moveit2](https://github.com/AndrejOrsula/panda_ign_moveit2) repository can be used. - -```bash -# RViz (fake) ROS 2 control -ros2 launch panda_moveit_config ex_fake_control.launch.py -# Gazebo (simulated) ROS 2 control -ros2 launch panda_moveit_config ex_ign_control.launch.py -``` - -After that, the individual scripts can be run. - -```bash -# Move to joint configuration -ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]" -# Move to Cartesian pose (motion in either joint or Cartesian space) -ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False -# Repeatadly toggle the gripper (or use "open"/"close" actions) -ros2 run pymoveit2 ex_gripper.py --ros-args -p action:="toggle" -# Example of using MoveIt 2 Servo to move the end-effector in a circular motion -ros2 run pymoveit2 ex_servo.py -# Example of adding a collision object with primitive geometry to the planning scene of MoveIt 2 -ros2 run pymoveit2 ex_collision_primitive.py --ros-args -p shape:="sphere" -p position:="[0.5, 0.0, 0.5]" -p dimensions:="[0.04]" -# Example of adding a collision object with mesh geometry to the planning scene of MoveIt 2 -ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p action:="add" -p position:="[0.5, 0.0, 0.5]" -p quat_xyzw:="[0.0, 0.0, -0.707, 0.707]" -``` - -## Directory Structure - -The following directory structure is utilised for this package. - -```bash -. -├── examples/ # [dir] Examples demonstrating the use of `pymoveit2` -├── pymoveit2/ # [dir] ROS 2 launch scripts - ├── robots/ # [dir] Presets for robots (data that can be extracted from URDF/SRDF) - ├── gripper_command.py # Interface for Gripper that is controlled by GripperCommand - ├── moveit2_gripper.py # Interface for MoveIt 2 Gripper that is controlled by JointTrajectoryController - ├── moveit2_servo.py # Interface for MoveIt 2 Servo that enables real-time control in Cartesian Space - └── moveit2.py # Interface for MoveIt 2 that enables planning and execution of trajectories -├── CMakeLists.txt # Colcon-enabled CMake recipe -└── package.xml # ROS 2 package metadata -``` diff --git a/dependencies/py_moveit2/package.xml b/dependencies/py_moveit2/package.xml deleted file mode 100644 index 2bac2b77..00000000 --- a/dependencies/py_moveit2/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - py_moveit2 - 4.0.0 - Basic Python interface for MoveIt 2 built on top of ROS 2 actions and services - Andrej Orsula - Andrej Orsula - BSD - - ament_cmake - ament_cmake_python - - action_msgs - control_msgs - geometry_msgs - moveit_msgs - rclpy - sensor_msgs - shape_msgs - std_srvs - trajectory_msgs - - - ament_cmake - - diff --git a/dependencies/py_moveit2/py_moveit2/__init__.py b/dependencies/py_moveit2/py_moveit2/__init__.py deleted file mode 100644 index 2933c2ba..00000000 --- a/dependencies/py_moveit2/py_moveit2/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -# License: BSD - -from .gripper_command import GripperCommand -from .gripper_interface import GripperInterface -from .moveit2 import MoveIt2, MoveIt2State -from .moveit2_gripper import MoveIt2Gripper -from .moveit2_servo import MoveIt2Servo diff --git a/dependencies/py_moveit2/py_moveit2/gripper_command.py b/dependencies/py_moveit2/py_moveit2/gripper_command.py deleted file mode 100644 index ee22516b..00000000 --- a/dependencies/py_moveit2/py_moveit2/gripper_command.py +++ /dev/null @@ -1,353 +0,0 @@ -# License: BSD -import threading -from typing import List, Optional, Union - -from action_msgs.msg import GoalStatus -from control_msgs.action import GripperCommand as GripperCommandAction -from rclpy.action import ActionClient -from rclpy.callback_groups import CallbackGroup -from rclpy.node import Node -from rclpy.qos import ( - QoSDurabilityPolicy, - QoSHistoryPolicy, - QoSProfile, - QoSReliabilityPolicy, -) -from sensor_msgs.msg import JointState - - -class GripperCommand: - """ - Python interface for Gripper that is controlled by GripperCommand. - """ - - def __init__( - self, - node: Node, - gripper_joint_names: List[str], - open_gripper_joint_positions: Union[float, List[float]], - closed_gripper_joint_positions: Union[float, List[float]], - max_effort: float = 0.0, - ignore_new_calls_while_executing: bool = True, - callback_group: Optional[CallbackGroup] = None, - gripper_command_action_name: str = "gripper_action_controller/gripper_command", - ): - """ - Construct an instance of `GripperCommand` interface. - - `node` - ROS 2 node that this interface is attached to - - `gripper_joint_names` - List of gripper joint names (can be extracted from URDF) - - `open_gripper_joint_positions` - Configuration of gripper joints when open - - `closed_gripper_joint_positions` - Configuration of gripper joints when fully closed - - `max_effort` - Max effort applied when closing - - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories - while previous is still being executed - - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) - - `gripper_command_action_name` - Name of the action server for the controller - """ - - self._node = node - self._callback_group = callback_group - - # Create subscriber for current joint states - self._node.create_subscription( - msg_type=JointState, - topic="joint_states", - callback=self.__joint_state_callback, - qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=self._callback_group, - ) - - # Create action client for move action - self.__gripper_command_action_client = ActionClient( - node=self._node, - action_type=GripperCommandAction, - action_name=gripper_command_action_name, - goal_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - result_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - cancel_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - feedback_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - status_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=self._callback_group, - ) - - # Initialise command goals for opening/closing - self.__open_gripper_joint_positions = open_gripper_joint_positions - self.__open_gripper_command_goal = self.__init_gripper_command_goal( - position=open_gripper_joint_positions, max_effort=max_effort - ) - self.__close_gripper_command_goal = self.__init_gripper_command_goal( - position=closed_gripper_joint_positions, max_effort=max_effort - ) - - # Initialise internals for determining whether the gripper is open or closed - self.__joint_state_mutex = threading.Lock() - self.__joint_state = None - self.__new_joint_state_available = False - # Tolerance used for checking whether the gripper is open or closed - self.__open_tolerance = [ - 0.1 - * abs(open_gripper_joint_positions[i] - closed_gripper_joint_positions[i]) - for i in range(len(open_gripper_joint_positions)) - ] - # Indices of gripper joint within the joint state message topic. - # It is assumed that the order of these does not change during execution. - self.__gripper_joint_indices: Optional[List[int]] = None - - # Flag that determines whether a new goal can be send while the previous one is being executed - self.__ignore_new_calls_while_executing = ignore_new_calls_while_executing - - # Store additional variables for later use - self.__joint_names = gripper_joint_names - - # Internal states that monitor the current motion requests and execution - self.__is_motion_requested = False - self.__is_executing = False - self.__wait_until_executed_rate = self._node.create_rate(1000.0) - - def __call__(self): - """ - Callable that is identical to `GripperCommand.toggle()`. - """ - - self.toggle() - - def toggle(self): - """ - Toggles the gripper between open and closed state. - """ - - if self.is_open: - self.close(skip_if_noop=False) - else: - self.open(skip_if_noop=False) - - def open(self, skip_if_noop: bool = False): - """ - Open the gripper. - - `skip_if_noop` - No action will be performed if the gripper is already open. - """ - - if skip_if_noop and self.is_open: - return - - if self.__ignore_new_calls_while_executing and self.__is_executing: - return - self.__is_motion_requested = True - - self.__send_goal_async_gripper_command(self.__open_gripper_command_goal) - - def close(self, skip_if_noop: bool = False): - """ - Close the gripper. - - `skip_if_noop` - No action will be performed if the gripper is not open. - """ - - if skip_if_noop and self.is_closed: - return - - if self.__ignore_new_calls_while_executing and self.__is_executing: - return - self.__is_motion_requested = True - - self.__send_goal_async_gripper_command(self.__close_gripper_command_goal) - - def reset_open(self, **kwargs): - """ - Reset into open configuration by sending a dummy joint trajectory. - This is useful for simulated robots that allow instantaneous reset of joints. - """ - - self.force_reset_executing_state() - self.__send_goal_async_gripper_command(self.__open_gripper_command_goal) - - def reset_closed(self, **kwargs): - """ - Reset into closed configuration by sending a dummy joint trajectory. - This is useful for simulated robots that allow instantaneous reset of joints. - """ - - self.force_reset_executing_state() - self.__send_goal_async_gripper_command(self.__close_gripper_command_goal) - - def force_reset_executing_state(self): - """ - Force reset of internal states that block execution while `ignore_new_calls_while_executing` is being - used. This function is applicable only in a very few edge-cases, so it should almost never be used. - """ - - self.__is_motion_requested = False - self.__is_executing = False - - def wait_until_executed(self) -> bool: - """ - Wait until the previously requested motion is finalised through either a success or failure. - """ - - if not self.__is_motion_requested: - self._node.get_logger().warn( - "Cannot wait until motion is executed (no motion is in progress)." - ) - return False - - while self.__is_motion_requested or self.__is_executing: - self.__wait_until_executed_rate.sleep() - return True - - def __joint_state_callback(self, msg: JointState): - # Update only if all relevant joints are included in the message - for joint_name in self.joint_names: - if not joint_name in msg.name: - return - - self.__joint_state_mutex.acquire() - self.__joint_state = msg - self.__new_joint_state_available = True - self.__joint_state_mutex.release() - - def __send_goal_async_gripper_command( - self, - goal: GripperCommandAction.Goal, - wait_for_server_timeout_sec: Optional[float] = 1.0, - ): - if not self.__gripper_command_action_client.wait_for_server( - timeout_sec=wait_for_server_timeout_sec - ): - self._node.get_logger().warn( - f"Action server {self.__gripper_command_action_client._action_name} is not yet ready. Better luck next time!" - ) - return - - action_result = self.__gripper_command_action_client.send_goal_async( - goal=goal, - feedback_callback=None, - ) - - action_result.add_done_callback(self.__response_callback_gripper_command) - - def __response_callback_gripper_command(self, response): - goal_handle = response.result() - if not goal_handle.accepted: - self._node.get_logger().warn( - f"Action '{self.__gripper_command_action_client._action_name}' was rejected." - ) - self.__is_motion_requested = False - return - - self.__is_executing = True - self.__is_motion_requested = False - - self.__get_result_future_gripper_command = goal_handle.get_result_async() - self.__get_result_future_gripper_command.add_done_callback( - self.__result_callback_gripper_command - ) - - def __result_callback_gripper_command(self, res): - if res.result().status != GoalStatus.STATUS_SUCCEEDED: - self._node.get_logger().error( - f"Action '{self.__gripper_command_action_client._action_name}' was unsuccessful: {res.result().status}" - ) - - self.__is_executing = False - - @classmethod - def __init_gripper_command_goal( - cls, position: Union[float, List[float]], max_effort: float - ) -> GripperCommandAction.Goal: - if hasattr(position, "__getitem__"): - position = position[0] - - gripper_cmd_goal = GripperCommandAction.Goal() - gripper_cmd_goal.command.position = position - gripper_cmd_goal.command.max_effort = max_effort - - return gripper_cmd_goal - - @property - def gripper_command_action_client(self) -> str: - return self.__gripper_command_action_client - - @property - def joint_names(self) -> List[str]: - return self.__joint_names - - @property - def joint_state(self) -> Optional[JointState]: - self.__joint_state_mutex.acquire() - joint_state = self.__joint_state - self.__joint_state_mutex.release() - return joint_state - - @property - def new_joint_state_available(self): - return self.__new_joint_state_available - - @property - def is_open(self) -> bool: - """ - Gripper is considered to be open if all of the joints are at their open position. - """ - - joint_state = self.joint_state - - # Assume the gripper is open if there are no joint state readings yet - if joint_state is None: - return True - - # For the sake of performance, find the indices of joints only once. - # This is especially useful for robots with many joints. - if self.__gripper_joint_indices is None: - self.__gripper_joint_indices: List[int] = [] - for joint_name in self.joint_names: - self.__gripper_joint_indices.append(joint_state.name.index(joint_name)) - - for local_joint_index, joint_state_index in enumerate( - self.__gripper_joint_indices - ): - if ( - abs( - joint_state.position[joint_state_index] - - self.__open_gripper_joint_positions[local_joint_index] - ) - > self.__open_tolerance[local_joint_index] - ): - return False - - return True - - @property - def is_closed(self) -> bool: - """ - Gripper is considered to be closed if any of the joints is outside of their open position. - """ - - return not self.is_open diff --git a/dependencies/py_moveit2/py_moveit2/gripper_interface.py b/dependencies/py_moveit2/py_moveit2/gripper_interface.py deleted file mode 100644 index 7a54108b..00000000 --- a/dependencies/py_moveit2/py_moveit2/gripper_interface.py +++ /dev/null @@ -1,204 +0,0 @@ -# License: BSD - -from typing import List, Optional - -from rclpy.callback_groups import CallbackGroup -from rclpy.node import Node - -from .gripper_command import GripperCommand -from .moveit2_gripper import MoveIt2Gripper - - -class GripperInterface(MoveIt2Gripper, GripperCommand): - """ - Python interface for MoveIt 2 Gripper that is controlled either by GripperCommand or JointTrajectoryController. - The appropriate interface is automatically selected based on the available action. - """ - - def __init__( - self, - node: Node, - gripper_joint_names: List[str], - open_gripper_joint_positions: List[float], - closed_gripper_joint_positions: List[float], - gripper_group_name: str = "gripper", - execute_via_moveit: bool = False, - ignore_new_calls_while_executing: bool = False, - skip_planning: bool = False, - skip_planning_fixed_motion_duration: float = 0.5, - max_effort: float = 0.0, - callback_group: Optional[CallbackGroup] = None, - follow_joint_trajectory_action_name: str = "DEPRECATED", - gripper_command_action_name: str = "gripper_action_controller/gripper_cmd", - use_move_group_action: bool = False, - ): - """ - Combination of `MoveIt2Gripper` and `GripperCommand` interfaces that automatically - selects the appropriate interface based on the available actions. - """ - - # Check for deprecated parameters - if execute_via_moveit: - node.get_logger().warn( - "Parameter `execute_via_moveit` is deprecated. Please use `use_move_group_action` instead." - ) - use_move_group_action = True - if follow_joint_trajectory_action_name != "DEPRECATED": - node.get_logger().warn( - "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead." - ) - - MoveIt2Gripper.__init__( - self=self, - node=node, - gripper_joint_names=gripper_joint_names, - open_gripper_joint_positions=open_gripper_joint_positions, - closed_gripper_joint_positions=closed_gripper_joint_positions, - gripper_group_name=gripper_group_name, - ignore_new_calls_while_executing=ignore_new_calls_while_executing, - skip_planning=skip_planning, - skip_planning_fixed_motion_duration=skip_planning_fixed_motion_duration, - callback_group=callback_group, - use_move_group_action=use_move_group_action, - ) - - GripperCommand.__init__( - self=self, - node=node, - gripper_joint_names=gripper_joint_names, - open_gripper_joint_positions=open_gripper_joint_positions, - closed_gripper_joint_positions=closed_gripper_joint_positions, - max_effort=max_effort, - ignore_new_calls_while_executing=ignore_new_calls_while_executing, - callback_group=callback_group, - gripper_command_action_name=gripper_command_action_name, - ) - - self.__determine_interface() - - def __determine_interface(self, timeout_sec=1.0): - if self.gripper_command_action_client.wait_for_server(timeout_sec=timeout_sec): - self._interface = GripperCommand - elif self._execute_trajectory_action_client.wait_for_server( - timeout_sec=timeout_sec - ): - self._interface = MoveIt2Gripper - else: - self._interface = None - - if self._interface is None: - self._node.get_logger().warn( - f"Unable to determine the appropriate interface for gripper." - ) - - def __call__(self): - """ - Callable that is identical to `MoveIt2Gripper.toggle()`. - """ - - self.toggle() - - def toggle(self): - """ - Toggles the gripper between open and closed state. - """ - - if self.is_open: - self.close(skip_if_noop=False) - else: - self.open(skip_if_noop=False) - - def open(self, skip_if_noop: bool = False): - """ - Open the gripper. - - `skip_if_noop` - No action will be performed if the gripper is already open. - """ - - if self._interface is None: - self.__determine_interface() - if self._interface is None: - self._node.get_logger().error( - f"Unable to close the gripper because the appropriate interface cannot be determined." - ) - return - - self._interface.open(self=self, skip_if_noop=skip_if_noop) - - def close(self, skip_if_noop: bool = False): - """ - Close the gripper. - - `skip_if_noop` - No action will be performed if the gripper is not open. - """ - - if self._interface is None: - self.__determine_interface() - if self._interface is None: - self._node.get_logger().error( - f"Unable to close the gripper because the appropriate interface cannot be determined." - ) - return - - self._interface.close(self=self, skip_if_noop=skip_if_noop) - - def reset_open(self, sync: bool = True): - """ - Reset into open configuration by sending a dummy joint trajectory. - This is useful for simulated robots that allow instantaneous reset of joints. - """ - - if self._interface is None: - self.__determine_interface() - if self._interface is None: - self._node.get_logger().error( - f"Unable to reset the gripper as open because the appropriate interface cannot be determined." - ) - return - - self._interface.reset_open(self=self, sync=sync) - - def reset_closed(self, sync: bool = True): - """ - Reset into closed configuration by sending a dummy joint trajectory. - This is useful for simulated robots that allow instantaneous reset of joints. - """ - - if self._interface is None: - self.__determine_interface() - if self._interface is None: - self._node.get_logger().error( - f"Unable to reset the gripper as closed because the appropriate interface cannot be determined." - ) - return - - self._interface.reset_closed(self=self, sync=sync) - - def force_reset_executing_state(self): - """ - Force reset of internal states that block execution while `ignore_new_calls_while_executing` is being - used. This function is applicable only in a very few edge-cases, so it should almost never be used. - """ - - if self._interface is None: - self.__determine_interface() - if self._interface is None: - self._node.get_logger().error( - f"Unable to reset the executing state because the appropriate interface cannot be determined." - ) - return - - self._interface.force_reset_executing_state(self=self) - - def wait_until_executed(self) -> bool: - """ - Wait until the previously requested motion is finalised through either a success or failure. - """ - - if self._interface is None: - self.__determine_interface() - if self._interface is None: - self._node.get_logger().error( - f"Unable to wait until a motion is executed because the appropriate interface cannot be determined." - ) - return False - - return self._interface.wait_until_executed(self=self) diff --git a/dependencies/py_moveit2/py_moveit2/moveit2.py b/dependencies/py_moveit2/py_moveit2/moveit2.py deleted file mode 100644 index e22d075c..00000000 --- a/dependencies/py_moveit2/py_moveit2/moveit2.py +++ /dev/null @@ -1,2441 +0,0 @@ -# License: BSD - -import copy -import threading -from enum import Enum -from typing import Any, List, Optional, Tuple, Union - -import numpy as np -from action_msgs.msg import GoalStatus -from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion -from moveit_msgs.action import ExecuteTrajectory, MoveGroup -from moveit_msgs.msg import ( - AllowedCollisionEntry, - AttachedCollisionObject, - CollisionObject, - Constraints, - JointConstraint, - MoveItErrorCodes, - OrientationConstraint, - PlanningScene, - PositionConstraint, -) -from moveit_msgs.srv import ( - ApplyPlanningScene, - GetCartesianPath, - GetMotionPlan, - GetPlanningScene, - GetPositionFK, - GetPositionIK, -) -from rclpy.action import ActionClient -from rclpy.callback_groups import CallbackGroup -from rclpy.node import Node -from rclpy.qos import ( - QoSDurabilityPolicy, - QoSHistoryPolicy, - QoSProfile, - QoSReliabilityPolicy, -) -from rclpy.task import Future -from sensor_msgs.msg import JointState -from shape_msgs.msg import Mesh, MeshTriangle, SolidPrimitive -from std_msgs.msg import Header, String -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint - - -class MoveIt2State(Enum): - """ - An enum the represents the current execution state of the MoveIt2 interface. - - IDLE: No motion is being requested or executed - - REQUESTING: Execution has been requested, but the request has not yet been - accepted. - - EXECUTING: Execution has been requested and accepted, and has not yet been - completed. - """ - - IDLE = 0 - REQUESTING = 1 - EXECUTING = 2 - - -class MoveIt2: - """ - Python interface for MoveIt 2 that enables planning and execution of trajectories. - For execution, this interface requires that robot utilises JointTrajectoryController. - """ - - def __init__( - self, - node: Node, - joint_names: List[str], - base_link_name: str, - end_effector_name: str, - group_name: str = "arm", - execute_via_moveit: bool = False, - ignore_new_calls_while_executing: bool = False, - callback_group: Optional[CallbackGroup] = None, - follow_joint_trajectory_action_name: str = "DEPRECATED", - use_move_group_action: bool = False, - ): - """ - Construct an instance of `MoveIt2` interface. - - `node` - ROS 2 node that this interface is attached to - - `joint_names` - List of joint names of the robot (can be extracted from URDF) - - `base_link_name` - Name of the robot base link - - `end_effector_name` - Name of the robot end effector - - `group_name` - Name of the planning group for robot arm - - [DEPRECATED] `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) - FollowJointTrajectory action (controller) is employed otherwise - together with a separate planning service client - - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories - while previous is still being executed - - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) - - [DEPRECATED] `follow_joint_trajectory_action_name` - Name of the action server for the controller - - `use_move_group_action` - Flag that enables execution via MoveGroup action (MoveIt 2) - ExecuteTrajectory action is employed otherwise - together with a separate planning service client - """ - - self._node = node - self._callback_group = callback_group - - # Check for deprecated parameters - if execute_via_moveit: - self._node.get_logger().warn( - "Parameter `execute_via_moveit` is deprecated. Please use `use_move_group_action` instead." - ) - use_move_group_action = True - if follow_joint_trajectory_action_name != "DEPRECATED": - self._node.get_logger().warn( - "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead." - ) - - # Create subscriber for current joint states - self._node.create_subscription( - msg_type=JointState, - topic="joint_states", - callback=self.__joint_state_callback, - qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=self._callback_group, - ) - - # Create action client for move action - self.__move_action_client = ActionClient( - node=self._node, - action_type=MoveGroup, - action_name="move_action", - goal_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - result_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - cancel_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - feedback_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - status_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=self._callback_group, - ) - - # Also create a separate service client for planning - self._plan_kinematic_path_service = self._node.create_client( - srv_type=GetMotionPlan, - srv_name="plan_kinematic_path", - qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=callback_group, - ) - self.__kinematic_path_request = GetMotionPlan.Request() - - # Create a separate service client for Cartesian planning - self._plan_cartesian_path_service = self._node.create_client( - srv_type=GetCartesianPath, - srv_name="compute_cartesian_path", - qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=callback_group, - ) - self.__cartesian_path_request = GetCartesianPath.Request() - - # Create action client for trajectory execution - self._execute_trajectory_action_client = ActionClient( - node=self._node, - action_type=ExecuteTrajectory, - action_name="execute_trajectory", - goal_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - result_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - cancel_service_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=5, - ), - feedback_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - status_sub_qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=self._callback_group, - ) - - # Create a service for getting the planning scene - self._get_planning_scene_service = self._node.create_client( - srv_type=GetPlanningScene, - srv_name="get_planning_scene", - qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=callback_group, - ) - self.__planning_scene = None - self.__old_planning_scene = None - self.__old_allowed_collision_matrix = None - - # Create a service for applying the planning scene - self._apply_planning_scene_service = self._node.create_client( - srv_type=ApplyPlanningScene, - srv_name="apply_planning_scene", - qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ), - callback_group=callback_group, - ) - - self.__collision_object_publisher = self._node.create_publisher( - CollisionObject, "/collision_object", 10 - ) - self.__attached_collision_object_publisher = self._node.create_publisher( - AttachedCollisionObject, "/attached_collision_object", 10 - ) - - self.__cancellation_pub = self._node.create_publisher( - String, "/trajectory_execution_event", 1 - ) - - self.__joint_state_mutex = threading.Lock() - self.__joint_state = None - self.__new_joint_state_available = False - self.__move_action_goal = self.__init_move_action_goal( - frame_id=base_link_name, - group_name=group_name, - end_effector=end_effector_name, - ) - - # Flag to determine whether to execute trajectories via Move Group Action, or rather by calling - # the separate ExecuteTrajectory action - # Applies to `move_to_pose()` and `move_to_configuration()` - self.__use_move_group_action = use_move_group_action - - # Flag that determines whether a new goal can be sent while the previous one is being executed - self.__ignore_new_calls_while_executing = ignore_new_calls_while_executing - - # Store additional variables for later use - self.__joint_names = joint_names - self.__base_link_name = base_link_name - self.__end_effector_name = end_effector_name - self.__group_name = group_name - - # Internal states that monitor the current motion requests and execution - self.__is_motion_requested = False - self.__is_executing = False - self.motion_suceeded = False - self.__execution_goal_handle = None - self.__last_error_code = None - self.__wait_until_executed_rate = self._node.create_rate(1000.0) - self.__execution_mutex = threading.Lock() - - # Event that enables waiting until async future is done - self.__future_done_event = threading.Event() - - #### Execution Polling Functions - def query_state(self) -> MoveIt2State: - with self.__execution_mutex: - if self.__is_motion_requested: - return MoveIt2State.REQUESTING - elif self.__is_executing: - return MoveIt2State.EXECUTING - else: - return MoveIt2State.IDLE - - def cancel_execution(self): - if self.query_state() != MoveIt2State.EXECUTING: - self._node.get_logger().warn("Attempted to cancel without active goal.") - return None - - cancel_string = String() - cancel_string.data = "stop" - self.__cancellation_pub.publish(cancel_string) - - def get_execution_future(self) -> Optional[Future]: - if self.query_state() != MoveIt2State.EXECUTING: - self._node.get_logger().warn("Need active goal for future.") - return None - - return self.__execution_goal_handle.get_result_async() - - def get_last_execution_error_code(self) -> Optional[MoveItErrorCodes]: - return self.__last_error_code - - #### - - def move_to_pose( - self, - pose: Optional[Union[PoseStamped, Pose]] = None, - position: Optional[Union[Point, Tuple[float, float, float]]] = None, - quat_xyzw: Optional[ - Union[Quaternion, Tuple[float, float, float, float]] - ] = None, - target_link: Optional[str] = None, - frame_id: Optional[str] = None, - tolerance_position: float = 0.001, - tolerance_orientation: float = 0.001, - weight_position: float = 1.0, - cartesian: bool = False, - weight_orientation: float = 1.0, - cartesian_max_step: float = 0.0025, - cartesian_fraction_threshold: float = 0.0, - ): - """ - Plan and execute motion based on previously set goals. Optional arguments can be - passed in to internally use `set_pose_goal()` to define a goal during the call. - """ - - if isinstance(pose, PoseStamped): - pose_stamped = pose - elif isinstance(pose, Pose): - pose_stamped = PoseStamped( - header=Header( - stamp=self._node.get_clock().now().to_msg(), - frame_id=( - frame_id if frame_id is not None else self.__base_link_name - ), - ), - pose=pose, - ) - else: - if not isinstance(position, Point): - position = Point( - x=float(position[0]), y=float(position[1]), z=float(position[2]) - ) - if not isinstance(quat_xyzw, Quaternion): - quat_xyzw = Quaternion( - x=float(quat_xyzw[0]), - y=float(quat_xyzw[1]), - z=float(quat_xyzw[2]), - w=float(quat_xyzw[3]), - ) - pose_stamped = PoseStamped( - header=Header( - stamp=self._node.get_clock().now().to_msg(), - frame_id=( - frame_id if frame_id is not None else self.__base_link_name - ), - ), - pose=Pose(position=position, orientation=quat_xyzw), - ) - - if self.__use_move_group_action and not cartesian: - if self.__ignore_new_calls_while_executing and ( - self.__is_motion_requested or self.__is_executing - ): - self._node.get_logger().warn( - "Controller is already following a trajectory. Skipping motion." - ) - return - - # Set goal - self.set_pose_goal( - position=pose_stamped.pose.position, - quat_xyzw=pose_stamped.pose.orientation, - frame_id=pose_stamped.header.frame_id, - target_link=target_link, - tolerance_position=tolerance_position, - tolerance_orientation=tolerance_orientation, - weight_position=weight_position, - weight_orientation=weight_orientation, - ) - # Define starting state as the current state - if self.joint_state is not None: - self.__move_action_goal.request.start_state.joint_state = ( - self.joint_state - ) - # Send to goal to the server (async) - both planning and execution - self._send_goal_async_move_action() - # Clear all previous goal constrains - self.clear_goal_constraints() - self.clear_path_constraints() - - else: - # Plan via MoveIt 2 and then execute directly with the controller - self.execute( - self.plan( - position=pose_stamped.pose.position, - quat_xyzw=pose_stamped.pose.orientation, - frame_id=pose_stamped.header.frame_id, - target_link=target_link, - tolerance_position=tolerance_position, - tolerance_orientation=tolerance_orientation, - weight_position=weight_position, - weight_orientation=weight_orientation, - cartesian=cartesian, - max_step=cartesian_max_step, - cartesian_fraction_threshold=cartesian_fraction_threshold, - ) - ) - - def move_to_configuration( - self, - joint_positions: List[float], - joint_names: Optional[List[str]] = None, - tolerance: float = 0.001, - weight: float = 1.0, - ): - """ - Plan and execute motion based on previously set goals. Optional arguments can be - passed in to internally use `set_joint_goal()` to define a goal during the call. - """ - - if self.__use_move_group_action: - if self.__ignore_new_calls_while_executing and ( - self.__is_motion_requested or self.__is_executing - ): - self._node.get_logger().warn( - "Controller is already following a trajectory. Skipping motion." - ) - return - - # Set goal - self.set_joint_goal( - joint_positions=joint_positions, - joint_names=joint_names, - tolerance=tolerance, - weight=weight, - ) - # Define starting state as the current state - if self.joint_state is not None: - self.__move_action_goal.request.start_state.joint_state = ( - self.joint_state - ) - # Send to goal to the server (async) - both planning and execution - self._send_goal_async_move_action() - # Clear all previous goal constrains - self.clear_goal_constraints() - self.clear_path_constraints() - - else: - # Plan via MoveIt 2 and then execute directly with the controller - self.execute( - self.plan( - joint_positions=joint_positions, - joint_names=joint_names, - tolerance_joint_position=tolerance, - weight_joint_position=weight, - ) - ) - - def plan( - self, - pose: Optional[Union[PoseStamped, Pose]] = None, - position: Optional[Union[Point, Tuple[float, float, float]]] = None, - quat_xyzw: Optional[ - Union[Quaternion, Tuple[float, float, float, float]] - ] = None, - joint_positions: Optional[List[float]] = None, - joint_names: Optional[List[str]] = None, - frame_id: Optional[str] = None, - target_link: Optional[str] = None, - tolerance_position: float = 0.001, - tolerance_orientation: Union[float, Tuple[float, float, float]] = 0.001, - tolerance_joint_position: float = 0.001, - weight_position: float = 1.0, - weight_orientation: float = 1.0, - weight_joint_position: float = 1.0, - start_joint_state: Optional[Union[JointState, List[float]]] = None, - cartesian: bool = False, - max_step: float = 0.0025, - cartesian_fraction_threshold: float = 0.0, - ) -> Optional[JointTrajectory]: - """ - Call plan_async and wait on future - """ - future = self.plan_async( - **{ - key: value - for key, value in locals().items() - if key not in ["self", "cartesian_fraction_threshold"] - } - ) - - if future is None: - return None - - # 100ms sleep - rate = self._node.create_rate(10) - while not future.done(): - rate.sleep() - - return self.get_trajectory( - future, - cartesian=cartesian, - cartesian_fraction_threshold=cartesian_fraction_threshold, - ) - - def plan_async( - self, - pose: Optional[Union[PoseStamped, Pose]] = None, - position: Optional[Union[Point, Tuple[float, float, float]]] = None, - quat_xyzw: Optional[ - Union[Quaternion, Tuple[float, float, float, float]] - ] = None, - joint_positions: Optional[List[float]] = None, - joint_names: Optional[List[str]] = None, - frame_id: Optional[str] = None, - target_link: Optional[str] = None, - tolerance_position: float = 0.001, - tolerance_orientation: Union[float, Tuple[float, float, float]] = 0.001, - tolerance_joint_position: float = 0.001, - weight_position: float = 1.0, - weight_orientation: float = 1.0, - weight_joint_position: float = 1.0, - start_joint_state: Optional[Union[JointState, List[float]]] = None, - cartesian: bool = False, - max_step: float = 0.0025, - ) -> Optional[Future]: - """ - Plan motion based on previously set goals. Optional arguments can be passed in to - internally use `set_position_goal()`, `set_orientation_goal()` or `set_joint_goal()` - to define a goal during the call. If no trajectory is found within the timeout - duration, `None` is returned. To plan from the different position than the current - one, optional argument `start_` can be defined. - """ - - pose_stamped = None - if pose is not None: - if isinstance(pose, PoseStamped): - pose_stamped = pose - elif isinstance(pose, Pose): - pose_stamped = PoseStamped( - header=Header( - stamp=self._node.get_clock().now().to_msg(), - frame_id=( - frame_id if frame_id is not None else self.__base_link_name - ), - ), - pose=pose, - ) - - self.set_position_goal( - position=pose_stamped.pose.position, - frame_id=pose_stamped.header.frame_id, - target_link=target_link, - tolerance=tolerance_position, - weight=weight_position, - ) - self.set_orientation_goal( - quat_xyzw=pose_stamped.pose.orientation, - frame_id=pose_stamped.header.frame_id, - target_link=target_link, - tolerance=tolerance_orientation, - weight=weight_orientation, - ) - else: - if position is not None: - if not isinstance(position, Point): - position = Point( - x=float(position[0]), y=float(position[1]), z=float(position[2]) - ) - - self.set_position_goal( - position=position, - frame_id=frame_id, - target_link=target_link, - tolerance=tolerance_position, - weight=weight_position, - ) - - if quat_xyzw is not None: - if not isinstance(quat_xyzw, Quaternion): - quat_xyzw = Quaternion( - x=float(quat_xyzw[0]), - y=float(quat_xyzw[1]), - z=float(quat_xyzw[2]), - w=float(quat_xyzw[3]), - ) - - self.set_orientation_goal( - quat_xyzw=quat_xyzw, - frame_id=frame_id, - target_link=target_link, - tolerance=tolerance_orientation, - weight=weight_orientation, - ) - - if joint_positions is not None: - self.set_joint_goal( - joint_positions=joint_positions, - joint_names=joint_names, - tolerance=tolerance_joint_position, - weight=weight_joint_position, - ) - - # Define starting state for the plan (default to the current state) - if start_joint_state is not None: - if isinstance(start_joint_state, JointState): - self.__move_action_goal.request.start_state.joint_state = ( - start_joint_state - ) - else: - self.__move_action_goal.request.start_state.joint_state = ( - init_joint_state( - joint_names=self.__joint_names, - joint_positions=start_joint_state, - ) - ) - elif self.joint_state is not None: - self.__move_action_goal.request.start_state.joint_state = self.joint_state - - # Plan trajectory asynchronously by service call - if cartesian: - future = self._plan_cartesian_path( - max_step=max_step, - frame_id=( - pose_stamped.header.frame_id - if pose_stamped is not None - else frame_id - ), - ) - else: - # Use service - future = self._plan_kinematic_path() - - # Clear all previous goal constrains - self.clear_goal_constraints() - self.clear_path_constraints() - - return future - - def get_trajectory( - self, - future: Future, - cartesian: bool = False, - cartesian_fraction_threshold: float = 0.0, - ) -> Optional[JointTrajectory]: - """ - Takes in a future returned by plan_async and returns the trajectory if the future is done - and planning was successful, else None. - - For cartesian plans, the plan is rejected if the fraction of the path that was completed is - less than `cartesian_fraction_threshold`. - """ - if not future.done(): - self._node.get_logger().warn( - "Cannot get trajectory because future is not done." - ) - return None - - res = future.result() - - # Cartesian - if cartesian: - if MoveItErrorCodes.SUCCESS == res.error_code.val: - if res.fraction >= cartesian_fraction_threshold: - return res.solution.joint_trajectory - else: - self._node.get_logger().warn( - f"Planning failed! Cartesian planner completed {res.fraction} " - f"of the trajectory, less than the threshold {cartesian_fraction_threshold}." - ) - return None - else: - self._node.get_logger().warn( - f"Planning failed! Error code: {res.error_code.val}." - ) - return None - - # Else Kinematic - res = res.motion_plan_response - if MoveItErrorCodes.SUCCESS == res.error_code.val: - return res.trajectory.joint_trajectory - else: - self._node.get_logger().warn( - f"Planning failed! Error code: {res.error_code.val}." - ) - return None - - def execute(self, joint_trajectory: JointTrajectory): - """ - Execute joint_trajectory by communicating directly with the controller. - """ - - if self.__ignore_new_calls_while_executing and ( - self.__is_motion_requested or self.__is_executing - ): - self._node.get_logger().warn( - "Controller is already following a trajectory. Skipping motion." - ) - return - - execute_trajectory_goal = init_execute_trajectory_goal( - joint_trajectory=joint_trajectory - ) - - if execute_trajectory_goal is None: - self._node.get_logger().warn( - "Cannot execute motion because the provided/planned trajectory is invalid." - ) - return - - self._send_goal_async_execute_trajectory(goal=execute_trajectory_goal) - - def wait_until_executed(self) -> bool: - """ - Wait until the previously requested motion is finalised through either a success or failure. - """ - - if not self.__is_motion_requested: - self._node.get_logger().warn( - "Cannot wait until motion is executed (no motion is in progress)." - ) - return False - - while self.__is_motion_requested or self.__is_executing: - self.__wait_until_executed_rate.sleep() - - return self.motion_suceeded - - def reset_controller( - self, joint_state: Union[JointState, List[float]], sync: bool = True - ): - """ - Reset controller to a given `joint_state` by sending a dummy joint trajectory. - This is useful for simulated robots that allow instantaneous reset of joints. - """ - - if not isinstance(joint_state, JointState): - joint_state = init_joint_state( - joint_names=self.__joint_names, - joint_positions=joint_state, - ) - joint_trajectory = init_dummy_joint_trajectory_from_state(joint_state) - execute_trajectory_goal = init_execute_trajectory_goal( - joint_trajectory=joint_trajectory - ) - - self._send_goal_async_execute_trajectory( - goal=execute_trajectory_goal, - wait_until_response=sync, - ) - - def set_pose_goal( - self, - pose: Optional[Union[PoseStamped, Pose]] = None, - position: Optional[Union[Point, Tuple[float, float, float]]] = None, - quat_xyzw: Optional[ - Union[Quaternion, Tuple[float, float, float, float]] - ] = None, - frame_id: Optional[str] = None, - target_link: Optional[str] = None, - tolerance_position: float = 0.001, - tolerance_orientation: Union[float, Tuple[float, float, float]] = 0.001, - weight_position: float = 1.0, - weight_orientation: float = 1.0, - ): - """ - This is direct combination of `set_position_goal()` and `set_orientation_goal()`. - """ - - if (pose is None) and (position is None or quat_xyzw is None): - raise ValueError( - "Either `pose` or `position` and `quat_xyzw` must be specified!" - ) - - if isinstance(pose, PoseStamped): - pose_stamped = pose - elif isinstance(pose, Pose): - pose_stamped = PoseStamped( - header=Header( - stamp=self._node.get_clock().now().to_msg(), - frame_id=( - frame_id if frame_id is not None else self.__base_link_name - ), - ), - pose=pose, - ) - else: - if not isinstance(position, Point): - position = Point( - x=float(position[0]), y=float(position[1]), z=float(position[2]) - ) - if not isinstance(quat_xyzw, Quaternion): - quat_xyzw = Quaternion( - x=float(quat_xyzw[0]), - y=float(quat_xyzw[1]), - z=float(quat_xyzw[2]), - w=float(quat_xyzw[3]), - ) - pose_stamped = PoseStamped( - header=Header( - stamp=self._node.get_clock().now().to_msg(), - frame_id=( - frame_id if frame_id is not None else self.__base_link_name - ), - ), - pose=Pose(position=position, orientation=quat_xyzw), - ) - - self.set_position_goal( - position=pose_stamped.pose.position, - frame_id=pose_stamped.header.frame_id, - target_link=target_link, - tolerance=tolerance_position, - weight=weight_position, - ) - self.set_orientation_goal( - quat_xyzw=pose_stamped.pose.orientation, - frame_id=pose_stamped.header.frame_id, - target_link=target_link, - tolerance=tolerance_orientation, - weight=weight_orientation, - ) - - def create_position_constraint( - self, - position: Union[Point, Tuple[float, float, float]], - frame_id: Optional[str] = None, - target_link: Optional[str] = None, - tolerance: float = 0.001, - weight: float = 1.0, - ) -> PositionConstraint: - """ - Create Cartesian position constraint of `target_link` with respect to `frame_id`. - - `frame_id` defaults to the base link - - `target_link` defaults to end effector - """ - - # Create new position constraint - constraint = PositionConstraint() - - # Define reference frame and target link - constraint.header.frame_id = ( - frame_id if frame_id is not None else self.__base_link_name - ) - constraint.link_name = ( - target_link if target_link is not None else self.__end_effector_name - ) - - # Define target position - constraint.constraint_region.primitive_poses.append(Pose()) - if isinstance(position, Point): - constraint.constraint_region.primitive_poses[0].position = position - else: - constraint.constraint_region.primitive_poses[0].position.x = float( - position[0] - ) - constraint.constraint_region.primitive_poses[0].position.y = float( - position[1] - ) - constraint.constraint_region.primitive_poses[0].position.z = float( - position[2] - ) - - # Define goal region as a sphere with radius equal to the tolerance - constraint.constraint_region.primitives.append(SolidPrimitive()) - constraint.constraint_region.primitives[0].type = 2 # Sphere - constraint.constraint_region.primitives[0].dimensions = [tolerance] - - # Set weight of the constraint - constraint.weight = weight - - return constraint - - def set_position_goal( - self, - position: Union[Point, Tuple[float, float, float]], - frame_id: Optional[str] = None, - target_link: Optional[str] = None, - tolerance: float = 0.001, - weight: float = 1.0, - ): - """ - Set Cartesian position goal of `target_link` with respect to `frame_id`. - - `frame_id` defaults to the base link - - `target_link` defaults to end effector - """ - - constraint = self.create_position_constraint( - position=position, - frame_id=frame_id, - target_link=target_link, - tolerance=tolerance, - weight=weight, - ) - - # Append to other constraints - self.__move_action_goal.request.goal_constraints[ - -1 - ].position_constraints.append(constraint) - - def create_orientation_constraint( - self, - quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], - frame_id: Optional[str] = None, - target_link: Optional[str] = None, - tolerance: Union[float, Tuple[float, float, float]] = 0.001, - weight: float = 1.0, - parameterization: int = 0, # 0: Euler, 1: Rotation Vector - ) -> OrientationConstraint: - """ - Create a Cartesian orientation constraint of `target_link` with respect to `frame_id`. - - `frame_id` defaults to the base link - - `target_link` defaults to end effector - """ - - # Create new position constraint - constraint = OrientationConstraint() - - # Define reference frame and target link - constraint.header.frame_id = ( - frame_id if frame_id is not None else self.__base_link_name - ) - constraint.link_name = ( - target_link if target_link is not None else self.__end_effector_name - ) - - # Define target orientation - if isinstance(quat_xyzw, Quaternion): - constraint.orientation = quat_xyzw - else: - constraint.orientation.x = float(quat_xyzw[0]) - constraint.orientation.y = float(quat_xyzw[1]) - constraint.orientation.z = float(quat_xyzw[2]) - constraint.orientation.w = float(quat_xyzw[3]) - - # Define tolerances - if type(tolerance) == float: - tolerance_xyz = (tolerance, tolerance, tolerance) - else: - tolerance_xyz = tolerance - constraint.absolute_x_axis_tolerance = tolerance_xyz[0] - constraint.absolute_y_axis_tolerance = tolerance_xyz[1] - constraint.absolute_z_axis_tolerance = tolerance_xyz[2] - - # Define parameterization (how to interpret the tolerance) - constraint.parameterization = parameterization - - # Set weight of the constraint - constraint.weight = weight - - return constraint - - def set_orientation_goal( - self, - quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], - frame_id: Optional[str] = None, - target_link: Optional[str] = None, - tolerance: Union[float, Tuple[float, float, float]] = 0.001, - weight: float = 1.0, - parameterization: int = 0, # 0: Euler, 1: Rotation Vector - ): - """ - Set Cartesian orientation goal of `target_link` with respect to `frame_id`. - - `frame_id` defaults to the base link - - `target_link` defaults to end effector - """ - - constraint = self.create_orientation_constraint( - quat_xyzw=quat_xyzw, - frame_id=frame_id, - target_link=target_link, - tolerance=tolerance, - weight=weight, - parameterization=parameterization, - ) - - # Append to other constraints - self.__move_action_goal.request.goal_constraints[ - -1 - ].orientation_constraints.append(constraint) - - def create_joint_constraints( - self, - joint_positions: List[float], - joint_names: Optional[List[str]] = None, - tolerance: float = 0.001, - weight: float = 1.0, - ) -> List[JointConstraint]: - """ - Creates joint space constraints. With `joint_names` specified, `joint_positions` can be - defined for specific joints in an arbitrary order. Otherwise, first **n** joints - passed into the constructor is used, where **n** is the length of `joint_positions`. - """ - - constraints = [] - - # Use default joint names if not specified - if joint_names == None: - joint_names = self.__joint_names - - for i in range(len(joint_positions)): - # Create a new constraint for each joint - constraint = JointConstraint() - - # Define joint name - constraint.joint_name = joint_names[i] - - # Define the target joint position - constraint.position = joint_positions[i] - - # Define telerances - constraint.tolerance_above = tolerance - constraint.tolerance_below = tolerance - - # Set weight of the constraint - constraint.weight = weight - - constraints.append(constraint) - - return constraints - - def set_joint_goal( - self, - joint_positions: List[float], - joint_names: Optional[List[str]] = None, - tolerance: float = 0.001, - weight: float = 1.0, - ): - """ - Set joint space goal. With `joint_names` specified, `joint_positions` can be - defined for specific joints in an arbitrary order. Otherwise, first **n** joints - passed into the constructor is used, where **n** is the length of `joint_positions`. - """ - - constraints = self.create_joint_constraints( - joint_positions=joint_positions, - joint_names=joint_names, - tolerance=tolerance, - weight=weight, - ) - - # Append to other constraints - self.__move_action_goal.request.goal_constraints[-1].joint_constraints.extend( - constraints - ) - - def clear_goal_constraints(self): - """ - Clear all goal constraints that were previously set. - Note that this function is called automatically after each `plan_kinematic_path()`. - """ - - self.__move_action_goal.request.goal_constraints = [Constraints()] - - def create_new_goal_constraint(self): - """ - Create a new set of goal constraints that will be set together with the request. Each - subsequent setting of goals with `set_joint_goal()`, `set_pose_goal()` and others will be - added under this newly created set of constraints. - """ - - self.__move_action_goal.request.goal_constraints.append(Constraints()) - - def set_path_joint_constraint( - self, - joint_positions: List[float], - joint_names: Optional[List[str]] = None, - tolerance: float = 0.001, - weight: float = 1.0, - ): - """ - Set joint space path constraints. With `joint_names` specified, `joint_positions` can be - defined for specific joints in an arbitrary order. Otherwise, first **n** joints - passed into the constructor is used, where **n** is the length of `joint_positions`. - """ - - constraints = self.create_joint_constraints( - joint_positions=joint_positions, - joint_names=joint_names, - tolerance=tolerance, - weight=weight, - ) - - # Append to other constraints - self.__move_action_goal.request.path_constraints.joint_constraints.extend( - constraints - ) - - def set_path_position_constraint( - self, - position: Union[Point, Tuple[float, float, float]], - frame_id: Optional[str] = None, - target_link: Optional[str] = None, - tolerance: float = 0.001, - weight: float = 1.0, - ): - """ - Set Cartesian position path constraint of `target_link` with respect to `frame_id`. - - `frame_id` defaults to the base link - - `target_link` defaults to end effector - """ - - constraint = self.create_position_constraint( - position=position, - frame_id=frame_id, - target_link=target_link, - tolerance=tolerance, - weight=weight, - ) - - # Append to other constraints - self.__move_action_goal.request.path_constraints.position_constraints.append( - constraint - ) - - def set_path_orientation_constraint( - self, - quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], - frame_id: Optional[str] = None, - target_link: Optional[str] = None, - tolerance: Union[float, Tuple[float, float, float]] = 0.001, - weight: float = 1.0, - parameterization: int = 0, # 0: Euler Angles, 1: Rotation Vector - ): - """ - Set Cartesian orientation path constraint of `target_link` with respect to `frame_id`. - - `frame_id` defaults to the base link - - `target_link` defaults to end effector - """ - - constraint = self.create_orientation_constraint( - quat_xyzw=quat_xyzw, - frame_id=frame_id, - target_link=target_link, - tolerance=tolerance, - weight=weight, - parameterization=parameterization, - ) - - # Append to other constraints - self.__move_action_goal.request.path_constraints.orientation_constraints.append( - constraint - ) - - def clear_path_constraints(self): - """ - Clear all path constraints that were previously set. - Note that this function is called automatically after each `plan_kinematic_path()`. - """ - - self.__move_action_goal.request.path_constraints = Constraints() - - def compute_fk( - self, - joint_state: Optional[Union[JointState, List[float]]] = None, - fk_link_names: Optional[List[str]] = None, - ) -> Optional[Union[PoseStamped, List[PoseStamped]]]: - """ - Call compute_fk_async and wait on future - """ - future = self.compute_fk_async( - **{key: value for key, value in locals().items() if key != "self"} - ) - - if future is None: - return None - - # 100ms sleep - rate = self._node.create_rate(10) - while not future.done(): - rate.sleep() - - return self.get_compute_fk_result(future, fk_link_names=fk_link_names) - - def get_compute_fk_result( - self, - future: Future, - fk_link_names: Optional[List[str]] = None, - ) -> Optional[Union[PoseStamped, List[PoseStamped]]]: - """ - Takes in a future returned by compute_fk_async and returns the poses - if the future is done and successful, else None. - """ - if not future.done(): - self._node.get_logger().warn( - "Cannot get FK result because future is not done." - ) - return None - - res = future.result() - - if MoveItErrorCodes.SUCCESS == res.error_code.val: - if fk_link_names is None: - return res.pose_stamped[0] - else: - return res.pose_stamped - else: - self._node.get_logger().warn( - f"FK computation failed! Error code: {res.error_code.val}." - ) - return None - - def compute_fk_async( - self, - joint_state: Optional[Union[JointState, List[float]]] = None, - fk_link_names: Optional[List[str]] = None, - ) -> Optional[Future]: - """ - Compute forward kinematics for all `fk_link_names` in a given `joint_state`. - - `fk_link_names` defaults to end-effector - - `joint_state` defaults to the current joint state - """ - - if not hasattr(self, "__compute_fk_client"): - self.__init_compute_fk() - - if fk_link_names is None: - self.__compute_fk_req.fk_link_names = [self.__end_effector_name] - else: - self.__compute_fk_req.fk_link_names = fk_link_names - - if joint_state is not None: - if isinstance(joint_state, JointState): - self.__compute_fk_req.robot_state.joint_state = joint_state - else: - self.__compute_fk_req.robot_state.joint_state = init_joint_state( - joint_names=self.__joint_names, - joint_positions=joint_state, - ) - elif self.joint_state is not None: - self.__compute_fk_req.robot_state.joint_state = self.joint_state - - stamp = self._node.get_clock().now().to_msg() - self.__compute_fk_req.header.stamp = stamp - - if not self.__compute_fk_client.service_is_ready(): - self._node.get_logger().warn( - f"Service '{self.__compute_fk_client.srv_name}' is not yet available. Better luck next time!" - ) - return None - - return self.__compute_fk_client.call_async(self.__compute_fk_req) - - def compute_ik( - self, - position: Union[Point, Tuple[float, float, float]], - quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], - start_joint_state: Optional[Union[JointState, List[float]]] = None, - constraints: Optional[Constraints] = None, - wait_for_server_timeout_sec: Optional[float] = 1.0, - ) -> Optional[JointState]: - """ - Call compute_ik_async and wait on future - """ - future = self.compute_ik_async( - **{key: value for key, value in locals().items() if key != "self"} - ) - - if future is None: - return None - - # 10ms sleep - rate = self._node.create_rate(10) - while not future.done(): - rate.sleep() - - return self.get_compute_ik_result(future) - - def get_compute_ik_result( - self, - future: Future, - ) -> Optional[JointState]: - """ - Takes in a future returned by compute_ik_async and returns the joint states - if the future is done and successful, else None. - """ - if not future.done(): - self._node.get_logger().warn( - "Cannot get IK result because future is not done." - ) - return None - - res = future.result() - - if MoveItErrorCodes.SUCCESS == res.error_code.val: - return res.solution.joint_state - else: - self._node.get_logger().warn( - f"IK computation failed! Error code: {res.error_code.val}." - ) - return None - - def compute_ik_async( - self, - position: Union[Point, Tuple[float, float, float]], - quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], - start_joint_state: Optional[Union[JointState, List[float]]] = None, - constraints: Optional[Constraints] = None, - wait_for_server_timeout_sec: Optional[float] = 1.0, - ) -> Optional[Future]: - """ - Compute inverse kinematics for the given pose. To indicate beginning of the search space, - `start_joint_state` can be specified. Furthermore, `constraints` can be imposed on the - computed IK. - - `start_joint_state` defaults to current joint state. - - `constraints` defaults to None. - """ - - if not hasattr(self, "__compute_ik_client"): - self.__init_compute_ik() - - if isinstance(position, Point): - self.__compute_ik_req.ik_request.pose_stamped.pose.position = position - else: - self.__compute_ik_req.ik_request.pose_stamped.pose.position.x = float( - position[0] - ) - self.__compute_ik_req.ik_request.pose_stamped.pose.position.y = float( - position[1] - ) - self.__compute_ik_req.ik_request.pose_stamped.pose.position.z = float( - position[2] - ) - if isinstance(quat_xyzw, Quaternion): - self.__compute_ik_req.ik_request.pose_stamped.pose.orientation = quat_xyzw - else: - self.__compute_ik_req.ik_request.pose_stamped.pose.orientation.x = float( - quat_xyzw[0] - ) - self.__compute_ik_req.ik_request.pose_stamped.pose.orientation.y = float( - quat_xyzw[1] - ) - self.__compute_ik_req.ik_request.pose_stamped.pose.orientation.z = float( - quat_xyzw[2] - ) - self.__compute_ik_req.ik_request.pose_stamped.pose.orientation.w = float( - quat_xyzw[3] - ) - - if start_joint_state is not None: - if isinstance(start_joint_state, JointState): - self.__compute_ik_req.ik_request.robot_state.joint_state = ( - start_joint_state - ) - else: - self.__compute_ik_req.ik_request.robot_state.joint_state = ( - init_joint_state( - joint_names=self.__joint_names, - joint_positions=start_joint_state, - ) - ) - elif self.joint_state is not None: - self.__compute_ik_req.ik_request.robot_state.joint_state = self.joint_state - - if constraints is not None: - self.__compute_ik_req.ik_request.constraints = constraints - - stamp = self._node.get_clock().now().to_msg() - self.__compute_ik_req.ik_request.pose_stamped.header.stamp = stamp - - if not self.__compute_ik_client.wait_for_service( - timeout_sec=wait_for_server_timeout_sec - ): - self._node.get_logger().warn( - f"Service '{self.__compute_ik_client.srv_name}' is not yet available. Better luck next time!" - ) - return None - - return self.__compute_ik_client.call_async(self.__compute_ik_req) - - def reset_new_joint_state_checker(self): - """ - Reset checker of the new joint state. - """ - - self.__joint_state_mutex.acquire() - self.__new_joint_state_available = False - self.__joint_state_mutex.release() - - def force_reset_executing_state(self): - """ - Force reset of internal states that block execution while `ignore_new_calls_while_executing` is being - used. This function is applicable only in a very few edge-cases, so it should almost never be used. - """ - - self.__execution_mutex.acquire() - self.__is_motion_requested = False - self.__is_executing = False - self.__execution_mutex.release() - - def add_collision_primitive( - self, - id: str, - primitive_type: int, - dimensions: Tuple[float, float, float], - pose: Optional[Union[PoseStamped, Pose]] = None, - position: Optional[Union[Point, Tuple[float, float, float]]] = None, - quat_xyzw: Optional[ - Union[Quaternion, Tuple[float, float, float, float]] - ] = None, - frame_id: Optional[str] = None, - operation: int = CollisionObject.ADD, - ): - """ - Add collision object with a primitive geometry specified by its dimensions. - - `primitive_type` can be one of the following: - - `SolidPrimitive.BOX` - - `SolidPrimitive.SPHERE` - - `SolidPrimitive.CYLINDER` - - `SolidPrimitive.CONE` - """ - - if (pose is None) and (position is None or quat_xyzw is None): - raise ValueError( - "Either `pose` or `position` and `quat_xyzw` must be specified!" - ) - - if isinstance(pose, PoseStamped): - pose_stamped = pose - elif isinstance(pose, Pose): - pose_stamped = PoseStamped( - header=Header( - stamp=self._node.get_clock().now().to_msg(), - frame_id=( - frame_id if frame_id is not None else self.__base_link_name - ), - ), - pose=pose, - ) - else: - if not isinstance(position, Point): - position = Point( - x=float(position[0]), y=float(position[1]), z=float(position[2]) - ) - if not isinstance(quat_xyzw, Quaternion): - quat_xyzw = Quaternion( - x=float(quat_xyzw[0]), - y=float(quat_xyzw[1]), - z=float(quat_xyzw[2]), - w=float(quat_xyzw[3]), - ) - pose_stamped = PoseStamped( - header=Header( - stamp=self._node.get_clock().now().to_msg(), - frame_id=( - frame_id if frame_id is not None else self.__base_link_name - ), - ), - pose=Pose(position=position, orientation=quat_xyzw), - ) - - msg = CollisionObject( - header=pose_stamped.header, - id=id, - operation=operation, - pose=pose_stamped.pose, - ) - - msg.primitives.append( - SolidPrimitive(type=primitive_type, dimensions=dimensions) - ) - - self.__collision_object_publisher.publish(msg) - - def add_collision_box( - self, - id: str, - size: Tuple[float, float, float], - pose: Optional[Union[PoseStamped, Pose]] = None, - position: Optional[Union[Point, Tuple[float, float, float]]] = None, - quat_xyzw: Optional[ - Union[Quaternion, Tuple[float, float, float, float]] - ] = None, - frame_id: Optional[str] = None, - operation: int = CollisionObject.ADD, - ): - """ - Add collision object with a box geometry specified by its size. - """ - - assert len(size) == 3, "Invalid size of the box!" - - self.add_collision_primitive( - id=id, - primitive_type=SolidPrimitive.BOX, - dimensions=size, - pose=pose, - position=position, - quat_xyzw=quat_xyzw, - frame_id=frame_id, - operation=operation, - ) - - def add_collision_sphere( - self, - id: str, - radius: float, - pose: Optional[Union[PoseStamped, Pose]] = None, - position: Optional[Union[Point, Tuple[float, float, float]]] = None, - quat_xyzw: Optional[ - Union[Quaternion, Tuple[float, float, float, float]] - ] = None, - frame_id: Optional[str] = None, - operation: int = CollisionObject.ADD, - ): - """ - Add collision object with a sphere geometry specified by its radius. - """ - - if quat_xyzw is None: - quat_xyzw = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) - - self.add_collision_primitive( - id=id, - primitive_type=SolidPrimitive.SPHERE, - dimensions=[ - radius, - ], - pose=pose, - position=position, - quat_xyzw=quat_xyzw, - frame_id=frame_id, - operation=operation, - ) - - def add_collision_cylinder( - self, - id: str, - height: float, - radius: float, - pose: Optional[Union[PoseStamped, Pose]] = None, - position: Optional[Union[Point, Tuple[float, float, float]]] = None, - quat_xyzw: Optional[ - Union[Quaternion, Tuple[float, float, float, float]] - ] = None, - frame_id: Optional[str] = None, - operation: int = CollisionObject.ADD, - ): - """ - Add collision object with a cylinder geometry specified by its height and radius. - """ - - self.add_collision_primitive( - id=id, - primitive_type=SolidPrimitive.CYLINDER, - dimensions=[height, radius], - pose=pose, - position=position, - quat_xyzw=quat_xyzw, - frame_id=frame_id, - operation=operation, - ) - - def add_collision_cone( - self, - id: str, - height: float, - radius: float, - pose: Optional[Union[PoseStamped, Pose]] = None, - position: Optional[Union[Point, Tuple[float, float, float]]] = None, - quat_xyzw: Optional[ - Union[Quaternion, Tuple[float, float, float, float]] - ] = None, - frame_id: Optional[str] = None, - operation: int = CollisionObject.ADD, - ): - """ - Add collision object with a cone geometry specified by its height and radius. - """ - - self.add_collision_primitive( - id=id, - primitive_type=SolidPrimitive.CONE, - dimensions=[height, radius], - pose=pose, - position=position, - quat_xyzw=quat_xyzw, - frame_id=frame_id, - operation=operation, - ) - - def add_collision_mesh( - self, - filepath: Optional[str], - id: str, - pose: Optional[Union[PoseStamped, Pose]] = None, - position: Optional[Union[Point, Tuple[float, float, float]]] = None, - quat_xyzw: Optional[ - Union[Quaternion, Tuple[float, float, float, float]] - ] = None, - frame_id: Optional[str] = None, - operation: int = CollisionObject.ADD, - scale: Union[float, Tuple[float, float, float]] = 1.0, - mesh: Optional[Any] = None, - ): - """ - Add collision object with a mesh geometry. Either `filepath` must be - specified or `mesh` must be provided. - Note: This function required 'trimesh' Python module to be installed. - """ - - # Load the mesh - try: - import trimesh - except ImportError as err: - raise ImportError( - "Python module 'trimesh' not found! Please install it manually in order " - "to add collision objects into the MoveIt 2 planning scene." - ) from err - - # Check the parameters - if (pose is None) and (position is None or quat_xyzw is None): - raise ValueError( - "Either `pose` or `position` and `quat_xyzw` must be specified!" - ) - if (filepath is None and mesh is None) or ( - filepath is not None and mesh is not None - ): - raise ValueError("Exactly one of `filepath` or `mesh` must be specified!") - if mesh is not None and not isinstance(mesh, trimesh.Trimesh): - raise ValueError("`mesh` must be an instance of `trimesh.Trimesh`!") - - if isinstance(pose, PoseStamped): - pose_stamped = pose - elif isinstance(pose, Pose): - pose_stamped = PoseStamped( - header=Header( - stamp=self._node.get_clock().now().to_msg(), - frame_id=( - frame_id if frame_id is not None else self.__base_link_name - ), - ), - pose=pose, - ) - else: - if not isinstance(position, Point): - position = Point( - x=float(position[0]), y=float(position[1]), z=float(position[2]) - ) - if not isinstance(quat_xyzw, Quaternion): - quat_xyzw = Quaternion( - x=float(quat_xyzw[0]), - y=float(quat_xyzw[1]), - z=float(quat_xyzw[2]), - w=float(quat_xyzw[3]), - ) - pose_stamped = PoseStamped( - header=Header( - stamp=self._node.get_clock().now().to_msg(), - frame_id=( - frame_id if frame_id is not None else self.__base_link_name - ), - ), - pose=Pose(position=position, orientation=quat_xyzw), - ) - - msg = CollisionObject( - header=pose_stamped.header, - id=id, - operation=operation, - pose=pose_stamped.pose, - ) - - if filepath is not None: - mesh = trimesh.load(filepath) - - # Scale the mesh - if isinstance(scale, float): - scale = (scale, scale, scale) - if not (scale[0] == scale[1] == scale[2] == 1.0): - # If the mesh was passed in as a parameter, make a copy of it to - # avoid transforming the original. - if filepath is not None: - mesh = mesh.copy() - # Transform the mesh - transform = np.eye(4) - np.fill_diagonal(transform, scale) - mesh.apply_transform(transform) - - msg.meshes.append( - Mesh( - triangles=[MeshTriangle(vertex_indices=face) for face in mesh.faces], - vertices=[ - Point(x=vert[0], y=vert[1], z=vert[2]) for vert in mesh.vertices - ], - ) - ) - - self.__collision_object_publisher.publish(msg) - - def remove_collision_object(self, id: str): - """ - Remove collision object specified by its `id`. - """ - - msg = CollisionObject() - msg.id = id - msg.operation = CollisionObject.REMOVE - msg.header.stamp = self._node.get_clock().now().to_msg() - self.__collision_object_publisher.publish(msg) - - def remove_collision_mesh(self, id: str): - """ - Remove collision mesh specified by its `id`. - Identical to `remove_collision_object()`. - """ - - self.remove_collision_object(id) - - def attach_collision_object( - self, - id: str, - link_name: Optional[str] = None, - touch_links: List[str] = [], - weight: float = 0.0, - ): - """ - Attach collision object to the robot. - """ - - if link_name is None: - link_name = self.__end_effector_name - - msg = AttachedCollisionObject( - object=CollisionObject(id=id, operation=CollisionObject.ADD) - ) - msg.link_name = link_name - msg.touch_links = touch_links - msg.weight = weight - - self.__attached_collision_object_publisher.publish(msg) - - def detach_collision_object(self, id: int): - """ - Detach collision object from the robot. - """ - - msg = AttachedCollisionObject( - object=CollisionObject(id=id, operation=CollisionObject.REMOVE) - ) - self.__attached_collision_object_publisher.publish(msg) - - def detach_all_collision_objects(self): - """ - Detach collision object from the robot. - """ - - msg = AttachedCollisionObject( - object=CollisionObject(operation=CollisionObject.REMOVE) - ) - self.__attached_collision_object_publisher.publish(msg) - - def move_collision( - self, - id: str, - position: Union[Point, Tuple[float, float, float]], - quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], - frame_id: Optional[str] = None, - ): - """ - Move collision object specified by its `id`. - """ - - msg = CollisionObject() - - if not isinstance(position, Point): - position = Point( - x=float(position[0]), y=float(position[1]), z=float(position[2]) - ) - if not isinstance(quat_xyzw, Quaternion): - quat_xyzw = Quaternion( - x=float(quat_xyzw[0]), - y=float(quat_xyzw[1]), - z=float(quat_xyzw[2]), - w=float(quat_xyzw[3]), - ) - - pose = Pose() - pose.position = position - pose.orientation = quat_xyzw - msg.pose = pose - msg.id = id - msg.operation = CollisionObject.MOVE - msg.header.frame_id = ( - frame_id if frame_id is not None else self.__base_link_name - ) - msg.header.stamp = self._node.get_clock().now().to_msg() - - self.__collision_object_publisher.publish(msg) - - def update_planning_scene(self) -> bool: - """ - Gets the current planning scene. Returns whether the service call was - successful. - """ - - if not self._get_planning_scene_service.service_is_ready(): - self._node.get_logger().warn( - f"Service '{self._get_planning_scene_service.srv_name}' is not yet available. Better luck next time!" - ) - return False - self.__planning_scene = self._get_planning_scene_service.call( - GetPlanningScene.Request() - ).scene - return True - - def allow_collisions(self, id: str, allow: bool) -> Optional[Future]: - """ - Takes in the ID of an element in the planning scene. Modifies the allowed - collision matrix to (dis)allow collisions between that object and all other - object. - - If `allow` is True, a plan will succeed even if the robot collides with that object. - If `allow` is False, a plan will fail if the robot collides with that object. - Returns whether it successfully updated the allowed collision matrix. - - Returns the future of the service call. - """ - # Update the planning scene - if not self.update_planning_scene(): - return None - allowed_collision_matrix = self.__planning_scene.allowed_collision_matrix - self.__old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix) - - # Get the location in the allowed collision matrix of the object - j = None - if id not in allowed_collision_matrix.entry_names: - allowed_collision_matrix.entry_names.append(id) - else: - j = allowed_collision_matrix.entry_names.index(id) - # For all other objects, (dis)allow collisions with the object with `id` - for i in range(len(allowed_collision_matrix.entry_values)): - if j is None: - allowed_collision_matrix.entry_values[i].enabled.append(allow) - elif i != j: - allowed_collision_matrix.entry_values[i].enabled[j] = allow - # For the object with `id`, (dis)allow collisions with all other objects - allowed_collision_entry = AllowedCollisionEntry( - enabled=[allow for _ in range(len(allowed_collision_matrix.entry_names))] - ) - if j is None: - allowed_collision_matrix.entry_values.append(allowed_collision_entry) - else: - allowed_collision_matrix.entry_values[j] = allowed_collision_entry - - # Apply the new planning scene - if not self._apply_planning_scene_service.service_is_ready(): - self._node.get_logger().warn( - f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!" - ) - return None - return self._apply_planning_scene_service.call_async( - ApplyPlanningScene.Request(scene=self.__planning_scene) - ) - - def process_allow_collision_future(self, future: Future) -> bool: - """ - Return whether the allow collision service call is done and has succeeded - or not. If it failed, reset the allowed collision matrix to the old one. - """ - if not future.done(): - return False - - # Get response - resp = future.result() - - # If it failed, restore the old planning scene - if not resp.success: - self.__planning_scene.allowed_collision_matrix = ( - self.__old_allowed_collision_matrix - ) - - return resp.success - - def clear_all_collision_objects(self) -> Optional[Future]: - """ - Removes all attached and un-attached collision objects from the planning scene. - - Returns a future for the ApplyPlanningScene service call. - """ - # Update the planning scene - if not self.update_planning_scene(): - return None - self.__old_planning_scene = copy.deepcopy(self.__planning_scene) - - # Remove all collision objects from the planning scene - self.__planning_scene.world.collision_objects = [] - self.__planning_scene.robot_state.attached_collision_objects = [] - - # Apply the new planning scene - if not self._apply_planning_scene_service.service_is_ready(): - self._node.get_logger().warn( - f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!" - ) - return None - return self._apply_planning_scene_service.call_async( - ApplyPlanningScene.Request(scene=self.__planning_scene) - ) - - def cancel_clear_all_collision_objects_future(self, future: Future): - """ - Cancel the clear all collision objects service call. - """ - self._apply_planning_scene_service.remove_pending_request(future) - - def process_clear_all_collision_objects_future(self, future: Future) -> bool: - """ - Return whether the clear all collision objects service call is done and has succeeded - or not. If it failed, restore the old planning scene. - """ - if not future.done(): - return False - - # Get response - resp = future.result() - - # If it failed, restore the old planning scene - if not resp.success: - self.__planning_scene = self.__old_planning_scene - - return resp.success - - def __joint_state_callback(self, msg: JointState): - # Update only if all relevant joints are included in the message - for joint_name in self.joint_names: - if not joint_name in msg.name: - return - - self.__joint_state_mutex.acquire() - self.__joint_state = msg - self.__new_joint_state_available = True - self.__joint_state_mutex.release() - - def _plan_kinematic_path(self) -> Optional[Future]: - # Reuse request from move action goal - self.__kinematic_path_request.motion_plan_request = ( - self.__move_action_goal.request - ) - - stamp = self._node.get_clock().now().to_msg() - self.__kinematic_path_request.motion_plan_request.workspace_parameters.header.stamp = ( - stamp - ) - for ( - constraints - ) in self.__kinematic_path_request.motion_plan_request.goal_constraints: - for position_constraint in constraints.position_constraints: - position_constraint.header.stamp = stamp - for orientation_constraint in constraints.orientation_constraints: - orientation_constraint.header.stamp = stamp - - if not self._plan_kinematic_path_service.service_is_ready(): - self._node.get_logger().warn( - f"Service '{self._plan_kinematic_path_service.srv_name}' is not yet available. Better luck next time!" - ) - return None - - return self._plan_kinematic_path_service.call_async( - self.__kinematic_path_request - ) - - def _plan_cartesian_path( - self, - max_step: float = 0.0025, - frame_id: Optional[str] = None, - ) -> Optional[Future]: - # Reuse request from move action goal - self.__cartesian_path_request.start_state = ( - self.__move_action_goal.request.start_state - ) - - # The below attributes were introduced in Iron and do not exist in Humble. - if hasattr(self.__cartesian_path_request, "max_velocity_scaling_factor"): - self.__cartesian_path_request.max_velocity_scaling_factor = ( - self.__move_action_goal.request.max_velocity_scaling_factor - ) - if hasattr(self.__cartesian_path_request, "max_acceleration_scaling_factor"): - self.__cartesian_path_request.max_acceleration_scaling_factor = ( - self.__move_action_goal.request.max_acceleration_scaling_factor - ) - - self.__cartesian_path_request.group_name = ( - self.__move_action_goal.request.group_name - ) - self.__cartesian_path_request.link_name = self.__end_effector_name - self.__cartesian_path_request.max_step = max_step - - self.__cartesian_path_request.header.frame_id = ( - frame_id if frame_id is not None else self.__base_link_name - ) - - stamp = self._node.get_clock().now().to_msg() - self.__cartesian_path_request.header.stamp = stamp - - self.__cartesian_path_request.path_constraints = ( - self.__move_action_goal.request.path_constraints - ) - for ( - position_constraint - ) in self.__cartesian_path_request.path_constraints.position_constraints: - position_constraint.header.stamp = stamp - for ( - orientation_constraint - ) in self.__cartesian_path_request.path_constraints.orientation_constraints: - orientation_constraint.header.stamp = stamp - # no header in joint_constraint message type - - target_pose = Pose() - target_pose.position = ( - self.__move_action_goal.request.goal_constraints[-1] - .position_constraints[-1] - .constraint_region.primitive_poses[0] - .position - ) - target_pose.orientation = ( - self.__move_action_goal.request.goal_constraints[-1] - .orientation_constraints[-1] - .orientation - ) - - self.__cartesian_path_request.waypoints = [target_pose] - - if not self._plan_cartesian_path_service.service_is_ready(): - self._node.get_logger().warn( - f"Service '{self._plan_cartesian_path_service.srv_name}' is not yet available. Better luck next time!" - ) - return None - - return self._plan_cartesian_path_service.call_async( - self.__cartesian_path_request - ) - - def _send_goal_async_move_action(self): - self.__execution_mutex.acquire() - stamp = self._node.get_clock().now().to_msg() - self.__move_action_goal.request.workspace_parameters.header.stamp = stamp - if not self.__move_action_client.server_is_ready(): - self._node.get_logger().warn( - f"Action server '{self.__move_action_client._action_name}' is not yet available. Better luck next time!" - ) - return - - self.__last_error_code = None - self.__is_motion_requested = True - self.__send_goal_future_move_action = self.__move_action_client.send_goal_async( - goal=self.__move_action_goal, - feedback_callback=None, - ) - - self.__send_goal_future_move_action.add_done_callback( - self.__response_callback_move_action - ) - - self.__execution_mutex.release() - - def __response_callback_move_action(self, response): - self.__execution_mutex.acquire() - goal_handle = response.result() - if not goal_handle.accepted: - self._node.get_logger().warn( - f"Action '{self.__move_action_client._action_name}' was rejected." - ) - self.__is_motion_requested = False - return - - self.__execution_goal_handle = goal_handle - self.__is_executing = True - self.__is_motion_requested = False - - self.__get_result_future_move_action = goal_handle.get_result_async() - self.__get_result_future_move_action.add_done_callback( - self.__result_callback_move_action - ) - self.__execution_mutex.release() - - def __result_callback_move_action(self, res): - self.__execution_mutex.acquire() - if res.result().status != GoalStatus.STATUS_SUCCEEDED: - self._node.get_logger().warn( - f"Action '{self.__move_action_client._action_name}' was unsuccessful: {res.result().status}." - ) - self.motion_suceeded = False - else: - self.motion_suceeded = True - - self.__last_error_code = res.result().result.error_code - - self.__execution_goal_handle = None - self.__is_executing = False - self.__execution_mutex.release() - - def _send_goal_async_execute_trajectory( - self, - goal: ExecuteTrajectory, - wait_until_response: bool = False, - ): - self.__execution_mutex.acquire() - - if not self._execute_trajectory_action_client.server_is_ready(): - self._node.get_logger().warn( - f"Action server '{self._execute_trajectory_action_client._action_name}' is not yet available. Better luck next time!" - ) - return - - self.__last_error_code = None - self.__is_motion_requested = True - self.__send_goal_future_execute_trajectory = ( - self._execute_trajectory_action_client.send_goal_async( - goal=goal, - feedback_callback=None, - ) - ) - - self.__send_goal_future_execute_trajectory.add_done_callback( - self.__response_callback_execute_trajectory - ) - self.__execution_mutex.release() - - def __response_callback_execute_trajectory(self, response): - self.__execution_mutex.acquire() - goal_handle = response.result() - if not goal_handle.accepted: - self._node.get_logger().warn( - f"Action '{self._execute_trajectory_action_client._action_name}' was rejected." - ) - self.__is_motion_requested = False - return - - self.__execution_goal_handle = goal_handle - self.__is_executing = True - self.__is_motion_requested = False - - self.__get_result_future_execute_trajectory = goal_handle.get_result_async() - self.__get_result_future_execute_trajectory.add_done_callback( - self.__result_callback_execute_trajectory - ) - self.__execution_mutex.release() - - def __response_callback_with_event_set_execute_trajectory(self, response): - self.__future_done_event.set() - - def __result_callback_execute_trajectory(self, res): - self.__execution_mutex.acquire() - if res.result().status != GoalStatus.STATUS_SUCCEEDED: - self._node.get_logger().warn( - f"Action '{self._execute_trajectory_action_client._action_name}' was unsuccessful: {res.result().status}." - ) - self.motion_suceeded = False - else: - self.motion_suceeded = True - - self.__last_error_code = res.result().result.error_code - - self.__execution_goal_handle = None - self.__is_executing = False - self.__execution_mutex.release() - - @classmethod - def __init_move_action_goal( - cls, frame_id: str, group_name: str, end_effector: str - ) -> MoveGroup.Goal: - move_action_goal = MoveGroup.Goal() - move_action_goal.request.workspace_parameters.header.frame_id = frame_id - # move_action_goal.request.workspace_parameters.header.stamp = "Set during request" - move_action_goal.request.workspace_parameters.min_corner.x = -1.0 - move_action_goal.request.workspace_parameters.min_corner.y = -1.0 - move_action_goal.request.workspace_parameters.min_corner.z = -1.0 - move_action_goal.request.workspace_parameters.max_corner.x = 1.0 - move_action_goal.request.workspace_parameters.max_corner.y = 1.0 - move_action_goal.request.workspace_parameters.max_corner.z = 1.0 - # move_action_goal.request.start_state = "Set during request" - move_action_goal.request.goal_constraints = [Constraints()] - move_action_goal.request.path_constraints = Constraints() - # move_action_goal.request.trajectory_constraints = "Ignored" - # move_action_goal.request.reference_trajectories = "Ignored" - move_action_goal.request.pipeline_id = "" - move_action_goal.request.planner_id = "" - move_action_goal.request.group_name = group_name - move_action_goal.request.num_planning_attempts = 5 - move_action_goal.request.allowed_planning_time = 0.5 - move_action_goal.request.max_velocity_scaling_factor = 0.0 - move_action_goal.request.max_acceleration_scaling_factor = 0.0 - # Note: Attribute was renamed in Iron (https://github.com/ros-planning/moveit_msgs/pull/130) - if hasattr(move_action_goal.request, "cartesian_speed_limited_link"): - move_action_goal.request.cartesian_speed_limited_link = end_effector - else: - move_action_goal.request.cartesian_speed_end_effector_link = end_effector - move_action_goal.request.max_cartesian_speed = 0.0 - - # move_action_goal.planning_options.planning_scene_diff = "Ignored" - move_action_goal.planning_options.plan_only = False - # move_action_goal.planning_options.look_around = "Ignored" - # move_action_goal.planning_options.look_around_attempts = "Ignored" - # move_action_goal.planning_options.max_safe_execution_cost = "Ignored" - # move_action_goal.planning_options.replan = "Ignored" - # move_action_goal.planning_options.replan_attempts = "Ignored" - # move_action_goal.planning_options.replan_delay = "Ignored" - - return move_action_goal - - def __init_compute_fk(self): - self.__compute_fk_client = self._node.create_client( - srv_type=GetPositionFK, - srv_name="compute_fk", - callback_group=self._callback_group, - ) - - self.__compute_fk_req = GetPositionFK.Request() - self.__compute_fk_req.header.frame_id = self.__base_link_name - # self.__compute_fk_req.header.stamp = "Set during request" - # self.__compute_fk_req.fk_link_names = "Set during request" - # self.__compute_fk_req.robot_state.joint_state = "Set during request" - # self.__compute_fk_req.robot_state.multi_dof_ = "Ignored" - # self.__compute_fk_req.robot_state.attached_collision_objects = "Ignored" - self.__compute_fk_req.robot_state.is_diff = False - - def __init_compute_ik(self): - # Service client for IK - self.__compute_ik_client = self._node.create_client( - srv_type=GetPositionIK, - srv_name="compute_ik", - callback_group=self._callback_group, - ) - - self.__compute_ik_req = GetPositionIK.Request() - self.__compute_ik_req.ik_request.group_name = self.__group_name - # self.__compute_ik_req.ik_request.robot_state.joint_state = "Set during request" - # self.__compute_ik_req.ik_request.robot_state.multi_dof_ = "Ignored" - # self.__compute_ik_req.ik_request.robot_state.attached_collision_objects = "Ignored" - self.__compute_ik_req.ik_request.robot_state.is_diff = False - # self.__compute_ik_req.ik_request.constraints = "Set during request OR Ignored" - self.__compute_ik_req.ik_request.avoid_collisions = True - # self.__compute_ik_req.ik_request.ik_link_name = "Ignored" - self.__compute_ik_req.ik_request.pose_stamped.header.frame_id = ( - self.__base_link_name - ) - # self.__compute_ik_req.ik_request.pose_stamped.header.stamp = "Set during request" - # self.__compute_ik_req.ik_request.pose_stamped.pose = "Set during request" - # self.__compute_ik_req.ik_request.ik_link_names = "Ignored" - # self.__compute_ik_req.ik_request.pose_stamped_vector = "Ignored" - # self.__compute_ik_req.ik_request.timeout.sec = "Ignored" - # self.__compute_ik_req.ik_request.timeout.nanosec = "Ignored" - - @property - def planning_scene(self) -> Optional[PlanningScene]: - return self.__planning_scene - - @property - def follow_joint_trajectory_action_client(self) -> str: - return self.__follow_joint_trajectory_action_client - - @property - def end_effector_name(self) -> str: - return self.__end_effector_name - - @property - def base_link_name(self) -> str: - return self.__base_link_name - - @property - def joint_names(self) -> List[str]: - return self.__joint_names - - @property - def joint_state(self) -> Optional[JointState]: - self.__joint_state_mutex.acquire() - joint_state = self.__joint_state - self.__joint_state_mutex.release() - return joint_state - - @property - def new_joint_state_available(self): - return self.__new_joint_state_available - - @property - def max_velocity(self) -> float: - return self.__move_action_goal.request.max_velocity_scaling_factor - - @max_velocity.setter - def max_velocity(self, value: float): - self.__move_action_goal.request.max_velocity_scaling_factor = value - - @property - def max_acceleration(self) -> float: - return self.__move_action_goal.request.max_acceleration_scaling_factor - - @max_acceleration.setter - def max_acceleration(self, value: float): - self.__move_action_goal.request.max_acceleration_scaling_factor = value - - @property - def num_planning_attempts(self) -> int: - return self.__move_action_goal.request.num_planning_attempts - - @num_planning_attempts.setter - def num_planning_attempts(self, value: int): - self.__move_action_goal.request.num_planning_attempts = value - - @property - def allowed_planning_time(self) -> float: - return self.__move_action_goal.request.allowed_planning_time - - @allowed_planning_time.setter - def allowed_planning_time(self, value: float): - self.__move_action_goal.request.allowed_planning_time = value - - @property - def cartesian_avoid_collisions(self) -> bool: - return self.__cartesian_path_request.request.avoid_collisions - - @cartesian_avoid_collisions.setter - def cartesian_avoid_collisions(self, value: bool): - self.__cartesian_path_request.avoid_collisions = value - - @property - def cartesian_jump_threshold(self) -> float: - return self.__cartesian_path_request.request.jump_threshold - - @cartesian_jump_threshold.setter - def cartesian_jump_threshold(self, value: float): - self.__cartesian_path_request.jump_threshold = value - - @property - def cartesian_prismatic_jump_threshold(self) -> float: - return self.__cartesian_path_request.request.prismatic_jump_threshold - - @cartesian_prismatic_jump_threshold.setter - def cartesian_prismatic_jump_threshold(self, value: float): - self.__cartesian_path_request.prismatic_jump_threshold = value - - @property - def cartesian_revolute_jump_threshold(self) -> float: - return self.__cartesian_path_request.request.revolute_jump_threshold - - @cartesian_revolute_jump_threshold.setter - def cartesian_revolute_jump_threshold(self, value: float): - self.__cartesian_path_request.revolute_jump_threshold = value - - @property - def pipeline_id(self) -> int: - return self.__move_action_goal.request.pipeline_id - - @pipeline_id.setter - def pipeline_id(self, value: str): - self.__move_action_goal.request.pipeline_id = value - - @property - def planner_id(self) -> int: - return self.__move_action_goal.request.planner_id - - @planner_id.setter - def planner_id(self, value: str): - self.__move_action_goal.request.planner_id = value - - -def init_joint_state( - joint_names: List[str], - joint_positions: Optional[List[str]] = None, - joint_velocities: Optional[List[str]] = None, - joint_effort: Optional[List[str]] = None, -) -> JointState: - joint_state = JointState() - - joint_state.name = joint_names - joint_state.position = ( - joint_positions if joint_positions is not None else [0.0] * len(joint_names) - ) - joint_state.velocity = ( - joint_velocities if joint_velocities is not None else [0.0] * len(joint_names) - ) - joint_state.effort = ( - joint_effort if joint_effort is not None else [0.0] * len(joint_names) - ) - - return joint_state - - -def init_execute_trajectory_goal( - joint_trajectory: JointTrajectory, -) -> Optional[ExecuteTrajectory.Goal]: - if joint_trajectory is None: - return None - - execute_trajectory_goal = ExecuteTrajectory.Goal() - - execute_trajectory_goal.trajectory.joint_trajectory = joint_trajectory - - return execute_trajectory_goal - - -def init_dummy_joint_trajectory_from_state( - joint_state: JointState, duration_sec: int = 0, duration_nanosec: int = 0 -) -> JointTrajectory: - joint_trajectory = JointTrajectory() - joint_trajectory.joint_names = joint_state.name - - point = JointTrajectoryPoint() - point.positions = joint_state.position - point.velocities = joint_state.velocity - point.accelerations = [0.0] * len(joint_trajectory.joint_names) - point.effort = joint_state.effort - point.time_from_start.sec = duration_sec - point.time_from_start.nanosec = duration_nanosec - joint_trajectory.points.append(point) - - return joint_trajectory diff --git a/dependencies/py_moveit2/py_moveit2/moveit2_gripper.py b/dependencies/py_moveit2/py_moveit2/moveit2_gripper.py deleted file mode 100644 index 622395ce..00000000 --- a/dependencies/py_moveit2/py_moveit2/moveit2_gripper.py +++ /dev/null @@ -1,252 +0,0 @@ -# License: BSD -import math -from typing import List, Optional - -from rclpy.callback_groups import CallbackGroup -from rclpy.node import Node - -from .moveit2 import * - - -class MoveIt2Gripper(MoveIt2): - """ - Python interface for MoveIt 2 Gripper that is controlled by JointTrajectoryController. - This implementation builds on MoveIt2 to reuse code (while keeping MoveIt2 standalone). - """ - - def __init__( - self, - node: Node, - gripper_joint_names: List[str], - open_gripper_joint_positions: List[float], - closed_gripper_joint_positions: List[float], - gripper_group_name: str = "gripper", - execute_via_moveit: bool = False, - ignore_new_calls_while_executing: bool = False, - skip_planning: bool = False, - skip_planning_fixed_motion_duration: float = 0.5, - callback_group: Optional[CallbackGroup] = None, - follow_joint_trajectory_action_name: str = "DEPRECATED", - use_move_group_action: bool = False, - ): - """ - Construct an instance of `MoveIt2Gripper` interface. - - `node` - ROS 2 node that this interface is attached to - - `gripper_joint_names` - List of gripper joint names (can be extracted from URDF) - - `open_gripper_joint_positions` - Configuration of gripper joints when open - - `closed_gripper_joint_positions` - Configuration of gripper joints when fully closed - - `gripper_group_name` - Name of the planning group for robot gripper - - [DEPRECATED] `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) - FollowJointTrajectory action (controller) is employed otherwise - together with a separate planning service client - - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories - while previous is still being executed - - `skip_planning` - If enabled, planning is skipped and a single joint trajectory point is published - for closing or opening. This enables much faster operation, but the collision - checking is disabled and the motion smoothness will depend on the controller. - - `skip_planning_fixed_motion_duration` - Desired duration for the closing and opening motions when - `skip_planning` mode is enabled. - - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) - - [DEPRECATED] `follow_joint_trajectory_action_name` - Name of the action server for the controller - - `use_move_group_action` - Flag that enables execution via MoveGroup action (MoveIt 2) - ExecuteTrajectory action is employed otherwise - together with a separate planning service client - """ - - # Check for deprecated parameters - if execute_via_moveit: - node.get_logger().warn( - "Parameter `execute_via_moveit` is deprecated. Please use `use_move_group_action` instead." - ) - use_move_group_action = True - if follow_joint_trajectory_action_name != "DEPRECATED": - node.get_logger().warn( - "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead." - ) - - super().__init__( - node=node, - joint_names=gripper_joint_names, - base_link_name="", - end_effector_name="", - group_name=gripper_group_name, - ignore_new_calls_while_executing=ignore_new_calls_while_executing, - callback_group=callback_group, - use_move_group_action=use_move_group_action, - ) - self.__del_redundant_attributes() - - assert ( - len(gripper_joint_names) - == len(open_gripper_joint_positions) - == len(closed_gripper_joint_positions) - ) - self.__open_gripper_joint_positions = open_gripper_joint_positions - self.__closed_gripper_joint_positions = closed_gripper_joint_positions - - self.__skip_planning = skip_planning - if skip_planning: - duration_sec = math.floor(skip_planning_fixed_motion_duration) - duration_nanosec = int( - 10e8 * (skip_planning_fixed_motion_duration - duration_sec) - ) - self.__open_dummy_trajectory_goal = init_follow_joint_trajectory_goal( - init_dummy_joint_trajectory_from_state( - init_joint_state( - joint_names=gripper_joint_names, - joint_positions=open_gripper_joint_positions, - ), - duration_sec=duration_sec, - duration_nanosec=duration_nanosec, - ) - ) - self.__close_dummy_trajectory_goal = init_follow_joint_trajectory_goal( - init_dummy_joint_trajectory_from_state( - init_joint_state( - joint_names=gripper_joint_names, - joint_positions=closed_gripper_joint_positions, - ), - duration_sec=duration_sec, - duration_nanosec=duration_nanosec, - ) - ) - - # Tolerance used for checking whether the gripper is open or closed - self.__open_tolerance = [ - 0.1 - * abs(open_gripper_joint_positions[i] - closed_gripper_joint_positions[i]) - for i in range(len(gripper_joint_names)) - ] - # Indices of gripper joint within the joint state message topic. - # It is assumed that the order of these does not change during execution. - self.__gripper_joint_indices: Optional[List[int]] = None - - def __call__(self): - """ - Callable that is identical to `MoveIt2Gripper.toggle()`. - """ - - self.toggle() - - def toggle(self): - """ - Toggles the gripper between open and closed state. - """ - - if self.is_open: - self.close(skip_if_noop=False) - else: - self.open(skip_if_noop=False) - - def open(self, skip_if_noop: bool = False): - """ - Open the gripper. - - `skip_if_noop` - No action will be performed if the gripper is already open. - """ - - if skip_if_noop and self.is_open: - return - - if self.__skip_planning: - self.__open_without_planning() - else: - self.move_to_configuration( - joint_positions=self.__open_gripper_joint_positions - ) - - def close(self, skip_if_noop: bool = False): - """ - Close the gripper. - - `skip_if_noop` - No action will be performed if the gripper is not open. - """ - - if skip_if_noop and self.is_closed: - return - - if self.__skip_planning: - self.__close_without_planning() - else: - self.move_to_configuration( - joint_positions=self.__closed_gripper_joint_positions - ) - - def reset_open(self, sync: bool = True): - """ - Reset into open configuration by sending a dummy joint trajectory. - This is useful for simulated robots that allow instantaneous reset of joints. - """ - - self.reset_controller( - joint_state=self.__open_gripper_joint_positions, sync=sync - ) - - def reset_closed(self, sync: bool = True): - """ - Reset into closed configuration by sending a dummy joint trajectory. - This is useful for simulated robots that allow instantaneous reset of joints. - """ - - self.reset_controller( - joint_state=self.__closed_gripper_joint_positions, sync=sync - ) - - def __open_without_planning(self): - self._send_goal_async_follow_joint_trajectory( - goal=self.__open_dummy_trajectory_goal, - wait_until_response=False, - ) - - def __close_without_planning(self): - self._send_goal_async_follow_joint_trajectory( - goal=self.__close_dummy_trajectory_goal, - wait_until_response=False, - ) - - def __del_redundant_attributes(self): - self.move_to_pose = None - self.set_pose_goal = None - self.set_position_goal = None - self.set_orientation_goal = None - self.compute_fk = None - self.compute_ik = None - - @property - def is_open(self) -> bool: - """ - Gripper is considered to be open if all of the joints are at their open position. - """ - - joint_state = self.joint_state - - # Assume the gripper is open if there are no joint state readings yet - if joint_state is None: - return True - - # For the sake of performance, find the indices of joints only once. - # This is especially useful for robots with many joints. - if self.__gripper_joint_indices is None: - self.__gripper_joint_indices: List[int] = [] - for joint_name in self.joint_names: - self.__gripper_joint_indices.append(joint_state.name.index(joint_name)) - - for local_joint_index, joint_state_index in enumerate( - self.__gripper_joint_indices - ): - if ( - abs( - joint_state.position[joint_state_index] - - self.__open_gripper_joint_positions[local_joint_index] - ) - > self.__open_tolerance[local_joint_index] - ): - return False - - return True - - @property - def is_closed(self) -> bool: - """ - Gripper is considered to be closed if any of the joints is outside of their open position. - """ - - return not self.is_open diff --git a/dependencies/py_moveit2/py_moveit2/moveit2_servo.py b/dependencies/py_moveit2/py_moveit2/moveit2_servo.py deleted file mode 100644 index afadd046..00000000 --- a/dependencies/py_moveit2/py_moveit2/moveit2_servo.py +++ /dev/null @@ -1,248 +0,0 @@ -# License: BSD -from copy import deepcopy -from typing import Optional, Tuple - -from geometry_msgs.msg import TwistStamped -from rclpy.callback_groups import CallbackGroup -from rclpy.node import Node -from rclpy.qos import ( - QoSDurabilityPolicy, - QoSHistoryPolicy, - QoSProfile, - QoSReliabilityPolicy, -) -from rclpy.task import Future -from std_srvs.srv import Trigger - - -class MoveIt2Servo: - """ - Python interface for MoveIt 2 Servo that enables real-time control in Cartesian Space. - This implementation is just a thin wrapper around TwistStamped message publisher. - """ - - def __init__( - self, - node: Node, - frame_id: str, - linear_speed: float = 1.0, - angular_speed: float = 1.0, - enable_at_init: bool = True, - callback_group: Optional[CallbackGroup] = None, - ): - """ - Construct an instance of `MoveIt2Servo` interface. - - `node` - ROS 2 node that this interface is attached to - - `frame_id` - Reference frame in which to publish command messages - - `linear_speed` - Factor that can be used to scale all input linear twist commands - - `angular_speed` - Factor that can be used to scale all input angular twist commands - - `enable_at_init` - Flag that enables initialisation of MoveIt 2 Servo during initialisation - Otherwise, `MoveIt2Servo.enable()` must be called explicitly - - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) - """ - - self._node = node - - # Create publisher - self.__twist_pub = self._node.create_publisher( - msg_type=TwistStamped, - topic="delta_twist_cmds", - qos_profile=QoSProfile( - durability=QoSDurabilityPolicy.VOLATILE, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_ALL, - ), - callback_group=callback_group, - ) - - # Create service clients - self.__start_service = self._node.create_client( - srv_type=Trigger, - srv_name="/servo_node/start_servo", - callback_group=callback_group, - ) - self.__stop_service = self._node.create_client( - srv_type=Trigger, - srv_name="/servo_node/stop_servo", - callback_group=callback_group, - ) - self.__trigger_req = Trigger.Request() - self.__is_enabled = False - - # Initialize message based on passed arguments - self.__twist_msg = TwistStamped() - self.__twist_msg.header.frame_id = frame_id - self.__twist_msg.twist.linear.x = linear_speed - self.__twist_msg.twist.linear.y = linear_speed - self.__twist_msg.twist.linear.z = linear_speed - self.__twist_msg.twist.angular.x = angular_speed - self.__twist_msg.twist.angular.y = angular_speed - self.__twist_msg.twist.angular.z = angular_speed - - # Enable servo immediately, if desired - if enable_at_init: - self.enable() - - def __del__(self): - """ - Try to stop MoveIt 2 Servo during destruction. - """ - - try: - if self.is_enabled: - self.__stop_service.call_async(self.__trigger_req) - except: - pass - - def __call__( - self, - linear: Tuple[float, float, float] = (0.0, 0.0, 0.0), - angular: Tuple[float, float, float] = (0.0, 0.0, 0.0), - ): - """ - Callable that is identical to `MoveIt2Servo.servo()`. - """ - - self.servo(linear=linear, angular=angular) - - def servo( - self, - linear: Tuple[float, float, float] = (0.0, 0.0, 0.0), - angular: Tuple[float, float, float] = (0.0, 0.0, 0.0), - enable_if_disabled: bool = True, - ): - """ - Apply linear and angular twist using MoveIt 2 Servo. - Input is scaled by `linear_speed` and `angular_speed`, respectively. - """ - - if not self.is_enabled: - self._node.get_logger().warn( - "Command failed because MoveIt Servo is not yet enabled." - ) - if enable_if_disabled: - self._node.get_logger().warn( - f"Calling '{self.__start_service.srv_name}' service to enable MoveIt Servo..." - ) - if not self.enable(): - return - else: - return - - twist_msg = deepcopy(self.__twist_msg) - twist_msg.header.stamp = self._node.get_clock().now().to_msg() - twist_msg.twist.linear.x *= linear[0] - twist_msg.twist.linear.y *= linear[1] - twist_msg.twist.linear.z *= linear[2] - twist_msg.twist.angular.x *= angular[0] - twist_msg.twist.angular.y *= angular[1] - twist_msg.twist.angular.z *= angular[2] - self.__twist_pub.publish(twist_msg) - - def enable( - self, wait_for_server_timeout_sec: Optional[float] = 1.0, sync: bool = False - ) -> bool: - """ - Enable MoveIt 2 Servo server via async service call. - """ - - while not self.__start_service.wait_for_service( - timeout_sec=wait_for_server_timeout_sec - ): - self._node.get_logger().warn( - f"Service '{self.__start_service.srv_name}' is not yet available..." - ) - return False - - if sync: - result = self.__start_service.call(self.__trigger_req) - if not result.success: - self._node.get_logger().error( - f"MoveIt Servo could not be enabled. ({result.message})" - ) - self.__is_enabled = result.success - return result.success - else: - start_service_future = self.__start_service.call_async(self.__trigger_req) - start_service_future.add_done_callback(self.__enable_done_callback) - return True - - def disable( - self, wait_for_server_timeout_sec: Optional[float] = 1.0, sync: bool = False - ) -> bool: - """ - Disable MoveIt 2 Servo server via async service call. - """ - - while not self.__stop_service.wait_for_service( - timeout_sec=wait_for_server_timeout_sec - ): - self._node.get_logger().warn( - f"Service '{self.__stop_service.srv_name}' is not yet available..." - ) - return False - - if sync: - result = self.__stop_service.call(self.__trigger_req) - if not result.success: - self._node.get_logger().error( - f"MoveIt Servo could not be disabled. ({result.message})" - ) - self.__is_enabled = not result.success - return result.success - else: - stop_service_future = self.__stop_service.call_async(self.__trigger_req) - stop_service_future.add_done_callback(self.__disable_done_callback) - return True - - def __enable_done_callback(self, future: Future): - result: Trigger.Response = future.result() - - if not result.success: - self._node.get_logger().error( - f"MoveIt Servo could not be enabled. ({result.message})" - ) - - self.__is_enabled = result.success - - def __disable_done_callback(self, future: Future): - result: Trigger.Response = future.result() - - if not result.success: - self._node.get_logger().error( - f"MoveIt Servo could not be disabled. ({result.message})" - ) - - self.__is_enabled = not result.success - - @property - def is_enabled(self) -> bool: - return self.__is_enabled - - @property - def frame_id(self) -> str: - return self.__twist_msg.header.frame_id - - @frame_id.setter - def frame_id(self, value: str): - self.__twist_msg.header.frame_id = value - - @property - def linear_speed(self) -> float: - return self.__twist_msg.twist.linear.x - - @linear_speed.setter - def linear_speed(self, value: float): - self.__twist_msg.twist.linear.x = value - self.__twist_msg.twist.linear.y = value - self.__twist_msg.twist.linear.z = value - - @property - def angular_speed(self) -> float: - return self.__twist_msg.twist.angular.x - - @angular_speed.setter - def angular_speed(self, value: float): - self.__twist_msg.twist.angular.x = value - self.__twist_msg.twist.angular.y = value - self.__twist_msg.twist.angular.z = value diff --git a/dependencies/pymoveit2 b/dependencies/pymoveit2 new file mode 160000 index 00000000..d38a6536 --- /dev/null +++ b/dependencies/pymoveit2 @@ -0,0 +1 @@ +Subproject commit d38a6536670b607faf19e516d68a112fe6348ce1 diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py index 06a3561c..7e7c99b7 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py @@ -17,7 +17,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node -from py_moveit2 import MoveIt2, MoveIt2State +from pymoveit2 import MoveIt2, MoveIt2State from arm_sim_scenario.robots import wx200 import py_trees From 45fbd0fbafa0ba66742fa24fa1b8d879626fc9e2 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 4 Jun 2024 15:56:33 +0200 Subject: [PATCH 08/62] add actor for arm --- examples/example_moveit/example_moveit.osc | 10 +- .../actions/move_to_joint_pose.py | 18 +- .../scenario_execution_ros/lib_osc/ros.osc | 12 +- scenario_execution_ros/setup.py | 2 +- .../arm_sim_scenario/robots/wx200.py | 2 +- .../launch/ignition_launch.py | 10 +- .../launch/sim_moveit_scenario_launch.py | 14 - .../rviz/xsarm_gz_classic.rviz | 253 ------------------ 8 files changed, 35 insertions(+), 286 deletions(-) delete mode 100644 simulation/gazebo/arm_sim_scenario/rviz/xsarm_gz_classic.rviz diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc index cf0eb5e9..c96dc0a5 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit/example_moveit.osc @@ -1,9 +1,17 @@ import osc.ros scenario example_moveit: + wx200: robotic_arm with: + keep(it.namespace == 'wx200') + keep(it.joint_names == '[\"waist\",\"shoulder\",\"elbow\",\"wrist_angle\",\"wrist_rotate\",\"left_finger\",\"right_finger\"]') + keep(it.base_link_name == 'base_link') + keep(it.end_effector_name == 'gripper_link') + keep(it.gripper_joint_names == '[\"right_finger\",\"left_finger\"]') + keep(it.move_group_arm == 'interbotix_arm') + keep(it.move_group_gripper == 'interbotix_gripper') do parallel: serial: - move_to_joint_pose() with: + wx200.move_to_joint_pose() with: keep(it.joint_pose == '[-1.5708, 0.0, 0.0, 0.0, 0.0]') wait elapsed(3s) emit end diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py index 7e7c99b7..75d96763 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py @@ -26,8 +26,13 @@ class MoveToJointPose(py_trees.behaviour.Behaviour): - def __init__(self, name: str, joint_pose: str): + def __init__(self, name: str, associated_actor, joint_pose: str): super().__init__(name) + self.namespace = associated_actor["namespace"] + self.joint_names = ast.literal_eval(associated_actor["joint_names"]) + self.base_link_name = associated_actor["base_link_name"] + self.end_effector_name = associated_actor["end_effector_name"] + self.move_group_arm = associated_actor["move_group_arm"] self.joint_pose = ast.literal_eval(joint_pose) self.execute = False @@ -40,16 +45,17 @@ def setup(self, **kwargs): raise KeyError(error_message) from e self.synchronous = True - # If non-positive, don't cancel. Only used if synchronous is False + + # # If non-positive, don't cancel. Only used if synchronous is False self.cancel_after_secs = 0.0 # Create MoveIt 2 interface self.moveit2 = MoveIt2( node=self.node, - joint_names=wx200.joint_names(), - base_link_name=wx200.base_link_name(), - end_effector_name=wx200.end_effector_name(), - group_name=wx200.MOVE_GROUP_ARM, + joint_names=self.joint_names, + base_link_name= self.namespace + '/' + self.base_link_name, + end_effector_name= self.namespace + '/' + self.end_effector_name, + group_name=self.move_group_arm, callback_group=ReentrantCallbackGroup() ) diff --git a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc index bd073939..c149d6f9 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc @@ -142,5 +142,15 @@ action wait_for_topics: # wait for topics to get available topics: list of topics -action move_to_joint_pose: +actor robotic_arm inherits robot: + namespace: string = '' + joint_names: string = '' + base_link_name: string = '' + end_effector_name: string = '' + gripper_joint_names: string = '' + move_group_arm: string = '' + move_group_gripper: string = '' + + +action robotic_arm.move_to_joint_pose: joint_pose: string \ No newline at end of file diff --git a/scenario_execution_ros/setup.py b/scenario_execution_ros/setup.py index 80de9789..982e0b3c 100644 --- a/scenario_execution_ros/setup.py +++ b/scenario_execution_ros/setup.py @@ -63,7 +63,7 @@ 'assert_topic_latency = scenario_execution_ros.actions.assert_topic_latency:AssertTopicLatency', 'assert_tf_moving = scenario_execution_ros.actions.assert_tf_moving:AssertTfMoving', 'assert_lifecycle_state = scenario_execution_ros.actions.assert_lifecycle_state:AssertLifecycleState', - 'move_to_joint_pose = scenario_execution_ros.actions.move_to_joint_pose:MoveToJointPose', + 'robotic_arm.move_to_joint_pose = scenario_execution_ros.actions.move_to_joint_pose:MoveToJointPose', ], 'scenario_execution.osc_libraries': [ 'ros = ' diff --git a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py index 1c79aee8..32ed738c 100644 --- a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py +++ b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py @@ -13,7 +13,7 @@ def joint_names() -> List[str]: "shoulder", "elbow", "wrist_angle", - "wrist_rotate" + "wrist_rotate", "left_finger", "right_finger", ] diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index 02ec9117..1c6fd5e6 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -41,12 +41,6 @@ DeclareLaunchArgument('headless', default_value='False', description='Whether to execute simulation gui'), - DeclareLaunchArgument('rviz_config', - default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), - 'rviz', - 'xsarm_gz_classic.rviz', - ]), - description='file path to the config file RViz should load.',) ] @@ -55,7 +49,6 @@ def generate_launch_description(): robot_model = LaunchConfiguration('robot_model') robot_name = LaunchConfiguration('robot_name') use_rviz = LaunchConfiguration('use_rviz') - rviz_config = LaunchConfiguration('rviz_config') use_sim_time = LaunchConfiguration('use_sim_time') env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], @@ -104,8 +97,7 @@ def generate_launch_description(): 'robot_model': robot_model, 'robot_name': robot_name, 'use_rviz': use_rviz, - 'use_sim_time': use_sim_time, - 'rviz_config': rviz_config + 'use_sim_time': use_sim_time }.items(), ) diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py index 0bb224d6..23c95179 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -27,8 +27,6 @@ def generate_launch_description(): arm_sim_scenario_dir = get_package_share_directory('arm_sim_scenario') scenario_execution_dir = get_package_share_directory('scenario_execution_ros') - # gazebo_tf_publisher_dir = get_package_share_directory('gazebo_tf_publisher') - tf_to_pose_publisher_dir = get_package_share_directory('tf_to_pose_publisher') scenario = LaunchConfiguration('scenario') scenario_execution = LaunchConfiguration('scenario_execution') @@ -46,13 +44,6 @@ def generate_launch_description(): PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'moveit_launch.py'])]), ) - # groundtruth_publisher = IncludeLaunchDescription( - # PythonLaunchDescriptionSource(os.path.join(gazebo_tf_publisher_dir, 'launch', 'gazebo_tf_publisher_launch.py')), - # launch_arguments=[ - # ('ign_pose_topic', ['/world/', world, '/dynamic_pose/info']), - # ] - # ) - scenario_exec = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([scenario_execution_dir, 'launch', 'scenario_launch.py'])]), condition=IfCondition(scenario_execution), @@ -61,10 +52,6 @@ def generate_launch_description(): ] ) - tf_to_pose = IncludeLaunchDescription( - PythonLaunchDescriptionSource([PathJoinSubstitution([tf_to_pose_publisher_dir, 'launch', 'tf_to_pose_launch.py'])]), - ) - ld = LaunchDescription([ arg_scenario, arg_scenario_execution @@ -72,5 +59,4 @@ def generate_launch_description(): ld.add_action(ignition) ld.add_action(moveit_bringup) ld.add_action(scenario_exec) - ld.add_action(tf_to_pose) return ld diff --git a/simulation/gazebo/arm_sim_scenario/rviz/xsarm_gz_classic.rviz b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_gz_classic.rviz deleted file mode 100644 index 5bba6c18..00000000 --- a/simulation/gazebo/arm_sim_scenario/rviz/xsarm_gz_classic.rviz +++ /dev/null @@ -1,253 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.6176470518112183 - Tree Height: 811 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.699999988079071 - Cell Size: 0.25 - Class: rviz_default_plugins/Grid - Color: 255; 255; 255 - 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 - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - world: - Alpha: 1 - Show Axes: false - Show Trail: false - wx200/base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wx200/ee_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - wx200/ee_gripper_link: - Alpha: 1 - Show Axes: false - Show Trail: false - wx200/fingers_link: - Alpha: 1 - Show Axes: false - Show Trail: false - wx200/forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wx200/gripper_bar_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wx200/gripper_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wx200/gripper_prop_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wx200/left_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wx200/right_finger_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wx200/shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wx200/upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wx200/wrist_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - world: - Value: true - wx200/base_link: - Value: true - wx200/ee_arm_link: - Value: true - wx200/ee_gripper_link: - Value: true - wx200/fingers_link: - Value: true - wx200/forearm_link: - Value: true - wx200/gripper_bar_link: - Value: true - wx200/gripper_link: - Value: true - wx200/gripper_prop_link: - Value: true - wx200/left_finger_link: - Value: true - wx200/right_finger_link: - Value: true - wx200/shoulder_link: - Value: true - wx200/upper_arm_link: - Value: true - wx200/wrist_link: - Value: true - Marker Scale: 0.15000000596046448 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world: - wx200/base_link: - wx200/shoulder_link: - wx200/upper_arm_link: - wx200/forearm_link: - wx200/wrist_link: - wx200/gripper_link: - wx200/ee_arm_link: - wx200/gripper_bar_link: - wx200/fingers_link: - wx200/ee_gripper_link: - {} - wx200/left_finger_link: - {} - wx200/right_finger_link: - {} - wx200/gripper_prop_link: - {} - Update Interval: 0 - Value: true - Enabled: true - Global Options: - Background Color: 0; 176; 240 - Fixed Frame: world - 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 - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - 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: 1.2599999904632568 - 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: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.7853981852531433 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.7853981852531433 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 923 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000018b00000368fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000001600000368000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000300049006e0074006500720062006f00740069007800200043006f006e00740072006f006c002000500061006e0065006c00000002420000013c00000000000000000000000100000110000002a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a20000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006080000004cfc0100000002fb0000000800540069006d00650100000000000006080000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004770000036800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 - Width: 1544 - X: 72 - Y: 27 From 7a5e3f01ba3d68865b3becc34b5260a7d0d35af2 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 4 Jun 2024 16:38:19 +0200 Subject: [PATCH 09/62] fix parsing error --- examples/example_moveit/example_moveit.osc | 4 ++-- .../actions/move_to_joint_pose.py | 10 ++++++---- .../scenario_execution_ros/lib_osc/ros.osc | 4 ++-- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc index c96dc0a5..8452ec71 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit/example_moveit.osc @@ -3,10 +3,10 @@ import osc.ros scenario example_moveit: wx200: robotic_arm with: keep(it.namespace == 'wx200') - keep(it.joint_names == '[\"waist\",\"shoulder\",\"elbow\",\"wrist_angle\",\"wrist_rotate\",\"left_finger\",\"right_finger\"]') + keep(it.joint_names == ["waist","shoulder","elbow","wrist_angle","wrist_rotate","left_finger","right_finger"]) keep(it.base_link_name == 'base_link') keep(it.end_effector_name == 'gripper_link') - keep(it.gripper_joint_names == '[\"right_finger\",\"left_finger\"]') + keep(it.gripper_joint_names == ["right_finger","left_finger"]) keep(it.move_group_arm == 'interbotix_arm') keep(it.move_group_gripper == 'interbotix_gripper') do parallel: diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py index 75d96763..1a51424d 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py @@ -29,7 +29,7 @@ class MoveToJointPose(py_trees.behaviour.Behaviour): def __init__(self, name: str, associated_actor, joint_pose: str): super().__init__(name) self.namespace = associated_actor["namespace"] - self.joint_names = ast.literal_eval(associated_actor["joint_names"]) + self.joint_names = associated_actor["joint_names"] self.base_link_name = associated_actor["base_link_name"] self.end_effector_name = associated_actor["end_effector_name"] self.move_group_arm = associated_actor["move_group_arm"] @@ -51,11 +51,11 @@ def setup(self, **kwargs): # Create MoveIt 2 interface self.moveit2 = MoveIt2( - node=self.node, - joint_names=self.joint_names, + node= self.node, + joint_names= self.joint_names, base_link_name= self.namespace + '/' + self.base_link_name, end_effector_name= self.namespace + '/' + self.end_effector_name, - group_name=self.move_group_arm, + group_name= self.move_group_arm, callback_group=ReentrantCallbackGroup() ) @@ -65,6 +65,8 @@ def setup(self, **kwargs): self.moveit2.max_velocity = 0.5 self.moveit2.max_acceleration = 0.5 + self.logger.info(f"{self.joint_names}") + def update(self) -> py_trees.common.Status: self.current_state = self.moveit2.query_state() self.logger.info(f"Current State: {self.current_state}") diff --git a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc index c149d6f9..65883ebb 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc @@ -144,10 +144,10 @@ action wait_for_topics: actor robotic_arm inherits robot: namespace: string = '' - joint_names: string = '' + joint_names: list of string base_link_name: string = '' end_effector_name: string = '' - gripper_joint_names: string = '' + gripper_joint_names: list of string move_group_arm: string = '' move_group_gripper: string = '' From 063697affbf5fc6b580499771e912d6cdbaee843 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 6 Jun 2024 11:20:59 +0200 Subject: [PATCH 10/62] add gripper action --- .../actions/move_gripper.py | 79 +++++++++++++++++++ .../actions/move_to_joint_pose.py | 2 - .../scenario_execution_ros/lib_osc/ros.osc | 5 +- scenario_execution_ros/setup.py | 1 + 4 files changed, 84 insertions(+), 3 deletions(-) create mode 100644 scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py b/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py new file mode 100644 index 00000000..bfa19894 --- /dev/null +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py @@ -0,0 +1,79 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node + +from pymoveit2 import MoveIt2, MoveIt2State, GripperInterface +from arm_sim_scenario.robots import wx200 + +import py_trees +import ast + + +class MoveGripper(py_trees.behaviour.Behaviour): + + def __init__(self, name: str, associated_actor, gripper: str): + super().__init__(name) + self.namespace = associated_actor["namespace"] + self.joint_names = associated_actor["joint_names"] + self.base_link_name = associated_actor["base_link_name"] + self.end_effector_name = associated_actor["end_effector_name"] + self.move_group_arm = associated_actor["move_group_arm"] + self.move_group_gripper = associated_actor["move_group_gripper"] + self.gripper_joint_names = associated_actor["gripper_joint_names"] + self.gripper = gripper + self.execute = False + + def setup(self, **kwargs): + try: + self.node: Node = kwargs['node'] + except KeyError as e: + error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format( + self.name, self.__class__.__name__) + raise KeyError(error_message) from e + + + # Create MoveIt 2 interface + self.gripper_interface = GripperInterface( + node= self.node, + gripper_joint_names= self.gripper_joint_names, + open_gripper_joint_positions=[0.037, -0.037] , + closed_gripper_joint_positions=[0.015, -0.015], + gripper_group_name= self.move_group_gripper, + callback_group=ReentrantCallbackGroup(), + gripper_command_action_name="gripper_action_controller/gripper_cmd" + ) + + def update(self) -> py_trees.common.Status: + self.current_state = self.gripper_interface.query_state() + self.logger.info(f"Current State: {self.current_state}") + if (self.current_state == MoveIt2State.IDLE): + if (self.execute == False): + self.gripper_interface() + result = py_trees.common.Status.RUNNING + else: + result = py_trees.common.Status.SUCCESS + elif (self.current_state == MoveIt2State.EXECUTING): + self.logger.info(f"Executing gripper....") + result = py_trees.common.Status.RUNNING + self.execute = True + else: + self.logger.info(f"Requesting gripper pose....") + result = py_trees.common.Status.RUNNING + + return result + diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py index 1a51424d..190eca20 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py @@ -65,8 +65,6 @@ def setup(self, **kwargs): self.moveit2.max_velocity = 0.5 self.moveit2.max_acceleration = 0.5 - self.logger.info(f"{self.joint_names}") - def update(self) -> py_trees.common.Status: self.current_state = self.moveit2.query_state() self.logger.info(f"Current State: {self.current_state}") diff --git a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc index 65883ebb..ac5aa02a 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc @@ -153,4 +153,7 @@ actor robotic_arm inherits robot: action robotic_arm.move_to_joint_pose: - joint_pose: string \ No newline at end of file + joint_pose: string + +action robotic_arm.move_gripper: + gripper: string # open, close \ No newline at end of file diff --git a/scenario_execution_ros/setup.py b/scenario_execution_ros/setup.py index 982e0b3c..c35ea898 100644 --- a/scenario_execution_ros/setup.py +++ b/scenario_execution_ros/setup.py @@ -64,6 +64,7 @@ 'assert_tf_moving = scenario_execution_ros.actions.assert_tf_moving:AssertTfMoving', 'assert_lifecycle_state = scenario_execution_ros.actions.assert_lifecycle_state:AssertLifecycleState', 'robotic_arm.move_to_joint_pose = scenario_execution_ros.actions.move_to_joint_pose:MoveToJointPose', + 'robotic_arm.move_gripper = scenario_execution_ros.actions.move_gripper:MoveGripper', ], 'scenario_execution.osc_libraries': [ 'ros = ' From f889630b3606980247f92343220302138e010f8e Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 6 Jun 2024 17:24:29 +0200 Subject: [PATCH 11/62] fix gripper --- simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro index 4c1b6058..c55b82e8 100644 --- a/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro @@ -51,7 +51,7 @@ - + From 47b085bb18edc7ab48bd13f717fcaa9ab17b5fe4 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 6 Jun 2024 17:32:56 +0200 Subject: [PATCH 12/62] add gripper action --- examples/example_moveit/example_moveit.osc | 3 +++ .../scenario_execution_ros/actions/move_gripper.py | 6 +++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc index 8452ec71..8c38542e 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit/example_moveit.osc @@ -14,6 +14,9 @@ scenario example_moveit: wx200.move_to_joint_pose() with: keep(it.joint_pose == '[-1.5708, 0.0, 0.0, 0.0, 0.0]') wait elapsed(3s) + wx200.move_gripper() with: + keep(it.gripper == 'open') + wait elapsed(3s) emit end time_out: serial: wait elapsed(60s) diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py b/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py index bfa19894..837c8354 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py @@ -51,8 +51,8 @@ def setup(self, **kwargs): self.gripper_interface = GripperInterface( node= self.node, gripper_joint_names= self.gripper_joint_names, - open_gripper_joint_positions=[0.037, -0.037] , - closed_gripper_joint_positions=[0.015, -0.015], + open_gripper_joint_positions=[-0.037, +0.037] , + closed_gripper_joint_positions=[-0.015, 0.015], gripper_group_name= self.move_group_gripper, callback_group=ReentrantCallbackGroup(), gripper_command_action_name="gripper_action_controller/gripper_cmd" @@ -63,7 +63,7 @@ def update(self) -> py_trees.common.Status: self.logger.info(f"Current State: {self.current_state}") if (self.current_state == MoveIt2State.IDLE): if (self.execute == False): - self.gripper_interface() + self.gripper_interface.open() result = py_trees.common.Status.RUNNING else: result = py_trees.common.Status.SUCCESS From 150aa1536373b6660fa192af860e4274e88edcde Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Fri, 7 Jun 2024 11:33:54 +0200 Subject: [PATCH 13/62] add move_to_pose action --- examples/example_moveit/example_moveit.osc | 2 + .../actions/move_to_pose.py | 103 ++++++++++++++++++ .../scenario_execution_ros/lib_osc/ros.osc | 5 +- scenario_execution_ros/setup.py | 1 + 4 files changed, 110 insertions(+), 1 deletion(-) create mode 100644 scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc index 8c38542e..47c1f50d 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit/example_moveit.osc @@ -14,6 +14,8 @@ scenario example_moveit: wx200.move_to_joint_pose() with: keep(it.joint_pose == '[-1.5708, 0.0, 0.0, 0.0, 0.0]') wait elapsed(3s) + wx200.move_to_pose(pose_3d(position: position_3d(x: 0.1m, y: 0.0m, z: 0.4m))) + wait elapsed(3s) wx200.move_gripper() with: keep(it.gripper == 'open') wait elapsed(3s) diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py b/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py new file mode 100644 index 00000000..7fe65a19 --- /dev/null +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py @@ -0,0 +1,103 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node + +from pymoveit2 import MoveIt2, MoveIt2State +from arm_sim_scenario.robots import wx200 +import py_trees +import ast + + +class MoveToPose(py_trees.behaviour.Behaviour): + + def __init__(self, name: str, associated_actor, goal_pose: list): + super().__init__(name) + self.namespace = associated_actor["namespace"] + self.joint_names = associated_actor["joint_names"] + self.base_link_name = associated_actor["base_link_name"] + self.end_effector_name = associated_actor["end_effector_name"] + self.move_group_arm = associated_actor["move_group_arm"] + self.position = [goal_pose['position']['x'], goal_pose['position']['y'], goal_pose['position']['z']] + self.orientation = [goal_pose['orientation']['roll'], goal_pose['orientation']['pitch'], goal_pose['orientation']['yaw'], 1.000000000] + self.cartesian = False + self.cartesian_max_step = 0.025 + self.cartesian_fraction_threshold = 0.0 + self.cartesian_jump_threshold = 0.0 + self.cartesian_avoid_collisions = False + self.execute = False + + + def setup(self, **kwargs): + try: + self.node: Node = kwargs['node'] + except KeyError as e: + error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format( + self.name, self.__class__.__name__) + raise KeyError(error_message) from e + + self.synchronous = True + + # # If non-positive, don't cancel. Only used if synchronous is False + self.cancel_after_secs = 0.0 + + # Create MoveIt 2 interface + self.moveit2 = MoveIt2( + node= self.node, + joint_names= self.joint_names, + base_link_name= self.namespace + '/' + self.base_link_name, + end_effector_name= self.namespace + '/' + self.end_effector_name, + group_name= self.move_group_arm, + callback_group=ReentrantCallbackGroup() + ) + + self.moveit2.planner_id = "RRTConnectkConfigDefault" + + # Scale down velocity and acceleration of joints (percentage of maximum) + self.moveit2.max_velocity = 0.5 + self.moveit2.max_acceleration = 0.5 + self.moveit2.cartesian_avoid_collisions = self.cartesian_avoid_collisions + self.moveit2.cartesian_jump_threshold = self.cartesian_jump_threshold + + def update(self) -> py_trees.common.Status: + self.current_state = self.moveit2.query_state() + if (self.current_state == MoveIt2State.IDLE): + if (self.execute == False): + self.move_to_pose() + result = py_trees.common.Status.RUNNING + else: + result = py_trees.common.Status.SUCCESS + elif (self.current_state == MoveIt2State.EXECUTING): + self.logger.info(f"Executing joint pose....") + result = py_trees.common.Status.RUNNING + self.execute = True + else: + self.logger.info(f"Requesting joint pose....") + result = py_trees.common.Status.RUNNING + + return result + + + def move_to_pose(self): + self.moveit2.move_to_pose( + position=self.position, + quat_xyzw=self.orientation, + cartesian=self.cartesian, + cartesian_max_step=self.cartesian_max_step, + cartesian_fraction_threshold=self.cartesian_fraction_threshold, + ) + diff --git a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc index ac5aa02a..55bf43e1 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc @@ -156,4 +156,7 @@ action robotic_arm.move_to_joint_pose: joint_pose: string action robotic_arm.move_gripper: - gripper: string # open, close \ No newline at end of file + gripper: string # open, close + +action robotic_arm.move_to_pose: + goal_pose: pose_3d \ No newline at end of file diff --git a/scenario_execution_ros/setup.py b/scenario_execution_ros/setup.py index c35ea898..4906823b 100644 --- a/scenario_execution_ros/setup.py +++ b/scenario_execution_ros/setup.py @@ -65,6 +65,7 @@ 'assert_lifecycle_state = scenario_execution_ros.actions.assert_lifecycle_state:AssertLifecycleState', 'robotic_arm.move_to_joint_pose = scenario_execution_ros.actions.move_to_joint_pose:MoveToJointPose', 'robotic_arm.move_gripper = scenario_execution_ros.actions.move_gripper:MoveGripper', + 'robotic_arm.move_to_pose = scenario_execution_ros.actions.move_to_pose:MoveToPose', ], 'scenario_execution.osc_libraries': [ 'ros = ' From 8e9eeeccabb4a6881e604754272affebe863702f Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Fri, 7 Jun 2024 11:54:19 +0200 Subject: [PATCH 14/62] cleanuo --- .../actions/move_gripper.py | 26 +++++++------- .../actions/move_to_joint_pose.py | 29 +++++++-------- .../actions/move_to_pose.py | 29 +++++++-------- .../arm_sim_scenario/__init__.py | 1 - .../arm_sim_scenario/robots/__init__.py | 1 - .../arm_sim_scenario/robots/wx200.py | 35 ------------------- .../launch/sim_moveit_scenario_launch.py | 4 +-- 7 files changed, 43 insertions(+), 82 deletions(-) delete mode 100644 simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/__init__.py delete mode 100644 simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py b/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py index 837c8354..3b189cd5 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py @@ -17,11 +17,8 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node -from pymoveit2 import MoveIt2, MoveIt2State, GripperInterface -from arm_sim_scenario.robots import wx200 - +from pymoveit2 import MoveIt2State, GripperInterface import py_trees -import ast class MoveGripper(py_trees.behaviour.Behaviour): @@ -37,6 +34,11 @@ def __init__(self, name: str, associated_actor, gripper: str): self.gripper_joint_names = associated_actor["gripper_joint_names"] self.gripper = gripper self.execute = False + self.node = None + self.synchronous = True + self.cancel_after_secs = 0.0 + self.gripper_interface = None + self.current_state = None def setup(self, **kwargs): try: @@ -46,14 +48,13 @@ def setup(self, **kwargs): self.name, self.__class__.__name__) raise KeyError(error_message) from e - # Create MoveIt 2 interface self.gripper_interface = GripperInterface( - node= self.node, - gripper_joint_names= self.gripper_joint_names, - open_gripper_joint_positions=[-0.037, +0.037] , + node=self.node, + gripper_joint_names=self.gripper_joint_names, + open_gripper_joint_positions=[-0.037, +0.037], closed_gripper_joint_positions=[-0.015, 0.015], - gripper_group_name= self.move_group_gripper, + gripper_group_name=self.move_group_gripper, callback_group=ReentrantCallbackGroup(), gripper_command_action_name="gripper_action_controller/gripper_cmd" ) @@ -61,13 +62,13 @@ def setup(self, **kwargs): def update(self) -> py_trees.common.Status: self.current_state = self.gripper_interface.query_state() self.logger.info(f"Current State: {self.current_state}") - if (self.current_state == MoveIt2State.IDLE): - if (self.execute == False): + if self.current_state == MoveIt2State.IDLE: + if not self.execute: self.gripper_interface.open() result = py_trees.common.Status.RUNNING else: result = py_trees.common.Status.SUCCESS - elif (self.current_state == MoveIt2State.EXECUTING): + elif self.current_state == MoveIt2State.EXECUTING: self.logger.info(f"Executing gripper....") result = py_trees.common.Status.RUNNING self.execute = True @@ -76,4 +77,3 @@ def update(self) -> py_trees.common.Status: result = py_trees.common.Status.RUNNING return result - diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py index 190eca20..6defdd11 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py @@ -18,8 +18,6 @@ from rclpy.node import Node from pymoveit2 import MoveIt2, MoveIt2State -from arm_sim_scenario.robots import wx200 - import py_trees import ast @@ -35,6 +33,11 @@ def __init__(self, name: str, associated_actor, joint_pose: str): self.move_group_arm = associated_actor["move_group_arm"] self.joint_pose = ast.literal_eval(joint_pose) self.execute = False + self.node = None + self.synchronous = True + self.cancel_after_secs = 0.0 + self.moveit2 = None + self.current_state = None def setup(self, **kwargs): try: @@ -44,18 +47,13 @@ def setup(self, **kwargs): self.name, self.__class__.__name__) raise KeyError(error_message) from e - self.synchronous = True - - # # If non-positive, don't cancel. Only used if synchronous is False - self.cancel_after_secs = 0.0 - # Create MoveIt 2 interface self.moveit2 = MoveIt2( - node= self.node, - joint_names= self.joint_names, - base_link_name= self.namespace + '/' + self.base_link_name, - end_effector_name= self.namespace + '/' + self.end_effector_name, - group_name= self.move_group_arm, + node=self.node, + joint_names=self.joint_names, + base_link_name=self.namespace + '/' + self.base_link_name, + end_effector_name=self.namespace + '/' + self.end_effector_name, + group_name=self.move_group_arm, callback_group=ReentrantCallbackGroup() ) @@ -68,13 +66,13 @@ def setup(self, **kwargs): def update(self) -> py_trees.common.Status: self.current_state = self.moveit2.query_state() self.logger.info(f"Current State: {self.current_state}") - if (self.current_state == MoveIt2State.IDLE): - if (self.execute == False): + if self.current_state == MoveIt2State.IDLE: + if not self.execute: self.moveit2.move_to_configuration(self.joint_pose) result = py_trees.common.Status.RUNNING else: result = py_trees.common.Status.SUCCESS - elif (self.current_state == MoveIt2State.EXECUTING): + elif self.current_state == MoveIt2State.EXECUTING: self.logger.info(f"Executing joint pose....") result = py_trees.common.Status.RUNNING self.execute = True @@ -83,4 +81,3 @@ def update(self) -> py_trees.common.Status: result = py_trees.common.Status.RUNNING return result - diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py b/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py index 7fe65a19..13cdf935 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py @@ -18,9 +18,7 @@ from rclpy.node import Node from pymoveit2 import MoveIt2, MoveIt2State -from arm_sim_scenario.robots import wx200 import py_trees -import ast class MoveToPose(py_trees.behaviour.Behaviour): @@ -33,14 +31,19 @@ def __init__(self, name: str, associated_actor, goal_pose: list): self.end_effector_name = associated_actor["end_effector_name"] self.move_group_arm = associated_actor["move_group_arm"] self.position = [goal_pose['position']['x'], goal_pose['position']['y'], goal_pose['position']['z']] - self.orientation = [goal_pose['orientation']['roll'], goal_pose['orientation']['pitch'], goal_pose['orientation']['yaw'], 1.000000000] + self.orientation = [goal_pose['orientation']['roll'], goal_pose['orientation'] + ['pitch'], goal_pose['orientation']['yaw'], 1.000000000] self.cartesian = False self.cartesian_max_step = 0.025 self.cartesian_fraction_threshold = 0.0 self.cartesian_jump_threshold = 0.0 self.cartesian_avoid_collisions = False self.execute = False - + self.node = None + self.synchronous = True + self.cancel_after_secs = 0.0 + self.moveit2 = None + self.current_state = None def setup(self, **kwargs): try: @@ -57,11 +60,11 @@ def setup(self, **kwargs): # Create MoveIt 2 interface self.moveit2 = MoveIt2( - node= self.node, - joint_names= self.joint_names, - base_link_name= self.namespace + '/' + self.base_link_name, - end_effector_name= self.namespace + '/' + self.end_effector_name, - group_name= self.move_group_arm, + node=self.node, + joint_names=self.joint_names, + base_link_name=self.namespace + '/' + self.base_link_name, + end_effector_name=self.namespace + '/' + self.end_effector_name, + group_name=self.move_group_arm, callback_group=ReentrantCallbackGroup() ) @@ -75,13 +78,13 @@ def setup(self, **kwargs): def update(self) -> py_trees.common.Status: self.current_state = self.moveit2.query_state() - if (self.current_state == MoveIt2State.IDLE): - if (self.execute == False): + if self.current_state == MoveIt2State.IDLE: + if not self.execute: self.move_to_pose() result = py_trees.common.Status.RUNNING else: result = py_trees.common.Status.SUCCESS - elif (self.current_state == MoveIt2State.EXECUTING): + elif self.current_state == MoveIt2State.EXECUTING: self.logger.info(f"Executing joint pose....") result = py_trees.common.Status.RUNNING self.execute = True @@ -90,7 +93,6 @@ def update(self) -> py_trees.common.Status: result = py_trees.common.Status.RUNNING return result - def move_to_pose(self): self.moveit2.move_to_pose( @@ -100,4 +102,3 @@ def move_to_pose(self): cartesian_max_step=self.cartesian_max_step, cartesian_fraction_threshold=self.cartesian_fraction_threshold, ) - diff --git a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py index dfdb5a49..e69de29b 100644 --- a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py +++ b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py @@ -1 +0,0 @@ -from . import robots diff --git a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/__init__.py b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/__init__.py deleted file mode 100644 index b5f41325..00000000 --- a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from . import wx200 diff --git a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py deleted file mode 100644 index 32ed738c..00000000 --- a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/robots/wx200.py +++ /dev/null @@ -1,35 +0,0 @@ -from typing import List - -MOVE_GROUP_ARM: str = "interbotix_arm" -MOVE_GROUP_GRIPPER: str = "interbotix_gripper" - -OPEN_GRIPPER_JOINT_POSITIONS: List[float] = [0.037, -0.037] -CLOSED_GRIPPER_JOINT_POSITIONS: List[float] = [0.0195, -0.0195] - - -def joint_names() -> List[str]: - return [ - "waist", - "shoulder", - "elbow", - "wrist_angle", - "wrist_rotate", - "left_finger", - "right_finger", - ] - - -def base_link_name(prefix: str = "wx200") -> str: - return prefix + "/base_link" - - -def end_effector_name(prefix: str = "wx200") -> str: - return prefix + "/gripper_link" - - -def gripper_joint_names() -> List[str]: - return [ - "right_finger", - "left_finger" - - ] diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py index 23c95179..2e6614c0 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -17,8 +17,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetLaunchConfiguration -from launch.conditions import IfCondition, UnlessCondition +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution From f8050e18f0ce54e83ab5eaf9575c2fe5eda40c39 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Fri, 7 Jun 2024 14:05:52 +0200 Subject: [PATCH 15/62] improve basic logic --- examples/example_moveit/example_moveit.osc | 3 ++ .../actions/move_gripper.py | 37 ++++++++++++------- .../actions/move_to_joint_pose.py | 32 ++++++++-------- .../actions/move_to_pose.py | 31 +++++++++------- 4 files changed, 60 insertions(+), 43 deletions(-) diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc index 47c1f50d..1b3323d3 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit/example_moveit.osc @@ -19,6 +19,9 @@ scenario example_moveit: wx200.move_gripper() with: keep(it.gripper == 'open') wait elapsed(3s) + wx200.move_gripper() with: + keep(it.gripper == 'close') + wait elapsed(3s) emit end time_out: serial: wait elapsed(60s) diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py b/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py index 3b189cd5..86ad2f94 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py @@ -35,8 +35,6 @@ def __init__(self, name: str, associated_actor, gripper: str): self.gripper = gripper self.execute = False self.node = None - self.synchronous = True - self.cancel_after_secs = 0.0 self.gripper_interface = None self.current_state = None @@ -59,21 +57,32 @@ def setup(self, **kwargs): gripper_command_action_name="gripper_action_controller/gripper_cmd" ) + if self.gripper not in ['open', 'close']: + raise ValueError("Invalid gripper state speficied. Allowed values are 'open' or 'close'.") + def update(self) -> py_trees.common.Status: self.current_state = self.gripper_interface.query_state() - self.logger.info(f"Current State: {self.current_state}") - if self.current_state == MoveIt2State.IDLE: - if not self.execute: - self.gripper_interface.open() + if not self.execute: + if self.current_state == MoveIt2State.EXECUTING: + self.logger.info("Another motion is in progress. Waiting for the current motion to complete...") result = py_trees.common.Status.RUNNING else: - result = py_trees.common.Status.SUCCESS - elif self.current_state == MoveIt2State.EXECUTING: - self.logger.info(f"Executing gripper....") - result = py_trees.common.Status.RUNNING - self.execute = True + if self.gripper == 'open': + self.gripper_interface.open() + else: + self.gripper_interface.close() + self.logger.info(f"No motion in progress. Setting gripper to {self.gripper} state...") + self.execute = True + result = py_trees.common.Status.RUNNING else: - self.logger.info(f"Requesting gripper pose....") - result = py_trees.common.Status.RUNNING - + if self.current_state == MoveIt2State.IDLE: + self.logger.info(f"Gripper state {self.gripper} set successfully.") + result = py_trees.common.Status.SUCCESS + self.execute = False # Reset for potential future executions + elif self.current_state == MoveIt2State.EXECUTING: + self.logger.info("Motion in progress...") + result = py_trees.common.Status.RUNNING + else: + self.logger.info("Unknown state encountered while executing motion. Requesting again...") + result = py_trees.common.Status.RUNNING return result diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py index 6defdd11..c25e47c9 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py @@ -34,8 +34,6 @@ def __init__(self, name: str, associated_actor, joint_pose: str): self.joint_pose = ast.literal_eval(joint_pose) self.execute = False self.node = None - self.synchronous = True - self.cancel_after_secs = 0.0 self.moveit2 = None self.current_state = None @@ -57,7 +55,7 @@ def setup(self, **kwargs): callback_group=ReentrantCallbackGroup() ) - self.moveit2.planner_id = "RRTConnectkConfigDefault" + self.moveit2.planner_id = "RRTConnect" # Scale down velocity and acceleration of joints (percentage of maximum) self.moveit2.max_velocity = 0.5 @@ -65,19 +63,23 @@ def setup(self, **kwargs): def update(self) -> py_trees.common.Status: self.current_state = self.moveit2.query_state() - self.logger.info(f"Current State: {self.current_state}") - if self.current_state == MoveIt2State.IDLE: - if not self.execute: - self.moveit2.move_to_configuration(self.joint_pose) + if not self.execute: + if self.current_state == MoveIt2State.EXECUTING: + self.logger.info("Another motion is in progress. Waiting for current motion to complete...") result = py_trees.common.Status.RUNNING else: - result = py_trees.common.Status.SUCCESS - elif self.current_state == MoveIt2State.EXECUTING: - self.logger.info(f"Executing joint pose....") - result = py_trees.common.Status.RUNNING - self.execute = True + self.logger.info("No motion in progress. Initiating move to joint pose...") + self.moveit2.move_to_configuration(self.joint_pose) + result = py_trees.common.Status.RUNNING + self.execute = True else: - self.logger.info(f"Requesting joint pose....") - result = py_trees.common.Status.RUNNING - + if self.current_state == MoveIt2State.IDLE: + self.logger.info("Motion to joint pose successful.") + result = py_trees.common.Status.SUCCESS + elif self.current_state == MoveIt2State.EXECUTING: + self.logger.info("Motion to joint pose in progress...") + result = py_trees.common.Status.RUNNING + else: + self.logger.info("Unknown state encountered while executing motion. Requesting joint pose again...") + result = py_trees.common.Status.RUNNING return result diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py b/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py index 13cdf935..5cb35203 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py @@ -40,8 +40,6 @@ def __init__(self, name: str, associated_actor, goal_pose: list): self.cartesian_avoid_collisions = False self.execute = False self.node = None - self.synchronous = True - self.cancel_after_secs = 0.0 self.moveit2 = None self.current_state = None @@ -68,7 +66,7 @@ def setup(self, **kwargs): callback_group=ReentrantCallbackGroup() ) - self.moveit2.planner_id = "RRTConnectkConfigDefault" + self.moveit2.planner_id = "RRTConnect" # Scale down velocity and acceleration of joints (percentage of maximum) self.moveit2.max_velocity = 0.5 @@ -78,20 +76,25 @@ def setup(self, **kwargs): def update(self) -> py_trees.common.Status: self.current_state = self.moveit2.query_state() - if self.current_state == MoveIt2State.IDLE: - if not self.execute: - self.move_to_pose() + if not self.execute: + if self.current_state == MoveIt2State.EXECUTING: + self.logger.info("Another motion is in progress. Waiting for current motion to complete...") result = py_trees.common.Status.RUNNING else: - result = py_trees.common.Status.SUCCESS - elif self.current_state == MoveIt2State.EXECUTING: - self.logger.info(f"Executing joint pose....") - result = py_trees.common.Status.RUNNING - self.execute = True + self.logger.info("No motion in progress. Initiating move to goal pose...") + self.move_to_pose() + result = py_trees.common.Status.RUNNING + self.execute = True else: - self.logger.info(f"Requesting joint pose....") - result = py_trees.common.Status.RUNNING - + if self.current_state == MoveIt2State.IDLE: + self.logger.info("Motion to goal pose successful.") + result = py_trees.common.Status.SUCCESS + elif self.current_state == MoveIt2State.EXECUTING: + self.logger.info("Motion to goal pose in progress...") + result = py_trees.common.Status.RUNNING + else: + self.logger.info("Unknown state encountered while executing motion. Requesting goal pose again...") + result = py_trees.common.Status.RUNNING return result def move_to_pose(self): From 66b326e0dcd411af15ebf9609523c6d8d88c1cbe Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Fri, 7 Jun 2024 14:08:54 +0200 Subject: [PATCH 16/62] format fix --- .../scenario_execution_ros/actions/move_to_pose.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py b/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py index 5cb35203..718a2d18 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py @@ -51,11 +51,6 @@ def setup(self, **kwargs): self.name, self.__class__.__name__) raise KeyError(error_message) from e - self.synchronous = True - - # # If non-positive, don't cancel. Only used if synchronous is False - self.cancel_after_secs = 0.0 - # Create MoveIt 2 interface self.moveit2 = MoveIt2( node=self.node, From bbb84dd02d27eed03a836d9cf93219442d769436 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Fri, 7 Jun 2024 15:44:11 +0200 Subject: [PATCH 17/62] add simulation scenario for pick place --- examples/example_moveit_simulation/README.md | 23 ++++++++++ .../example_moveit_simulation/__init__.py | 15 +++++++ .../example_moveit_simulation/models/box.sdf | 37 ++++++++++++++++ .../example_moveit_simulation/package.xml | 25 +++++++++++ .../resource/example_moveit_simulation | 0 .../scenarios/example_moveit_simulation.osc} | 21 ++++++--- examples/example_moveit_simulation/setup.py | 43 +++++++++++++++++++ .../example_simulation/models/box_moveit.sdf | 37 ++++++++++++++++ 8 files changed, 196 insertions(+), 5 deletions(-) create mode 100644 examples/example_moveit_simulation/README.md create mode 100644 examples/example_moveit_simulation/example_moveit_simulation/__init__.py create mode 100644 examples/example_moveit_simulation/models/box.sdf create mode 100644 examples/example_moveit_simulation/package.xml create mode 100644 examples/example_moveit_simulation/resource/example_moveit_simulation rename examples/{example_moveit/example_moveit.osc => example_moveit_simulation/scenarios/example_moveit_simulation.osc} (53%) create mode 100644 examples/example_moveit_simulation/setup.py create mode 100644 examples/example_simulation/models/box_moveit.sdf diff --git a/examples/example_moveit_simulation/README.md b/examples/example_moveit_simulation/README.md new file mode 100644 index 00000000..ece80a34 --- /dev/null +++ b/examples/example_moveit_simulation/README.md @@ -0,0 +1,23 @@ +# Example Simulation Navigation + +To run the Example Simulation Moveit with scenario, first build the `example_moveit_simulation` package: + +```bash +colcon build --packages-up-to example_moveit_simulation +``` + +Source the workspace: + +```bash +source install/setup.bash +``` + +Now, run the following command to launch the scenario: + +```bash +ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:=examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc +``` + +A turtlebot is initialised with nav2 which drives to a goal and back. During the ride an obstacle is spawned in front of the turtlebot which will then drive around the object. + +For a more detailed understanding of the code structure and scenario implementation please refer to the [tutorial documentation](https://intellabs.github.io/scenario_execution/tutorials.html). diff --git a/examples/example_moveit_simulation/example_moveit_simulation/__init__.py b/examples/example_moveit_simulation/example_moveit_simulation/__init__.py new file mode 100644 index 00000000..3ba13780 --- /dev/null +++ b/examples/example_moveit_simulation/example_moveit_simulation/__init__.py @@ -0,0 +1,15 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/examples/example_moveit_simulation/models/box.sdf b/examples/example_moveit_simulation/models/box.sdf new file mode 100644 index 00000000..c8011261 --- /dev/null +++ b/examples/example_moveit_simulation/models/box.sdf @@ -0,0 +1,37 @@ + + + + 0 0 0.5 0 0 0 + + + 0.1 + + 0.083 + 0.0 + 0.0 + 0.083 + 0.0 + 0.083 + + + + + + 0.03 0.03 0.03 + + + + + + + 0.03 0.03 0.03 + + + + 0.1 0.1 0.1 1 + 0.4 0.4 0.4 1 + + + + + \ No newline at end of file diff --git a/examples/example_moveit_simulation/package.xml b/examples/example_moveit_simulation/package.xml new file mode 100644 index 00000000..ad8ae3d5 --- /dev/null +++ b/examples/example_moveit_simulation/package.xml @@ -0,0 +1,25 @@ + + + + example_moveit_simulation + 1.1.0 + Scenario Execution Example for Moveit Simulation + Intel Labs + Intel Labs + Apache-2.0 + + scenario_execution + scenario_execution_gazebo + arm_sim_scenario + + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/examples/example_moveit_simulation/resource/example_moveit_simulation b/examples/example_moveit_simulation/resource/example_moveit_simulation new file mode 100644 index 00000000..e69de29b diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc similarity index 53% rename from examples/example_moveit/example_moveit.osc rename to examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc index 1b3323d3..9bbe863f 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc @@ -1,4 +1,5 @@ import osc.ros +import osc.gazebo scenario example_moveit: wx200: robotic_arm with: @@ -9,18 +10,28 @@ scenario example_moveit: keep(it.gripper_joint_names == ["right_finger","left_finger"]) keep(it.move_group_arm == 'interbotix_arm') keep(it.move_group_gripper == 'interbotix_gripper') + box: osc_actor do parallel: + serial: + box.spawn(spawn_pose: pose_3d( + position: position_3d(x: 0.25m, y: 0.0m, z: 0.02m), + orientation: orientation_3d(yaw: 0.0rad)), + model: 'example_moveit_simulation://models/box.sdf',world_name: 'empty') serial: wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[-1.5708, 0.0, 0.0, 0.0, 0.0]') - wait elapsed(3s) - wx200.move_to_pose(pose_3d(position: position_3d(x: 0.1m, y: 0.0m, z: 0.4m))) - wait elapsed(3s) + keep(it.joint_pose == '[0.0, 0.122173, 0.296706, 1.0472, 0.0]') wx200.move_gripper() with: keep(it.gripper == 'open') - wait elapsed(3s) + wx200.move_to_joint_pose() with: + keep(it.joint_pose == '[0.0, 0.296706, 0.296706, 1.22173, 0.0]') wx200.move_gripper() with: keep(it.gripper == 'close') + wx200.move_to_pose(pose_3d(position: position_3d(x: 0.1m, y: 0.0m, z: 0.4m))) + wx200.move_to_joint_pose() with: + keep(it.joint_pose == '[0.5235, 0.0, 0.0, 0.0, 0.0]') + wx200.move_gripper() with: + keep(it.gripper == 'open') + wx200.move_to_pose(pose_3d(position: position_3d(x: 0.1m, y: 0.0m, z: 0.4m))) wait elapsed(3s) emit end time_out: serial: diff --git a/examples/example_moveit_simulation/setup.py b/examples/example_moveit_simulation/setup.py new file mode 100644 index 00000000..a3c2fbfb --- /dev/null +++ b/examples/example_moveit_simulation/setup.py @@ -0,0 +1,43 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from glob import glob +import os +from setuptools import setup + +PACKAGE_NAME = 'example_moveit_simulation' + +setup( + name=PACKAGE_NAME, + version='1.1.0', + packages=[PACKAGE_NAME], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), + (os.path.join('share', PACKAGE_NAME, 'scenarios'), glob('scenarios/*.osc')), + (os.path.join('share', PACKAGE_NAME, 'models'), glob('models/*.sdf')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Intel Labs', + maintainer_email='scenario-execution@intel.com', + description='Scenario Execution Example for Moveit Simulation', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + }, +) diff --git a/examples/example_simulation/models/box_moveit.sdf b/examples/example_simulation/models/box_moveit.sdf new file mode 100644 index 00000000..c8011261 --- /dev/null +++ b/examples/example_simulation/models/box_moveit.sdf @@ -0,0 +1,37 @@ + + + + 0 0 0.5 0 0 0 + + + 0.1 + + 0.083 + 0.0 + 0.0 + 0.083 + 0.0 + 0.083 + + + + + + 0.03 0.03 0.03 + + + + + + + 0.03 0.03 0.03 + + + + 0.1 0.1 0.1 1 + 0.4 0.4 0.4 1 + + + + + \ No newline at end of file From 5995aa46115ff7b911227615847cdf976b9019ce Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 12 Jun 2024 11:58:50 +0200 Subject: [PATCH 18/62] move file to seperate moveit pkg --- .../example_moveit_simulation/package.xml | 3 +- .../scenarios/example_moveit_simulation.osc | 2 +- scenario_execution_moveit/MANIFEST.in | 1 + scenario_execution_moveit/README.md | 4 ++ scenario_execution_moveit/moveit.osc | 20 +++++++ scenario_execution_moveit/package.xml | 30 ++++++++++ .../resource/scenario_execution_moveit | 0 .../scenario_execution_moveit/__init__.py | 23 ++++++++ .../actions/__init__.py | 15 +++++ .../actions/move_gripper.py | 5 +- .../actions/move_to_joint_pose.py | 5 +- .../actions/move_to_pose.py | 5 +- .../get_osc_library.py | 19 +++++++ .../lib_osc/moveit.osc | 20 +++++++ scenario_execution_moveit/setup.cfg | 4 ++ scenario_execution_moveit/setup.py | 55 +++++++++++++++++++ .../scenario_execution_ros/lib_osc/ros.osc | 21 +------ scenario_execution_ros/setup.py | 3 - 18 files changed, 205 insertions(+), 30 deletions(-) create mode 100644 scenario_execution_moveit/MANIFEST.in create mode 100644 scenario_execution_moveit/README.md create mode 100644 scenario_execution_moveit/moveit.osc create mode 100644 scenario_execution_moveit/package.xml create mode 100644 scenario_execution_moveit/resource/scenario_execution_moveit create mode 100644 scenario_execution_moveit/scenario_execution_moveit/__init__.py create mode 100644 scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py rename {scenario_execution_ros/scenario_execution_ros => scenario_execution_moveit/scenario_execution_moveit}/actions/move_gripper.py (97%) rename {scenario_execution_ros/scenario_execution_ros => scenario_execution_moveit/scenario_execution_moveit}/actions/move_to_joint_pose.py (97%) rename {scenario_execution_ros/scenario_execution_ros => scenario_execution_moveit/scenario_execution_moveit}/actions/move_to_pose.py (97%) create mode 100644 scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py create mode 100644 scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc create mode 100644 scenario_execution_moveit/setup.cfg create mode 100644 scenario_execution_moveit/setup.py diff --git a/examples/example_moveit_simulation/package.xml b/examples/example_moveit_simulation/package.xml index ad8ae3d5..40ef3ef6 100644 --- a/examples/example_moveit_simulation/package.xml +++ b/examples/example_moveit_simulation/package.xml @@ -11,7 +11,8 @@ scenario_execution scenario_execution_gazebo arm_sim_scenario - + scenario_execution_moveit + rclpy ament_copyright diff --git a/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc index 9bbe863f..48cca11a 100644 --- a/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc +++ b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc @@ -1,4 +1,4 @@ -import osc.ros +import osc.moveit import osc.gazebo scenario example_moveit: diff --git a/scenario_execution_moveit/MANIFEST.in b/scenario_execution_moveit/MANIFEST.in new file mode 100644 index 00000000..cb3eff09 --- /dev/null +++ b/scenario_execution_moveit/MANIFEST.in @@ -0,0 +1 @@ +include scenario_execution_moveit/lib_osc/*.osc \ No newline at end of file diff --git a/scenario_execution_moveit/README.md b/scenario_execution_moveit/README.md new file mode 100644 index 00000000..53717d50 --- /dev/null +++ b/scenario_execution_moveit/README.md @@ -0,0 +1,4 @@ +# Scenario Execution MOVEIT + +The `scenario_execution_moveit` package provides actions to control robotic manipulator using moveit library. + diff --git a/scenario_execution_moveit/moveit.osc b/scenario_execution_moveit/moveit.osc new file mode 100644 index 00000000..54681122 --- /dev/null +++ b/scenario_execution_moveit/moveit.osc @@ -0,0 +1,20 @@ +import osc.robotics + +actor robotic_arm inherits robot: + namespace: string = '' + joint_names: list of string + base_link_name: string = '' + end_effector_name: string = '' + gripper_joint_names: list of string + move_group_arm: string = '' + move_group_gripper: string = '' + + +action robotic_arm.move_to_joint_pose: + joint_pose: string + +action robotic_arm.move_gripper: + gripper: string # open, close + +action robotic_arm.move_to_pose: + goal_pose: pose_3d \ No newline at end of file diff --git a/scenario_execution_moveit/package.xml b/scenario_execution_moveit/package.xml new file mode 100644 index 00000000..cc80edf7 --- /dev/null +++ b/scenario_execution_moveit/package.xml @@ -0,0 +1,30 @@ + + + + scenario_execution_moveit + 1.1.0 + Scenario Execution for MOVEIT + Intel Labs + Intel Labs + Apache-2.0 + + scenario_execution + + rclpy + py_trees + py_trees_ros + visualization_msgs + nav2_simple_commander + xacro + rcl_interfaces + scenario_status + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/scenario_execution_moveit/resource/scenario_execution_moveit b/scenario_execution_moveit/resource/scenario_execution_moveit new file mode 100644 index 00000000..e69de29b diff --git a/scenario_execution_moveit/scenario_execution_moveit/__init__.py b/scenario_execution_moveit/scenario_execution_moveit/__init__.py new file mode 100644 index 00000000..73e52bde --- /dev/null +++ b/scenario_execution_moveit/scenario_execution_moveit/__init__.py @@ -0,0 +1,23 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +""" Main entry for scenario execution """ + +from . import actions + +__all__ = [ + 'actions', +] diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py b/scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py new file mode 100644 index 00000000..3ba13780 --- /dev/null +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py @@ -0,0 +1,15 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py similarity index 97% rename from scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py rename to scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py index 86ad2f94..b7d5e1ce 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_gripper.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py @@ -16,7 +16,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node - +from rclpy.logging import get_logger from pymoveit2 import MoveIt2State, GripperInterface import py_trees @@ -35,6 +35,7 @@ def __init__(self, name: str, associated_actor, gripper: str): self.gripper = gripper self.execute = False self.node = None + self.logger = None self.gripper_interface = None self.current_state = None @@ -45,7 +46,7 @@ def setup(self, **kwargs): error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format( self.name, self.__class__.__name__) raise KeyError(error_message) from e - + self.logger = get_logger(self.name) # Create MoveIt 2 interface self.gripper_interface = GripperInterface( node=self.node, diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py similarity index 97% rename from scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py rename to scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py index c25e47c9..769bd684 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_to_joint_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py @@ -16,7 +16,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node - +from rclpy.logging import get_logger from pymoveit2 import MoveIt2, MoveIt2State import py_trees import ast @@ -36,6 +36,7 @@ def __init__(self, name: str, associated_actor, joint_pose: str): self.node = None self.moveit2 = None self.current_state = None + self.logger = None def setup(self, **kwargs): try: @@ -44,7 +45,7 @@ def setup(self, **kwargs): error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format( self.name, self.__class__.__name__) raise KeyError(error_message) from e - + self.logger = get_logger(self.name) # Create MoveIt 2 interface self.moveit2 = MoveIt2( node=self.node, diff --git a/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py similarity index 97% rename from scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py rename to scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py index 718a2d18..4fc7dec2 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/move_to_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py @@ -16,7 +16,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node - +from rclpy.logging import get_logger from pymoveit2 import MoveIt2, MoveIt2State import py_trees @@ -42,6 +42,7 @@ def __init__(self, name: str, associated_actor, goal_pose: list): self.node = None self.moveit2 = None self.current_state = None + self.logger = None def setup(self, **kwargs): try: @@ -51,6 +52,8 @@ def setup(self, **kwargs): self.name, self.__class__.__name__) raise KeyError(error_message) from e + self.logger = get_logger(self.name) + # Create MoveIt 2 interface self.moveit2 = MoveIt2( node=self.node, diff --git a/scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py b/scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py new file mode 100644 index 00000000..bff78ee4 --- /dev/null +++ b/scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py @@ -0,0 +1,19 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + + +def get_moveit_library(): + return 'scenario_execution_moveit', 'moveit.osc' diff --git a/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc b/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc new file mode 100644 index 00000000..54681122 --- /dev/null +++ b/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc @@ -0,0 +1,20 @@ +import osc.robotics + +actor robotic_arm inherits robot: + namespace: string = '' + joint_names: list of string + base_link_name: string = '' + end_effector_name: string = '' + gripper_joint_names: list of string + move_group_arm: string = '' + move_group_gripper: string = '' + + +action robotic_arm.move_to_joint_pose: + joint_pose: string + +action robotic_arm.move_gripper: + gripper: string # open, close + +action robotic_arm.move_to_pose: + goal_pose: pose_3d \ No newline at end of file diff --git a/scenario_execution_moveit/setup.cfg b/scenario_execution_moveit/setup.cfg new file mode 100644 index 00000000..e910f915 --- /dev/null +++ b/scenario_execution_moveit/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/scenario_execution_moveit +[install] +install_scripts=$base/lib/scenario_execution_moveit diff --git a/scenario_execution_moveit/setup.py b/scenario_execution_moveit/setup.py new file mode 100644 index 00000000..c5568e7b --- /dev/null +++ b/scenario_execution_moveit/setup.py @@ -0,0 +1,55 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +""" Setup python package """ +from glob import glob +import os +from setuptools import find_namespace_packages, setup + +PACKAGE_NAME = 'scenario_execution_moveit' + +setup( + name=PACKAGE_NAME, + version='1.1.0', + packages=find_namespace_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), + (os.path.join('share', PACKAGE_NAME, 'scenarios'), glob('scenarios/*.osc')), + (os.path.join('share', PACKAGE_NAME, 'scenarios', 'test'), glob('scenarios/test/*osc')), + (os.path.join('share', PACKAGE_NAME, 'launch'), glob('launch/*launch.py')) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Intel Labs', + maintainer_email='scenario-execution@intel.com', + description='Scenario Execution for MOVEIT', + license='Apache License 2.0', + tests_require=['pytest'], + include_package_data=True, + entry_points={ + 'scenario_execution.actions': [ + 'robotic_arm.move_to_joint_pose = scenario_execution_moveit.actions.move_to_joint_pose:MoveToJointPose', + 'robotic_arm.move_gripper = scenario_execution_moveit.actions.move_gripper:MoveGripper', + 'robotic_arm.move_to_pose = scenario_execution_moveit.actions.move_to_pose:MoveToPose', + ], + 'scenario_execution.osc_libraries': [ + 'moveit = ' + 'scenario_execution_moveit.get_osc_library:get_moveit_library', + ] + }, +) diff --git a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc index 55bf43e1..7eacfb08 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc @@ -140,23 +140,4 @@ action wait_for_data: action wait_for_topics: # wait for topics to get available - topics: list of topics - -actor robotic_arm inherits robot: - namespace: string = '' - joint_names: list of string - base_link_name: string = '' - end_effector_name: string = '' - gripper_joint_names: list of string - move_group_arm: string = '' - move_group_gripper: string = '' - - -action robotic_arm.move_to_joint_pose: - joint_pose: string - -action robotic_arm.move_gripper: - gripper: string # open, close - -action robotic_arm.move_to_pose: - goal_pose: pose_3d \ No newline at end of file + topics: list of topics \ No newline at end of file diff --git a/scenario_execution_ros/setup.py b/scenario_execution_ros/setup.py index 4906823b..c33ff6eb 100644 --- a/scenario_execution_ros/setup.py +++ b/scenario_execution_ros/setup.py @@ -63,9 +63,6 @@ 'assert_topic_latency = scenario_execution_ros.actions.assert_topic_latency:AssertTopicLatency', 'assert_tf_moving = scenario_execution_ros.actions.assert_tf_moving:AssertTfMoving', 'assert_lifecycle_state = scenario_execution_ros.actions.assert_lifecycle_state:AssertLifecycleState', - 'robotic_arm.move_to_joint_pose = scenario_execution_ros.actions.move_to_joint_pose:MoveToJointPose', - 'robotic_arm.move_gripper = scenario_execution_ros.actions.move_gripper:MoveGripper', - 'robotic_arm.move_to_pose = scenario_execution_ros.actions.move_to_pose:MoveToPose', ], 'scenario_execution.osc_libraries': [ 'ros = ' From da3dca8c94fc124cb743480f317531f4a4994582 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 12 Jun 2024 13:35:16 +0200 Subject: [PATCH 19/62] additional fix --- .github/workflows/test_build.yml | 11 ---------- scenario_execution_moveit/moveit.osc | 20 ------------------- .../actions/move_gripper.py | 2 +- .../arm_sim_scenario/__init__.py | 15 ++++++++++++++ 4 files changed, 16 insertions(+), 32 deletions(-) delete mode 100644 scenario_execution_moveit/moveit.osc diff --git a/.github/workflows/test_build.yml b/.github/workflows/test_build.yml index d62af1f9..df4fdb2a 100644 --- a/.github/workflows/test_build.yml +++ b/.github/workflows/test_build.yml @@ -86,17 +86,6 @@ jobs: rosdep install --rosdistro=humble --from-paths . --ignore-src -r -y apt install -y ros-humble-gazebo-ros2-control pip3 install -r requirements.txt - - name: Build IGN_ROS_CONTROL # build from source for now because base package has errors. - shell: bash - run: | - mkdir -p /ros_ign/src - cd /ros_ign/src - git clone --branch humble https://github.com/ros-controls/gz_ros2_control.git - rosdep install -r --from-paths . --ignore-src --rosdistro humble -y - cd /ros_ign - source /opt/ros/humble/setup.bash - colcon build --continue-on-error - source install/setup.bash - name: Build shell: bash run: | diff --git a/scenario_execution_moveit/moveit.osc b/scenario_execution_moveit/moveit.osc deleted file mode 100644 index 54681122..00000000 --- a/scenario_execution_moveit/moveit.osc +++ /dev/null @@ -1,20 +0,0 @@ -import osc.robotics - -actor robotic_arm inherits robot: - namespace: string = '' - joint_names: list of string - base_link_name: string = '' - end_effector_name: string = '' - gripper_joint_names: list of string - move_group_arm: string = '' - move_group_gripper: string = '' - - -action robotic_arm.move_to_joint_pose: - joint_pose: string - -action robotic_arm.move_gripper: - gripper: string # open, close - -action robotic_arm.move_to_pose: - goal_pose: pose_3d \ No newline at end of file diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py index b7d5e1ce..5cf07ef4 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py @@ -35,7 +35,7 @@ def __init__(self, name: str, associated_actor, gripper: str): self.gripper = gripper self.execute = False self.node = None - self.logger = None + self.gripper_interface = None self.current_state = None diff --git a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py index e69de29b..3ba13780 100644 --- a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py +++ b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py @@ -0,0 +1,15 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 From 621a163dc5220e784ebd19efa92574d15aa4d004 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 12 Jun 2024 13:51:42 +0200 Subject: [PATCH 20/62] fix format --- .github/workflows/test_build.yml | 3 --- .../scenario_execution_moveit/actions/move_gripper.py | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/.github/workflows/test_build.yml b/.github/workflows/test_build.yml index df4fdb2a..071f5dbc 100644 --- a/.github/workflows/test_build.yml +++ b/.github/workflows/test_build.yml @@ -133,7 +133,6 @@ jobs: - name: Test Example Nav2 shell: bash run: | - source /ros_ign/install/setup.bash source /opt/ros/humble/setup.bash source install/setup.bash Xvfb :1 -screen 0 800x600x16 & @@ -147,7 +146,6 @@ jobs: - name: Test Example Simulation shell: bash run: | - source /ros_ign/install/setup.bash source /opt/ros/humble/setup.bash source install/setup.bash Xvfb :1 -screen 0 800x600x16 & @@ -161,7 +159,6 @@ jobs: - name: Test Example Multirobot shell: bash run: | - source /ros_ign/install/setup.bash source /opt/ros/humble/setup.bash source install/setup.bash Xvfb :1 -screen 0 800x600x16 & diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py index 5cf07ef4..0ec1e52f 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py @@ -35,9 +35,9 @@ def __init__(self, name: str, associated_actor, gripper: str): self.gripper = gripper self.execute = False self.node = None - self.gripper_interface = None self.current_state = None + self.logger = None def setup(self, **kwargs): try: From 18ebbf89ac1d1eaee3a8aaf7fe2022a46e9a21b2 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 12 Jun 2024 16:09:23 +0200 Subject: [PATCH 21/62] description cleanup --- .../launch/arm_description_launch.py | 61 +++++++++++---- .../rviz/xsarm_description.rviz | 4 - .../arm_sim_scenario/urdf/ar_tag.urdf.xacro | 55 -------------- .../arm_sim_scenario/urdf/control.urdf.xacro | 74 ++++++++++++------- .../arm_sim_scenario/urdf/wx200.urdf.xacro | 10 +-- 5 files changed, 97 insertions(+), 107 deletions(-) delete mode 100644 simulation/gazebo/arm_sim_scenario/urdf/ar_tag.urdf.xacro diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py index 12c8d541..c7376593 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py @@ -28,20 +28,37 @@ DeclareLaunchArgument('robot_model', default_value='wx200', choices=['wx200'], description='robot_model type of the Interbotix Arm'), - DeclareLaunchArgument('use_sim_time', default_value='false', - choices=['true', 'false'], - description='use_sim_time'), - DeclareLaunchArgument('robot_name', default_value=LaunchConfiguration('robot_model'), + DeclareLaunchArgument('robot_name', default_value=LaunchConfiguration('robot_model'), #namespace description='Robot name'), DeclareLaunchArgument('use_rviz', default_value='true', choices=['true', 'false'], description='launches RViz if set to `true`.'), DeclareLaunchArgument('rviz_config', - default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), - 'rviz', - 'xsarm_description.rviz', - ]), - description='file path to the config file RViz should load.',) + default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), + 'rviz', + 'xsarm_description.rviz', + ]), + description='file path to the config file RViz should load.'), + DeclareLaunchArgument('use_sim_time', default_value='false', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('use_joint_pub', + default_value='false', + choices=('true', 'false'), + description='launches the joint_state_publisher node.', + ), + DeclareLaunchArgument('use_joint_pub_gui', + default_value='false', + choices=('true', 'false'), + description='launches the joint_state_publisher GUI.', + ), + DeclareLaunchArgument('hardware_type', + default_value='ignition', + choices=['actual', 'fake', 'gz_classic', 'ignition'], + description='configure robot_description to use actual, fake, or simulated hardware', + ) + + ] @@ -50,11 +67,14 @@ def generate_launch_description(): xacro_file = PathJoinSubstitution([pkg_arm_sim_scenario, 'urdf', 'wx200.urdf.xacro']) - use_rviz = LaunchConfiguration('use_rviz') - robot_name = LaunchConfiguration('robot_name') robot_model = LaunchConfiguration('robot_model') - use_sim_time = LaunchConfiguration('use_sim_time') + robot_name = LaunchConfiguration('robot_name') + use_rviz = LaunchConfiguration('use_rviz') rviz_config = LaunchConfiguration('rviz_config') + use_sim_time = LaunchConfiguration('use_sim_time') + use_joint_pub = LaunchConfiguration('use_joint_pub') + use_joint_pub_gui = LaunchConfiguration('use_joint_pub_gui') + hardware_type = LaunchConfiguration('hardware_type') robot_state_publisher = Node( package='robot_state_publisher', @@ -65,21 +85,21 @@ def generate_launch_description(): {'use_sim_time': use_sim_time}, {'robot_description': ParameterValue(Command([ 'xacro', ' ', xacro_file, ' ', - 'gazebo:=ignition', ' ', + 'robot_model:=', robot_model, ' ', + 'robot_name:=', robot_name, ' ', 'base_link_frame:=', 'base_link', ' ', 'use_gripper:=', 'true', ' ', 'show_ar_tag:=', 'false', ' ', 'show_gripper_bar:=', 'true', ' ', 'show_gripper_fingers:=', 'true', ' ', 'use_world_frame:=', 'true', ' ', - 'robot_model:=', robot_model, ' ', - 'robot_name:=', robot_name, ' ', - 'hardware_type:=', 'gz_classic']), value_type=str)}, + 'hardware_type:=', hardware_type]), value_type=str)}, ], namespace=robot_name ) joint_state_publisher = Node( + condition=IfCondition(use_joint_pub), package='joint_state_publisher', executable='joint_state_publisher', name='joint_state_publisher', @@ -88,6 +108,14 @@ def generate_launch_description(): namespace=robot_name ) + joint_state_publisher_gui = Node( + condition=IfCondition(use_joint_pub_gui), + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + namespace=robot_name, + output={'both': 'log'}, + ) + rviz2 = Node( condition=IfCondition(use_rviz), package='rviz2', @@ -106,5 +134,6 @@ def generate_launch_description(): ld = LaunchDescription(ARGUMENTS) ld.add_action(robot_state_publisher) ld.add_action(joint_state_publisher) + ld.add_action(joint_state_publisher_gui) ld.add_action(rviz2) return ld diff --git a/simulation/gazebo/arm_sim_scenario/rviz/xsarm_description.rviz b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_description.rviz index ac0a8870..a527ca51 100644 --- a/simulation/gazebo/arm_sim_scenario/rviz/xsarm_description.rviz +++ b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_description.rviz @@ -6,8 +6,6 @@ Panels: Expanded: ~ Splitter Ratio: 0.6176470518112183 Tree Height: 503 - - Class: interbotix_xs_rviz/Interbotix Control Panel - Name: Interbotix Control Panel RobotNameSpace: "" Visualization Manager: Class: "" @@ -138,8 +136,6 @@ Window Geometry: Height: 923 Hide Left Dock: false Hide Right Dock: true - Interbotix Control Panel: - collapsed: false QMainWindow State: 000000ff00000000fd00000004000000000000018b00000368fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000001600000226000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000300049006e0074006500720062006f00740069007800200043006f006e00740072006f006c002000500061006e0065006c01000002420000013c0000012e000001510000000100000110000002a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a20000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006080000004cfc0100000002fb0000000800540069006d00650100000000000006080000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004770000036800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 Width: 1544 X: 72 diff --git a/simulation/gazebo/arm_sim_scenario/urdf/ar_tag.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/ar_tag.urdf.xacro deleted file mode 100644 index 9e63b914..00000000 --- a/simulation/gazebo/arm_sim_scenario/urdf/ar_tag.urdf.xacro +++ /dev/null @@ -1,55 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro index 169314aa..7c097d36 100644 --- a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro @@ -20,10 +20,14 @@ - - - ign_ros2_control/IgnitionSystem - + + gazebo_ros2_control/GazeboSystem + + + + + ign_ros2_control/IgnitionSystem + @@ -80,12 +84,12 @@ - - - - - - + + + + + + @@ -96,7 +100,7 @@ - + @@ -106,27 +110,43 @@ - + - - - - - - /$(arg robot_name) - - robot_description - robot_state_publisher - $(find arm_sim_scenario)/config/trajectory_controllers/${robot_model}_trajectory_controllers.yaml - - - - - + + + + + + /$(arg robot_name) + + robot_description + robot_state_publisher + $(find arm_sim_scenario)/config/trajectory_controllers/${robot_model}_trajectory_controllers.yaml + + + + + + + + + + + + /$(arg robot_name) + + robot_description + robot_state_publisher + $(find arm_sim_scenario)/config/trajectory_controllers/${robot_model}_trajectory_controllers.yaml + + + + + diff --git a/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro index c55b82e8..cef57cc9 100644 --- a/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro @@ -16,7 +16,7 @@ - + @@ -51,7 +51,7 @@ - + @@ -627,7 +627,7 @@ - + - + - + From 955d2a7f88b5f5fed13c5adfd2620e91c1fcdef4 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 12 Jun 2024 17:48:56 +0200 Subject: [PATCH 22/62] cleanup --- .../arm_sim_scenario/launch/ignition_launch.py | 16 +++++++++++----- .../arm_sim_scenario/urdf/wx200.urdf.xacro | 2 +- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index 1c6fd5e6..e894e008 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -29,17 +29,21 @@ DeclareLaunchArgument('robot_model', default_value='wx200', choices=['wx200'], description='model type of the Interbotix Arm'), - DeclareLaunchArgument('use_sim_time', default_value='true', - choices=['true', 'false'], - description='use_sim_time'), DeclareLaunchArgument('robot_name', default_value='wx200', description='Robot name'), DeclareLaunchArgument('use_rviz', default_value='false', choices=['true', 'false'], description='launches RViz if set to `true`.'), - + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), DeclareLaunchArgument('headless', default_value='False', description='Whether to execute simulation gui'), + DeclareLaunchArgument('hardware_type', + default_value='ignition', + choices=['actual', 'fake', 'gz_classic', 'ignition'], + description='configure robot_description to use actual, fake, or simulated hardware', + ) ] @@ -50,6 +54,7 @@ def generate_launch_description(): robot_name = LaunchConfiguration('robot_name') use_rviz = LaunchConfiguration('use_rviz') use_sim_time = LaunchConfiguration('use_sim_time') + hardware_type = LaunchConfiguration('hardware_type') env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname( @@ -97,7 +102,8 @@ def generate_launch_description(): 'robot_model': robot_model, 'robot_name': robot_name, 'use_rviz': use_rviz, - 'use_sim_time': use_sim_time + 'use_sim_time': use_sim_time, + 'hardware_type' : hardware_type }.items(), ) diff --git a/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro index cef57cc9..4ea14bdf 100644 --- a/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro @@ -705,7 +705,7 @@ - + From 87c2521202d8edd7bc30f60569c1588c3caaeeb7 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 13 Jun 2024 16:08:16 +0200 Subject: [PATCH 23/62] enhance functions --- .../actions/move_gripper.py | 11 +++--- .../actions/move_to_joint_pose.py | 27 +++++++++----- .../actions/move_to_pose.py | 28 ++++++++++----- .../lib_osc/moveit.osc | 2 ++ .../launch/arm_description_launch.py | 36 +++++++++---------- .../launch/ignition_launch.py | 17 ++++++--- 6 files changed, 77 insertions(+), 44 deletions(-) diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py index 0ec1e52f..93fe35e5 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py @@ -19,6 +19,7 @@ from rclpy.logging import get_logger from pymoveit2 import MoveIt2State, GripperInterface import py_trees +import ast class MoveGripper(py_trees.behaviour.Behaviour): @@ -32,6 +33,8 @@ def __init__(self, name: str, associated_actor, gripper: str): self.move_group_arm = associated_actor["move_group_arm"] self.move_group_gripper = associated_actor["move_group_gripper"] self.gripper_joint_names = associated_actor["gripper_joint_names"] + self.open_gripper_position = ast.literal_eval(associated_actor["open_gripper_position"]) + self.close_gripper_position = ast.literal_eval(associated_actor["close_gripper_position"]) self.gripper = gripper self.execute = False self.node = None @@ -47,12 +50,13 @@ def setup(self, **kwargs): self.name, self.__class__.__name__) raise KeyError(error_message) from e self.logger = get_logger(self.name) + # Create MoveIt 2 interface self.gripper_interface = GripperInterface( node=self.node, gripper_joint_names=self.gripper_joint_names, - open_gripper_joint_positions=[-0.037, +0.037], - closed_gripper_joint_positions=[-0.015, 0.015], + open_gripper_joint_positions=self.open_gripper_position, + closed_gripper_joint_positions=self.close_gripper_position, gripper_group_name=self.move_group_gripper, callback_group=ReentrantCallbackGroup(), gripper_command_action_name="gripper_action_controller/gripper_cmd" @@ -72,14 +76,13 @@ def update(self) -> py_trees.common.Status: self.gripper_interface.open() else: self.gripper_interface.close() - self.logger.info(f"No motion in progress. Setting gripper to {self.gripper} state...") + self.logger.info(f"Setting gripper to {self.gripper} state...") self.execute = True result = py_trees.common.Status.RUNNING else: if self.current_state == MoveIt2State.IDLE: self.logger.info(f"Gripper state {self.gripper} set successfully.") result = py_trees.common.Status.SUCCESS - self.execute = False # Reset for potential future executions elif self.current_state == MoveIt2State.EXECUTING: self.logger.info("Motion in progress...") result = py_trees.common.Status.RUNNING diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py index 769bd684..03a5ddd0 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py @@ -14,6 +14,7 @@ # # SPDX-License-Identifier: Apache-2.0 +from time import sleep from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.logging import get_logger @@ -64,6 +65,7 @@ def setup(self, **kwargs): def update(self) -> py_trees.common.Status: self.current_state = self.moveit2.query_state() + result = py_trees.common.Status.RUNNING if not self.execute: if self.current_state == MoveIt2State.EXECUTING: self.logger.info("Another motion is in progress. Waiting for current motion to complete...") @@ -73,14 +75,23 @@ def update(self) -> py_trees.common.Status: self.moveit2.move_to_configuration(self.joint_pose) result = py_trees.common.Status.RUNNING self.execute = True - else: - if self.current_state == MoveIt2State.IDLE: - self.logger.info("Motion to joint pose successful.") - result = py_trees.common.Status.SUCCESS - elif self.current_state == MoveIt2State.EXECUTING: - self.logger.info("Motion to joint pose in progress...") - result = py_trees.common.Status.RUNNING + elif self.current_state == MoveIt2State.EXECUTING: + future = self.moveit2.get_execution_future() + if future: + while not future.done(): + self.logger.info("Motion to joint pose in progress...") + sleep(1) + if str(future.result().status) == '4': + self.logger.info("Motion to joint pose successful.") + result = py_trees.common.Status.SUCCESS + else: + self.logger.info(f"{str(future.result().result.error_code)}") + result = py_trees.common.Status.FAILURE else: - self.logger.info("Unknown state encountered while executing motion. Requesting joint pose again...") + self.logger.info("Waiting for response from arm...") result = py_trees.common.Status.RUNNING + elif self.current_state == MoveIt2State.IDLE: + self.logger.info("Postion not reachable or arm is already at the specified position!") + result = py_trees.common.Status.FAILURE + return result diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py index 4fc7dec2..392b9578 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py @@ -19,6 +19,7 @@ from rclpy.logging import get_logger from pymoveit2 import MoveIt2, MoveIt2State import py_trees +from time import sleep class MoveToPose(py_trees.behaviour.Behaviour): @@ -74,25 +75,34 @@ def setup(self, **kwargs): def update(self) -> py_trees.common.Status: self.current_state = self.moveit2.query_state() + result = py_trees.common.Status.RUNNING if not self.execute: if self.current_state == MoveIt2State.EXECUTING: self.logger.info("Another motion is in progress. Waiting for current motion to complete...") result = py_trees.common.Status.RUNNING else: - self.logger.info("No motion in progress. Initiating move to goal pose...") + self.logger.info("No motion in progress. Initiating move to joint pose...") self.move_to_pose() result = py_trees.common.Status.RUNNING self.execute = True - else: - if self.current_state == MoveIt2State.IDLE: - self.logger.info("Motion to goal pose successful.") - result = py_trees.common.Status.SUCCESS - elif self.current_state == MoveIt2State.EXECUTING: - self.logger.info("Motion to goal pose in progress...") - result = py_trees.common.Status.RUNNING + elif self.current_state == MoveIt2State.EXECUTING: + future = self.moveit2.get_execution_future() + if future: + while not future.done(): + self.logger.info("Motion to goal pose in progress...") + sleep(1) + if str(future.result().status) == '4': + self.logger.info("Motion to goal pose successful.") + result = py_trees.common.Status.SUCCESS + else: + self.logger.info(f"{str(future.result().result.error_code)}") + result = py_trees.common.Status.FAILURE else: - self.logger.info("Unknown state encountered while executing motion. Requesting goal pose again...") + self.logger.info("Waiting for response from arm...") result = py_trees.common.Status.RUNNING + elif self.current_state == MoveIt2State.IDLE: + self.logger.info("pose not reachable or arm is already at the specified goal pose!") + result = py_trees.common.Status.FAILURE return result def move_to_pose(self): diff --git a/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc b/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc index 54681122..f85a10c2 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc +++ b/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc @@ -8,6 +8,8 @@ actor robotic_arm inherits robot: gripper_joint_names: list of string move_group_arm: string = '' move_group_gripper: string = '' + open_gripper_position: string = '' + close_gripper_position: string = '' action robotic_arm.move_to_joint_pose: diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py index c7376593..388c2f09 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py @@ -28,35 +28,35 @@ DeclareLaunchArgument('robot_model', default_value='wx200', choices=['wx200'], description='robot_model type of the Interbotix Arm'), - DeclareLaunchArgument('robot_name', default_value=LaunchConfiguration('robot_model'), #namespace + DeclareLaunchArgument('robot_name', default_value=LaunchConfiguration('robot_model'), # namespace description='Robot name'), DeclareLaunchArgument('use_rviz', default_value='true', choices=['true', 'false'], description='launches RViz if set to `true`.'), DeclareLaunchArgument('rviz_config', - default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), - 'rviz', - 'xsarm_description.rviz', - ]), - description='file path to the config file RViz should load.'), + default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), + 'rviz', + 'xsarm_description.rviz', + ]), + description='file path to the config file RViz should load.'), DeclareLaunchArgument('use_sim_time', default_value='false', choices=['true', 'false'], description='use_sim_time'), DeclareLaunchArgument('use_joint_pub', - default_value='false', - choices=('true', 'false'), - description='launches the joint_state_publisher node.', - ), + default_value='false', + choices=('true', 'false'), + description='launches the joint_state_publisher node.', + ), DeclareLaunchArgument('use_joint_pub_gui', - default_value='false', - choices=('true', 'false'), - description='launches the joint_state_publisher GUI.', - ), + default_value='false', + choices=('true', 'false'), + description='launches the joint_state_publisher GUI.', + ), DeclareLaunchArgument('hardware_type', - default_value='ignition', - choices=['actual', 'fake', 'gz_classic', 'ignition'], - description='configure robot_description to use actual, fake, or simulated hardware', - ) + default_value='ignition', + choices=['actual', 'fake', 'gz_classic', 'ignition'], + description='configure robot_description to use actual, fake, or simulated hardware', + ) ] diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index e894e008..485c58b5 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -40,10 +40,15 @@ DeclareLaunchArgument('headless', default_value='False', description='Whether to execute simulation gui'), DeclareLaunchArgument('hardware_type', - default_value='ignition', - choices=['actual', 'fake', 'gz_classic', 'ignition'], - description='configure robot_description to use actual, fake, or simulated hardware', - ) + default_value='ignition', + choices=['actual', 'fake', 'gz_classic', 'ignition'], + description='configure robot_description to use actual, fake, or simulated hardware', + ), + DeclareLaunchArgument('use_joint_pub_gui', + default_value='false', + choices=('true', 'false'), + description='launches the joint_state_publisher GUI.', + ), ] @@ -55,6 +60,7 @@ def generate_launch_description(): use_rviz = LaunchConfiguration('use_rviz') use_sim_time = LaunchConfiguration('use_sim_time') hardware_type = LaunchConfiguration('hardware_type') + use_joint_pub_gui = LaunchConfiguration('use_joint_pub_gui') env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname( @@ -103,7 +109,8 @@ def generate_launch_description(): 'robot_name': robot_name, 'use_rviz': use_rviz, 'use_sim_time': use_sim_time, - 'hardware_type' : hardware_type + 'hardware_type': hardware_type, + 'use_joint_pub_gui': use_joint_pub_gui }.items(), ) From 8b8cd74b870b82c3d412e3ff90b7cdcd18fa38a7 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 13 Jun 2024 16:58:30 +0200 Subject: [PATCH 24/62] fix gripper initial pos --- .../gazebo/arm_sim_scenario/urdf/control.urdf.xacro | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro index 7c097d36..446d619c 100644 --- a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro @@ -97,7 +97,9 @@ "${finger_limit_lower}" "${-finger_limit_lower}" - + + 0.0195 + @@ -108,7 +110,9 @@ "${-finger_limit_lower}" "${finger_limit_lower}" - + + -0.0195 + From 412f0aee406aaed2876d40f2e8897462f99fe443 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 13 Jun 2024 17:30:07 +0200 Subject: [PATCH 25/62] fix launch file --- .../launch/ignition_launch.py | 8 +-- .../arm_sim_scenario/launch/moveit_launch.py | 55 ++++++++++--------- 2 files changed, 32 insertions(+), 31 deletions(-) diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index 485c58b5..f3d31154 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -45,10 +45,10 @@ description='configure robot_description to use actual, fake, or simulated hardware', ), DeclareLaunchArgument('use_joint_pub_gui', - default_value='false', - choices=('true', 'false'), - description='launches the joint_state_publisher GUI.', - ), + default_value='false', + choices=('true', 'false'), + description='launches the joint_state_publisher GUI.', + ), ] diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py index e766e593..17b05980 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -47,28 +47,31 @@ def load_yaml(package_name, file_path): DeclareLaunchArgument('robot_model', default_value='wx200', choices=['wx200'], description='robot_model type of the Interbotix Arm'), - DeclareLaunchArgument('use_sim_time', default_value='true', - choices=['true', 'false'], - description='use_sim_time'), DeclareLaunchArgument('robot_name', default_value=LaunchConfiguration('robot_model'), description='Robot name'), DeclareLaunchArgument('use_rviz', default_value='false', choices=['true', 'false'], description='launches RViz if set to `true`.'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), DeclareLaunchArgument('rviz_config', default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), 'rviz', 'xsarm_moveit.rviz', ]), description='file path to the config file RViz should load.',), - DeclareLaunchArgument( - 'rviz_frame', - default_value='world', - description=( - 'defines the fixed frame parameter in RViz. Note that if `use_world_frame` is ' - '`false`, this parameter should be changed to a frame that exists.' - ), - ) + DeclareLaunchArgument('hardware_type', + default_value='ignition', + choices=['actual', 'fake', 'gz_classic', 'ignition'], + description='configure robot_description to use actual, fake, or simulated hardware', + ), + DeclareLaunchArgument('rviz_frame', default_value='world', + description=( + 'defines the fixed frame parameter in RViz. Note that if `use_world_frame` is ' + '`false`, this parameter should be changed to a frame that exists.' + ), + ) ] @@ -76,12 +79,13 @@ def generate_launch_description(): pkg_arm_sim_scenario = get_package_share_directory('arm_sim_scenario') - use_sim_time = LaunchConfiguration('use_sim_time') - rviz_config = LaunchConfiguration('rviz_config') + robot_model = LaunchConfiguration('robot_model') + robot_name = LaunchConfiguration('robot_name') use_rviz = LaunchConfiguration('use_rviz') + rviz_config = LaunchConfiguration('rviz_config') rviz_frame = LaunchConfiguration('rviz_frame') - robot_name = LaunchConfiguration('robot_name') - robot_model = LaunchConfiguration('robot_model') + use_sim_time = LaunchConfiguration('use_sim_time') + hardware_type = LaunchConfiguration('hardware_type') xacro_file = PathJoinSubstitution([pkg_arm_sim_scenario, 'urdf', @@ -107,7 +111,7 @@ def generate_launch_description(): 'use_world_frame:=', 'true', ' ', 'robot_model:=', robot_model, ' ', 'robot_name:=', robot_name, ' ', - 'hardware_type:=', 'gz_classic']), value_type=str)} + 'hardware_type:=', hardware_type]), value_type=str)} robot_description_semantic = {'robot_description_semantic': ParameterValue(Command([ 'xacro', ' ', srdf_xacro_file, ' ', @@ -120,7 +124,7 @@ def generate_launch_description(): 'use_world_frame:=', 'true', ' ', 'external_urdf_loc:=', '', ' ', 'external_srdf_loc:=', '', ' ', - 'hardware_type:=', 'gz_classic']), value_type=str)} + 'hardware_type:=', hardware_type]), value_type=str)} ompl_planning_pipeline_config = { 'move_group': { @@ -184,30 +188,28 @@ def generate_launch_description(): remappings = [ ( - 'wx200/get_planning_scene', - 'wx200/get_planning_scene' + [robot_name, '/get_planning_scene'], + [robot_name, '/get_planning_scene'] ), ( - '/arm_controller/follow_joint_trajectory', - 'wx200/arm_controller/follow_joint_trajectory' + '/arm_controller/follow_joint_trajectory', [robot_name, '/arm_controller/follow_joint_trajectory'] + ), ( - '/gripper_controller/follow_joint_trajectory', - 'wx200/gripper_controller/follow_joint_trajectory' + '/gripper_controller/follow_joint_trajectory', [robot_name, '/gripper_controller/follow_joint_trajectory'] + ), ] move_group_node = Node( package='moveit_ros_move_group', executable='move_group', - # namespace=robot_name_launch_arg, parameters=[ { 'planning_scene_monitor_options': { 'robot_description': 'robot_description', - 'joint_state_topic': - 'wx200/joint_states', + 'joint_state_topic': [robot_name, '/joint_states'], }, 'use_sim_time': use_sim_time, }, @@ -230,7 +232,6 @@ def generate_launch_description(): package='rviz2', executable='rviz2', name='rviz2', - # namespace=robot_name_launch_arg, arguments=[ '-d', rviz_config, '-f', rviz_frame, From 5440da5ddd8e3a42164a571c7a2cade204d939e5 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 13 Jun 2024 17:40:46 +0200 Subject: [PATCH 26/62] fix scenario file --- .../scenarios/example_moveit_simulation.osc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc index 48cca11a..89b3bf07 100644 --- a/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc +++ b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc @@ -10,6 +10,8 @@ scenario example_moveit: keep(it.gripper_joint_names == ["right_finger","left_finger"]) keep(it.move_group_arm == 'interbotix_arm') keep(it.move_group_gripper == 'interbotix_gripper') + keep(it.open_gripper_position == '[-0.037, +0.037]') + keep(it.close_gripper_position == '[-0.015, +0.015]') box: osc_actor do parallel: serial: From e03bdbb2a34a6895e03382093e0ad173845fe579 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 24 Jun 2024 12:50:58 +0200 Subject: [PATCH 27/62] cleanup --- examples/example_moveit_simulation/README.md | 2 +- .../scenarios/example_moveit_simulation.osc | 34 +++++++++++++------ scenario_execution_moveit/README.md | 2 +- scenario_execution_moveit/package.xml | 2 +- .../actions/move_gripper.py | 2 ++ .../actions/move_to_joint_pose.py | 1 + 6 files changed, 29 insertions(+), 14 deletions(-) diff --git a/examples/example_moveit_simulation/README.md b/examples/example_moveit_simulation/README.md index ece80a34..159a2e88 100644 --- a/examples/example_moveit_simulation/README.md +++ b/examples/example_moveit_simulation/README.md @@ -1,4 +1,4 @@ -# Example Simulation Navigation +# Example Simulation MoveIt To run the Example Simulation Moveit with scenario, first build the `example_moveit_simulation` package: diff --git a/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc index 89b3bf07..a2f3c80e 100644 --- a/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc +++ b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc @@ -11,29 +11,41 @@ scenario example_moveit: keep(it.move_group_arm == 'interbotix_arm') keep(it.move_group_gripper == 'interbotix_gripper') keep(it.open_gripper_position == '[-0.037, +0.037]') - keep(it.close_gripper_position == '[-0.015, +0.015]') + keep(it.close_gripper_position == '[-0.030, +0.030]') box: osc_actor + box2: osc_actor do parallel: serial: - box.spawn(spawn_pose: pose_3d( - position: position_3d(x: 0.25m, y: 0.0m, z: 0.02m), - orientation: orientation_3d(yaw: 0.0rad)), - model: 'example_moveit_simulation://models/box.sdf',world_name: 'empty') - serial: + wx200.move_gripper() with: + keep(it.gripper == 'open') + wx200.move_to_joint_pose() with: + keep(it.joint_pose == '[0.0, 0.576332, -0.0523599, 0.418879, 0.0]') + wx200.move_gripper() with: + keep(it.gripper == 'close') wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.0, 0.122173, 0.296706, 1.0472, 0.0]') + keep(it.joint_pose == '[0.0, -0.8377, -0.0523599, 0.418879, 0.0]') + wx200.move_to_joint_pose() with: + keep(it.joint_pose == '[0.5235, 0.5, -0.0523599, 0.418879, 0.0]') wx200.move_gripper() with: keep(it.gripper == 'open') wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.0, 0.296706, 0.296706, 1.22173, 0.0]') + keep(it.joint_pose == '[0.5235, 0.1, -0.0523599, 0.418879, 0.0]') + wx200.move_to_joint_pose() with: + keep(it.joint_pose == '[1.22, 0.1, -0.0523599, 0.418879, 0.0]') + wx200.move_to_joint_pose() with: + keep(it.joint_pose == '[1.22, 0.576332, -0.0523599, 0.418879, 0.0]') wx200.move_gripper() with: keep(it.gripper == 'close') - wx200.move_to_pose(pose_3d(position: position_3d(x: 0.1m, y: 0.0m, z: 0.4m))) wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.5235, 0.0, 0.0, 0.0, 0.0]') + keep(it.joint_pose == '[1.22, 0.1, -0.0523599, 0.418879, 0.0]') + wx200.move_to_joint_pose() with: + keep(it.joint_pose == '[0.0, 0.1, -0.0523599, 0.418879, 0.0]') + wx200.move_to_joint_pose() with: + keep(it.joint_pose == '[0.0, 0.436332, -0.0523599, 0.418879, 0.0]') wx200.move_gripper() with: keep(it.gripper == 'open') - wx200.move_to_pose(pose_3d(position: position_3d(x: 0.1m, y: 0.0m, z: 0.4m))) + wx200.move_to_joint_pose() with: + keep(it.joint_pose == '[0.0, -1.40, 1.55, 0.8, 0.0]') wait elapsed(3s) emit end time_out: serial: diff --git a/scenario_execution_moveit/README.md b/scenario_execution_moveit/README.md index 53717d50..6de657f5 100644 --- a/scenario_execution_moveit/README.md +++ b/scenario_execution_moveit/README.md @@ -1,4 +1,4 @@ # Scenario Execution MOVEIT -The `scenario_execution_moveit` package provides actions to control robotic manipulator using moveit library. +The `scenario_execution_moveit` package provides actions to control robotic manipulator using [Moveit](https://moveit.picknik.ai/main/index.html) library. diff --git a/scenario_execution_moveit/package.xml b/scenario_execution_moveit/package.xml index cc80edf7..af041292 100644 --- a/scenario_execution_moveit/package.xml +++ b/scenario_execution_moveit/package.xml @@ -14,7 +14,7 @@ py_trees py_trees_ros visualization_msgs - nav2_simple_commander + pymoveit2 xacro rcl_interfaces scenario_status diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py index 93fe35e5..5b8039da 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py @@ -74,9 +74,11 @@ def update(self) -> py_trees.common.Status: else: if self.gripper == 'open': self.gripper_interface.open() + self.feedback_message = f"Setting gripper to {self.gripper} state." # pylint: disable= attribute-defined-outside-init else: self.gripper_interface.close() self.logger.info(f"Setting gripper to {self.gripper} state...") + self.feedback_message = f"Setting gripper to {self.gripper} state." # pylint: disable= attribute-defined-outside-init self.execute = True result = py_trees.common.Status.RUNNING else: diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py index 03a5ddd0..d81c496e 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py @@ -72,6 +72,7 @@ def update(self) -> py_trees.common.Status: result = py_trees.common.Status.RUNNING else: self.logger.info("No motion in progress. Initiating move to joint pose...") + self.feedback_message = f"Moving to joint pose {self.joint_pose}." # pylint: disable= attribute-defined-outside-init self.moveit2.move_to_configuration(self.joint_pose) result = py_trees.common.Status.RUNNING self.execute = True From 8fc11b31e77fc9d4bd4c3e2746a435e631d4fd88 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 24 Jun 2024 12:53:35 +0200 Subject: [PATCH 28/62] add world file --- .../arm_sim_scenario/__init__.py | 15 - .../launch/ignition_launch.py | 7 +- .../gazebo/arm_sim_scenario/worlds/maze.sdf | 1066 +++++++++++++++++ 3 files changed, 1071 insertions(+), 17 deletions(-) delete mode 100644 simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py create mode 100644 simulation/gazebo/arm_sim_scenario/worlds/maze.sdf diff --git a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py b/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py deleted file mode 100644 index 3ba13780..00000000 --- a/simulation/gazebo/arm_sim_scenario/arm_sim_scenario/__init__.py +++ /dev/null @@ -1,15 +0,0 @@ -# Copyright (C) 2024 Intel Corporation -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, -# software distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions -# and limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index f3d31154..23789bfa 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -49,12 +49,15 @@ choices=('true', 'false'), description='launches the joint_state_publisher GUI.', ), + DeclareLaunchArgument('world', default_value='maze', + description='Ignition World'), ] def generate_launch_description(): - + pkg_arm_sim_scenario = get_package_share_directory( + 'arm_sim_scenario') robot_model = LaunchConfiguration('robot_model') robot_name = LaunchConfiguration('robot_name') use_rviz = LaunchConfiguration('use_rviz') @@ -67,7 +70,7 @@ def generate_launch_description(): get_package_share_directory('arm_sim_scenario'))} ignition_gazebo = ExecuteProcess( - cmd=['ign', 'gazebo', 'empty.sdf', '-r', '-v', '4'], + cmd=['ign', 'gazebo', [PathJoinSubstitution([pkg_arm_sim_scenario, 'worlds', LaunchConfiguration('world')]), '.sdf'], '-r', '-v', '4'], output='screen', additional_env=env, on_exit=Shutdown(), diff --git a/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf b/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf new file mode 100644 index 00000000..274a8620 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf @@ -0,0 +1,1066 @@ + + + + + + 0.003 + 1 + 1000 + + + + + + + ogre + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.90000000000000002 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0 0 0 0 -0 0 + + + + 0 0 1 0 0 0 + true + + -10.005 0 0 0 0 0 + true + + + + 0.01 20 2 + + + + + + + 0.01 20 2 + + + + 0.1 0.1 0.1 1 + 0 0.01 0.05 1 + 0 0.01 0.05 1 + + + + + 10.005 0 0 0 0 0 + true + + + + 0.01 20 2 + + + + + + + 0.01 20 2 + + + + 0.1 0.1 0.1 1 + 0 0.01 0.05 1 + 0 0.01 0.05 1 + + + + + 0 10.005 0 0 0 0 + true + + + + 20 0.01 2 + + + + + + + 20 0.01 2 + + + + 0.1 0.1 0.1 1 + 0 0.01 0.05 1 + 0 0.01 0.05 1 + + + + + 0 -10.005 0 0 0 0 + true + + + + 20 0.01 2 + + + + 0.1 0.1 0.1 1 + 0 0.01 0.05 1 + 0 0.01 0.05 1 + + + + + + 20 0.01 2 + + + + 0.1 0.1 0.1 1 + 0 0.01 0.05 1 + 0 0.01 0.05 1 + + + + + + + 0 0 0.5 0 0 0 + true + + -6.5 -9 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -8.5 -6.5 0 0 0 0 + true + + + + 3 1 1 + + + + + + + 3 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -3.5 -7 0 0 0 0 + true + + + + 1 6 1 + + + + + + + 1 6 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 2.5 -9 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 7 -8.5 0 0 0 0 + true + + + + 2 1 1 + + + + + + + 2 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -9 -0.5 0 0 0 0 + true + + + + 2 1 1 + + + + + + + 2 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -8.5 -2.5 0 0 0 0 + true + + + + 1 3 1 + + + + + + + 1 3 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -4.5 -1.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -6 1 0 0 0 0 + true + + + + 2 2 1 + + + + + + + 2 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -7.5 4.5 0 0 0 0 + true + + + + 5 1 1 + + + + + + + 5 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -3 7.5 0 0 0 0 + true + + + + 4 1 1 + + + + + + + 4 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + 0 5.5 0 0 0 0 + true + + + + 2 9 1 + + + + + + + 2 9 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 0 -2.5 0 0 0 0 + true + + + + 2 1 1 + + + + + + + 2 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -0.5 -1.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -0.5 -4.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 0.5 -5.5 0 0 0 0 + true + + + + 3 1 1 + + + + + + + 3 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 2.5 -4.5 0 0 0 0 + true + + + + 3 1 1 + + + + + + + 3 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 4.5 -5.5 0 0 0 0 + true + + + + 3 1 1 + + + + + + + 3 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 6.5 -4.5 0 0 0 0 + true + + + + 3 1 1 + + + + + + + 3 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 8.5 -5.5 0 0 0 0 + true + + + + 3 1 1 + + + + + + + 3 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 9.5 -4.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 3.5 -0.5 0 0 0 0 + true + + + + 1 3 1 + + + + + + + 1 3 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 7 0.5 0 0 0 0 + true + + + + 6 1 1 + + + + + + + 6 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 7.5 2 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 1.5 5.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 2.5 6 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 3.5 7.5 0 0 0 0 + true + + + + 1 3 1 + + + + + + + 1 3 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 4.5 8.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 5.5 9 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 6.5 9.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 4.5 3 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 5.5 4 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 6.5 5 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 7.5 6 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 8.5 7 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 9.5 8 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + \ No newline at end of file From 7a525a89ebe50fa2f20596f72bb551bd8585a3bb Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 24 Jun 2024 14:29:30 +0200 Subject: [PATCH 29/62] add singleton --- .../scenarios/example_moveit_simulation.osc | 34 ++++------ .../actions/move_gripper.py | 5 +- .../actions/move_to_joint_pose.py | 5 +- .../actions/move_to_pose.py | 5 +- .../moveit_common.py | 62 +++++++++++++++++++ .../gazebo/arm_sim_scenario/CMakeLists.txt | 4 +- 6 files changed, 83 insertions(+), 32 deletions(-) create mode 100644 scenario_execution_moveit/scenario_execution_moveit/moveit_common.py diff --git a/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc index a2f3c80e..89b3bf07 100644 --- a/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc +++ b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc @@ -11,41 +11,29 @@ scenario example_moveit: keep(it.move_group_arm == 'interbotix_arm') keep(it.move_group_gripper == 'interbotix_gripper') keep(it.open_gripper_position == '[-0.037, +0.037]') - keep(it.close_gripper_position == '[-0.030, +0.030]') + keep(it.close_gripper_position == '[-0.015, +0.015]') box: osc_actor - box2: osc_actor do parallel: serial: - wx200.move_gripper() with: - keep(it.gripper == 'open') - wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.0, 0.576332, -0.0523599, 0.418879, 0.0]') - wx200.move_gripper() with: - keep(it.gripper == 'close') - wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.0, -0.8377, -0.0523599, 0.418879, 0.0]') + box.spawn(spawn_pose: pose_3d( + position: position_3d(x: 0.25m, y: 0.0m, z: 0.02m), + orientation: orientation_3d(yaw: 0.0rad)), + model: 'example_moveit_simulation://models/box.sdf',world_name: 'empty') + serial: wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.5235, 0.5, -0.0523599, 0.418879, 0.0]') + keep(it.joint_pose == '[0.0, 0.122173, 0.296706, 1.0472, 0.0]') wx200.move_gripper() with: keep(it.gripper == 'open') wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.5235, 0.1, -0.0523599, 0.418879, 0.0]') - wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[1.22, 0.1, -0.0523599, 0.418879, 0.0]') - wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[1.22, 0.576332, -0.0523599, 0.418879, 0.0]') + keep(it.joint_pose == '[0.0, 0.296706, 0.296706, 1.22173, 0.0]') wx200.move_gripper() with: keep(it.gripper == 'close') + wx200.move_to_pose(pose_3d(position: position_3d(x: 0.1m, y: 0.0m, z: 0.4m))) wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[1.22, 0.1, -0.0523599, 0.418879, 0.0]') - wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.0, 0.1, -0.0523599, 0.418879, 0.0]') - wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.0, 0.436332, -0.0523599, 0.418879, 0.0]') + keep(it.joint_pose == '[0.5235, 0.0, 0.0, 0.0, 0.0]') wx200.move_gripper() with: keep(it.gripper == 'open') - wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.0, -1.40, 1.55, 0.8, 0.0]') + wx200.move_to_pose(pose_3d(position: position_3d(x: 0.1m, y: 0.0m, z: 0.4m))) wait elapsed(3s) emit end time_out: serial: diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py index 5b8039da..c8e16370 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py @@ -17,7 +17,8 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.logging import get_logger -from pymoveit2 import MoveIt2State, GripperInterface +from pymoveit2 import MoveIt2State +from scenario_execution_moveit.moveit_common import Gripper import py_trees import ast @@ -52,7 +53,7 @@ def setup(self, **kwargs): self.logger = get_logger(self.name) # Create MoveIt 2 interface - self.gripper_interface = GripperInterface( + self.gripper_interface = Gripper( node=self.node, gripper_joint_names=self.gripper_joint_names, open_gripper_joint_positions=self.open_gripper_position, diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py index d81c496e..dd7f42d0 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py @@ -18,7 +18,8 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.logging import get_logger -from pymoveit2 import MoveIt2, MoveIt2State +from pymoveit2 import MoveIt2State +from scenario_execution_moveit.moveit_common import MoveIt2Interface import py_trees import ast @@ -48,7 +49,7 @@ def setup(self, **kwargs): raise KeyError(error_message) from e self.logger = get_logger(self.name) # Create MoveIt 2 interface - self.moveit2 = MoveIt2( + self.moveit2 = MoveIt2Interface( node=self.node, joint_names=self.joint_names, base_link_name=self.namespace + '/' + self.base_link_name, diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py index 392b9578..c5aee355 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py @@ -17,7 +17,8 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.logging import get_logger -from pymoveit2 import MoveIt2, MoveIt2State +from pymoveit2 import MoveIt2State +from scenario_execution_moveit.moveit_common import MoveIt2Interface import py_trees from time import sleep @@ -56,7 +57,7 @@ def setup(self, **kwargs): self.logger = get_logger(self.name) # Create MoveIt 2 interface - self.moveit2 = MoveIt2( + self.moveit2 = MoveIt2Interface( node=self.node, joint_names=self.joint_names, base_link_name=self.namespace + '/' + self.base_link_name, diff --git a/scenario_execution_moveit/scenario_execution_moveit/moveit_common.py b/scenario_execution_moveit/scenario_execution_moveit/moveit_common.py new file mode 100644 index 00000000..4049488f --- /dev/null +++ b/scenario_execution_moveit/scenario_execution_moveit/moveit_common.py @@ -0,0 +1,62 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from pymoveit2 import MoveIt2, GripperInterface + +class _MoveIt2InterfaceSingleton: + _instance = None + + def __init__(self, node, joint_names, base_link_name, end_effector_name, group_name, callback_group) -> None: + self.moveit2 = MoveIt2( + node=node, + joint_names=joint_names, + base_link_name=base_link_name, + end_effector_name=end_effector_name, + group_name=group_name, + callback_group=callback_group + ) + + def __getattr__(self, name): + return getattr(self.moveit2, name) + +class _GripperSingleton: + _instance = None + + def __init__(self, node, gripper_joint_names, open_gripper_joint_positions, closed_gripper_joint_positions, gripper_group_name, callback_group, gripper_command_action_name) -> None: + self.gripper_interface = GripperInterface( + node=node, + gripper_joint_names=gripper_joint_names, + open_gripper_joint_positions=open_gripper_joint_positions, + closed_gripper_joint_positions=closed_gripper_joint_positions, + gripper_group_name=gripper_group_name, + callback_group=callback_group, + gripper_command_action_name=gripper_command_action_name + ) + + def __getattr__(self, name): + return getattr(self.gripper_interface, name) + + +def MoveIt2Interface(node, joint_names, base_link_name, end_effector_name, group_name, callback_group): + if _MoveIt2InterfaceSingleton._instance is None: + _MoveIt2InterfaceSingleton._instance = _MoveIt2InterfaceSingleton(node, joint_names, base_link_name, end_effector_name, group_name, callback_group) + return _MoveIt2InterfaceSingleton._instance + + +def Gripper(node, gripper_joint_names, open_gripper_joint_positions, closed_gripper_joint_positions, gripper_group_name, callback_group, gripper_command_action_name): + if _GripperSingleton._instance is None: + _GripperSingleton._instance = _GripperSingleton(node, gripper_joint_names, open_gripper_joint_positions, closed_gripper_joint_positions, gripper_group_name, callback_group, gripper_command_action_name) + return _GripperSingleton._instance diff --git a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt index c7797401..41393d8d 100644 --- a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt +++ b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt @@ -8,10 +8,8 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -ament_python_install_package(arm_sim_scenario) - install( - DIRECTORY config launch urdf config rviz meshes + DIRECTORY config launch urdf config rviz meshes worlds DESTINATION share/${PROJECT_NAME} ) From df126c46aa39617c1aa069437cbe4430c49a04bb Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 24 Jun 2024 14:36:55 +0200 Subject: [PATCH 30/62] format --- .../moveit_common.py | 26 +++++++++++-------- .../launch/ignition_launch.py | 3 ++- 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/scenario_execution_moveit/scenario_execution_moveit/moveit_common.py b/scenario_execution_moveit/scenario_execution_moveit/moveit_common.py index 4049488f..6b05a4f5 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/moveit_common.py +++ b/scenario_execution_moveit/scenario_execution_moveit/moveit_common.py @@ -16,6 +16,7 @@ from pymoveit2 import MoveIt2, GripperInterface + class _MoveIt2InterfaceSingleton: _instance = None @@ -32,11 +33,12 @@ def __init__(self, node, joint_names, base_link_name, end_effector_name, group_n def __getattr__(self, name): return getattr(self.moveit2, name) + class _GripperSingleton: _instance = None def __init__(self, node, gripper_joint_names, open_gripper_joint_positions, closed_gripper_joint_positions, gripper_group_name, callback_group, gripper_command_action_name) -> None: - self.gripper_interface = GripperInterface( + self.gripper_interface = GripperInterface( node=node, gripper_joint_names=gripper_joint_names, open_gripper_joint_positions=open_gripper_joint_positions, @@ -45,18 +47,20 @@ def __init__(self, node, gripper_joint_names, open_gripper_joint_positions, clos callback_group=callback_group, gripper_command_action_name=gripper_command_action_name ) - + def __getattr__(self, name): return getattr(self.gripper_interface, name) - -def MoveIt2Interface(node, joint_names, base_link_name, end_effector_name, group_name, callback_group): - if _MoveIt2InterfaceSingleton._instance is None: - _MoveIt2InterfaceSingleton._instance = _MoveIt2InterfaceSingleton(node, joint_names, base_link_name, end_effector_name, group_name, callback_group) - return _MoveIt2InterfaceSingleton._instance + +def MoveIt2Interface(node, joint_names, base_link_name, end_effector_name, group_name, callback_group): # pylint: disable=C0103 + if _MoveIt2InterfaceSingleton._instance is None: # pylint: disable=protected-access + _MoveIt2InterfaceSingleton._instance = _MoveIt2InterfaceSingleton( # pylint: disable=protected-access + node, joint_names, base_link_name, end_effector_name, group_name, callback_group) + return _MoveIt2InterfaceSingleton._instance # pylint: disable=protected-access -def Gripper(node, gripper_joint_names, open_gripper_joint_positions, closed_gripper_joint_positions, gripper_group_name, callback_group, gripper_command_action_name): - if _GripperSingleton._instance is None: - _GripperSingleton._instance = _GripperSingleton(node, gripper_joint_names, open_gripper_joint_positions, closed_gripper_joint_positions, gripper_group_name, callback_group, gripper_command_action_name) - return _GripperSingleton._instance +def Gripper(node, gripper_joint_names, open_gripper_joint_positions, closed_gripper_joint_positions, gripper_group_name, callback_group, gripper_command_action_name): # pylint: disable=C0103 + if _GripperSingleton._instance is None: # pylint: disable=protected-access + _GripperSingleton._instance = _GripperSingleton(node, gripper_joint_names, open_gripper_joint_positions, # pylint: disable=protected-access + closed_gripper_joint_positions, gripper_group_name, callback_group, gripper_command_action_name) + return _GripperSingleton._instance # pylint: disable=protected-access diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index 23789bfa..7f12b7dd 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -70,7 +70,8 @@ def generate_launch_description(): get_package_share_directory('arm_sim_scenario'))} ignition_gazebo = ExecuteProcess( - cmd=['ign', 'gazebo', [PathJoinSubstitution([pkg_arm_sim_scenario, 'worlds', LaunchConfiguration('world')]), '.sdf'], '-r', '-v', '4'], + cmd=['ign', 'gazebo', [PathJoinSubstitution( + [pkg_arm_sim_scenario, 'worlds', LaunchConfiguration('world')]), '.sdf'], '-r', '-v', '4'], output='screen', additional_env=env, on_exit=Shutdown(), From d9bb54d361a7e34e8bb2ce018efc065da8e0e62c Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 24 Jun 2024 17:04:13 +0200 Subject: [PATCH 31/62] seperate launch files --- .../launch/ignition_arm_launch.py | 164 ++++++++++++++++++ .../launch/ignition_launch.py | 135 +------------- 2 files changed, 165 insertions(+), 134 deletions(-) create mode 100644 simulation/gazebo/arm_sim_scenario/launch/ignition_arm_launch.py diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_arm_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_arm_launch.py new file mode 100644 index 00000000..133ef637 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_arm_launch.py @@ -0,0 +1,164 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +ARGUMENTS = [ + DeclareLaunchArgument('robot_model', default_value='wx200', + choices=['wx200'], + description='model type of the Interbotix Arm'), + DeclareLaunchArgument('robot_name', default_value='wx200', + description='Robot name'), + DeclareLaunchArgument('use_rviz', default_value='false', + choices=['true', 'false'], + description='launches RViz if set to `true`.'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('headless', default_value='False', + description='Whether to execute simulation gui'), + DeclareLaunchArgument('hardware_type', + default_value='ignition', + choices=['actual', 'fake', 'gz_classic', 'ignition'], + description='configure robot_description to use actual, fake, or simulated hardware', + ), + DeclareLaunchArgument('use_joint_pub_gui', + default_value='false', + choices=('true', 'false'), + description='launches the joint_state_publisher GUI.', + ), + +] + + +def generate_launch_description(): + robot_model = LaunchConfiguration('robot_model') + robot_name = LaunchConfiguration('robot_name') + use_rviz = LaunchConfiguration('use_rviz') + use_sim_time = LaunchConfiguration('use_sim_time') + hardware_type = LaunchConfiguration('hardware_type') + use_joint_pub_gui = LaunchConfiguration('use_joint_pub_gui') + + spawn_robot_node = Node( + package='ros_gz_sim', + executable='create', + arguments=['-name', 'spawn_wx200', + '-x', '0.0', + '-y', '0.0', + '-z', '0.0', + '-Y', '0.0', + '-topic', 'wx200/robot_description'], + output='screen' + ) + + arm_description_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('arm_sim_scenario'), + 'launch', + 'arm_description_launch.py' + ]) + ]), + launch_arguments={ + 'robot_model': robot_model, + 'robot_name': robot_name, + 'use_rviz': use_rviz, + 'use_sim_time': use_sim_time, + 'hardware_type': hardware_type, + 'use_joint_pub_gui': use_joint_pub_gui + }.items(), + ) + + spawn_joint_state_broadcaster_node = Node( + name='joint_state_broadcaster_spawner', + package='controller_manager', + executable='spawner', + namespace=robot_name, + arguments=[ + '-c', + 'controller_manager', + 'joint_state_broadcaster', + ], + parameters=[{ + 'use_sim_time': use_sim_time, + }], + ) + + spawn_arm_controller_node = Node( + name='arm_controller_spawner', + package='controller_manager', + executable='spawner', + namespace=robot_name, + arguments=[ + '-c', + 'controller_manager', + 'arm_controller', + ], + parameters=[{ + 'use_sim_time': use_sim_time, + }] + ) + + spawn_gripper_controller_node = Node( + name='gripper_controller_spawner', + package='controller_manager', + executable='spawner', + namespace=robot_name, + arguments=[ + '-c', + 'controller_manager', + 'gripper_controller', + ], + parameters=[{ + 'use_sim_time': use_sim_time, + }] + ) + + load_joint_state_broadcaster_event = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_robot_node, + on_exit=[spawn_joint_state_broadcaster_node] + ) + ) + + load_arm_controller_event = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_joint_state_broadcaster_node, + on_exit=[spawn_arm_controller_node] + ) + ) + + load_gripper_controller_event = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_joint_state_broadcaster_node, + on_exit=[spawn_gripper_controller_node] + ) + ) + + ld = LaunchDescription(ARGUMENTS) + ld.add_action(spawn_robot_node) + ld.add_action(load_joint_state_broadcaster_event) + ld.add_action(load_arm_controller_event) + ld.add_action(load_gripper_controller_event) + ld.add_action(arm_description_launch) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index 7f12b7dd..ec19b955 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -17,38 +17,12 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, IncludeLaunchDescription, Shutdown +from launch.actions import DeclareLaunchArgument, ExecuteProcess, Shutdown from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare ARGUMENTS = [ - DeclareLaunchArgument('robot_model', default_value='wx200', - choices=['wx200'], - description='model type of the Interbotix Arm'), - DeclareLaunchArgument('robot_name', default_value='wx200', - description='Robot name'), - DeclareLaunchArgument('use_rviz', default_value='false', - choices=['true', 'false'], - description='launches RViz if set to `true`.'), - DeclareLaunchArgument('use_sim_time', default_value='true', - choices=['true', 'false'], - description='use_sim_time'), - DeclareLaunchArgument('headless', default_value='False', - description='Whether to execute simulation gui'), - DeclareLaunchArgument('hardware_type', - default_value='ignition', - choices=['actual', 'fake', 'gz_classic', 'ignition'], - description='configure robot_description to use actual, fake, or simulated hardware', - ), - DeclareLaunchArgument('use_joint_pub_gui', - default_value='false', - choices=('true', 'false'), - description='launches the joint_state_publisher GUI.', - ), DeclareLaunchArgument('world', default_value='maze', description='Ignition World'), @@ -58,12 +32,6 @@ def generate_launch_description(): pkg_arm_sim_scenario = get_package_share_directory( 'arm_sim_scenario') - robot_model = LaunchConfiguration('robot_model') - robot_name = LaunchConfiguration('robot_name') - use_rviz = LaunchConfiguration('use_rviz') - use_sim_time = LaunchConfiguration('use_sim_time') - hardware_type = LaunchConfiguration('hardware_type') - use_joint_pub_gui = LaunchConfiguration('use_joint_pub_gui') env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname( @@ -81,18 +49,6 @@ def generate_launch_description(): emulate_tty=True ) - spawn_robot_node = Node( - package='ros_gz_sim', - executable='create', - arguments=['-name', 'spawn_wx200', - '-x', '0.0', - '-y', '0.0', - '-z', '0.0', - '-Y', '0.0', - '-topic', 'wx200/robot_description'], - output='screen' - ) - clock_bridge = Node( package='ros_gz_bridge', executable='parameter_bridge', @@ -100,96 +56,7 @@ def generate_launch_description(): output='screen' ) - arm_description_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('arm_sim_scenario'), - 'launch', - 'arm_description_launch.py' - ]) - ]), - launch_arguments={ - 'robot_model': robot_model, - 'robot_name': robot_name, - 'use_rviz': use_rviz, - 'use_sim_time': use_sim_time, - 'hardware_type': hardware_type, - 'use_joint_pub_gui': use_joint_pub_gui - }.items(), - ) - - spawn_joint_state_broadcaster_node = Node( - name='joint_state_broadcaster_spawner', - package='controller_manager', - executable='spawner', - namespace=robot_name, - arguments=[ - '-c', - 'controller_manager', - 'joint_state_broadcaster', - ], - parameters=[{ - 'use_sim_time': use_sim_time, - }], - ) - - spawn_arm_controller_node = Node( - name='arm_controller_spawner', - package='controller_manager', - executable='spawner', - namespace=robot_name, - arguments=[ - '-c', - 'controller_manager', - 'arm_controller', - ], - parameters=[{ - 'use_sim_time': use_sim_time, - }] - ) - - spawn_gripper_controller_node = Node( - name='gripper_controller_spawner', - package='controller_manager', - executable='spawner', - namespace=robot_name, - arguments=[ - '-c', - 'controller_manager', - 'gripper_controller', - ], - parameters=[{ - 'use_sim_time': use_sim_time, - }] - ) - - load_joint_state_broadcaster_event = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=spawn_robot_node, - on_exit=[spawn_joint_state_broadcaster_node] - ) - ) - - load_arm_controller_event = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=spawn_joint_state_broadcaster_node, - on_exit=[spawn_arm_controller_node] - ) - ) - - load_gripper_controller_event = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=spawn_joint_state_broadcaster_node, - on_exit=[spawn_gripper_controller_node] - ) - ) - ld = LaunchDescription(ARGUMENTS) ld.add_action(ignition_gazebo) - ld.add_action(spawn_robot_node) - ld.add_action(load_joint_state_broadcaster_event) - ld.add_action(load_arm_controller_event) - ld.add_action(load_gripper_controller_event) - ld.add_action(arm_description_launch) ld.add_action(clock_bridge) return ld From 7d57d93c290d3245f0a277c61f896932a2e566ec Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 25 Jun 2024 17:30:58 +0200 Subject: [PATCH 32/62] add robot launch --- .../arm_sim_scenario/launch/sim_moveit_scenario_launch.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py index 2e6614c0..48c3aaa3 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -40,6 +40,10 @@ def generate_launch_description(): PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'ignition_launch.py'])]), ) + robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'ignition_arm_launch.py'])]), + ) + moveit_bringup = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'moveit_launch.py'])]), ) @@ -57,6 +61,7 @@ def generate_launch_description(): arg_scenario_execution ]) ld.add_action(ignition) + ld.add_action(robot) ld.add_action(moveit_bringup) ld.add_action(scenario_exec) return ld From b77d26832964bf0ad2b7074cc7c770d4088ba84e Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 3 Jul 2024 16:05:00 +0200 Subject: [PATCH 33/62] fix dependency --- simulation/gazebo/arm_sim_scenario/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml index 5f2205f7..c0458404 100644 --- a/simulation/gazebo/arm_sim_scenario/package.xml +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -15,6 +15,7 @@ gazebo_ros gazebo_ros2_control joint_trajectory_controller + moveit ament_lint_auto ament_lint_common From 4686e5d519fef239d07284a0edde56ade9814e3c Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 3 Jul 2024 18:07:05 +0200 Subject: [PATCH 34/62] update actions --- .../scenarios/example_moveit_simulation.osc | 18 ++++++------------ .../actions/move_gripper.py | 7 ++++--- .../actions/move_to_joint_pose.py | 7 ++++--- .../actions/move_to_pose.py | 7 ++++--- 4 files changed, 18 insertions(+), 21 deletions(-) diff --git a/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc index 89b3bf07..6ac43088 100644 --- a/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc +++ b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc @@ -20,19 +20,13 @@ scenario example_moveit: orientation: orientation_3d(yaw: 0.0rad)), model: 'example_moveit_simulation://models/box.sdf',world_name: 'empty') serial: - wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.0, 0.122173, 0.296706, 1.0472, 0.0]') - wx200.move_gripper() with: - keep(it.gripper == 'open') - wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.0, 0.296706, 0.296706, 1.22173, 0.0]') - wx200.move_gripper() with: - keep(it.gripper == 'close') + wx200.move_to_joint_pose(joint_pose: '[0.0, 0.122173, 0.296706, 1.0472, 0.0]') + wx200.move_gripper(gripper:'open') + wx200.move_to_joint_pose(joint_pose: '[0.0, 0.296706, 0.296706, 1.22173, 0.0]') + wx200.move_gripper(gripper: 'close') wx200.move_to_pose(pose_3d(position: position_3d(x: 0.1m, y: 0.0m, z: 0.4m))) - wx200.move_to_joint_pose() with: - keep(it.joint_pose == '[0.5235, 0.0, 0.0, 0.0, 0.0]') - wx200.move_gripper() with: - keep(it.gripper == 'open') + wx200.move_to_joint_pose(joint_pose: '[0.5235, 0.0, 0.0, 0.0, 0.0]') + wx200.move_gripper(gripper: 'open') wx200.move_to_pose(pose_3d(position: position_3d(x: 0.1m, y: 0.0m, z: 0.4m))) wait elapsed(3s) emit end diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py index c8e16370..423aadb9 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py @@ -19,14 +19,15 @@ from rclpy.logging import get_logger from pymoveit2 import MoveIt2State from scenario_execution_moveit.moveit_common import Gripper +from scenario_execution.actions.base_action import BaseAction import py_trees import ast -class MoveGripper(py_trees.behaviour.Behaviour): +class MoveGripper(BaseAction): - def __init__(self, name: str, associated_actor, gripper: str): - super().__init__(name) + def __init__(self, associated_actor, gripper: str): + super().__init__() self.namespace = associated_actor["namespace"] self.joint_names = associated_actor["joint_names"] self.base_link_name = associated_actor["base_link_name"] diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py index dd7f42d0..ea605948 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py @@ -20,14 +20,15 @@ from rclpy.logging import get_logger from pymoveit2 import MoveIt2State from scenario_execution_moveit.moveit_common import MoveIt2Interface +from scenario_execution.actions.base_action import BaseAction import py_trees import ast -class MoveToJointPose(py_trees.behaviour.Behaviour): +class MoveToJointPose(BaseAction): - def __init__(self, name: str, associated_actor, joint_pose: str): - super().__init__(name) + def __init__(self,associated_actor, joint_pose: str): + super().__init__() self.namespace = associated_actor["namespace"] self.joint_names = associated_actor["joint_names"] self.base_link_name = associated_actor["base_link_name"] diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py index c5aee355..4987eaf0 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py @@ -19,14 +19,15 @@ from rclpy.logging import get_logger from pymoveit2 import MoveIt2State from scenario_execution_moveit.moveit_common import MoveIt2Interface +from scenario_execution.actions.base_action import BaseAction import py_trees from time import sleep -class MoveToPose(py_trees.behaviour.Behaviour): +class MoveToPose(BaseAction): - def __init__(self, name: str, associated_actor, goal_pose: list): - super().__init__(name) + def __init__(self, associated_actor, goal_pose: list): + super().__init__() self.namespace = associated_actor["namespace"] self.joint_names = associated_actor["joint_names"] self.base_link_name = associated_actor["base_link_name"] From 2f83bc172280ea0b3d6455ac3015ac33c8b59467 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 9 Jul 2024 15:35:08 +0200 Subject: [PATCH 35/62] fix actions --- .../actions/move_gripper.py | 75 ++++++++--- .../actions/move_to_joint_pose.py | 120 +++++++++++++---- .../actions/move_to_pose.py | 122 ++++++++++++++---- 3 files changed, 249 insertions(+), 68 deletions(-) diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py index 423aadb9..fbbbef54 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py @@ -14,12 +14,14 @@ # # SPDX-License-Identifier: Apache-2.0 +import time from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.logging import get_logger from pymoveit2 import MoveIt2State from scenario_execution_moveit.moveit_common import Gripper from scenario_execution.actions.base_action import BaseAction +from moveit_msgs.srv import GetMotionPlan import py_trees import ast @@ -43,6 +45,12 @@ def __init__(self, associated_actor, gripper: str): self.gripper_interface = None self.current_state = None self.logger = None + self.service_available = False + self.state = None + # timeout + self.service_start_time = None + self.execution_start_time = None + self.timeout_duration = 10 # Timeout duration in seconds def setup(self, **kwargs): try: @@ -67,30 +75,61 @@ def setup(self, **kwargs): if self.gripper not in ['open', 'close']: raise ValueError("Invalid gripper state speficied. Allowed values are 'open' or 'close'.") - def update(self) -> py_trees.common.Status: + self.client = self.node.create_client(GetMotionPlan, 'plan_kinematic_path') + + def update(self) -> py_trees.common.Status: # pylint: disable=R0911 self.current_state = self.gripper_interface.query_state() - if not self.execute: - if self.current_state == MoveIt2State.EXECUTING: - self.logger.info("Another motion is in progress. Waiting for the current motion to complete...") - result = py_trees.common.Status.RUNNING + result = py_trees.common.Status.RUNNING + self.wait_for_service_plan_kinematic_path() + # Handle service availability + if not self.service_available: + if self.service_start_time is None: + self.service_start_time = time.time() + if self.check_timeout(self.service_start_time): + self.feedback_message = f"Timeout waiting for MoveIt to become active." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + self.feedback_message = "Waiting for MoveIt to become active..." # pylint: disable= attribute-defined-outside-init + return result + # Handle executing state + if self.current_state == MoveIt2State.EXECUTING: + if not self.execute: + self.feedback_message = "Another motion is in progress. Waiting for current motion to complete..." # pylint: disable= attribute-defined-outside-init + return result else: + self.feedback_message = f"Motion gripper {self.gripper} in progress..." # pylint: disable= attribute-defined-outside-init + return result + # Handle idle state + if self.current_state == MoveIt2State.IDLE: + if not self.execute: + self.logger.info(f"No motion in progress. Initiating gripper to {self.gripper} state...") if self.gripper == 'open': self.gripper_interface.open() - self.feedback_message = f"Setting gripper to {self.gripper} state." # pylint: disable= attribute-defined-outside-init else: self.gripper_interface.close() - self.logger.info(f"Setting gripper to {self.gripper} state...") - self.feedback_message = f"Setting gripper to {self.gripper} state." # pylint: disable= attribute-defined-outside-init + self.feedback_message = f"Moving gripper to {self.gripper} state." # pylint: disable= attribute-defined-outside-init self.execute = True - result = py_trees.common.Status.RUNNING - else: - if self.current_state == MoveIt2State.IDLE: - self.logger.info(f"Gripper state {self.gripper} set successfully.") - result = py_trees.common.Status.SUCCESS - elif self.current_state == MoveIt2State.EXECUTING: - self.logger.info("Motion in progress...") - result = py_trees.common.Status.RUNNING + return result else: - self.logger.info("Unknown state encountered while executing motion. Requesting again...") - result = py_trees.common.Status.RUNNING + self.logger.info(f"Gripper state {self.gripper} set successfully.") + self.feedback_message = f"Gripper state {self.gripper} set successfully." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.SUCCESS + if self.execution_start_time is None: + self.execution_start_time = time.time() + if self.check_timeout(self.execution_start_time): + self.feedback_message = f"Timeout waiting for current state." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + self.feedback_message = "Waiting for a valid current state..." # pylint: disable= attribute-defined-outside-init return result + + def wait_for_service_plan_kinematic_path(self): + if self.client.wait_for_service(1.0): + self.service_available = True + else: + self.service_available = False + + def check_timeout(self, start_time): + if start_time is None: + return False + if time.time() - start_time > self.timeout_duration: + return True + return False diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py index ea605948..a7709a09 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py @@ -14,20 +14,22 @@ # # SPDX-License-Identifier: Apache-2.0 -from time import sleep +import time from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.logging import get_logger from pymoveit2 import MoveIt2State from scenario_execution_moveit.moveit_common import MoveIt2Interface from scenario_execution.actions.base_action import BaseAction +from moveit_msgs.srv import GetMotionPlan +from moveit_msgs.msg import MoveItErrorCodes import py_trees import ast class MoveToJointPose(BaseAction): - def __init__(self,associated_actor, joint_pose: str): + def __init__(self, associated_actor, joint_pose: str): super().__init__() self.namespace = associated_actor["namespace"] self.joint_names = associated_actor["joint_names"] @@ -40,6 +42,15 @@ def __init__(self,associated_actor, joint_pose: str): self.moveit2 = None self.current_state = None self.logger = None + self.service_available = False + self.state = None + self.last_error_code = None + # timeout + self.service_start_time = None + self.execution_start_time = None + self.retry_start_time = None + self.timeout_duration = 10 # Timeout duration in seconds + self.future_called = False def setup(self, **kwargs): try: @@ -49,6 +60,7 @@ def setup(self, **kwargs): self.name, self.__class__.__name__) raise KeyError(error_message) from e self.logger = get_logger(self.name) + # Create MoveIt 2 interface self.moveit2 = MoveIt2Interface( node=self.node, @@ -65,36 +77,94 @@ def setup(self, **kwargs): self.moveit2.max_velocity = 0.5 self.moveit2.max_acceleration = 0.5 - def update(self) -> py_trees.common.Status: + self.client = self.node.create_client(GetMotionPlan, 'plan_kinematic_path') + + def update(self) -> py_trees.common.Status: # pylint: disable=R0911 self.current_state = self.moveit2.query_state() + self.last_error_code = self.moveit2.get_last_execution_error_code() result = py_trees.common.Status.RUNNING - if not self.execute: - if self.current_state == MoveIt2State.EXECUTING: - self.logger.info("Another motion is in progress. Waiting for current motion to complete...") - result = py_trees.common.Status.RUNNING + self.wait_for_service_plan_kinematic_path() + # Handle service availability + if not self.service_available: + if self.service_start_time is None: + self.service_start_time = time.time() + if self.check_timeout(self.service_start_time): + self.feedback_message = f"Timeout waiting for MoveIt to become active." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + self.feedback_message = "Waiting for MoveIt to become active..." # pylint: disable= attribute-defined-outside-init + return result + # Handle executing state + if self.current_state == MoveIt2State.EXECUTING: + if not self.execute: + self.feedback_message = "Another motion is in progress. Waiting for current motion to complete..." # pylint: disable= attribute-defined-outside-init + return result else: + if not self.future_called: + future = self.moveit2.get_execution_future() + future.add_done_callback(self.future_done_callback) + self.future_called = True + return result + else: + self.feedback_message = "Motion to joint pose in progress..." # pylint: disable= attribute-defined-outside-init + return result + # Handle idle state + if self.current_state == MoveIt2State.IDLE: + if not self.execute: self.logger.info("No motion in progress. Initiating move to joint pose...") self.feedback_message = f"Moving to joint pose {self.joint_pose}." # pylint: disable= attribute-defined-outside-init self.moveit2.move_to_configuration(self.joint_pose) - result = py_trees.common.Status.RUNNING self.execute = True - elif self.current_state == MoveIt2State.EXECUTING: - future = self.moveit2.get_execution_future() - if future: - while not future.done(): - self.logger.info("Motion to joint pose in progress...") - sleep(1) - if str(future.result().status) == '4': + self.retry_start_time = None # Reset execution timeout for new action + return result + if self.state: + if self.state.status == 4: self.logger.info("Motion to joint pose successful.") - result = py_trees.common.Status.SUCCESS + self.feedback_message = "Motion to joint pose successful." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.SUCCESS else: - self.logger.info(f"{str(future.result().result.error_code)}") - result = py_trees.common.Status.FAILURE - else: - self.logger.info("Waiting for response from arm...") - result = py_trees.common.Status.RUNNING - elif self.current_state == MoveIt2State.IDLE: - self.logger.info("Postion not reachable or arm is already at the specified position!") - result = py_trees.common.Status.FAILURE - + self.logger.info(f"Motion failed with error code: {str(self.state.result.error_code)}") + return py_trees.common.Status.FAILURE + if self.last_error_code: + if self.last_error_code.val == 1: + self.logger.info("Arm is already at the specified joint pose.") + self.feedback_message = "Motion to joint pose successful." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.SUCCESS + else: + error_info = self.log_moveit_error(self.last_error_code) + self.logger.info(f"{error_info}. Retrying...") + if self.retry_start_time is None: + self.retry_start_time = time.time() + if self.check_timeout(self.retry_start_time): + self.feedback_message = f"Timeout retrying to move to joint pose." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + return result + if self.execution_start_time is None: + self.execution_start_time = time.time() + if self.check_timeout(self.execution_start_time): + self.feedback_message = f"Timeout waiting for current state." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + self.feedback_message = "Waiting for a valid current state..." # pylint: disable= attribute-defined-outside-init return result + + def wait_for_service_plan_kinematic_path(self): + if self.client.wait_for_service(1.0): + self.service_available = True + else: + self.service_available = False + + def future_done_callback(self, future): + if not future.result(): + return + self.state = future.result() + + def check_timeout(self, start_time): + if start_time is None: + return False + if time.time() - start_time > self.timeout_duration: + return True + return False + + def log_moveit_error(self, error_code): + error_code_mapping = {v: k for k, v in MoveItErrorCodes.__dict__.items() if isinstance(v, int)} + description = error_code_mapping.get(error_code.val, "UNKNOWN_ERROR_CODE") + return description diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py index 4987eaf0..5aaada67 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py @@ -14,14 +14,16 @@ # # SPDX-License-Identifier: Apache-2.0 +import time from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.logging import get_logger from pymoveit2 import MoveIt2State from scenario_execution_moveit.moveit_common import MoveIt2Interface from scenario_execution.actions.base_action import BaseAction +from moveit_msgs.srv import GetMotionPlan +from moveit_msgs.msg import MoveItErrorCodes import py_trees -from time import sleep class MoveToPose(BaseAction): @@ -46,6 +48,16 @@ def __init__(self, associated_actor, goal_pose: list): self.moveit2 = None self.current_state = None self.logger = None + self.service_available = False + self.future = None + self.state = None + self.last_error_code = None + # timeout + self.service_start_time = None + self.execution_start_time = None + self.retry_start_time = None + self.timeout_duration = 10 # Timeout duration in seconds + self.future_called = False def setup(self, **kwargs): try: @@ -75,38 +87,98 @@ def setup(self, **kwargs): self.moveit2.cartesian_avoid_collisions = self.cartesian_avoid_collisions self.moveit2.cartesian_jump_threshold = self.cartesian_jump_threshold - def update(self) -> py_trees.common.Status: + self.client = self.node.create_client(GetMotionPlan, 'plan_kinematic_path') + + def update(self) -> py_trees.common.Status: # pylint: disable=R0911 self.current_state = self.moveit2.query_state() + self.last_error_code = self.moveit2.get_last_execution_error_code() result = py_trees.common.Status.RUNNING - if not self.execute: - if self.current_state == MoveIt2State.EXECUTING: - self.logger.info("Another motion is in progress. Waiting for current motion to complete...") - result = py_trees.common.Status.RUNNING + self.wait_for_service_plan_kinematic_path() + # Handle service availability + if not self.service_available: + if self.service_start_time is None: + self.service_start_time = time.time() + if self.check_timeout(self.service_start_time): + self.feedback_message = f"Timeout waiting for MoveIt to become active." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + self.feedback_message = "Waiting for MoveIt to become active..." # pylint: disable= attribute-defined-outside-init + return result + # Handle executing state + if self.current_state == MoveIt2State.EXECUTING: + if not self.execute: + self.feedback_message = "Another motion is in progress. Waiting for current motion to complete..." # pylint: disable= attribute-defined-outside-init + return result else: - self.logger.info("No motion in progress. Initiating move to joint pose...") + if not self.future_called: + future = self.moveit2.get_execution_future() + future.add_done_callback(self.future_done_callback) + self.future_called = True + return result + else: + self.feedback_message = "Motion to pose in progress..." # pylint: disable= attribute-defined-outside-init + return result + # Handle idle state + if self.current_state == MoveIt2State.IDLE: + if not self.execute: + self.logger.info("No motion in progress. Initiating move to pose...") + self.feedback_message = f"Moving to pose." # pylint: disable= attribute-defined-outside-init self.move_to_pose() - result = py_trees.common.Status.RUNNING self.execute = True - elif self.current_state == MoveIt2State.EXECUTING: - future = self.moveit2.get_execution_future() - if future: - while not future.done(): - self.logger.info("Motion to goal pose in progress...") - sleep(1) - if str(future.result().status) == '4': - self.logger.info("Motion to goal pose successful.") - result = py_trees.common.Status.SUCCESS + self.retry_start_time = None # Reset execution timeout for new action + return result + if self.state: + if self.state.status == 4: + self.logger.info("Motion to pose successful.") + self.feedback_message = "Motion to pose successful." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.SUCCESS else: - self.logger.info(f"{str(future.result().result.error_code)}") - result = py_trees.common.Status.FAILURE - else: - self.logger.info("Waiting for response from arm...") - result = py_trees.common.Status.RUNNING - elif self.current_state == MoveIt2State.IDLE: - self.logger.info("pose not reachable or arm is already at the specified goal pose!") - result = py_trees.common.Status.FAILURE + self.logger.info(f"Motion failed with error code: {str(self.state.result.error_code)}") + return py_trees.common.Status.FAILURE + if self.last_error_code: + if self.last_error_code.val == 1: + self.logger.info("Arm is already at the specified pose.") + self.feedback_message = "Motion to pose successful." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.SUCCESS + else: + error_info = self.log_moveit_error(self.last_error_code) + self.logger.info(f"{error_info}. Retrying...") + if self.retry_start_time is None: + self.retry_start_time = time.time() + if self.check_timeout(self.retry_start_time): + self.feedback_message = f"Timeout retrying to move to pose." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + return result + if self.execution_start_time is None: + self.execution_start_time = time.time() + if self.check_timeout(self.execution_start_time): + self.feedback_message = f"Timeout waiting for current state." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + self.feedback_message = "Waiting for a valid current state..." # pylint: disable= attribute-defined-outside-init return result + def wait_for_service_plan_kinematic_path(self): + if self.client.wait_for_service(1.0): + self.service_available = True + else: + self.service_available = False + + def future_done_callback(self, future): + if not future.result(): + return + self.state = future.result() + + def check_timeout(self, start_time): + if start_time is None: + return False + if time.time() - start_time > self.timeout_duration: + return True + return False + + def log_moveit_error(self, error_code): + error_code_mapping = {v: k for k, v in MoveItErrorCodes.__dict__.items() if isinstance(v, int)} + description = error_code_mapping.get(error_code.val, "UNKNOWN_ERROR_CODE") + return description + def move_to_pose(self): self.moveit2.move_to_pose( position=self.position, From 811758c0d336d28888fa3917c8bdb508534a8383 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 9 Jul 2024 15:42:58 +0200 Subject: [PATCH 36/62] rmeove merge files --- .gitmodules.orig | 12 -- .../lib_osc/ros.osc.orig | 175 ------------------ 2 files changed, 187 deletions(-) delete mode 100644 .gitmodules.orig delete mode 100644 scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc.orig diff --git a/.gitmodules.orig b/.gitmodules.orig deleted file mode 100644 index 1a0d6ffa..00000000 --- a/.gitmodules.orig +++ /dev/null @@ -1,12 +0,0 @@ -[submodule "dependencies/py_trees_ros"] - path = dependencies/py_trees_ros - url = https://github.com/splintered-reality/py_trees_ros.git -<<<<<<< HEAD -[submodule "dependencies/py_trees"] - path = dependencies/py_trees - url = https://github.com/splintered-reality/py_trees.git -[submodule "dependencies/pymoveit2"] - path = dependencies/pymoveit2 - url = https://github.com/AndrejOrsula/pymoveit2.git -======= ->>>>>>> main diff --git a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc.orig b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc.orig deleted file mode 100644 index 2409e833..00000000 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc.orig +++ /dev/null @@ -1,175 +0,0 @@ -import osc.robotics - -enum comparison_operator: [ - lt, - le, - eq, - ne, - ge, - gt -] - -enum qos_preset_profiles: [ - parameters, - parameter_events, - sensor_data, - services_default, - system_default -] - -enum lifecycle_state: [ - unconfigured, - inactive, - active, - finalized -] - -actor differential_drive_robot inherits robot: - namespace: string = '' - -action action_call: - # Call a ros action and wait for the result - action_name: string # name of the action to connect to - action_type: string # class of the message type - data: string # call content - -action assert_lifecycle_state: - # Checks for the state of a lifecycle-managed node. - node_name: string # Name of lifecycle-managed node - state_sequence: list of lifecycle_state # list of allowed lifecycle states to follow - allow_initial_skip: bool = false # if true, allows skipping of states within the state_sequence without reporting failure - fail_on_unexpected: bool = true # if true and an unexpected transition or final state occurs, the action fails. Otherwise it succeed. - keep_running: bool = true # if true, the action keeps running while the last state in the state_sequence remains - -action assert_tf_moving: - # Checks that a tf frame_id keeps moving in respect to a parent_frame_id. If there is no movement within timeout the action ends, - # depending on fail_on_finish, either with success or failure. Speeds below threshold_translation and threshold_rotation - # are discarded. By default the action waits for the first transform to get available before starting the timeout timer. - # This can be changed by setting wait_for_first_transform to false. If the tf topics are not available on /tf and /tf_static - # you can specify a namespace by setting tf_topic_namespace. - frame_id: string # frame_id to check for movement - parent_frame_id: string = "map" # parent frame_id against which the movement is checked - timeout: time # timeout without movement - threshold_translation: speed = 0.01mps # translation speed, below this threshold is skipped - threshold_rotation: angular_rate = 0.01radps # rotational speed, below this threshold is skipped - fail_on_finish: bool = true # if false, the action should success if no movement - wait_for_first_transform: bool = true # start measuring with the first received message - tf_topic_namespace: string = '' # if set, it's used as namespace - use_sim_time: bool = false # in simulation, we need to look up the transform at a different time as the scenario execution node is not allowed to use the sim time - -action assert_topic_latency: - # Check the latency of the specified topic (in system time). If the check with comparison_operator gets true, the action ends, depending on fail_on_finish, either with success or failure. - topic_name: string # topic name to wait for message - latency: time # the time to compare against - comparison_operator: comparison_operator = comparison_operator!le # the comparison is done using the python operator module - fail_on_finish: bool = true # if false, the action should succeed if comparison is true - rolling_average_count: int = 1 # the check is done aganist the rolling average over x elements - wait_for_first_message: bool = true # start measuring with the first received message - topic_type: string # class of message type, only required when wait_for_first_message is set to false (e.g. std_msgs.msg.String) - -action check_data: - # Compare received topic messages using the given comparison_operator, against the specified value. Either the whole message gets compared or a member defined by member_name. - topic_name: string # name of the topic to connect to - topic_type: string # class of the message type (e.g. std_msgs.msg.String) - qos_profile: qos_preset_profiles = qos_preset_profiles!system_default # qos profile for the subscriber - member_name: string = "" # name of the member to check, if empty the whole message is checked - expected_value: string # expected value of the variable - comparison_operator: comparison_operator = comparison_operator!eq # one from the python `operator module`_ - fail_if_no_data: bool = false # py_trees.common.Status.FAILURE instead of py_trees.common.Status.RUNNING if there is no data on action execution - fail_if_bad_comparison: bool = false # py_trees.common.Status.FAILURE instead of py_trees.common.Status.RUNNING if comparison failed - wait_for_first_message: bool = true # start checking with the first received message after action execution. If false, the check is executed on the last received message - -action differential_drive_robot.init_nav2: - # Initialize nav2 - initial_pose: pose_3d # the initial pose to set during initialization - base_frame_id: string = 'base_link' # the base frame id - use_initial_pose: bool = true # if false, no initial_pose is needed (useful when using slam instead of amcl for localization) - namespace_override: string = '' # if set, it's used as namespace (instead of the associated actor's name) - wait_for_initial_pose: bool = false # if true the initial pose needs to be set externally (e.g. manually through rviz) - -action differential_drive_robot.nav_through_poses: - # Use nav2 to navigate through poses. - goal_poses: list of pose_3d # goal poses to navigate through - namespace_override: string = '' # if set, it's used as namespace (instead of the associated actor's name) - monitor_progress: bool = true # if true, the action returns after the goal is reached or on failure. If no, the action returns after request. - -action differential_drive_robot.nav_to_pose: - # Nav2 to navigate to goal pose. - goal_pose: pose_3d # goal pose to navigate to - namespace_override: string = '' # if set, it's used as namespace (instead of the associated actor's name) - action_topic: string = 'navigate_to_pose' # Name of action - monitor_progress: bool = true # if yes, the action returns after the goal is reached or on failure. If no, the action returns after request. - -action differential_drive_robot.odometry_distance_traveled: - # Wait until a defined distance was traveled, based on odometry - namespace_override: string = '' # if set, it's used as namespace (instead of the associated actor's name) - distance: length # traveled distance at which the action succeeds. - -action differential_drive_robot.tf_close_to: - # Wait until a TF frame is close to a defined reference point - namespace_override: string = '' # if set, it's used as namespace (instead of the associated actor's name) - reference_point: position_3d # z is not considered - threshold: length # distance at which the action succeeds - sim: bool = false # in simulation, we need to look up the transform map --> base_link at a different time as the scenario execution node is not allowed to use the sim time - robot_frame_id: string = 'base_link' # defines the TF frame id of the robot - -action log_check: - # Check the ROS log for specific output - module_name: string = '' # If specified, a matching message must also match the module name - values: list of string # string to check for. If found, action succeeds - -action record_bag: - # Record a dataset, stored in output_dir defined by command-line parameter (default: '.') - topics: list of topics # Topics to records, if empty all topics are recorded - timestamp_suffix: bool = true # Add a timestamp suffix to output directory name - hidden_topics: bool = false # whether to record hidden topics - storage: string = '' # storage type to use (empty string: use default) - -action ros_launch: - # Execute a ros launch file - package_name: string # package that contains the launch file - launch_file: string # launch file name - arguments: list of key_value # ros arguments (get forwarded as key:=value pairs) - wait_for_shutdown: bool = true # if true, the action waits until the execution has finished - shutdown_timeout: time = 10s # (only used if wait_for_shutdown is false) time to wait between SIGINT and SIGKILL getting sent, if process is still running on scenario shutdown - -action service_call: - # Call a ROS service and wait for the reply. - service_name: string # name of the service to connect to - service_type: string # class of the message type (e.g. std_srvs.msg.Empty) - data: string # call content - -action set_node_parameter: - # Set a parameter of a node. - node_name: string # name of the node - parameter_name: string # name of the parameter - parameter_value: string # new value of the parameter - -action topic_monitor: - # subscribe to a topic and store the last message within a variable. - topic_name: string # name of the topic to connect to - topic_type: string # class of the message type (e.g. std_msgs.msg.String) - target_variable: string # name of the variable (e.g. a 'var' within an actor instance) - qos_profile: qos_preset_profiles = qos_preset_profiles!system_default # qos profile for the subscriber - -action topic_publish: - # publish a message on a topic - topic_name: string # name of the topic to connect to - topic_type: string # class of the message type (e.g. std_msgs.msg.String) - value: string # value to publish (can either be a string that gets parsed, a struct or a message object stored within a variable, e.g. stored by topic_monitor()) - qos_profile: qos_preset_profiles = qos_preset_profiles!system_default # qos profile for the subscriber - -action wait_for_data: - # Wait for any message on a ROS topic. - topic_name: string # name of the topic to connect to - topic_type: string # class of the message type (e.g. std_msgs.msg.String) - qos_profile: qos_preset_profiles = qos_preset_profiles!system_default # qos profile for the subscriber - -action wait_for_topics: -<<<<<<< HEAD - # wait for topics to get available - topics: list of topics -======= - # Wait for topics to get available (i.e. publisher gets available). - topics: list of topics # list of topics to wait for ->>>>>>> main From 27f05dd65ab2c221c79c2dc2e81dc06b2da03e8a Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 9 Jul 2024 16:43:07 +0200 Subject: [PATCH 37/62] fix submodule update --- dependencies/py_trees | 1 - 1 file changed, 1 deletion(-) delete mode 160000 dependencies/py_trees diff --git a/dependencies/py_trees b/dependencies/py_trees deleted file mode 160000 index 8d8fc19d..00000000 --- a/dependencies/py_trees +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8d8fc19d5f6c81bc2107039a7f0baa763a01bb79 From 7efd3b86f257442406d1814267dfefd4b7005324 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 9 Jul 2024 17:20:26 +0200 Subject: [PATCH 38/62] increase timeout --- .../scenario_execution_moveit/actions/move_gripper.py | 2 +- .../scenario_execution_moveit/actions/move_to_joint_pose.py | 2 +- .../scenario_execution_moveit/actions/move_to_pose.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py index fbbbef54..2fb9fb5f 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py @@ -50,7 +50,7 @@ def __init__(self, associated_actor, gripper: str): # timeout self.service_start_time = None self.execution_start_time = None - self.timeout_duration = 10 # Timeout duration in seconds + self.timeout_duration = 30 # Timeout duration in seconds def setup(self, **kwargs): try: diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py index a7709a09..885d7215 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py @@ -49,7 +49,7 @@ def __init__(self, associated_actor, joint_pose: str): self.service_start_time = None self.execution_start_time = None self.retry_start_time = None - self.timeout_duration = 10 # Timeout duration in seconds + self.timeout_duration = 30 # Timeout duration in seconds self.future_called = False def setup(self, **kwargs): diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py index 5aaada67..7fd195df 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py @@ -56,7 +56,7 @@ def __init__(self, associated_actor, goal_pose: list): self.service_start_time = None self.execution_start_time = None self.retry_start_time = None - self.timeout_duration = 10 # Timeout duration in seconds + self.timeout_duration = 30 # Timeout duration in seconds self.future_called = False def setup(self, **kwargs): From f5f81932bbb07ef931d476148846224fdbbcc422 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 9 Jul 2024 17:49:47 +0200 Subject: [PATCH 39/62] fix retrying --- .../scenario_execution_moveit/actions/move_to_joint_pose.py | 1 + .../scenario_execution_moveit/actions/move_to_pose.py | 1 + 2 files changed, 2 insertions(+) diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py index 885d7215..ebb24ba4 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py @@ -132,6 +132,7 @@ def update(self) -> py_trees.common.Status: # pylint: disable=R0911 else: error_info = self.log_moveit_error(self.last_error_code) self.logger.info(f"{error_info}. Retrying...") + self.execute = False if self.retry_start_time is None: self.retry_start_time = time.time() if self.check_timeout(self.retry_start_time): diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py index 7fd195df..c1592921 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py +++ b/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py @@ -142,6 +142,7 @@ def update(self) -> py_trees.common.Status: # pylint: disable=R0911 else: error_info = self.log_moveit_error(self.last_error_code) self.logger.info(f"{error_info}. Retrying...") + self.execute = False if self.retry_start_time is None: self.retry_start_time = time.time() if self.check_timeout(self.retry_start_time): From 7c61c55f963c850a1983e70333d71e514a4995e2 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 15 Jul 2024 17:06:16 +0200 Subject: [PATCH 40/62] test target workflow --- .github/workflows/target_workflow.yml | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 .github/workflows/target_workflow.yml diff --git a/.github/workflows/target_workflow.yml b/.github/workflows/target_workflow.yml new file mode 100644 index 00000000..0aad20d9 --- /dev/null +++ b/.github/workflows/target_workflow.yml @@ -0,0 +1,25 @@ +name: Trigger Target Workflow +on: + push: + branches: [moveit-humble] +jobs: + trigger: + runs-on: ubuntu-latest + + steps: + - name: Trigger Workflow in Another Repository + run: | + # Set the required variables + repo_owner="Intel-Innersource" + repo_name="applications.robotics.safety.manipulation-failure-mitigation" + event_type="trigger-workflow" + service="moveit-humble" + version="v0.0.1" + + curl -L \ + -X POST \ + -H "Accept: application/vnd.github+json" \ + -H "Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}" \ + -H "X-GitHub-Api-Version: 2022-11-28" \ + https://api.github.com/repos/$repo_owner/$repo_name/dispatches \ + -d "{\"event_type\": \"$event_type\", \"client_payload\": {\"service\": \"$service\", \"version\": \"$version\", \"unit\": false, \"integration\": true}}" \ No newline at end of file From c3c6af666ed26b927fcefb399598d11dd5f53cce Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 15 Jul 2024 17:10:05 +0200 Subject: [PATCH 41/62] fix permission --- .github/workflows/target_workflow.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/target_workflow.yml b/.github/workflows/target_workflow.yml index 0aad20d9..4e60edb5 100644 --- a/.github/workflows/target_workflow.yml +++ b/.github/workflows/target_workflow.yml @@ -2,6 +2,7 @@ name: Trigger Target Workflow on: push: branches: [moveit-humble] +permissions: read-all jobs: trigger: runs-on: ubuntu-latest From a489d133d74bf248b0fc43ddafd81832b86fa83d Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 15 Jul 2024 17:14:40 +0200 Subject: [PATCH 42/62] fix branch --- .github/workflows/target_workflow.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/target_workflow.yml b/.github/workflows/target_workflow.yml index 4e60edb5..68c4cd01 100644 --- a/.github/workflows/target_workflow.yml +++ b/.github/workflows/target_workflow.yml @@ -1,7 +1,7 @@ name: Trigger Target Workflow on: push: - branches: [moveit-humble] + branches: [main] permissions: read-all jobs: trigger: From b5f6da9b1f3745417f832788f63c1e8238ddfe91 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 15 Jul 2024 17:17:11 +0200 Subject: [PATCH 43/62] fix file --- .github/workflows/image.yml | 21 ++++++++++++++++++++- .github/workflows/target_workflow.yml | 26 -------------------------- 2 files changed, 20 insertions(+), 27 deletions(-) delete mode 100644 .github/workflows/target_workflow.yml diff --git a/.github/workflows/image.yml b/.github/workflows/image.yml index 67f5a326..a57aab40 100644 --- a/.github/workflows/image.yml +++ b/.github/workflows/image.yml @@ -39,4 +39,23 @@ jobs: context: . file: .github/workflows/Dockerfile push: true - tags: ghcr.io/intellabs/scenario-execution:humble \ No newline at end of file + tags: ghcr.io/intellabs/scenario-execution:humble + trigger: + runs-on: intellabs-01 + steps: + - name: Trigger Workflow in Another Repository + run: | + # Set the required variables + repo_owner="Intel-Innersource" + repo_name="applications.robotics.safety.manipulation-failure-mitigation" + event_type="trigger-workflow" + service="moveit-humble" + version="v0.0.1" + + curl -L \ + -X POST \ + -H "Accept: application/vnd.github+json" \ + -H "Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}" \ + -H "X-GitHub-Api-Version: 2022-11-28" \ + https://api.github.com/repos/$repo_owner/$repo_name/dispatches \ + -d "{\"event_type\": \"$event_type\", \"client_payload\": {\"service\": \"$service\", \"version\": \"$version\", \"unit\": false, \"integration\": true}}" \ No newline at end of file diff --git a/.github/workflows/target_workflow.yml b/.github/workflows/target_workflow.yml deleted file mode 100644 index 68c4cd01..00000000 --- a/.github/workflows/target_workflow.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Trigger Target Workflow -on: - push: - branches: [main] -permissions: read-all -jobs: - trigger: - runs-on: ubuntu-latest - - steps: - - name: Trigger Workflow in Another Repository - run: | - # Set the required variables - repo_owner="Intel-Innersource" - repo_name="applications.robotics.safety.manipulation-failure-mitigation" - event_type="trigger-workflow" - service="moveit-humble" - version="v0.0.1" - - curl -L \ - -X POST \ - -H "Accept: application/vnd.github+json" \ - -H "Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}" \ - -H "X-GitHub-Api-Version: 2022-11-28" \ - https://api.github.com/repos/$repo_owner/$repo_name/dispatches \ - -d "{\"event_type\": \"$event_type\", \"client_payload\": {\"service\": \"$service\", \"version\": \"$version\", \"unit\": false, \"integration\": true}}" \ No newline at end of file From 69852e3430d0eee11a272495f89e2de052d8c551 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 15 Jul 2024 17:18:50 +0200 Subject: [PATCH 44/62] fix syntax --- .github/workflows/image.yml | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/.github/workflows/image.yml b/.github/workflows/image.yml index a57aab40..fe26f4ef 100644 --- a/.github/workflows/image.yml +++ b/.github/workflows/image.yml @@ -41,21 +41,21 @@ jobs: push: true tags: ghcr.io/intellabs/scenario-execution:humble trigger: - runs-on: intellabs-01 - steps: - - name: Trigger Workflow in Another Repository - run: | - # Set the required variables - repo_owner="Intel-Innersource" - repo_name="applications.robotics.safety.manipulation-failure-mitigation" - event_type="trigger-workflow" - service="moveit-humble" - version="v0.0.1" + runs-on: intellabs-01 + steps: + - name: Trigger Workflow in Another Repository + run: | + # Set the required variables + repo_owner="Intel-Innersource" + repo_name="applications.robotics.safety.manipulation-failure-mitigation" + event_type="trigger-workflow" + service="moveit-humble" + version="v0.0.1" - curl -L \ - -X POST \ - -H "Accept: application/vnd.github+json" \ - -H "Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}" \ - -H "X-GitHub-Api-Version: 2022-11-28" \ - https://api.github.com/repos/$repo_owner/$repo_name/dispatches \ - -d "{\"event_type\": \"$event_type\", \"client_payload\": {\"service\": \"$service\", \"version\": \"$version\", \"unit\": false, \"integration\": true}}" \ No newline at end of file + curl -L \ + -X POST \ + -H "Accept: application/vnd.github+json" \ + -H "Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}" \ + -H "X-GitHub-Api-Version: 2022-11-28" \ + https://api.github.com/repos/$repo_owner/$repo_name/dispatches \ + -d "{\"event_type\": \"$event_type\", \"client_payload\": {\"service\": \"$service\", \"version\": \"$version\", \"unit\": false, \"integration\": true}}" \ No newline at end of file From e651423816c4289b3c4b3aba69ff4c37968ff563 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 15 Jul 2024 17:36:59 +0200 Subject: [PATCH 45/62] fix url --- .github/workflows/image.yml | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/.github/workflows/image.yml b/.github/workflows/image.yml index fe26f4ef..7485d1e2 100644 --- a/.github/workflows/image.yml +++ b/.github/workflows/image.yml @@ -45,17 +45,10 @@ jobs: steps: - name: Trigger Workflow in Another Repository run: | - # Set the required variables - repo_owner="Intel-Innersource" - repo_name="applications.robotics.safety.manipulation-failure-mitigation" - event_type="trigger-workflow" - service="moveit-humble" - version="v0.0.1" - curl -L \ -X POST \ -H "Accept: application/vnd.github+json" \ -H "Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}" \ -H "X-GitHub-Api-Version: 2022-11-28" \ - https://api.github.com/repos/$repo_owner/$repo_name/dispatches \ - -d "{\"event_type\": \"$event_type\", \"client_payload\": {\"service\": \"$service\", \"version\": \"$version\", \"unit\": false, \"integration\": true}}" \ No newline at end of file + https://api.github.com/repos/Intel-Innersource/applications.robotics.safety.manipulation-failure-mitigation/actions/workflows/target_workflow.yml/dispatches \ + -d '{"ref":"main"}' \ No newline at end of file From 758a0bba18da076e3354d904bc3e7ca81628e801 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 15 Jul 2024 17:46:30 +0200 Subject: [PATCH 46/62] test url] --- .github/workflows/image.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.github/workflows/image.yml b/.github/workflows/image.yml index 7485d1e2..da5c52a6 100644 --- a/.github/workflows/image.yml +++ b/.github/workflows/image.yml @@ -46,9 +46,7 @@ jobs: - name: Trigger Workflow in Another Repository run: | curl -L \ - -X POST \ -H "Accept: application/vnd.github+json" \ -H "Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}" \ -H "X-GitHub-Api-Version: 2022-11-28" \ - https://api.github.com/repos/Intel-Innersource/applications.robotics.safety.manipulation-failure-mitigation/actions/workflows/target_workflow.yml/dispatches \ - -d '{"ref":"main"}' \ No newline at end of file + https://api.github.com/repos/intel-innersource/applications.robotics.safety.manipulation-failure-mitigation/actions/workflows \ No newline at end of file From 48d50f4202277ac85c0d71d66b16f4a84f0fdc66 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 15 Jul 2024 17:52:57 +0200 Subject: [PATCH 47/62] fix syntax --- .github/workflows/image.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/image.yml b/.github/workflows/image.yml index da5c52a6..0a16c4bc 100644 --- a/.github/workflows/image.yml +++ b/.github/workflows/image.yml @@ -49,4 +49,4 @@ jobs: -H "Accept: application/vnd.github+json" \ -H "Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}" \ -H "X-GitHub-Api-Version: 2022-11-28" \ - https://api.github.com/repos/intel-innersource/applications.robotics.safety.manipulation-failure-mitigation/actions/workflows \ No newline at end of file + 'https://api.github.com/repos/intel-innersource/applications.robotics.safety.manipulation-failure-mitigation/actions/workflows' \ No newline at end of file From 4ccb556b7c0bfa90fdc48736f7e28f18e24647cb Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 15 Jul 2024 17:56:53 +0200 Subject: [PATCH 48/62] fix --- .github/workflows/image.yml | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/.github/workflows/image.yml b/.github/workflows/image.yml index 0a16c4bc..0667a299 100644 --- a/.github/workflows/image.yml +++ b/.github/workflows/image.yml @@ -45,8 +45,7 @@ jobs: steps: - name: Trigger Workflow in Another Repository run: | - curl -L \ - -H "Accept: application/vnd.github+json" \ - -H "Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}" \ - -H "X-GitHub-Api-Version: 2022-11-28" \ - 'https://api.github.com/repos/intel-innersource/applications.robotics.safety.manipulation-failure-mitigation/actions/workflows' \ No newline at end of file + curl -X POST 'https://api.github.com/repos/intel-innersource/applications.robotics.safety.manipulation-failure-mitigation/actions/workflows/ci-development.yml/dispatches' \ + -H 'Accept: application/vnd.github.everest-preview+json' \ + -H 'Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}' \ + -d '{"ref":"main"}' \ No newline at end of file From 15effba69cbf397cd66571c4d2b5aa8d02e159e4 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 15 Jul 2024 17:58:59 +0200 Subject: [PATCH 49/62] fix name --- .github/workflows/image.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/image.yml b/.github/workflows/image.yml index 0667a299..c0d0fdf2 100644 --- a/.github/workflows/image.yml +++ b/.github/workflows/image.yml @@ -45,7 +45,7 @@ jobs: steps: - name: Trigger Workflow in Another Repository run: | - curl -X POST 'https://api.github.com/repos/intel-innersource/applications.robotics.safety.manipulation-failure-mitigation/actions/workflows/ci-development.yml/dispatches' \ + curl -X POST 'https://api.github.com/repos/intel-innersource/applications.robotics.safety.manipulation-failure-mitigation/actions/workflows/target_workflow.yml/dispatches' \ -H 'Accept: application/vnd.github.everest-preview+json' \ -H 'Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}' \ -d '{"ref":"main"}' \ No newline at end of file From fe4d5b42f941606aada6ac2a2b6c3be7b1de4515 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Fri, 2 Aug 2024 15:36:44 +0200 Subject: [PATCH 50/62] move to libs --- .../scenario_execution_moveit}/MANIFEST.in | 0 .../scenario_execution_moveit}/README.md | 0 .../scenario_execution_moveit}/package.xml | 9 ++------- .../resource/scenario_execution_moveit | 0 .../scenario_execution_moveit/__init__.py | 4 +--- .../scenario_execution_moveit/actions/__init__.py | 0 .../scenario_execution_moveit/actions/move_gripper.py | 0 .../actions/move_to_joint_pose.py | 0 .../scenario_execution_moveit/actions/move_to_pose.py | 0 .../scenario_execution_moveit/get_osc_library.py | 0 .../scenario_execution_moveit/lib_osc/moveit.osc | 1 + .../scenario_execution_moveit/moveit_common.py | 0 .../scenario_execution_moveit}/setup.cfg | 0 .../scenario_execution_moveit}/setup.py | 0 14 files changed, 4 insertions(+), 10 deletions(-) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/MANIFEST.in (100%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/README.md (100%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/package.xml (73%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/resource/scenario_execution_moveit (100%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/scenario_execution_moveit/__init__.py (91%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/scenario_execution_moveit/actions/__init__.py (100%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/scenario_execution_moveit/actions/move_gripper.py (100%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/scenario_execution_moveit/actions/move_to_joint_pose.py (100%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/scenario_execution_moveit/actions/move_to_pose.py (100%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/scenario_execution_moveit/get_osc_library.py (100%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/scenario_execution_moveit/lib_osc/moveit.osc (95%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/scenario_execution_moveit/moveit_common.py (100%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/setup.cfg (100%) rename {scenario_execution_moveit => libs/scenario_execution_moveit}/setup.py (100%) diff --git a/scenario_execution_moveit/MANIFEST.in b/libs/scenario_execution_moveit/MANIFEST.in similarity index 100% rename from scenario_execution_moveit/MANIFEST.in rename to libs/scenario_execution_moveit/MANIFEST.in diff --git a/scenario_execution_moveit/README.md b/libs/scenario_execution_moveit/README.md similarity index 100% rename from scenario_execution_moveit/README.md rename to libs/scenario_execution_moveit/README.md diff --git a/scenario_execution_moveit/package.xml b/libs/scenario_execution_moveit/package.xml similarity index 73% rename from scenario_execution_moveit/package.xml rename to libs/scenario_execution_moveit/package.xml index af041292..9764c54a 100644 --- a/scenario_execution_moveit/package.xml +++ b/libs/scenario_execution_moveit/package.xml @@ -8,16 +8,11 @@ Intel Labs Apache-2.0 - scenario_execution + scenario_execution_ros rclpy - py_trees - py_trees_ros - visualization_msgs pymoveit2 - xacro - rcl_interfaces - scenario_status + moveit_msgs ament_copyright ament_flake8 diff --git a/scenario_execution_moveit/resource/scenario_execution_moveit b/libs/scenario_execution_moveit/resource/scenario_execution_moveit similarity index 100% rename from scenario_execution_moveit/resource/scenario_execution_moveit rename to libs/scenario_execution_moveit/resource/scenario_execution_moveit diff --git a/scenario_execution_moveit/scenario_execution_moveit/__init__.py b/libs/scenario_execution_moveit/scenario_execution_moveit/__init__.py similarity index 91% rename from scenario_execution_moveit/scenario_execution_moveit/__init__.py rename to libs/scenario_execution_moveit/scenario_execution_moveit/__init__.py index 73e52bde..ec44632b 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/__init__.py +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/__init__.py @@ -14,10 +14,8 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Main entry for scenario execution """ - from . import actions __all__ = [ - 'actions', + 'actions' ] diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py similarity index 100% rename from scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py rename to libs/scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py similarity index 100% rename from scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py rename to libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_gripper.py diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py similarity index 100% rename from scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py rename to libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py diff --git a/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py similarity index 100% rename from scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py rename to libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py diff --git a/scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py b/libs/scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py similarity index 100% rename from scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py rename to libs/scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py diff --git a/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc b/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc similarity index 95% rename from scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc rename to libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc index f85a10c2..37f93aa3 100644 --- a/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc @@ -1,4 +1,5 @@ import osc.robotics +import osc.standard.base actor robotic_arm inherits robot: namespace: string = '' diff --git a/scenario_execution_moveit/scenario_execution_moveit/moveit_common.py b/libs/scenario_execution_moveit/scenario_execution_moveit/moveit_common.py similarity index 100% rename from scenario_execution_moveit/scenario_execution_moveit/moveit_common.py rename to libs/scenario_execution_moveit/scenario_execution_moveit/moveit_common.py diff --git a/scenario_execution_moveit/setup.cfg b/libs/scenario_execution_moveit/setup.cfg similarity index 100% rename from scenario_execution_moveit/setup.cfg rename to libs/scenario_execution_moveit/setup.cfg diff --git a/scenario_execution_moveit/setup.py b/libs/scenario_execution_moveit/setup.py similarity index 100% rename from scenario_execution_moveit/setup.py rename to libs/scenario_execution_moveit/setup.py From b168b34dfb4a000457026fe0907d91e8f6f1e6cf Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 19 Aug 2024 15:17:57 +0200 Subject: [PATCH 51/62] fix move_to_pose --- .../actions/move_to_joint_pose.py | 45 +++++++------------ 1 file changed, 17 insertions(+), 28 deletions(-) diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py index ebb24ba4..4e90b1a7 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py @@ -18,6 +18,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.logging import get_logger +from rclpy.action.client import GoalStatus from pymoveit2 import MoveIt2State from scenario_execution_moveit.moveit_common import MoveIt2Interface from scenario_execution.actions.base_action import BaseAction @@ -43,7 +44,7 @@ def __init__(self, associated_actor, joint_pose: str): self.current_state = None self.logger = None self.service_available = False - self.state = None + self.goal_status = None self.last_error_code = None # timeout self.service_start_time = None @@ -84,61 +85,49 @@ def update(self) -> py_trees.common.Status: # pylint: disable=R0911 self.last_error_code = self.moveit2.get_last_execution_error_code() result = py_trees.common.Status.RUNNING self.wait_for_service_plan_kinematic_path() - # Handle service availability if not self.service_available: if self.service_start_time is None: self.service_start_time = time.time() if self.check_timeout(self.service_start_time): self.feedback_message = f"Timeout waiting for MoveIt to become active." # pylint: disable= attribute-defined-outside-init return py_trees.common.Status.FAILURE + self.logger.info("Waiting for MoveIt to become active...") self.feedback_message = "Waiting for MoveIt to become active..." # pylint: disable= attribute-defined-outside-init return result - # Handle executing state if self.current_state == MoveIt2State.EXECUTING: if not self.execute: self.feedback_message = "Another motion is in progress. Waiting for current motion to complete..." # pylint: disable= attribute-defined-outside-init return result else: + self.feedback_message = "Motion to joint pose in progress..." # pylint: disable= attribute-defined-outside-init if not self.future_called: future = self.moveit2.get_execution_future() future.add_done_callback(self.future_done_callback) self.future_called = True - return result - else: - self.feedback_message = "Motion to joint pose in progress..." # pylint: disable= attribute-defined-outside-init - return result - # Handle idle state + return result if self.current_state == MoveIt2State.IDLE: if not self.execute: self.logger.info("No motion in progress. Initiating move to joint pose...") self.feedback_message = f"Moving to joint pose {self.joint_pose}." # pylint: disable= attribute-defined-outside-init self.moveit2.move_to_configuration(self.joint_pose) self.execute = True - self.retry_start_time = None # Reset execution timeout for new action return result - if self.state: - if self.state.status == 4: + else: + if self.goal_status is None or self.goal_status == GoalStatus.STATUS_UNKNOWN: + self.logger.debug(f"Goal aborted by the controller {self.log_moveit_error(self.last_error_code)}. Retrying...") + self.feedback_message = f"Goal aborted by the controller {self.log_moveit_error(self.last_error_code)}. Retrying..." # pylint: disable= attribute-defined-outside-init + self.execute = False + self.future_called = False + self.goal_status = None + return result + if self.goal_status == GoalStatus.STATUS_SUCCEEDED: self.logger.info("Motion to joint pose successful.") self.feedback_message = "Motion to joint pose successful." # pylint: disable= attribute-defined-outside-init return py_trees.common.Status.SUCCESS else: - self.logger.info(f"Motion failed with error code: {str(self.state.result.error_code)}") + self.logger.info(f"Motion move to joint position failed.") return py_trees.common.Status.FAILURE - if self.last_error_code: - if self.last_error_code.val == 1: - self.logger.info("Arm is already at the specified joint pose.") - self.feedback_message = "Motion to joint pose successful." # pylint: disable= attribute-defined-outside-init - return py_trees.common.Status.SUCCESS - else: - error_info = self.log_moveit_error(self.last_error_code) - self.logger.info(f"{error_info}. Retrying...") - self.execute = False - if self.retry_start_time is None: - self.retry_start_time = time.time() - if self.check_timeout(self.retry_start_time): - self.feedback_message = f"Timeout retrying to move to joint pose." # pylint: disable= attribute-defined-outside-init - return py_trees.common.Status.FAILURE - return result + if self.execution_start_time is None: self.execution_start_time = time.time() if self.check_timeout(self.execution_start_time): @@ -156,7 +145,7 @@ def wait_for_service_plan_kinematic_path(self): def future_done_callback(self, future): if not future.result(): return - self.state = future.result() + self.goal_status = future.result().status def check_timeout(self, start_time): if start_time is None: From a9af826c4186f556f44f9b9e3802d1f72c0af73a Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 19 Aug 2024 17:06:33 +0200 Subject: [PATCH 52/62] fix siulation --- .../scenarios/example_moveit_simulation.osc | 5 +-- .../actions/move_to_joint_pose.py | 2 + .../actions/move_to_pose.py | 42 +++++++------------ .../launch/ignition_launch.py | 40 ++++++++++-------- .../gazebo/arm_sim_scenario/worlds/maze.sdf | 2 +- 5 files changed, 43 insertions(+), 48 deletions(-) diff --git a/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc index 6ac43088..c546640d 100644 --- a/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc +++ b/examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc @@ -17,9 +17,8 @@ scenario example_moveit: serial: box.spawn(spawn_pose: pose_3d( position: position_3d(x: 0.25m, y: 0.0m, z: 0.02m), - orientation: orientation_3d(yaw: 0.0rad)), - model: 'example_moveit_simulation://models/box.sdf',world_name: 'empty') - serial: + orientation: orientation_3d(yaw: 0.0rad)), + model: 'example_moveit_simulation://models/box.sdf') wx200.move_to_joint_pose(joint_pose: '[0.0, 0.122173, 0.296706, 1.0472, 0.0]') wx200.move_gripper(gripper:'open') wx200.move_to_joint_pose(joint_pose: '[0.0, 0.296706, 0.296706, 1.22173, 0.0]') diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py index 4e90b1a7..109270ba 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py @@ -94,6 +94,7 @@ def update(self) -> py_trees.common.Status: # pylint: disable=R0911 self.logger.info("Waiting for MoveIt to become active...") self.feedback_message = "Waiting for MoveIt to become active..." # pylint: disable= attribute-defined-outside-init return result + # Handle executing state if self.current_state == MoveIt2State.EXECUTING: if not self.execute: self.feedback_message = "Another motion is in progress. Waiting for current motion to complete..." # pylint: disable= attribute-defined-outside-init @@ -105,6 +106,7 @@ def update(self) -> py_trees.common.Status: # pylint: disable=R0911 future.add_done_callback(self.future_done_callback) self.future_called = True return result + # Handle idle state if self.current_state == MoveIt2State.IDLE: if not self.execute: self.logger.info("No motion in progress. Initiating move to joint pose...") diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py index c1592921..b495438e 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py @@ -18,6 +18,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.logging import get_logger +from rclpy.action.client import GoalStatus from pymoveit2 import MoveIt2State from scenario_execution_moveit.moveit_common import MoveIt2Interface from scenario_execution.actions.base_action import BaseAction @@ -49,8 +50,7 @@ def __init__(self, associated_actor, goal_pose: list): self.current_state = None self.logger = None self.service_available = False - self.future = None - self.state = None + self.goal_status = None self.last_error_code = None # timeout self.service_start_time = None @@ -84,6 +84,7 @@ def setup(self, **kwargs): # Scale down velocity and acceleration of joints (percentage of maximum) self.moveit2.max_velocity = 0.5 self.moveit2.max_acceleration = 0.5 + self.moveit2.cartesian_avoid_collisions = self.cartesian_avoid_collisions self.moveit2.cartesian_jump_threshold = self.cartesian_jump_threshold @@ -109,14 +110,12 @@ def update(self) -> py_trees.common.Status: # pylint: disable=R0911 self.feedback_message = "Another motion is in progress. Waiting for current motion to complete..." # pylint: disable= attribute-defined-outside-init return result else: + self.feedback_message = "Motion to pose in progress..." # pylint: disable= attribute-defined-outside-init if not self.future_called: future = self.moveit2.get_execution_future() future.add_done_callback(self.future_done_callback) self.future_called = True - return result - else: - self.feedback_message = "Motion to pose in progress..." # pylint: disable= attribute-defined-outside-init - return result + return result # Handle idle state if self.current_state == MoveIt2State.IDLE: if not self.execute: @@ -124,31 +123,22 @@ def update(self) -> py_trees.common.Status: # pylint: disable=R0911 self.feedback_message = f"Moving to pose." # pylint: disable= attribute-defined-outside-init self.move_to_pose() self.execute = True - self.retry_start_time = None # Reset execution timeout for new action return result - if self.state: - if self.state.status == 4: + else: + if self.goal_status is None or self.goal_status == GoalStatus.STATUS_UNKNOWN: + self.logger.debug(f"Goal aborted by the controller {self.log_moveit_error(self.last_error_code)}. Retrying...") + self.feedback_message = f"Goal aborted by the controller {self.log_moveit_error(self.last_error_code)}. Retrying..." # pylint: disable= attribute-defined-outside-init + self.execute = False + self.future_called = False + self.goal_status = None + return result + if self.goal_status == GoalStatus.STATUS_SUCCEEDED: self.logger.info("Motion to pose successful.") self.feedback_message = "Motion to pose successful." # pylint: disable= attribute-defined-outside-init return py_trees.common.Status.SUCCESS else: - self.logger.info(f"Motion failed with error code: {str(self.state.result.error_code)}") + self.logger.info(f"Motion move to pose failed.") return py_trees.common.Status.FAILURE - if self.last_error_code: - if self.last_error_code.val == 1: - self.logger.info("Arm is already at the specified pose.") - self.feedback_message = "Motion to pose successful." # pylint: disable= attribute-defined-outside-init - return py_trees.common.Status.SUCCESS - else: - error_info = self.log_moveit_error(self.last_error_code) - self.logger.info(f"{error_info}. Retrying...") - self.execute = False - if self.retry_start_time is None: - self.retry_start_time = time.time() - if self.check_timeout(self.retry_start_time): - self.feedback_message = f"Timeout retrying to move to pose." # pylint: disable= attribute-defined-outside-init - return py_trees.common.Status.FAILURE - return result if self.execution_start_time is None: self.execution_start_time = time.time() if self.check_timeout(self.execution_start_time): @@ -166,7 +156,7 @@ def wait_for_service_plan_kinematic_path(self): def future_done_callback(self, future): if not future.result(): return - self.state = future.result() + self.goal_status = future.result().status def check_timeout(self, start_time): if start_time is None: diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index ec19b955..56c3768b 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -22,24 +22,27 @@ from launch_ros.actions import Node -ARGUMENTS = [ - DeclareLaunchArgument('world', default_value='maze', - description='Ignition World'), - -] - def generate_launch_description(): + pkg_arm_sim_scenario = get_package_share_directory( 'arm_sim_scenario') - env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], - 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname( - get_package_share_directory('arm_sim_scenario'))} + arguments = [ + + DeclareLaunchArgument('world', default_value=os.path.join(pkg_arm_sim_scenario, 'worlds', 'maze.sdf'), + description='Simulation World File'), + ] + + env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': + ':'.join([os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''), + os.environ.get('LD_LIBRARY_PATH', default='')]), + 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname(pkg_arm_sim_scenario)} + + # Ignition gazebo ignition_gazebo = ExecuteProcess( - cmd=['ign', 'gazebo', [PathJoinSubstitution( - [pkg_arm_sim_scenario, 'worlds', LaunchConfiguration('world')]), '.sdf'], '-r', '-v', '4'], + cmd=['ign', 'gazebo', LaunchConfiguration('world'), '-r', '-v', '4'], output='screen', additional_env=env, on_exit=Shutdown(), @@ -49,14 +52,15 @@ def generate_launch_description(): emulate_tty=True ) - clock_bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], - output='screen' - ) - ld = LaunchDescription(ARGUMENTS) + clock_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock' + ]) + + ld = LaunchDescription(arguments) ld.add_action(ignition_gazebo) ld.add_action(clock_bridge) return ld diff --git a/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf b/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf index 274a8620..cde97194 100644 --- a/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf +++ b/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf @@ -17,7 +17,7 @@ and limitations under the License. SPDX-License-Identifier: Apache-2.0 --> - + 0.003 1 From 87728b0d1260f2bc1d43f8c98d06ba4c8554483e Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 21 Aug 2024 13:28:01 +0200 Subject: [PATCH 53/62] fix packages --- examples/example_moveit_simulation/package.xml | 4 +--- simulation/gazebo/arm_sim_scenario/package.xml | 4 ++++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/examples/example_moveit_simulation/package.xml b/examples/example_moveit_simulation/package.xml index 40ef3ef6..cfa95143 100644 --- a/examples/example_moveit_simulation/package.xml +++ b/examples/example_moveit_simulation/package.xml @@ -8,10 +8,8 @@ Intel Labs Apache-2.0 - scenario_execution - scenario_execution_gazebo + arm_sim_scenario - scenario_execution_moveit rclpy diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml index c0458404..70419ea1 100644 --- a/simulation/gazebo/arm_sim_scenario/package.xml +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -16,6 +16,10 @@ gazebo_ros2_control joint_trajectory_controller moveit + scenario_execution + scenario_execution_ros + scenario_execution_moveit + scenario_execution_gazebo ament_lint_auto ament_lint_common From 32faf9735b18a3179e686476eb6aea631030b177 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 22 Aug 2024 15:17:07 +0200 Subject: [PATCH 54/62] ci --- simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index 56c3768b..a69b8090 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -18,11 +18,10 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess, Shutdown -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node - def generate_launch_description(): pkg_arm_sim_scenario = get_package_share_directory( @@ -34,7 +33,6 @@ def generate_launch_description(): description='Simulation World File'), ] - env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': ':'.join([os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''), os.environ.get('LD_LIBRARY_PATH', default='')]), @@ -52,7 +50,6 @@ def generate_launch_description(): emulate_tty=True ) - clock_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', name='clock_bridge', output='screen', From 7bfcb7e2aa11a4e7c731bdb99219eb23f24e1efe Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Fri, 30 Aug 2024 15:03:31 +0200 Subject: [PATCH 55/62] fix urdf --- .../arm_sim_scenario/urdf/control.urdf.xacro | 46 +++++++++++-------- .../arm_sim_scenario/urdf/wx200.urdf.xacro | 16 ++++++- 2 files changed, 42 insertions(+), 20 deletions(-) diff --git a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro index 446d619c..4fb92062 100644 --- a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro @@ -23,8 +23,8 @@ gazebo_ros2_control/GazeboSystem - - + + ign_ros2_control/IgnitionSystem @@ -36,7 +36,9 @@ "${waist_limit_lower}" "${waist_limit_upper}" - + + ${waist_initial_value} + @@ -44,7 +46,9 @@ "${shoulder_limit_lower}" "${shoulder_limit_upper}" - + + ${shoulder_initial_value} + @@ -52,7 +56,9 @@ "${elbow_limit_lower}" "${elbow_limit_upper}" - + + ${elbow_initial_value} + @@ -61,7 +67,9 @@ "${forearm_roll_limit_lower}" "${forearm_roll_limit_upper}" - + + ${forearm_roll_initial_value} + @@ -70,14 +78,16 @@ "${wrist_angle_limit_lower}" "${wrist_angle_limit_upper}" - + + ${wrist_angle_initial_value} + "${wrist_rotate_limit_lower}" - "${wrist_rotate_limit_upper}" + ${wrist_rotate_limit_upper} @@ -98,7 +108,7 @@ "${-finger_limit_lower}" - 0.0195 + ${finger_initial_value} @@ -110,10 +120,10 @@ "${-finger_limit_lower}" "${finger_limit_lower}" - - -0.0195 - - + + -${finger_initial_value} + + @@ -123,23 +133,20 @@ - /$(arg robot_name) robot_description robot_state_publisher - $(find arm_sim_scenario)/config/trajectory_controllers/${robot_model}_trajectory_controllers.yaml + $(find interbotix_xsarm_sim)/config/trajectory_controllers/${robot_model}_trajectory_controllers.yaml - - + - /$(arg robot_name) @@ -148,9 +155,10 @@ robot_state_publisher $(find arm_sim_scenario)/config/trajectory_controllers/${robot_model}_trajectory_controllers.yaml - + + diff --git a/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro index 4ea14bdf..33b19bda 100644 --- a/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro @@ -16,7 +16,15 @@ - + + + + + + + + + @@ -27,26 +35,31 @@ + + + + + @@ -54,6 +67,7 @@ + From bbfa477ee8cf89cb23d926a5f49d20c4c0b42631 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Fri, 30 Aug 2024 19:44:04 +0200 Subject: [PATCH 56/62] re-structure launch files --- .../config/srdf/wx200.srdf.xacro | 2 +- .../wx200_trajectory_controllers.yaml | 16 +- ...wx200_trajectory_controllers_ignition.yaml | 49 +++++ .../launch/arm_controller_launch.py | 182 ++++++++++++++++++ .../launch/arm_description_launch.py | 30 ++- .../launch/ignition_arm_launch.py | 41 +--- .../arm_sim_scenario/launch/moveit_launch.py | 43 ++--- .../launch/sim_moveit_scenario_launch.py | 65 ++++++- .../arm_sim_scenario/urdf/control.urdf.xacro | 2 +- 9 files changed, 324 insertions(+), 106 deletions(-) create mode 100644 simulation/gazebo/arm_sim_scenario/config/trajectory_controllers/wx200_trajectory_controllers_ignition.yaml create mode 100644 simulation/gazebo/arm_sim_scenario/launch/arm_controller_launch.py diff --git a/simulation/gazebo/arm_sim_scenario/config/srdf/wx200.srdf.xacro b/simulation/gazebo/arm_sim_scenario/config/srdf/wx200.srdf.xacro index 1e2475fa..59b2ec00 100644 --- a/simulation/gazebo/arm_sim_scenario/config/srdf/wx200.srdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/config/srdf/wx200.srdf.xacro @@ -48,7 +48,7 @@ - + diff --git a/simulation/gazebo/arm_sim_scenario/config/trajectory_controllers/wx200_trajectory_controllers.yaml b/simulation/gazebo/arm_sim_scenario/config/trajectory_controllers/wx200_trajectory_controllers.yaml index 02d4716f..8177c5b5 100644 --- a/simulation/gazebo/arm_sim_scenario/config/trajectory_controllers/wx200_trajectory_controllers.yaml +++ b/simulation/gazebo/arm_sim_scenario/config/trajectory_controllers/wx200_trajectory_controllers.yaml @@ -1,13 +1,13 @@ -/wx200: +/$(var robot_name): controller_manager: ros__parameters: - update_rate: 1000 - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster + update_rate: 50 # Hz arm_controller: type: joint_trajectory_controller/JointTrajectoryController gripper_controller: type: joint_trajectory_controller/JointTrajectoryController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster arm_controller: ros__parameters: joints: @@ -16,13 +16,12 @@ - elbow - wrist_angle - wrist_rotate - interface_name: position command_interfaces: - position state_interfaces: - position - state_publish_rate: 50.0 - action_monitor_rate: 20.0 + state_publish_rate: 50.0 # Hz + action_monitor_rate: 20.0 # Hz constraints: stopped_velocity_tolerance: 0.02 goal_time: 1.0 @@ -30,8 +29,6 @@ ros__parameters: joints: - left_finger - - right_finger - interface_name: position command_interfaces: - position state_interfaces: @@ -46,4 +43,3 @@ - wrist_rotate - gripper - left_finger - - right_finger diff --git a/simulation/gazebo/arm_sim_scenario/config/trajectory_controllers/wx200_trajectory_controllers_ignition.yaml b/simulation/gazebo/arm_sim_scenario/config/trajectory_controllers/wx200_trajectory_controllers_ignition.yaml new file mode 100644 index 00000000..02d4716f --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/trajectory_controllers/wx200_trajectory_controllers_ignition.yaml @@ -0,0 +1,49 @@ +/wx200: + controller_manager: + ros__parameters: + update_rate: 1000 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + gripper_controller: + type: joint_trajectory_controller/JointTrajectoryController + arm_controller: + ros__parameters: + joints: + - waist + - shoulder + - elbow + - wrist_angle + - wrist_rotate + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + state_publish_rate: 50.0 + action_monitor_rate: 20.0 + constraints: + stopped_velocity_tolerance: 0.02 + goal_time: 1.0 + gripper_controller: + ros__parameters: + joints: + - left_finger + - right_finger + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + joint_state_broadcaster: + ros__parameters: + joints: + - waist + - shoulder + - elbow + - wrist_angle + - wrist_rotate + - gripper + - left_finger + - right_finger diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_controller_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_controller_launch.py new file mode 100644 index 00000000..3204aaa1 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_controller_launch.py @@ -0,0 +1,182 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch_ros.parameter_descriptions import ParameterFile +from ament_index_python.packages import get_package_share_directory +from launch_ros.descriptions import ParameterValue + +ARGUMENTS = [ + DeclareLaunchArgument('robot_model', default_value='wx200', + choices=['wx200'], + description='model type of the Interbotix Arm'), + DeclareLaunchArgument('robot_name', default_value='wx200', + description='Robot name'), + DeclareLaunchArgument('use_rviz', default_value='false', + choices=['true', 'false'], + description='launches RViz if set to `true`.'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('headless', default_value='False', + description='Whether to execute simulation gui'), + DeclareLaunchArgument('hardware_type', + default_value='fake', + choices=['actual', 'fake', 'gz_classic', 'ignition'], + description='configure robot_description to use actual, fake, or simulated hardware', + ), + DeclareLaunchArgument('use_joint_pub_gui', + default_value='false', + choices=('true', 'false'), + description='launches the joint_state_publisher GUI.', + ), + DeclareLaunchArgument('waist_initial_value', + default_value='0', + description='waist joint initial value.', + ), + DeclareLaunchArgument('shoulder_initial_value', + default_value='0', + description='shoulder joint initial value.', + ), + DeclareLaunchArgument('elbow_initial_value', + default_value='0', + description='elbow joint initial value.', + ), + DeclareLaunchArgument('wrist_angle_initial_value', + default_value='0', + description='wrist_angle joint initial value.', + ), + DeclareLaunchArgument('wrist_rotate_initial_value', + default_value='0', + description='wrist_rotate joint initial value.', + ), + DeclareLaunchArgument('finger_initial_value', + default_value='0', + description='gripper finger initial value.', + ), + +] + + +def generate_launch_description(): + robot_model = LaunchConfiguration('robot_model') + robot_name = LaunchConfiguration('robot_name') + use_sim_time = LaunchConfiguration('use_sim_time') + hardware_type = LaunchConfiguration('hardware_type') + # Intial joint states + waist_initial_value = LaunchConfiguration('waist_initial_value') + shoulder_initial_value = LaunchConfiguration('shoulder_initial_value') + elbow_initial_value = LaunchConfiguration('elbow_initial_value') + wrist_angle_initial_value = LaunchConfiguration('wrist_angle_initial_value') + wrist_rotate_initial_value = LaunchConfiguration('wrist_rotate_initial_value') + finger_initial_value = LaunchConfiguration('finger_initial_value') + + + pkg_arm_sim_scenario = get_package_share_directory('arm_sim_scenario') + + + ros2_control_controllers_config_parameter_file = ParameterFile( + param_file=PathJoinSubstitution([ + FindPackageShare('arm_sim_scenario'), + 'config', + 'trajectory_controllers', + 'wx200_trajectory_controllers.yaml', + ]), + allow_substs=True + ) + + xacro_file = PathJoinSubstitution([pkg_arm_sim_scenario, + 'urdf', + 'wx200.urdf.xacro']) + controller_manager_node = Node( + package='controller_manager', + executable='ros2_control_node', + namespace=robot_name, + parameters=[ + {'robot_description': ParameterValue(Command([ + 'xacro ',xacro_file, ' ', + 'robot_model:=',robot_model,' ', + 'robot_name:=',robot_name,' ', + 'use_world_frame:=', 'true ', + 'waist_initial_value:=',waist_initial_value,' ', + 'shoulder_initial_value:=',shoulder_initial_value,' ', + 'elbow_initial_value:=',elbow_initial_value,' ', + 'wrist_angle_initial_value:=',wrist_angle_initial_value,' ', + 'wrist_rotate_initial_value:=',wrist_rotate_initial_value,' ', + 'finger_initial_value:=',finger_initial_value,' ', + 'hardware_type:=',hardware_type]), value_type=str)}, + ros2_control_controllers_config_parameter_file, + ], + output={'both': 'screen'}, + ) + + spawn_joint_state_broadcaster_node = Node( + name='joint_state_broadcaster_spawner', + package='controller_manager', + executable='spawner', + namespace=robot_name, + arguments=[ + '-c', + 'controller_manager', + 'joint_state_broadcaster', + ], + parameters=[{ + 'use_sim_time': use_sim_time, + }], + ) + + spawn_arm_controller_node = Node( + name='arm_controller_spawner', + package='controller_manager', + executable='spawner', + namespace=robot_name, + arguments=[ + '-c', + 'controller_manager', + 'arm_controller', + ], + parameters=[{ + 'use_sim_time': use_sim_time, + }] + ) + + spawn_gripper_controller_node = Node( + name='gripper_controller_spawner', + package='controller_manager', + executable='spawner', + namespace=robot_name, + arguments=[ + '-c', + 'controller_manager', + 'gripper_controller', + ], + parameters=[{ + 'use_sim_time': use_sim_time, + }] + ) + + ld = LaunchDescription(ARGUMENTS) + ld.add_action(controller_manager_node) + ld.add_action(spawn_joint_state_broadcaster_node) + ld.add_action(spawn_arm_controller_node) + ld.add_action(spawn_gripper_controller_node) + return ld \ No newline at end of file diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py index 388c2f09..38a5afb9 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py @@ -39,7 +39,7 @@ 'xsarm_description.rviz', ]), description='file path to the config file RViz should load.'), - DeclareLaunchArgument('use_sim_time', default_value='false', + DeclareLaunchArgument('use_sim_time', default_value='true', choices=['true', 'false'], description='use_sim_time'), DeclareLaunchArgument('use_joint_pub', @@ -53,12 +53,10 @@ description='launches the joint_state_publisher GUI.', ), DeclareLaunchArgument('hardware_type', - default_value='ignition', + default_value='fake', choices=['actual', 'fake', 'gz_classic', 'ignition'], description='configure robot_description to use actual, fake, or simulated hardware', ) - - ] @@ -80,32 +78,28 @@ def generate_launch_description(): package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', - output='screen', parameters=[ {'use_sim_time': use_sim_time}, {'robot_description': ParameterValue(Command([ - 'xacro', ' ', xacro_file, ' ', - 'robot_model:=', robot_model, ' ', - 'robot_name:=', robot_name, ' ', - 'base_link_frame:=', 'base_link', ' ', - 'use_gripper:=', 'true', ' ', - 'show_ar_tag:=', 'false', ' ', - 'show_gripper_bar:=', 'true', ' ', - 'show_gripper_fingers:=', 'true', ' ', - 'use_world_frame:=', 'true', ' ', - 'hardware_type:=', hardware_type]), value_type=str)}, + 'xacro ',xacro_file, ' ', + 'robot_model:=',robot_model,' ', + 'robot_name:=',robot_name,' ', + 'use_world_frame:=', 'true ' + 'hardware_type:=',hardware_type]), value_type=str)}, ], - namespace=robot_name + namespace=robot_name, + output={'both': 'log'}, ) + joint_state_publisher = Node( condition=IfCondition(use_joint_pub), package='joint_state_publisher', executable='joint_state_publisher', name='joint_state_publisher', - output='screen', parameters=[{'use_sim_time': use_sim_time}], - namespace=robot_name + namespace=robot_name, + output={'both': 'log'}, ) joint_state_publisher_gui = Node( diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_arm_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_arm_launch.py index 133ef637..fb6ff4e2 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_arm_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_arm_launch.py @@ -29,35 +29,17 @@ description='model type of the Interbotix Arm'), DeclareLaunchArgument('robot_name', default_value='wx200', description='Robot name'), - DeclareLaunchArgument('use_rviz', default_value='false', - choices=['true', 'false'], - description='launches RViz if set to `true`.'), DeclareLaunchArgument('use_sim_time', default_value='true', choices=['true', 'false'], - description='use_sim_time'), - DeclareLaunchArgument('headless', default_value='False', - description='Whether to execute simulation gui'), - DeclareLaunchArgument('hardware_type', - default_value='ignition', - choices=['actual', 'fake', 'gz_classic', 'ignition'], - description='configure robot_description to use actual, fake, or simulated hardware', - ), - DeclareLaunchArgument('use_joint_pub_gui', - default_value='false', - choices=('true', 'false'), - description='launches the joint_state_publisher GUI.', - ), + description='use_sim_time') ] def generate_launch_description(): - robot_model = LaunchConfiguration('robot_model') + robot_name = LaunchConfiguration('robot_name') - use_rviz = LaunchConfiguration('use_rviz') use_sim_time = LaunchConfiguration('use_sim_time') - hardware_type = LaunchConfiguration('hardware_type') - use_joint_pub_gui = LaunchConfiguration('use_joint_pub_gui') spawn_robot_node = Node( package='ros_gz_sim', @@ -71,24 +53,6 @@ def generate_launch_description(): output='screen' ) - arm_description_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('arm_sim_scenario'), - 'launch', - 'arm_description_launch.py' - ]) - ]), - launch_arguments={ - 'robot_model': robot_model, - 'robot_name': robot_name, - 'use_rviz': use_rviz, - 'use_sim_time': use_sim_time, - 'hardware_type': hardware_type, - 'use_joint_pub_gui': use_joint_pub_gui - }.items(), - ) - spawn_joint_state_broadcaster_node = Node( name='joint_state_broadcaster_spawner', package='controller_manager', @@ -160,5 +124,4 @@ def generate_launch_description(): ld.add_action(load_joint_state_broadcaster_event) ld.add_action(load_arm_controller_event) ld.add_action(load_gripper_controller_event) - ld.add_action(arm_description_launch) return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py index 17b05980..8c5d375c 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -49,7 +49,7 @@ def load_yaml(package_name, file_path): description='robot_model type of the Interbotix Arm'), DeclareLaunchArgument('robot_name', default_value=LaunchConfiguration('robot_model'), description='Robot name'), - DeclareLaunchArgument('use_rviz', default_value='false', + DeclareLaunchArgument('use_rviz_moveit', default_value='false', choices=['true', 'false'], description='launches RViz if set to `true`.'), DeclareLaunchArgument('use_sim_time', default_value='true', @@ -62,7 +62,7 @@ def load_yaml(package_name, file_path): ]), description='file path to the config file RViz should load.',), DeclareLaunchArgument('hardware_type', - default_value='ignition', + default_value='fake', choices=['actual', 'fake', 'gz_classic', 'ignition'], description='configure robot_description to use actual, fake, or simulated hardware', ), @@ -81,7 +81,7 @@ def generate_launch_description(): robot_model = LaunchConfiguration('robot_model') robot_name = LaunchConfiguration('robot_name') - use_rviz = LaunchConfiguration('use_rviz') + use_rviz_moveit = LaunchConfiguration('use_rviz_moveit') rviz_config = LaunchConfiguration('rviz_config') rviz_frame = LaunchConfiguration('rviz_frame') use_sim_time = LaunchConfiguration('use_sim_time') @@ -101,29 +101,19 @@ def generate_launch_description(): ]) robot_description = {'robot_description': ParameterValue(Command([ - 'xacro', ' ', xacro_file, ' ', - 'gazebo:=ignition', ' ', - 'base_link_frame:=', 'base_link', ' ', - 'use_gripper:=', 'true', ' ', - 'show_ar_tag:=', 'false', ' ', - 'show_gripper_bar:=', 'true', ' ', - 'show_gripper_fingers:=', 'true', ' ', - 'use_world_frame:=', 'true', ' ', - 'robot_model:=', robot_model, ' ', - 'robot_name:=', robot_name, ' ', - 'hardware_type:=', hardware_type]), value_type=str)} + 'xacro', ' ',xacro_file,' ', + 'gazebo:=ignition',' ', + 'base_link_frame:=','base_link ', + 'use_world_frame:=','true ', + 'robot_model:=',robot_model,' ', + 'robot_name:=',robot_name,' ', + 'hardware_type:=',hardware_type]), value_type=str)} robot_description_semantic = {'robot_description_semantic': ParameterValue(Command([ - 'xacro', ' ', srdf_xacro_file, ' ', - 'robot_name:=', robot_name, ' ', - 'base_link_frame:=', 'base_link', ' ', - 'use_gripper:=', 'true', ' ', - 'show_ar_tag:=', 'false', ' ', - 'show_gripper_bar:=', 'true', ' ', - 'show_gripper_fingers:=', 'true', ' ', - 'use_world_frame:=', 'true', ' ', - 'external_urdf_loc:=', '', ' ', - 'external_srdf_loc:=', '', ' ', + 'xacro', ' ', srdf_xacro_file,' ', + 'robot_name:=', robot_name,' ', + 'base_link_frame:=', 'base_link ', + 'use_world_frame:=','true ', 'hardware_type:=', hardware_type]), value_type=str)} ompl_planning_pipeline_config = { @@ -207,8 +197,7 @@ def generate_launch_description(): parameters=[ { 'planning_scene_monitor_options': { - 'robot_description': - 'robot_description', + 'robot_description': 'robot_description', 'joint_state_topic': [robot_name, '/joint_states'], }, 'use_sim_time': use_sim_time, @@ -228,7 +217,7 @@ def generate_launch_description(): ) moveit_rviz_node = Node( - condition=IfCondition(use_rviz), + condition=IfCondition(use_rviz_moveit), package='rviz2', executable='rviz2', name='rviz2', diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py index 48c3aaa3..08d01eb5 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -21,31 +21,77 @@ from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import LaunchConfigurationEquals + + +ARGUMENTS = [ + DeclareLaunchArgument('use_rviz', default_value='false', + choices=['true', 'false'], + description='launches RViz if set to `true`.'), + DeclareLaunchArgument('hardware_type', + default_value='fake', + choices=['actual', 'fake', 'gz_classic', 'ignition'], + description='configure robot_description to use actual, fake, or simulated hardware', + ), + DeclareLaunchArgument('scenario', + default_value=' ', + description='Scenario file to execute', + ), + DeclareLaunchArgument('scenario_execution', default_value='true', + choices=['true', 'false'], + description='Wether to execute scenario execution'), +] def generate_launch_description(): arm_sim_scenario_dir = get_package_share_directory('arm_sim_scenario') scenario_execution_dir = get_package_share_directory('scenario_execution_ros') - + use_rviz = LaunchConfiguration('use_rviz') + hardware_type = LaunchConfiguration('hardware_type') scenario = LaunchConfiguration('scenario') scenario_execution = LaunchConfiguration('scenario_execution') - arg_scenario = DeclareLaunchArgument('scenario', - description='Scenario file to execute') - arg_scenario_execution = DeclareLaunchArgument( - 'scenario_execution', default_value='True', - description='Wether to execute scenario execution') ignition = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'ignition_launch.py'])]), + condition=LaunchConfigurationEquals( + launch_configuration_name='hardware_type', + expected_value='ignition' + ), ) robot = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'ignition_arm_launch.py'])]), + condition=LaunchConfigurationEquals( + launch_configuration_name='hardware_type', + expected_value='ignition' + ), ) moveit_bringup = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'moveit_launch.py'])]), + launch_arguments={ + 'hardware_type': hardware_type, + }.items(), + ) + + arm_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'arm_description_launch.py'])]), + launch_arguments={ + 'use_rviz': use_rviz, + 'hardware_type': hardware_type, + }.items() + ) + + arm_controllers = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'arm_controller_launch.py'])]), + launch_arguments={ + 'hardware_type': hardware_type, + }.items(), + condition=LaunchConfigurationEquals( + launch_configuration_name='hardware_type', + expected_value='fake' + ), ) scenario_exec = IncludeLaunchDescription( @@ -56,10 +102,9 @@ def generate_launch_description(): ] ) - ld = LaunchDescription([ - arg_scenario, - arg_scenario_execution - ]) + ld = LaunchDescription(ARGUMENTS) + ld.add_action(arm_controllers) + ld.add_action(arm_description) ld.add_action(ignition) ld.add_action(robot) ld.add_action(moveit_bringup) diff --git a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro index 4fb92062..4377c1b5 100644 --- a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro @@ -153,7 +153,7 @@ robot_description robot_state_publisher - $(find arm_sim_scenario)/config/trajectory_controllers/${robot_model}_trajectory_controllers.yaml + $(find arm_sim_scenario)/config/trajectory_controllers/${robot_model}_trajectory_controllers_ignition.yaml From dba56a4793cce4231dc9e1f44362bfe646596ffc Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Fri, 30 Aug 2024 19:50:18 +0200 Subject: [PATCH 57/62] format --- .../launch/arm_controller_launch.py | 31 +++++++++---------- .../launch/arm_description_launch.py | 9 +++--- .../launch/ignition_arm_launch.py | 6 ++-- .../arm_sim_scenario/launch/moveit_launch.py | 20 ++++++------ 4 files changed, 30 insertions(+), 36 deletions(-) diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_controller_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_controller_launch.py index 3204aaa1..6e09488a 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/arm_controller_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_controller_launch.py @@ -15,15 +15,14 @@ # SPDX-License-Identifier: Apache-2.0 from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command -from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch_ros.parameter_descriptions import ParameterFile -from ament_index_python.packages import get_package_share_directory from launch_ros.descriptions import ParameterValue +from ament_index_python.packages import get_package_share_directory + ARGUMENTS = [ DeclareLaunchArgument('robot_model', default_value='wx200', @@ -90,10 +89,8 @@ def generate_launch_description(): wrist_rotate_initial_value = LaunchConfiguration('wrist_rotate_initial_value') finger_initial_value = LaunchConfiguration('finger_initial_value') - pkg_arm_sim_scenario = get_package_share_directory('arm_sim_scenario') - ros2_control_controllers_config_parameter_file = ParameterFile( param_file=PathJoinSubstitution([ FindPackageShare('arm_sim_scenario'), @@ -113,17 +110,17 @@ def generate_launch_description(): namespace=robot_name, parameters=[ {'robot_description': ParameterValue(Command([ - 'xacro ',xacro_file, ' ', - 'robot_model:=',robot_model,' ', - 'robot_name:=',robot_name,' ', + 'xacro ', xacro_file, ' ', + 'robot_model:=', robot_model, ' ', + 'robot_name:=', robot_name, ' ', 'use_world_frame:=', 'true ', - 'waist_initial_value:=',waist_initial_value,' ', - 'shoulder_initial_value:=',shoulder_initial_value,' ', - 'elbow_initial_value:=',elbow_initial_value,' ', - 'wrist_angle_initial_value:=',wrist_angle_initial_value,' ', - 'wrist_rotate_initial_value:=',wrist_rotate_initial_value,' ', - 'finger_initial_value:=',finger_initial_value,' ', - 'hardware_type:=',hardware_type]), value_type=str)}, + 'waist_initial_value:=', waist_initial_value, ' ', + 'shoulder_initial_value:=', shoulder_initial_value, ' ', + 'elbow_initial_value:=', elbow_initial_value, ' ', + 'wrist_angle_initial_value:=', wrist_angle_initial_value, ' ', + 'wrist_rotate_initial_value:=', wrist_rotate_initial_value, ' ', + 'finger_initial_value:=', finger_initial_value, ' ', + 'hardware_type:=', hardware_type]), value_type=str)}, ros2_control_controllers_config_parameter_file, ], output={'both': 'screen'}, @@ -179,4 +176,4 @@ def generate_launch_description(): ld.add_action(spawn_joint_state_broadcaster_node) ld.add_action(spawn_arm_controller_node) ld.add_action(spawn_gripper_controller_node) - return ld \ No newline at end of file + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py index 38a5afb9..fff19e34 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py @@ -81,17 +81,16 @@ def generate_launch_description(): parameters=[ {'use_sim_time': use_sim_time}, {'robot_description': ParameterValue(Command([ - 'xacro ',xacro_file, ' ', - 'robot_model:=',robot_model,' ', - 'robot_name:=',robot_name,' ', + 'xacro ', xacro_file, ' ', + 'robot_model:=', robot_model, ' ', + 'robot_name:=', robot_name, ' ', 'use_world_frame:=', 'true ' - 'hardware_type:=',hardware_type]), value_type=str)}, + 'hardware_type:=', hardware_type]), value_type=str)}, ], namespace=robot_name, output={'both': 'log'}, ) - joint_state_publisher = Node( condition=IfCondition(use_joint_pub), package='joint_state_publisher', diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_arm_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_arm_launch.py index fb6ff4e2..8645de05 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_arm_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_arm_launch.py @@ -15,12 +15,10 @@ # SPDX-License-Identifier: Apache-2.0 from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.substitutions import LaunchConfiguration from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare ARGUMENTS = [ diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py index 8c5d375c..223cacec 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -101,19 +101,19 @@ def generate_launch_description(): ]) robot_description = {'robot_description': ParameterValue(Command([ - 'xacro', ' ',xacro_file,' ', - 'gazebo:=ignition',' ', - 'base_link_frame:=','base_link ', - 'use_world_frame:=','true ', - 'robot_model:=',robot_model,' ', - 'robot_name:=',robot_name,' ', - 'hardware_type:=',hardware_type]), value_type=str)} + 'xacro', ' ', xacro_file, ' ', + 'gazebo:=ignition', ' ', + 'base_link_frame:=', 'base_link ', + 'use_world_frame:=', 'true ', + 'robot_model:=', robot_model, ' ', + 'robot_name:=', robot_name, ' ', + 'hardware_type:=', hardware_type]), value_type=str)} robot_description_semantic = {'robot_description_semantic': ParameterValue(Command([ - 'xacro', ' ', srdf_xacro_file,' ', - 'robot_name:=', robot_name,' ', + 'xacro', ' ', srdf_xacro_file, ' ', + 'robot_name:=', robot_name, ' ', 'base_link_frame:=', 'base_link ', - 'use_world_frame:=','true ', + 'use_world_frame:=', 'true ', 'hardware_type:=', hardware_type]), value_type=str)} ompl_planning_pipeline_config = { From 532c73c90e9ca8b4dc50c52fa5f2e07058f47c64 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 2 Sep 2024 17:32:38 +0200 Subject: [PATCH 58/62] add msg_conversion and improvements --- .../external_methods/__init__.py | 15 ++++++++++ .../external_methods/msg_conversion.py | 29 +++++++++++++++++++ .../lib_osc/moveit.osc | 3 ++ .../launch/arm_controller_launch.py | 24 +++++++-------- .../rviz/xsarm_description.rviz | 4 +++ .../arm_sim_scenario/rviz/xsarm_moveit.rviz | 4 +++ .../arm_sim_scenario/urdf/control.urdf.xacro | 6 ++-- .../arm_sim_scenario/urdf/wx200.urdf.xacro | 2 +- 8 files changed, 71 insertions(+), 16 deletions(-) create mode 100644 libs/scenario_execution_moveit/scenario_execution_moveit/external_methods/__init__.py create mode 100644 libs/scenario_execution_moveit/scenario_execution_moveit/external_methods/msg_conversion.py diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/external_methods/__init__.py b/libs/scenario_execution_moveit/scenario_execution_moveit/external_methods/__init__.py new file mode 100644 index 00000000..49cb9db1 --- /dev/null +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/external_methods/__init__.py @@ -0,0 +1,15 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 \ No newline at end of file diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/external_methods/msg_conversion.py b/libs/scenario_execution_moveit/scenario_execution_moveit/external_methods/msg_conversion.py new file mode 100644 index 00000000..0cff7f59 --- /dev/null +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/external_methods/msg_conversion.py @@ -0,0 +1,29 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import sensor_msgs.msg + +def generate_launch_params(in_value): + if isinstance(in_value, sensor_msgs.msg.JointState): + key_value_list = [] + for name, position in zip(in_value.name, in_value.position): + key_value_list.append({'key': name, 'value': position}) + return key_value_list + elif isinstance(in_value, dict) and in_value == {}: + return in_value + else: + raise ValueError(f"to_joint_state_key_value_list not implemented for type {type(in_value)}") + \ No newline at end of file diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc b/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc index 37f93aa3..8b2081a8 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc @@ -1,6 +1,9 @@ import osc.robotics import osc.standard.base +struct joint_state_msg_conversion: + def generate_launch_params(in_value: string) -> list of key_value is external scenario_execution_moveit.external_methods.msg_conversion.generate_launch_params() + actor robotic_arm inherits robot: namespace: string = '' joint_names: list of string diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_controller_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_controller_launch.py index 6e09488a..99a39d3e 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/arm_controller_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_controller_launch.py @@ -48,27 +48,27 @@ choices=('true', 'false'), description='launches the joint_state_publisher GUI.', ), - DeclareLaunchArgument('waist_initial_value', + DeclareLaunchArgument('waist', default_value='0', description='waist joint initial value.', ), - DeclareLaunchArgument('shoulder_initial_value', + DeclareLaunchArgument('shoulder', default_value='0', description='shoulder joint initial value.', ), - DeclareLaunchArgument('elbow_initial_value', + DeclareLaunchArgument('elbow', default_value='0', description='elbow joint initial value.', ), - DeclareLaunchArgument('wrist_angle_initial_value', + DeclareLaunchArgument('wrist_angle', default_value='0', description='wrist_angle joint initial value.', ), - DeclareLaunchArgument('wrist_rotate_initial_value', + DeclareLaunchArgument('wrist_rotate', default_value='0', description='wrist_rotate joint initial value.', ), - DeclareLaunchArgument('finger_initial_value', + DeclareLaunchArgument('finger', default_value='0', description='gripper finger initial value.', ), @@ -82,12 +82,12 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') hardware_type = LaunchConfiguration('hardware_type') # Intial joint states - waist_initial_value = LaunchConfiguration('waist_initial_value') - shoulder_initial_value = LaunchConfiguration('shoulder_initial_value') - elbow_initial_value = LaunchConfiguration('elbow_initial_value') - wrist_angle_initial_value = LaunchConfiguration('wrist_angle_initial_value') - wrist_rotate_initial_value = LaunchConfiguration('wrist_rotate_initial_value') - finger_initial_value = LaunchConfiguration('finger_initial_value') + waist_initial_value = LaunchConfiguration('waist') + shoulder_initial_value = LaunchConfiguration('shoulder') + elbow_initial_value = LaunchConfiguration('elbow') + wrist_angle_initial_value = LaunchConfiguration('wrist_angle') + wrist_rotate_initial_value = LaunchConfiguration('wrist_rotate') + finger_initial_value = LaunchConfiguration('finger') pkg_arm_sim_scenario = get_package_share_directory('arm_sim_scenario') diff --git a/simulation/gazebo/arm_sim_scenario/rviz/xsarm_description.rviz b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_description.rviz index a527ca51..6ec49332 100644 --- a/simulation/gazebo/arm_sim_scenario/rviz/xsarm_description.rviz +++ b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_description.rviz @@ -7,6 +7,10 @@ Panels: Splitter Ratio: 0.6176470518112183 Tree Height: 503 RobotNameSpace: "" + - Class: scenario_execution_rviz/Scenario Control + Name: Scenario Control + - Class: scenario_execution_rviz/Scenario View + Name: Scenario View Visualization Manager: Class: "" Displays: diff --git a/simulation/gazebo/arm_sim_scenario/rviz/xsarm_moveit.rviz b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_moveit.rviz index c9239ef3..255a1d18 100644 --- a/simulation/gazebo/arm_sim_scenario/rviz/xsarm_moveit.rviz +++ b/simulation/gazebo/arm_sim_scenario/rviz/xsarm_moveit.rviz @@ -25,6 +25,10 @@ Panels: Name: Time SyncMode: 0 SyncSource: "" + - Class: scenario_execution_rviz/Scenario Control + Name: Scenario Control + - Class: scenario_execution_rviz/Scenario View + Name: Scenario View Visualization Manager: Class: "" Displays: diff --git a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro index 4377c1b5..e4cddda2 100644 --- a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro @@ -87,7 +87,7 @@ "${wrist_rotate_limit_lower}" - ${wrist_rotate_limit_upper} + "${wrist_rotate_limit_upper}" @@ -139,10 +139,10 @@ robot_description robot_state_publisher - $(find interbotix_xsarm_sim)/config/trajectory_controllers/${robot_model}_trajectory_controllers.yaml + $(find arm_sim_scenario)/config/trajectory_controllers/${robot_model}_trajectory_controllers.yaml - + diff --git a/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro index 33b19bda..d01bc8dc 100644 --- a/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/urdf/wx200.urdf.xacro @@ -24,7 +24,7 @@ - + From 2f3851d8532241db5c0c4e592d8cd3b5f227c7a1 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 3 Sep 2024 16:40:53 +0200 Subject: [PATCH 59/62] fix actions --- .../actions/move_to_joint_pose.py | 39 ++++++++++--------- .../actions/move_to_pose.py | 36 ++++++++--------- 2 files changed, 38 insertions(+), 37 deletions(-) diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py index 109270ba..afddf65c 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py @@ -82,6 +82,8 @@ def setup(self, **kwargs): def update(self) -> py_trees.common.Status: # pylint: disable=R0911 self.current_state = self.moveit2.query_state() + self.logger.debug(str(self.current_state)) + self.logger.debug(str(self.goal_status)) self.last_error_code = self.moveit2.get_last_execution_error_code() result = py_trees.common.Status.RUNNING self.wait_for_service_plan_kinematic_path() @@ -101,10 +103,6 @@ def update(self) -> py_trees.common.Status: # pylint: disable=R0911 return result else: self.feedback_message = "Motion to joint pose in progress..." # pylint: disable= attribute-defined-outside-init - if not self.future_called: - future = self.moveit2.get_execution_future() - future.add_done_callback(self.future_done_callback) - self.future_called = True return result # Handle idle state if self.current_state == MoveIt2State.IDLE: @@ -115,21 +113,24 @@ def update(self) -> py_trees.common.Status: # pylint: disable=R0911 self.execute = True return result else: - if self.goal_status is None or self.goal_status == GoalStatus.STATUS_UNKNOWN: - self.logger.debug(f"Goal aborted by the controller {self.log_moveit_error(self.last_error_code)}. Retrying...") - self.feedback_message = f"Goal aborted by the controller {self.log_moveit_error(self.last_error_code)}. Retrying..." # pylint: disable= attribute-defined-outside-init - self.execute = False - self.future_called = False - self.goal_status = None - return result - if self.goal_status == GoalStatus.STATUS_SUCCEEDED: - self.logger.info("Motion to joint pose successful.") - self.feedback_message = "Motion to joint pose successful." # pylint: disable= attribute-defined-outside-init - return py_trees.common.Status.SUCCESS - else: - self.logger.info(f"Motion move to joint position failed.") - return py_trees.common.Status.FAILURE - + if not self.future_called: + future = self.moveit2.get_execution_future() + future.add_done_callback(self.future_done_callback) + self.future_called = True + if self.goal_status is not None: + if self.goal_status == GoalStatus.STATUS_SUCCEEDED: + self.logger.info("Motion to joint pose successful.") + self.feedback_message = "Motion to joint pose successful." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.SUCCESS + elif self.goal_status == GoalStatus.STATUS_CANCELED: + self.feedback_message = f"Goal canceled." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + elif self.goal_status == GoalStatus.STATUS_ABORTED: + self.feedback_message = f"Goal aborted." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + else: + self.feedback_message = f"Goal status unknown." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE if self.execution_start_time is None: self.execution_start_time = time.time() if self.check_timeout(self.execution_start_time): diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py index b495438e..9cabad04 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py @@ -111,10 +111,6 @@ def update(self) -> py_trees.common.Status: # pylint: disable=R0911 return result else: self.feedback_message = "Motion to pose in progress..." # pylint: disable= attribute-defined-outside-init - if not self.future_called: - future = self.moveit2.get_execution_future() - future.add_done_callback(self.future_done_callback) - self.future_called = True return result # Handle idle state if self.current_state == MoveIt2State.IDLE: @@ -125,20 +121,24 @@ def update(self) -> py_trees.common.Status: # pylint: disable=R0911 self.execute = True return result else: - if self.goal_status is None or self.goal_status == GoalStatus.STATUS_UNKNOWN: - self.logger.debug(f"Goal aborted by the controller {self.log_moveit_error(self.last_error_code)}. Retrying...") - self.feedback_message = f"Goal aborted by the controller {self.log_moveit_error(self.last_error_code)}. Retrying..." # pylint: disable= attribute-defined-outside-init - self.execute = False - self.future_called = False - self.goal_status = None - return result - if self.goal_status == GoalStatus.STATUS_SUCCEEDED: - self.logger.info("Motion to pose successful.") - self.feedback_message = "Motion to pose successful." # pylint: disable= attribute-defined-outside-init - return py_trees.common.Status.SUCCESS - else: - self.logger.info(f"Motion move to pose failed.") - return py_trees.common.Status.FAILURE + if not self.future_called: + future = self.moveit2.get_execution_future() + future.add_done_callback(self.future_done_callback) + self.future_called = True + if self.goal_status is not None: + if self.goal_status == GoalStatus.STATUS_SUCCEEDED: + self.logger.info("Motion to pose successful.") + self.feedback_message = "Motion to pose successful." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.SUCCESS + elif self.goal_status == GoalStatus.STATUS_CANCELED: + self.feedback_message = f"Goal canceled." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + elif self.goal_status == GoalStatus.STATUS_ABORTED: + self.feedback_message = f"Goal aborted." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + else: + self.feedback_message = f"Goal status unknown." # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE if self.execution_start_time is None: self.execution_start_time = time.time() if self.check_timeout(self.execution_start_time): From b502411c638405bdee7b341d2994174c78b91fe6 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 4 Sep 2024 11:42:33 +0200 Subject: [PATCH 60/62] add action server --- libs/scenario_execution_moveit/package.xml | 1 + .../action_server.py | 96 +++++++++++++++++++ .../scenario_execution_moveit/wx200.py | 35 +++++++ libs/scenario_execution_moveit/setup.py | 4 + .../CMakeLists.txt | 23 +++++ .../action/MoveToJointPose.action | 9 ++ .../package.xml | 22 +++++ 7 files changed, 190 insertions(+) create mode 100644 libs/scenario_execution_moveit/scenario_execution_moveit/action_server.py create mode 100644 libs/scenario_execution_moveit/scenario_execution_moveit/wx200.py create mode 100644 libs/scenario_execution_moveit_interface/CMakeLists.txt create mode 100644 libs/scenario_execution_moveit_interface/action/MoveToJointPose.action create mode 100644 libs/scenario_execution_moveit_interface/package.xml diff --git a/libs/scenario_execution_moveit/package.xml b/libs/scenario_execution_moveit/package.xml index 9764c54a..b21921a6 100644 --- a/libs/scenario_execution_moveit/package.xml +++ b/libs/scenario_execution_moveit/package.xml @@ -10,6 +10,7 @@ scenario_execution_ros + scenario_execution_moveit_interface rclpy pymoveit2 moveit_msgs diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/action_server.py b/libs/scenario_execution_moveit/scenario_execution_moveit/action_server.py new file mode 100644 index 00000000..cdbaefb4 --- /dev/null +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/action_server.py @@ -0,0 +1,96 @@ +import time +import rclpy +from rclpy.node import Node +from rclpy.action import ActionServer, GoalResponse +from rclpy.action.client import GoalStatus +from scenario_execution_moveit_interface.action import MoveToJointPose +from rclpy.action.server import ServerGoalHandle +from pymoveit2 import MoveIt2State +from scenario_execution_moveit.moveit_common import MoveIt2Interface +from rclpy.callback_groups import ReentrantCallbackGroup +from scenario_execution_moveit import wx200 +from rclpy.executors import MultiThreadedExecutor + +class MoveToJointPoseNode(Node): + def __init__(self): + super().__init__("move_to_joint_pose") + + self.moveit2 = MoveIt2Interface( + node=self, + joint_names=wx200.joint_names(), + base_link_name=wx200.base_link_name(), + end_effector_name=wx200.end_effector_name(), + group_name=wx200.MOVE_GROUP_ARM, + callback_group=ReentrantCallbackGroup() + ) + self.moveit2.planner_id = "RRTConnect" + self.moveit2.max_velocity = 0.5 + self.moveit2.max_acceleration = 0.5 + + self.move_to_joint_pose_server_ = ActionServer( + self, + MoveToJointPose, + "move_to_joint_pose", + goal_callback=self.goal_callback, + execute_callback=self.execute_callback, + callback_group=ReentrantCallbackGroup() + ) + self.get_logger().info("Action server move to joint pose has been started.") + + def goal_callback(self, goal_request: MoveToJointPose.Goal): + self.get_logger().info("Received a goal request") + current_state = self.moveit2.query_state() + if current_state == MoveIt2State.EXECUTING: + self.get_logger().info("Another motion is in progress. Rejecting the goal...") + return GoalResponse.REJECT + self.get_logger().info("Accepting the goal") + return GoalResponse.ACCEPT + + def execute_callback(self, goal_handle: ServerGoalHandle): + target = goal_handle.request.joint_positions + self.get_logger().info(f"Executing the goal...{target}") + result = MoveToJointPose.Result() + self.moveit2.move_to_configuration(target) + timeout = 1 + start_time = time.time() + future = None + + while future is None: + if time.time() - start_time > timeout: + self.get_logger().info("Timeout reached while waiting for the future.") + goal_handle.abort() + result.error_code = 6 + return result + if self.moveit2.query_state() == MoveIt2State.EXECUTING: + future = self.moveit2.get_execution_future() + time.sleep(0.01) + while not future.done(): + time.sleep(0.01) + + if future.result().status == GoalStatus.STATUS_SUCCEEDED: + self.get_logger().info("Goal succeeded.") + goal_handle.succeed() + result.error_code = 1 + elif future.result().status == GoalStatus.STATUS_ABORTED: + self.get_logger().info("Goal aborted.") + goal_handle.abort() + result.error_code = 6 + elif future.result().status == GoalStatus.STATUS_CANCELED: + self.get_logger().info("Goal canceled.") + goal_handle.canceled() + result.error_code = 3 + + return result + +def main(args=None): + rclpy.init(args=args) + node = MoveToJointPoseNode() + executor = MultiThreadedExecutor(num_threads=2) + executor.add_node(node) + executor.spin() + + rclpy.shutdown() + +if __name__ == "__main__": + main() + diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/wx200.py b/libs/scenario_execution_moveit/scenario_execution_moveit/wx200.py new file mode 100644 index 00000000..466e6cd1 --- /dev/null +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/wx200.py @@ -0,0 +1,35 @@ +from typing import List + +MOVE_GROUP_ARM: str = "interbotix_arm" +MOVE_GROUP_GRIPPER: str = "interbotix_gripper" + +OPEN_GRIPPER_JOINT_POSITIONS: List[float] = [-0.037, +0.037] +CLOSED_GRIPPER_JOINT_POSITIONS: List[float] = [-0.0195, +0.0195] + + +def joint_names() -> List[str]: + return [ + "waist", + "shoulder", + "elbow", + "wrist_angle", + "wrist_rotate" + "left_finger", + "right_finger", + ] + + +def base_link_name() -> str: + return "wx200/base_link" + + +def end_effector_name() -> str: + return "wx200/ee_gripper_link" + + +def gripper_joint_names() -> List[str]: + return [ + "right_finger", + "left_finger" + + ] diff --git a/libs/scenario_execution_moveit/setup.py b/libs/scenario_execution_moveit/setup.py index c5568e7b..5d50d271 100644 --- a/libs/scenario_execution_moveit/setup.py +++ b/libs/scenario_execution_moveit/setup.py @@ -50,6 +50,10 @@ 'scenario_execution.osc_libraries': [ 'moveit = ' 'scenario_execution_moveit.get_osc_library:get_moveit_library', + ], + 'console_scripts': [ + + "move_to_joint_pose_server = scenario_execution_moveit.action_server:main" ] }, ) diff --git a/libs/scenario_execution_moveit_interface/CMakeLists.txt b/libs/scenario_execution_moveit_interface/CMakeLists.txt new file mode 100644 index 00000000..a9536729 --- /dev/null +++ b/libs/scenario_execution_moveit_interface/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.8) +project(scenario_execution_moveit_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + + +rosidl_generate_interfaces(${PROJECT_NAME} + "action/MoveToJointPose.action" +) + +ament_export_dependencies(rosidl_default_runtime) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() \ No newline at end of file diff --git a/libs/scenario_execution_moveit_interface/action/MoveToJointPose.action b/libs/scenario_execution_moveit_interface/action/MoveToJointPose.action new file mode 100644 index 00000000..7fa02324 --- /dev/null +++ b/libs/scenario_execution_moveit_interface/action/MoveToJointPose.action @@ -0,0 +1,9 @@ +# Goal +float64[5] joint_positions # Array of 5 joint positions + +--- +# Result +int64 error_code + +--- +# Feedback diff --git a/libs/scenario_execution_moveit_interface/package.xml b/libs/scenario_execution_moveit_interface/package.xml new file mode 100644 index 00000000..eb123e42 --- /dev/null +++ b/libs/scenario_execution_moveit_interface/package.xml @@ -0,0 +1,22 @@ + + + + scenario_execution_moveit_interface + 0.0.0 + TODO: Package description + nsinghal + TODO: License declaration + + ament_cmake + + rosidl_default_generators + rosidl_interface_packages + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 809ad337fd445871d77e58e80487a96f7057a3f8 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 4 Sep 2024 13:09:40 +0200 Subject: [PATCH 61/62] launch files setup --- ...ver.py => move_to_joint_pose_action_server.py} | 0 libs/scenario_execution_moveit/setup.py | 2 +- .../arm_sim_scenario/launch/moveit_launch.py | 15 ++++++++++++++- .../launch/sim_moveit_scenario_launch.py | 10 +++++++++- 4 files changed, 24 insertions(+), 3 deletions(-) rename libs/scenario_execution_moveit/scenario_execution_moveit/{action_server.py => move_to_joint_pose_action_server.py} (100%) diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/action_server.py b/libs/scenario_execution_moveit/scenario_execution_moveit/move_to_joint_pose_action_server.py similarity index 100% rename from libs/scenario_execution_moveit/scenario_execution_moveit/action_server.py rename to libs/scenario_execution_moveit/scenario_execution_moveit/move_to_joint_pose_action_server.py diff --git a/libs/scenario_execution_moveit/setup.py b/libs/scenario_execution_moveit/setup.py index 5d50d271..73fb3dac 100644 --- a/libs/scenario_execution_moveit/setup.py +++ b/libs/scenario_execution_moveit/setup.py @@ -53,7 +53,7 @@ ], 'console_scripts': [ - "move_to_joint_pose_server = scenario_execution_moveit.action_server:main" + "move_to_joint_pose_server = scenario_execution_moveit.move_to_joint_pose_action_server:main" ] }, ) diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py index 223cacec..3408a642 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -71,7 +71,10 @@ def load_yaml(package_name, file_path): 'defines the fixed frame parameter in RViz. Note that if `use_world_frame` is ' '`false`, this parameter should be changed to a frame that exists.' ), - ) + ), + DeclareLaunchArgument('use_movie_to_joint_pose_action_server', default_value='true', + choices=['true', 'false'], + description='launches movie to joint pose action server'), ] @@ -86,6 +89,7 @@ def generate_launch_description(): rviz_frame = LaunchConfiguration('rviz_frame') use_sim_time = LaunchConfiguration('use_sim_time') hardware_type = LaunchConfiguration('hardware_type') + use_movie_to_joint_pose_action_server = LaunchConfiguration('use_movie_to_joint_pose_action_server') xacro_file = PathJoinSubstitution([pkg_arm_sim_scenario, 'urdf', @@ -236,7 +240,16 @@ def generate_launch_description(): output={'both': 'log'}, ) + movie_to_joint_pose_action_server = Node( + condition=IfCondition(use_movie_to_joint_pose_action_server), + package='scenario_execution_moveit', + executable='move_to_joint_pose_server', + name='move_to_joint_pose_server', + output={'both': 'log'}, + ) + ld = LaunchDescription(ARGUMENTS) ld.add_action(move_group_node) ld.add_action(moveit_rviz_node) + ld.add_action(movie_to_joint_pose_action_server) return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py index 08d01eb5..5c3bffed 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -34,12 +34,15 @@ description='configure robot_description to use actual, fake, or simulated hardware', ), DeclareLaunchArgument('scenario', - default_value=' ', + default_value='', description='Scenario file to execute', ), DeclareLaunchArgument('scenario_execution', default_value='true', choices=['true', 'false'], description='Wether to execute scenario execution'), + DeclareLaunchArgument('use_moveit', default_value='true', + choices=['true', 'false'], + description='Wether to launch moveit'), ] @@ -51,6 +54,7 @@ def generate_launch_description(): hardware_type = LaunchConfiguration('hardware_type') scenario = LaunchConfiguration('scenario') scenario_execution = LaunchConfiguration('scenario_execution') + use_moveit = LaunchConfiguration('use_moveit') ignition = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'ignition_launch.py'])]), @@ -70,6 +74,10 @@ def generate_launch_description(): moveit_bringup = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'moveit_launch.py'])]), + condition=LaunchConfigurationEquals( + launch_configuration_name='use_moveit', + expected_value='true' + ), launch_arguments={ 'hardware_type': hardware_type, }.items(), From 48cba715a2ebdcf8c76eac5a3fcc8a47ecf61c38 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 4 Sep 2024 13:58:25 +0200 Subject: [PATCH 62/62] fix action defintion --- libs/scenario_execution_moveit/package.xml | 3 +-- .../move_to_joint_pose_action_server.py | 8 +++---- .../CMakeLists.txt | 23 ------------------- .../action/MoveToJointPose.action | 9 -------- .../package.xml | 22 ------------------ 5 files changed, 5 insertions(+), 60 deletions(-) delete mode 100644 libs/scenario_execution_moveit_interface/CMakeLists.txt delete mode 100644 libs/scenario_execution_moveit_interface/action/MoveToJointPose.action delete mode 100644 libs/scenario_execution_moveit_interface/package.xml diff --git a/libs/scenario_execution_moveit/package.xml b/libs/scenario_execution_moveit/package.xml index b21921a6..86951e81 100644 --- a/libs/scenario_execution_moveit/package.xml +++ b/libs/scenario_execution_moveit/package.xml @@ -9,8 +9,7 @@ Apache-2.0 scenario_execution_ros - - scenario_execution_moveit_interface + joint_to_pose rclpy pymoveit2 moveit_msgs diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/move_to_joint_pose_action_server.py b/libs/scenario_execution_moveit/scenario_execution_moveit/move_to_joint_pose_action_server.py index cdbaefb4..64964c1f 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/move_to_joint_pose_action_server.py +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/move_to_joint_pose_action_server.py @@ -3,7 +3,7 @@ from rclpy.node import Node from rclpy.action import ActionServer, GoalResponse from rclpy.action.client import GoalStatus -from scenario_execution_moveit_interface.action import MoveToJointPose +from joint_to_pose.action import JointToPose from rclpy.action.server import ServerGoalHandle from pymoveit2 import MoveIt2State from scenario_execution_moveit.moveit_common import MoveIt2Interface @@ -29,7 +29,7 @@ def __init__(self): self.move_to_joint_pose_server_ = ActionServer( self, - MoveToJointPose, + JointToPose, "move_to_joint_pose", goal_callback=self.goal_callback, execute_callback=self.execute_callback, @@ -37,7 +37,7 @@ def __init__(self): ) self.get_logger().info("Action server move to joint pose has been started.") - def goal_callback(self, goal_request: MoveToJointPose.Goal): + def goal_callback(self, goal_request: JointToPose.Goal): self.get_logger().info("Received a goal request") current_state = self.moveit2.query_state() if current_state == MoveIt2State.EXECUTING: @@ -49,7 +49,7 @@ def goal_callback(self, goal_request: MoveToJointPose.Goal): def execute_callback(self, goal_handle: ServerGoalHandle): target = goal_handle.request.joint_positions self.get_logger().info(f"Executing the goal...{target}") - result = MoveToJointPose.Result() + result = JointToPose.Result() self.moveit2.move_to_configuration(target) timeout = 1 start_time = time.time() diff --git a/libs/scenario_execution_moveit_interface/CMakeLists.txt b/libs/scenario_execution_moveit_interface/CMakeLists.txt deleted file mode 100644 index a9536729..00000000 --- a/libs/scenario_execution_moveit_interface/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(scenario_execution_moveit_interface) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) - - -rosidl_generate_interfaces(${PROJECT_NAME} - "action/MoveToJointPose.action" -) - -ament_export_dependencies(rosidl_default_runtime) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() \ No newline at end of file diff --git a/libs/scenario_execution_moveit_interface/action/MoveToJointPose.action b/libs/scenario_execution_moveit_interface/action/MoveToJointPose.action deleted file mode 100644 index 7fa02324..00000000 --- a/libs/scenario_execution_moveit_interface/action/MoveToJointPose.action +++ /dev/null @@ -1,9 +0,0 @@ -# Goal -float64[5] joint_positions # Array of 5 joint positions - ---- -# Result -int64 error_code - ---- -# Feedback diff --git a/libs/scenario_execution_moveit_interface/package.xml b/libs/scenario_execution_moveit_interface/package.xml deleted file mode 100644 index eb123e42..00000000 --- a/libs/scenario_execution_moveit_interface/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - scenario_execution_moveit_interface - 0.0.0 - TODO: Package description - nsinghal - TODO: License declaration - - ament_cmake - - rosidl_default_generators - rosidl_interface_packages - rosidl_default_runtime - - ament_lint_auto - ament_lint_common - - - ament_cmake - -