From a89e572b210c7acec26d727fdbac8a36d3809f62 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 29 Feb 2024 11:31:54 +0000 Subject: [PATCH 1/5] Configure ros2_control JTC including position joint --- .../config/default_joint_limits.yaml | 5 ++ .../config/ompl_planning.yaml | 4 +- .../config/ros_controllers.yaml | 4 +- .../config/stretch_base.ros2_control.xacro | 16 +++++- stretch_moveit_config/launch/demo.launch.py | 56 ++++++++----------- 5 files changed, 46 insertions(+), 39 deletions(-) diff --git a/stretch_moveit_config/config/default_joint_limits.yaml b/stretch_moveit_config/config/default_joint_limits.yaml index 9367cbea..d7cdbd58 100644 --- a/stretch_moveit_config/config/default_joint_limits.yaml +++ b/stretch_moveit_config/config/default_joint_limits.yaml @@ -63,6 +63,11 @@ joint_limits: max_velocity: 0.15 has_acceleration_limits: true max_acceleration: 0.2 + position/y: + has_velocity_limits: true + max_velocity: 0.15 + has_acceleration_limits: true + max_acceleration: 0.2 position/theta: has_velocity_limits: true max_velocity: 0.15 diff --git a/stretch_moveit_config/config/ompl_planning.yaml b/stretch_moveit_config/config/ompl_planning.yaml index 914f55dd..a85da7de 100644 --- a/stretch_moveit_config/config/ompl_planning.yaml +++ b/stretch_moveit_config/config/ompl_planning.yaml @@ -7,10 +7,10 @@ request_adapters: - default_planning_request_adapters/CheckStartStateBounds - default_planning_request_adapters/CheckStartStateCollision response_adapters: - # - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/AddTimeOptimalParameterization - default_planning_response_adapters/ValidateSolution - default_planning_response_adapters/DisplayMotionPath - +start_state_max_bounds_error: 0.1 planner_configs: AnytimePathShortening: type: geometric::AnytimePathShortening diff --git a/stretch_moveit_config/config/ros_controllers.yaml b/stretch_moveit_config/config/ros_controllers.yaml index add337c1..d0f4f21a 100644 --- a/stretch_moveit_config/config/ros_controllers.yaml +++ b/stretch_moveit_config/config/ros_controllers.yaml @@ -27,4 +27,6 @@ stretch_controller: - joint_head_tilt - joint_gripper_finger_left - joint_gripper_finger_right - - position + - position/x + - position/y + - position/theta diff --git a/stretch_moveit_config/config/stretch_base.ros2_control.xacro b/stretch_moveit_config/config/stretch_base.ros2_control.xacro index 353c5e2a..b175431b 100644 --- a/stretch_moveit_config/config/stretch_base.ros2_control.xacro +++ b/stretch_moveit_config/config/stretch_base.ros2_control.xacro @@ -7,8 +7,20 @@ mock_components/GenericSystem - - + + 0.0 + + + + + + 0.0 + + + + + + 0.0 diff --git a/stretch_moveit_config/launch/demo.launch.py b/stretch_moveit_config/launch/demo.launch.py index df526e5e..c63a0a4c 100644 --- a/stretch_moveit_config/launch/demo.launch.py +++ b/stretch_moveit_config/launch/demo.launch.py @@ -80,7 +80,7 @@ def load_joint_limits_from_config(mode='default'): def generate_launch_description(): parser = argparse.ArgumentParser() - parser.add_argument('--use_fake_controller', default=False, type=eval, choices=[True, False]) + parser.add_argument('--use_fake_controller', default=True, type=eval, choices=[True, False]) args, _ = parser.parse_known_args([arg for sys_arg in sys.argv[4:] for arg in ('--' + sys_arg).split(':=')]) ld = LaunchDescription() @@ -104,25 +104,9 @@ def generate_launch_description(): joint_limits_yaml = {'robot_description_planning': load_joint_limits_from_config()} # Planning Functionality - planning_pipelines_config = { - "default_planning_pipeline": "ompl", - "planning_pipelines": ["ompl"], - "ompl": { - "planning_plugins": ["ompl_interface/OMPLPlanner"], - "request_adapters": [ - "default_planning_request_adapters/ResolveConstraintFrames", - "default_planning_request_adapters/ValidateWorkspaceBounds", - "default_planning_request_adapters/CheckStartStateBounds", - "default_planning_request_adapters/CheckStartStateCollision", - ], - "response_adapters": [ - "default_planning_response_adapters/AddTimeOptimalParameterization", - "default_planning_response_adapters/ValidateSolution", - ], - }, - } - ompl_planning_yaml = load_yaml("stretch_moveit_config", "config/ompl_planning.yaml") - planning_pipelines_config["ompl"].update(ompl_planning_yaml) + ompl_planning_pipeline_config = {'move_group': {}} + ompl_planning_yaml = load_yaml('stretch_moveit_config', 'config/ompl_planning.yaml') + ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) # Trajectory Execution Functionality controllers_yaml = load_yaml('stretch_moveit_config', 'config/moveit_simple_controllers.yaml') @@ -148,7 +132,7 @@ def generate_launch_description(): robot_description_kinematics, sensors_yaml, joint_limits_yaml, - planning_pipelines_config, + ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters]) @@ -163,8 +147,7 @@ def generate_launch_description(): arguments=['-d', rviz_config_file], parameters=[robot_description, robot_description_semantic, - planning_pipelines_config, - kinematics_yaml]) + robot_description_kinematics]) ld.add_action(rviz_node) if args.use_fake_controller: @@ -184,22 +167,27 @@ def generate_launch_description(): ld.add_action(robot_state_publisher) # Fake joint driver - fake_joint_driver_node = Node( + controller_manager_node = Node( package='controller_manager', executable='ros2_control_node', parameters=[robot_description, os.path.join(get_package_share_directory('stretch_moveit_config'), 'config', 'ros_controllers.yaml')], ) - ld.add_action(fake_joint_driver_node) - - for controller in ['stretch_controller', 'joint_state_broadcaster']: - ld.add_action( - ExecuteProcess( - cmd=['ros2 run controller_manager spawner --controller-manager-timeout 180 {}'.format(controller)], - shell=True, - output='screen', - ) - ) + ld.add_action(controller_manager_node) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + ld.add_action(joint_state_broadcaster_spawner) + + controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["stretch_controller", "--controller-manager", "/controller_manager"], + ) + ld.add_action(controller_spawner) return ld From 96975c6ef7485f48edcf6124830273194c21d7fe Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Mon, 18 Mar 2024 10:38:56 +0000 Subject: [PATCH 2/5] Set base joint limits to actual max --- stretch_moveit_config/config/default_joint_limits.yaml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/stretch_moveit_config/config/default_joint_limits.yaml b/stretch_moveit_config/config/default_joint_limits.yaml index d7cdbd58..9056ef25 100644 --- a/stretch_moveit_config/config/default_joint_limits.yaml +++ b/stretch_moveit_config/config/default_joint_limits.yaml @@ -58,18 +58,22 @@ joint_limits: max_velocity: 3.0 has_acceleration_limits: true max_acceleration: 8.0 + +# The max velocity of the stretch robot is 0.6m/s +# Since the limits are applied by dimension, diagonal motions will allow a squared norm of the x/y limits. +# The 0.42m/s is set to not exceed the max velocity in diagonal motions. position/x: has_velocity_limits: true - max_velocity: 0.15 + max_velocity: 0.42 has_acceleration_limits: true max_acceleration: 0.2 position/y: has_velocity_limits: true - max_velocity: 0.15 + max_velocity: 0.42 has_acceleration_limits: true max_acceleration: 0.2 position/theta: has_velocity_limits: true - max_velocity: 0.15 + max_velocity: 0.42 has_acceleration_limits: true max_acceleration: 0.2 From cfc3e5645391de7a9f3f5afa40366a65820255c3 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Mon, 18 Mar 2024 10:42:28 +0000 Subject: [PATCH 3/5] Configure controller chain: JTC+DiffDriveController --- .../config/gripper.ros2_control.xacro | 4 +- .../config/moveit_simple_controllers.yaml | 11 +++- .../config/ros_controllers.yaml | 66 +++++++++++++++++++ .../config/stretch_base.ros2_control.xacro | 27 ++++++-- stretch_moveit_config/launch/demo.launch.py | 36 +++++++--- 5 files changed, 126 insertions(+), 18 deletions(-) diff --git a/stretch_moveit_config/config/gripper.ros2_control.xacro b/stretch_moveit_config/config/gripper.ros2_control.xacro index 204dc977..07db3333 100644 --- a/stretch_moveit_config/config/gripper.ros2_control.xacro +++ b/stretch_moveit_config/config/gripper.ros2_control.xacro @@ -5,7 +5,7 @@ - fake_components/GenericSystem + mock_components/GenericSystem 0.0 @@ -23,4 +23,4 @@ - \ No newline at end of file + diff --git a/stretch_moveit_config/config/moveit_simple_controllers.yaml b/stretch_moveit_config/config/moveit_simple_controllers.yaml index 60ec68c0..26e2ca3b 100644 --- a/stretch_moveit_config/config/moveit_simple_controllers.yaml +++ b/stretch_moveit_config/config/moveit_simple_controllers.yaml @@ -1,5 +1,6 @@ controller_names: - stretch_controller + - stretch_base_controller stretch_controller: action_ns: follow_joint_trajectory @@ -16,4 +17,12 @@ stretch_controller: - joint_head_tilt - joint_gripper_finger_left - joint_gripper_finger_right - - position + +stretch_base_controller: + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - position/x + - position/y + - position/theta diff --git a/stretch_moveit_config/config/ros_controllers.yaml b/stretch_moveit_config/config/ros_controllers.yaml index d0f4f21a..63e88e0c 100644 --- a/stretch_moveit_config/config/ros_controllers.yaml +++ b/stretch_moveit_config/config/ros_controllers.yaml @@ -5,9 +5,15 @@ controller_manager: stretch_controller: type: joint_trajectory_controller/JointTrajectoryController + stretch_base_controller: + type: joint_trajectory_controller/JointTrajectoryController + joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + diff_drive_controller: + type: diff_drive_controller/DiffDriveController + stretch_controller: ros__parameters: command_interfaces: @@ -27,6 +33,66 @@ stretch_controller: - joint_head_tilt - joint_gripper_finger_left - joint_gripper_finger_right + +stretch_base_controller: + ros__parameters: + command_interfaces: + - velocity + state_interfaces: + - position + - velocity + allow_partial_joints_goal: true + open_loop_control: true # disables PID + allow_integration_in_goal_trajectories: true + joints: - position/x - position/y - position/theta + command_joints: + - diff_drive_controller/x + - diff_drive_controller/y + - diff_drive_controller/theta + +# Controller configuration for HelloRobot Stretch RE1 +# See https://docs.hello-robot.com/0.2/stretch-hardware-guides/docs/hardware_guide_re1/#base +diff_drive_controller: + ros__parameters: + left_wheel_names: ["joint_left_wheel"] + right_wheel_names: ["joint_right_wheel"] + + wheel_separation: 0.3 # estimate! (340mm - 2x20mm wheel width) + wheels_per_side: 1 + wheel_radius: 0.0508 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 50.0 + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + + open_loop: true + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + #velocity_rolling_window_size: 10 + + # Velocity and acceleration limits, should match MoveIt's default_joint_limits.yaml + # Whenever a min_* is unspecified, default to -max_* + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: true + linear.x.has_jerk_limits: false + linear.x.max_velocity: 0.42 + linear.x.max_acceleration: 0.2 + linear.x.max_jerk: 0.0 + + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: true + angular.z.has_jerk_limits: false + angular.z.max_velocity: 0.42 + angular.z.max_acceleration: 0.2 + angular.z.max_jerk: 0.0 diff --git a/stretch_moveit_config/config/stretch_base.ros2_control.xacro b/stretch_moveit_config/config/stretch_base.ros2_control.xacro index b175431b..cf9a971f 100644 --- a/stretch_moveit_config/config/stretch_base.ros2_control.xacro +++ b/stretch_moveit_config/config/stretch_base.ros2_control.xacro @@ -6,22 +6,37 @@ mock_components/GenericSystem + true - + + + 0.0 - + - + 0.0 - + - + 0.0 - + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/demo.launch.py b/stretch_moveit_config/launch/demo.launch.py index c63a0a4c..1add36e5 100644 --- a/stretch_moveit_config/launch/demo.launch.py +++ b/stretch_moveit_config/launch/demo.launch.py @@ -5,7 +5,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ExecuteProcess +from launch.actions import ExecuteProcess, RegisterEventHandler +from launch.event_handlers import OnProcessExit from launch_ros.actions import Node @@ -151,12 +152,6 @@ def generate_launch_description(): ld.add_action(rviz_node) if args.use_fake_controller: - static_tf = Node(package='tf2_ros', - executable='static_transform_publisher', - name='static_transform_publisher', - output='log', - arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'odom', 'base_link']) - ld.add_action(static_tf) # Publish TF robot_state_publisher = Node(package='robot_state_publisher', @@ -183,11 +178,34 @@ def generate_launch_description(): ) ld.add_action(joint_state_broadcaster_spawner) - controller_spawner = Node( + trajectory_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=["stretch_controller", "--controller-manager", "/controller_manager"], ) - ld.add_action(controller_spawner) + ld.add_action(trajectory_controller_spawner) + + diff_drive_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diff_drive_controller", "--controller-manager", "/controller_manager"], + ) + ld.add_action(diff_drive_controller_spawner) + + # The base JTC is chained to the diff drive controller, so we run the spawner on a delayed lifecycle event + base_trajectory_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["stretch_base_controller", "--controller-manager", "/controller_manager"], + ) + delay_base_trajectory_controller_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=diff_drive_controller_spawner, + on_exit=[base_trajectory_controller_spawner], + ) + ) + ) + ld.add_action(delay_base_trajectory_controller_spawner) return ld From 58f033faf12d5983a42f8e0b56a77be7380d1b70 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 22 Mar 2024 15:56:52 +0000 Subject: [PATCH 4/5] Configure TEM to run multi-dof trajectories as joints --- stretch_moveit_config/launch/demo.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/stretch_moveit_config/launch/demo.launch.py b/stretch_moveit_config/launch/demo.launch.py index 1add36e5..f0fe43aa 100644 --- a/stretch_moveit_config/launch/demo.launch.py +++ b/stretch_moveit_config/launch/demo.launch.py @@ -117,7 +117,8 @@ def generate_launch_description(): trajectory_execution = {'moveit_manage_controllers': True, 'trajectory_execution.allowed_execution_duration_scaling': 1.2, 'trajectory_execution.allowed_goal_duration_margin': 0.5, - 'trajectory_execution.allowed_start_tolerance': 0.01} + 'trajectory_execution.allowed_start_tolerance': 0.01, + 'trajectory_execution.control_multi_dof_joint_variables': True} planning_scene_monitor_parameters = {'publish_planning_scene': True, 'publish_geometry_updates': True, From e258aa292dfa35f8e74343da50377a34ea20a910 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 30 Apr 2024 16:49:41 +0000 Subject: [PATCH 5/5] Use picknik_diff_drive_controller::DiffDriveController --- .../config/ros_controllers.yaml | 2 +- stretch_moveit_config/package.xml | 1 + stretch_ros.repos | 39 +++---------------- 3 files changed, 8 insertions(+), 34 deletions(-) diff --git a/stretch_moveit_config/config/ros_controllers.yaml b/stretch_moveit_config/config/ros_controllers.yaml index 63e88e0c..0c8b5786 100644 --- a/stretch_moveit_config/config/ros_controllers.yaml +++ b/stretch_moveit_config/config/ros_controllers.yaml @@ -12,7 +12,7 @@ controller_manager: type: joint_state_broadcaster/JointStateBroadcaster diff_drive_controller: - type: diff_drive_controller/DiffDriveController + type: picknik_diff_drive_controller/DiffDriveController stretch_controller: ros__parameters: diff --git a/stretch_moveit_config/package.xml b/stretch_moveit_config/package.xml index 80d2ea5b..e4d0731d 100644 --- a/stretch_moveit_config/package.xml +++ b/stretch_moveit_config/package.xml @@ -22,6 +22,7 @@ moveit_planners_ompl moveit_ros_move_group moveit_ros_visualization + picknik_diff_drive_controller robot_state_publisher diff --git a/stretch_ros.repos b/stretch_ros.repos index d756e27c..3069da24 100644 --- a/stretch_ros.repos +++ b/stretch_ros.repos @@ -2,42 +2,15 @@ repositories: stretch_ros: type: git url: https://github.com/PickNikRobotics/stretch_ros - version: galactic_devel + version: ros2 + stretch_moveit_plugins: type: git url: https://github.com/PickNikRobotics/stretch_moveit_plugins version: main - moveit2: - type: git - url: https://github.com/DLu/moveit2 - version: dlu_galactic - - moveit_task_constructor: - type: git - url: https://github.com/DLu/moveit_task_constructor - version: galactic - - realsense-ros: # For realsense2_description - type: git - url: https://github.com/IntelRealSense/realsense-ros - version: foxy - - rosparam_shortcuts: # Used by stretch_moveit_plugins + # Includes chainable picknik_diff_drive_controller/DiffDriveController + picknik_controllers: type: git - url: https://github.com/PickNikRobotics/rosparam_shortcuts - version: ros2 - -# Moveit Dependencies not yet released on Galactic - geometric_shapes: - type: git - url: https://github.com/ros-planning/geometric_shapes - version: ros2 - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control - version: master - ros2_controllers: - type: git - url: https://github.com/ros-controls/ros2_controllers - version: master + url: https://github.com/PickNikRobotics/picknik_controllers + version: main