diff --git a/stretch_moveit_config/config/default_joint_limits.yaml b/stretch_moveit_config/config/default_joint_limits.yaml index 9367cbea..9056ef25 100644 --- a/stretch_moveit_config/config/default_joint_limits.yaml +++ b/stretch_moveit_config/config/default_joint_limits.yaml @@ -58,13 +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.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 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/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..0c8b5786 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: picknik_diff_drive_controller/DiffDriveController + stretch_controller: ros__parameters: command_interfaces: @@ -27,4 +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 353c5e2a..cf9a971f 100644 --- a/stretch_moveit_config/config/stretch_base.ros2_control.xacro +++ b/stretch_moveit_config/config/stretch_base.ros2_control.xacro @@ -6,10 +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 df526e5e..f0fe43aa 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 @@ -80,7 +81,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 +105,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') @@ -132,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, @@ -148,7 +134,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,17 +149,10 @@ 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: - 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', @@ -184,22 +163,50 @@ 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) + + trajectory_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["stretch_controller", "--controller-manager", "/controller_manager"], + ) + 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 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