From ae4a066e20a8aa63255b9dea642ebc2c7f9c8bd7 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Mon, 18 Mar 2024 10:56:53 +0000 Subject: [PATCH 01/19] Pull in stretch_ros:base_controller_support --- .gitmodules | 1 + src/stretch_moveit_plugins | 2 +- src/stretch_ros | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index cf210e0..d9585a5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,7 @@ [submodule "src/stretch_ros"] path = src/stretch_ros url = https://github.com/PickNikRobotics/stretch_ros.git + branch=base_controller_support [submodule "src/stretch_moveit_plugins"] path = src/stretch_moveit_plugins url = https://github.com/PickNikRobotics/stretch_moveit_plugins.git diff --git a/src/stretch_moveit_plugins b/src/stretch_moveit_plugins index a99514b..fd18c35 160000 --- a/src/stretch_moveit_plugins +++ b/src/stretch_moveit_plugins @@ -1 +1 @@ -Subproject commit a99514b7ff6fd988dbfbfe204f5c8a450866c608 +Subproject commit fd18c352101e531d947e362b0b0b0987a77c9778 diff --git a/src/stretch_ros b/src/stretch_ros index 364285a..2f01c74 160000 --- a/src/stretch_ros +++ b/src/stretch_ros @@ -1 +1 @@ -Subproject commit 364285a51ef4ae237fe76e3615be77a7e75d7f51 +Subproject commit 2f01c74815b600b8dee43f8f0e1da262a73f8adc From ba474f5ca078e5f526ffb68c5cb0fd3eef5da96e Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Mon, 18 Mar 2024 11:44:33 +0000 Subject: [PATCH 02/19] Apply diff drive controller changes from MoveIt config --- src/stretch_re1_pro_config/config/config.yaml | 8 +- .../config/control/stretch.ros2_control.yaml | 96 ++++++++++++------- .../config/moveit/joint_limits.yaml | 13 ++- .../moveit/moveit_simple_controllers.yaml | 28 ++++++ .../stretch_base.ros2_control.xacro | 33 ++++++- 5 files changed, 136 insertions(+), 42 deletions(-) create mode 100644 src/stretch_re1_pro_config/config/moveit/moveit_simple_controllers.yaml diff --git a/src/stretch_re1_pro_config/config/config.yaml b/src/stretch_re1_pro_config/config/config.yaml index 003d098..c9a0b3d 100644 --- a/src/stretch_re1_pro_config/config/config.yaml +++ b/src/stretch_re1_pro_config/config/config.yaml @@ -47,9 +47,9 @@ hardware: # Specify any additional launch files for running the robot in simulation mode. # Used when hardware.simulated is True. # [Optional, defaults to a blank launch file if not specified] - simulated_robot_driver_persist_launch_file: - package: "stretch_re1_pro_config" - path: "launch/sim/robot_drivers_to_persist_sim.launch.py" + # simulated_robot_driver_persist_launch_file: + # package: "stretch_re1_pro_config" + # path: "launch/sim/robot_drivers_to_persist_sim.launch.py" # simulated_hardware_launch_file: # package: "moveit_studio_agent" # path: "launch/blank.launch.py" @@ -163,11 +163,13 @@ ros2_control: # [Optional, default=[]] controllers_active_at_startup: - "joint_state_broadcaster" + - "diff_drive_controller" - "servo_controller" # Load but do not start these controllers so they can be activated later if needed. # [Optional, default=[]] controllers_inactive_at_startup: - "stretch_controller" + - "stretch_base_controller" # Any controllers here will not be spawned by MoveIt Pro. # [Optional, default=[]] controllers_not_managed: [] diff --git a/src/stretch_re1_pro_config/config/control/stretch.ros2_control.yaml b/src/stretch_re1_pro_config/config/control/stretch.ros2_control.yaml index 07b9e31..2f86002 100644 --- a/src/stretch_re1_pro_config/config/control/stretch.ros2_control.yaml +++ b/src/stretch_re1_pro_config/config/control/stretch.ros2_control.yaml @@ -11,6 +11,12 @@ controller_manager: servo_controller: type: joint_trajectory_controller/JointTrajectoryController + stretch_base_controller: + type: joint_trajectory_controller/JointTrajectoryController + + diff_drive_controller: + type: diff_drive_controller/DiffDriveController + stretch_controller: ros__parameters: command_interfaces: @@ -30,44 +36,66 @@ stretch_controller: - joint_head_tilt - joint_gripper_finger_left - joint_gripper_finger_right - - position -servo_controller: +stretch_base_controller: ros__parameters: - joints: - - joint_arm_l0 - - joint_arm_l1 - - joint_arm_l2 - - joint_arm_l3 - - joint_lift - - joint_wrist_yaw command_interfaces: - - position + - 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: - - joint_arm_l0 - - joint_arm_l1 - - joint_arm_l2 - - joint_arm_l3 - - joint_lift - - joint_wrist_yaw - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - allow_partial_joints_goal: false - constraints: - stopped_velocity_tolerance: 0.0 - goal_time: 0.0 - joint_arm_l0: - goal: 0.05 - joint_arm_l1: - goal: 0.05 - joint_arm_l2: - goal: 0.05 - joint_arm_l3: - goal: 0.05 - joint_lift: - goal: 0.05 - joint_wrist_yaw: - goal: 0.05 + - 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/src/stretch_re1_pro_config/config/moveit/joint_limits.yaml b/src/stretch_re1_pro_config/config/moveit/joint_limits.yaml index 9367cbe..9056ef2 100644 --- a/src/stretch_re1_pro_config/config/moveit/joint_limits.yaml +++ b/src/stretch_re1_pro_config/config/moveit/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/src/stretch_re1_pro_config/config/moveit/moveit_simple_controllers.yaml b/src/stretch_re1_pro_config/config/moveit/moveit_simple_controllers.yaml new file mode 100644 index 0000000..26e2ca3 --- /dev/null +++ b/src/stretch_re1_pro_config/config/moveit/moveit_simple_controllers.yaml @@ -0,0 +1,28 @@ +controller_names: + - stretch_controller + - stretch_base_controller + +stretch_controller: + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint_lift + - joint_arm_l3 + - joint_arm_l2 + - joint_arm_l1 + - joint_arm_l0 + - joint_wrist_yaw + - joint_head_pan + - joint_head_tilt + - joint_gripper_finger_left + - joint_gripper_finger_right + +stretch_base_controller: + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - position/x + - position/y + - position/theta diff --git a/src/stretch_re1_pro_config/description/stretch_base.ros2_control.xacro b/src/stretch_re1_pro_config/description/stretch_base.ros2_control.xacro index 1de5d18..57c37ec 100644 --- a/src/stretch_re1_pro_config/description/stretch_base.ros2_control.xacro +++ b/src/stretch_re1_pro_config/description/stretch_base.ros2_control.xacro @@ -6,10 +6,37 @@ fake_components/GenericSystem + true - - - + + + + 0.0 + + + + + + 0.0 + + + + + + 0.0 + + + + + + + + + + + + + From 4a4fa479a0aa5a03c00c533e7b0aa13ad57aef1e Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Mon, 18 Mar 2024 12:46:45 +0000 Subject: [PATCH 03/19] Pull in DiffDriveController --- .gitmodules | 6 +++++- src/ros2_controllers | 1 + 2 files changed, 6 insertions(+), 1 deletion(-) create mode 160000 src/ros2_controllers diff --git a/.gitmodules b/.gitmodules index d9585a5..189f4a7 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,7 +1,11 @@ [submodule "src/stretch_ros"] path = src/stretch_ros url = https://github.com/PickNikRobotics/stretch_ros.git - branch=base_controller_support + branch = base_controller_support [submodule "src/stretch_moveit_plugins"] path = src/stretch_moveit_plugins url = https://github.com/PickNikRobotics/stretch_moveit_plugins.git +[submodule "src/ros2_controllers"] + path = src/ros2_controllers + url = https://github.com/henningkayser/ros2_controllers + branch = chainable-jtc-diff-drive diff --git a/src/ros2_controllers b/src/ros2_controllers new file mode 160000 index 0000000..bcd2205 --- /dev/null +++ b/src/ros2_controllers @@ -0,0 +1 @@ +Subproject commit bcd220543068f0c9a90923ac21f035cc7a4b8c95 From 991cad4ceab3acefb101882d3ed4bbd03341f235 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 19 Mar 2024 15:47:51 +0000 Subject: [PATCH 04/19] Publish static tf world->odom --- src/stretch_re1_pro_config/config/config.yaml | 6 +++--- .../launch/sim/robot_drivers_to_persist_sim.launch.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/stretch_re1_pro_config/config/config.yaml b/src/stretch_re1_pro_config/config/config.yaml index c9a0b3d..e8a610c 100644 --- a/src/stretch_re1_pro_config/config/config.yaml +++ b/src/stretch_re1_pro_config/config/config.yaml @@ -47,9 +47,9 @@ hardware: # Specify any additional launch files for running the robot in simulation mode. # Used when hardware.simulated is True. # [Optional, defaults to a blank launch file if not specified] - # simulated_robot_driver_persist_launch_file: - # package: "stretch_re1_pro_config" - # path: "launch/sim/robot_drivers_to_persist_sim.launch.py" + simulated_robot_driver_persist_launch_file: + package: "stretch_re1_pro_config" + path: "launch/sim/robot_drivers_to_persist_sim.launch.py" # simulated_hardware_launch_file: # package: "moveit_studio_agent" # path: "launch/blank.launch.py" diff --git a/src/stretch_re1_pro_config/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/stretch_re1_pro_config/launch/sim/robot_drivers_to_persist_sim.launch.py index 03aefb2..a55173a 100644 --- a/src/stretch_re1_pro_config/launch/sim/robot_drivers_to_persist_sim.launch.py +++ b/src/stretch_re1_pro_config/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -36,6 +36,6 @@ def generate_launch_description(): 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']) + arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'odom']) return LaunchDescription([static_tf]) From c5cd8b0fb2594701a80ac3882950c424cfc2f687 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 19 Mar 2024 15:48:39 +0000 Subject: [PATCH 05/19] Configure PILZ planner --- src/stretch_re1_pro_config/config/config.yaml | 14 +++++++------- .../config/moveit/pilz_cartesian_limits.yaml | 6 ++++++ .../pilz_industrial_motion_planner_planning.yaml | 14 ++++++++++++++ 3 files changed, 27 insertions(+), 7 deletions(-) create mode 100644 src/stretch_re1_pro_config/config/moveit/pilz_cartesian_limits.yaml create mode 100644 src/stretch_re1_pro_config/config/moveit/pilz_industrial_motion_planner_planning.yaml diff --git a/src/stretch_re1_pro_config/config/config.yaml b/src/stretch_re1_pro_config/config/config.yaml index e8a610c..744cf36 100644 --- a/src/stretch_re1_pro_config/config/config.yaml +++ b/src/stretch_re1_pro_config/config/config.yaml @@ -106,9 +106,9 @@ moveit_params: stomp_planning: package: "stretch_re1_pro_config" path: "config/moveit/stomp_planning.yaml" - # pilz_planning: - # package: "stretch_re1_pro_config" - # path: "config/moveit/pilz_industrial_motion_planner_planning.yaml" + pilz_planning: + package: "stretch_re1_pro_config" + path: "config/moveit/pilz_industrial_motion_planner_planning.yaml" kinematics: package: "stretch_re1_pro_config" path: "config/moveit/kinematics.yaml" @@ -120,16 +120,16 @@ moveit_params: path: "config/moveit/sensors_3d.yaml" servo_kinematics: package: "stretch_re1_pro_config" - path: "config/moveit/trac_ik_kinematics_speed.yaml" + path: "config/moveit/kinematics.yaml" joint_limits: package: "stretch_re1_pro_config" path: "config/moveit/joint_limits.yaml" servo_joint_limits: package: "stretch_re1_pro_config" path: "config/moveit/joint_limits.yaml" - # pilz_cartesian_limits: - # package: "stretch_re1_pro_config" - # path: "config/moveit/pilz_cartesian_limits.yaml" + pilz_cartesian_limits: + package: "stretch_re1_pro_config" + path: "config/moveit/pilz_cartesian_limits.yaml" publish: planning_scene: True diff --git a/src/stretch_re1_pro_config/config/moveit/pilz_cartesian_limits.yaml b/src/stretch_re1_pro_config/config/moveit/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..1633938 --- /dev/null +++ b/src/stretch_re1_pro_config/config/moveit/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Cartesian limits for the Pilz planner +cartesian_limits: + max_trans_vel: 0.1 + max_trans_acc: 0.1 + max_trans_dec: -0.1 + max_rot_vel: 0.1 diff --git a/src/stretch_re1_pro_config/config/moveit/pilz_industrial_motion_planner_planning.yaml b/src/stretch_re1_pro_config/config/moveit/pilz_industrial_motion_planner_planning.yaml new file mode 100644 index 0000000..6d79138 --- /dev/null +++ b/src/stretch_re1_pro_config/config/moveit/pilz_industrial_motion_planner_planning.yaml @@ -0,0 +1,14 @@ +planning_plugins: + - pilz_industrial_motion_planner/CommandPlanner +default_planner_config: LIN +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/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath +capabilities: >- + pilz_industrial_motion_planner/MoveGroupSequenceAction + pilz_industrial_motion_planner/MoveGroupSequenceService From 1c03fe0903e2f96f22baffb827798e16011dd16f Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 19 Mar 2024 15:49:03 +0000 Subject: [PATCH 06/19] Remove redundant UI grasp link --- src/stretch_re1_pro_config/config/config.yaml | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/stretch_re1_pro_config/config/config.yaml b/src/stretch_re1_pro_config/config/config.yaml index 744cf36..0825b5c 100644 --- a/src/stretch_re1_pro_config/config/config.yaml +++ b/src/stretch_re1_pro_config/config/config.yaml @@ -143,14 +143,6 @@ moveit_params: allowed_goal_duration_margin: 5.0 allowed_start_tolerance: 0.01 -# Additional configurable parameters for the MoveIt Pro user interface. -# TODO - Find the right frame -# [Required] -ui_params: - # By default, we use a frame named "grasp_link" for tool grasp pose rendering and planning. - # [Required] - servo_endpoint_frame_id: "grasp_link" - # Configuration for launching ros2_control processes. # [Required, if using ros2_control] ros2_control: From 95ad6df8af387e3d24a79b136a2b20d28c1eeb57 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 19 Mar 2024 15:49:45 +0000 Subject: [PATCH 07/19] Configure Servo --- src/stretch_re1_pro_config/config/config.yaml | 1 - src/stretch_re1_pro_config/config/moveit/stretch_servo.yaml | 4 ++-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/stretch_re1_pro_config/config/config.yaml b/src/stretch_re1_pro_config/config/config.yaml index 0825b5c..d8404ad 100644 --- a/src/stretch_re1_pro_config/config/config.yaml +++ b/src/stretch_re1_pro_config/config/config.yaml @@ -156,7 +156,6 @@ ros2_control: controllers_active_at_startup: - "joint_state_broadcaster" - "diff_drive_controller" - - "servo_controller" # Load but do not start these controllers so they can be activated later if needed. # [Optional, default=[]] controllers_inactive_at_startup: diff --git a/src/stretch_re1_pro_config/config/moveit/stretch_servo.yaml b/src/stretch_re1_pro_config/config/moveit/stretch_servo.yaml index b392183..cd43ec0 100644 --- a/src/stretch_re1_pro_config/config/moveit/stretch_servo.yaml +++ b/src/stretch_re1_pro_config/config/moveit/stretch_servo.yaml @@ -27,7 +27,7 @@ command_out_type: trajectory_msgs/JointTrajectory # What to publish? Can save some bandwidth as most robots only require positions or velocities publish_joint_positions: true -publish_joint_velocities: true +publish_joint_velocities: false publish_joint_accelerations: false ## Plugins for smoothing outgoing commands @@ -53,7 +53,7 @@ cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian t joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /joint_states status_topic: ~/status # Publish status to this topic -command_out_topic: /servo_controller/joint_trajectory # Publish outgoing commands here +command_out_topic: /stretch_controller/joint_trajectory # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? From 26349118edf6ff9bc0f105cb2299972758563d79 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 19 Mar 2024 15:50:02 +0000 Subject: [PATCH 08/19] Update controllers, group names in objectives --- .../objectives/move_to_joint_state.xml | 36 +++++++++---------- .../objectives/move_to_pose.xml | 4 +-- .../objectives/request_teleoperation.xml | 4 +-- .../objectives/teleoperate.xml | 4 +-- .../objectives/teleoperate_joint_jog.xml | 8 ----- .../objectives/teleoperate_twist.xml | 8 ----- 6 files changed, 24 insertions(+), 40 deletions(-) delete mode 100644 src/stretch_re1_pro_config/objectives/teleoperate_joint_jog.xml delete mode 100644 src/stretch_re1_pro_config/objectives/teleoperate_twist.xml diff --git a/src/stretch_re1_pro_config/objectives/move_to_joint_state.xml b/src/stretch_re1_pro_config/objectives/move_to_joint_state.xml index 24d2ca2..fdc4ff6 100644 --- a/src/stretch_re1_pro_config/objectives/move_to_joint_state.xml +++ b/src/stretch_re1_pro_config/objectives/move_to_joint_state.xml @@ -1,20 +1,20 @@ - + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + diff --git a/src/stretch_re1_pro_config/objectives/move_to_pose.xml b/src/stretch_re1_pro_config/objectives/move_to_pose.xml index dd8f561..ce5793b 100644 --- a/src/stretch_re1_pro_config/objectives/move_to_pose.xml +++ b/src/stretch_re1_pro_config/objectives/move_to_pose.xml @@ -4,9 +4,9 @@ - + - + diff --git a/src/stretch_re1_pro_config/objectives/request_teleoperation.xml b/src/stretch_re1_pro_config/objectives/request_teleoperation.xml index bf3d5b7..2a81906 100644 --- a/src/stretch_re1_pro_config/objectives/request_teleoperation.xml +++ b/src/stretch_re1_pro_config/objectives/request_teleoperation.xml @@ -28,8 +28,8 @@ - - + + diff --git a/src/stretch_re1_pro_config/objectives/teleoperate.xml b/src/stretch_re1_pro_config/objectives/teleoperate.xml index 1ac62b7..d3c8b30 100644 --- a/src/stretch_re1_pro_config/objectives/teleoperate.xml +++ b/src/stretch_re1_pro_config/objectives/teleoperate.xml @@ -1,7 +1,7 @@ - + - + diff --git a/src/stretch_re1_pro_config/objectives/teleoperate_joint_jog.xml b/src/stretch_re1_pro_config/objectives/teleoperate_joint_jog.xml deleted file mode 100644 index 641eb94..0000000 --- a/src/stretch_re1_pro_config/objectives/teleoperate_joint_jog.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/src/stretch_re1_pro_config/objectives/teleoperate_twist.xml b/src/stretch_re1_pro_config/objectives/teleoperate_twist.xml deleted file mode 100644 index e2a0267..0000000 --- a/src/stretch_re1_pro_config/objectives/teleoperate_twist.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - From 2ee3aa40ba19742505a94ea5af5673523ab0aa7b Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 19 Mar 2024 15:50:21 +0000 Subject: [PATCH 09/19] Add mobile base waypoints --- .../waypoints/stretch_waypoints.yaml | 76 ++++++++++++++++++- 1 file changed, 73 insertions(+), 3 deletions(-) diff --git a/src/stretch_re1_pro_config/waypoints/stretch_waypoints.yaml b/src/stretch_re1_pro_config/waypoints/stretch_waypoints.yaml index 80ed05e..0643422 100644 --- a/src/stretch_re1_pro_config/waypoints/stretch_waypoints.yaml +++ b/src/stretch_re1_pro_config/waypoints/stretch_waypoints.yaml @@ -26,7 +26,7 @@ position: [0.1033, 0.13, 0.13, 0.13, 0.13, 4] velocity: [] effort: [] -- name: Extended +- name: ExtendedFullGroup favorite: true joint_group_names: - mobile_base_arm @@ -36,7 +36,77 @@ stamp: sec: 0 nanosec: 0 - name: [joint_lift, joint_arm_l0, joint_arm_l1, joint_arm_l2, joint_arm_l3, joint_wrist_yaw, joint_head_pan, joint_head_tilt, joint_gripper_finger_left, joint_gripper_finger_right, position] - position: [0.1033, 0.13, 0.13, 0.13, 0.13, 4, 0, 0, 0, 0, 0] + name: [joint_lift, joint_arm_l0, joint_arm_l1, joint_arm_l2, joint_arm_l3, joint_wrist_yaw, joint_head_pan, joint_head_tilt, joint_gripper_finger_left, joint_gripper_finger_right, position/x, position/y, position/theta] + position: [0.1033, 0.13, 0.13, 0.13, 0.13, 4, 0, 0, 0, 0, 0.1, 0, 0] + velocity: [] + effort: [] +- name: PositionForward10CM + favorite: true + joint_group_names: + - position + joint_state: + header: + frame_id: world + stamp: + sec: 0 + nanosec: 0 + name: [position/x, position/y, position/theta] + position: [0.1, 0, 0] + velocity: [] + effort: [] +- name: PositionBackwards10CM + favorite: true + joint_group_names: + - position + joint_state: + header: + frame_id: world + stamp: + sec: 0 + nanosec: 0 + name: [position/x, position/y, position/theta] + position: [-0.1, 0, 0] + velocity: [] + effort: [] +- name: PositionLeft45Degrees + favorite: true + joint_group_names: + - position + joint_state: + header: + frame_id: world + stamp: + sec: 0 + nanosec: 0 + name: [position/x, position/y, position/theta] + position: [0, 0, .785] + velocity: [] + effort: [] +- name: PositionRight45Degrees + favorite: true + joint_group_names: + - position + joint_state: + header: + frame_id: world + stamp: + sec: 0 + nanosec: 0 + name: [position/x, position/y, position/theta] + position: [0, 0, -0.785] + velocity: [] + effort: [] +- name: PositionHome + favorite: true + joint_group_names: + - position + joint_state: + header: + frame_id: world + stamp: + sec: 0 + nanosec: 0 + name: [position/x, position/y, position/theta] + position: [0, 0, 0] velocity: [] effort: [] From f86a36f3c0e3ec06bd66be57151fdd4149c06a22 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 19 Mar 2024 15:53:03 +0000 Subject: [PATCH 10/19] Commented config for MoveItSimpleControllerManager --- src/stretch_re1_pro_config/config/config.yaml | 4 ++++ ...controllers.yaml => moveit_simple_controller_manager.yaml} | 0 2 files changed, 4 insertions(+) rename src/stretch_re1_pro_config/config/moveit/{moveit_simple_controllers.yaml => moveit_simple_controller_manager.yaml} (100%) diff --git a/src/stretch_re1_pro_config/config/config.yaml b/src/stretch_re1_pro_config/config/config.yaml index d8404ad..ac93f01 100644 --- a/src/stretch_re1_pro_config/config/config.yaml +++ b/src/stretch_re1_pro_config/config/config.yaml @@ -112,6 +112,10 @@ moveit_params: kinematics: package: "stretch_re1_pro_config" path: "config/moveit/kinematics.yaml" + # NOTE: This is not supported yet + # moveit_simple_controller_manager: + # package: "stretch_re1_pro_config" + # path: "config/moveit/moveit_simple_controller_manager.yaml" servo: package: "stretch_re1_pro_config" path: "config/moveit/stretch_servo.yaml" diff --git a/src/stretch_re1_pro_config/config/moveit/moveit_simple_controllers.yaml b/src/stretch_re1_pro_config/config/moveit/moveit_simple_controller_manager.yaml similarity index 100% rename from src/stretch_re1_pro_config/config/moveit/moveit_simple_controllers.yaml rename to src/stretch_re1_pro_config/config/moveit/moveit_simple_controller_manager.yaml From 59afc9968b4dba692be2f42e8e3aec1723b335b5 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 19 Mar 2024 15:54:32 +0000 Subject: [PATCH 11/19] Update stretch_ros submodule --- src/stretch_ros | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/stretch_ros b/src/stretch_ros index 2f01c74..cfc3e56 160000 --- a/src/stretch_ros +++ b/src/stretch_ros @@ -1 +1 @@ -Subproject commit 2f01c74815b600b8dee43f8f0e1da262a73f8adc +Subproject commit cfc3e5645391de7a9f3f5afa40366a65820255c3 From 813b469c21edb0e51c9af868e242f4900da01962 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 21 Mar 2024 22:46:15 +0000 Subject: [PATCH 12/19] Add Odometry to MultiDOFJointState republisher --- src/stretch_re1_pro_config/CMakeLists.txt | 5 +++ .../robot_drivers_to_persist_sim.launch.py | 8 +++- src/stretch_re1_pro_config/package.xml | 5 ++- .../repub_odometry_mdof_joint_states.py | 41 +++++++++++++++++++ 4 files changed, 56 insertions(+), 3 deletions(-) create mode 100755 src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py diff --git a/src/stretch_re1_pro_config/CMakeLists.txt b/src/stretch_re1_pro_config/CMakeLists.txt index a6d31a1..fcfdbf5 100644 --- a/src/stretch_re1_pro_config/CMakeLists.txt +++ b/src/stretch_re1_pro_config/CMakeLists.txt @@ -14,6 +14,11 @@ install( share/${PROJECT_NAME} ) +install(PROGRAMS + scripts/repub_odometry_mdof_joint_states.py + DESTINATION lib/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/src/stretch_re1_pro_config/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/stretch_re1_pro_config/launch/sim/robot_drivers_to_persist_sim.launch.py index a55173a..d062009 100644 --- a/src/stretch_re1_pro_config/launch/sim/robot_drivers_to_persist_sim.launch.py +++ b/src/stretch_re1_pro_config/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -38,4 +38,10 @@ def generate_launch_description(): output='log', arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'odom']) - return LaunchDescription([static_tf]) + # TODO(henningkayser): make mdof_joint_state_publisher a proper MoveGroup capability, or ros2_controller? + odom_to_mdof_joint_state_repub = Node(package='stretch_re1_pro_config', + executable='repub_odometry_mdof_joint_states.py', + name='odom_to_mdof_joint_state_repub', + output='log') + + return LaunchDescription([static_tf, odom_to_mdof_joint_state_repub]) diff --git a/src/stretch_re1_pro_config/package.xml b/src/stretch_re1_pro_config/package.xml index dd9b7f8..627ff03 100644 --- a/src/stretch_re1_pro_config/package.xml +++ b/src/stretch_re1_pro_config/package.xml @@ -5,17 +5,18 @@ Base configuration package for Stretch RE1 - MoveIt Pro Maintainer + Henning Kayser BSD-3-Clause ament_cmake + geometry_msgs moveit_planners_stomp moveit_ros_perception moveit_studio_agent moveit_studio_behavior - trac_ik_kinematics_plugin + rclpy stretch_description ament_lint_auto diff --git a/src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py b/src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py new file mode 100755 index 0000000..4df3752 --- /dev/null +++ b/src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py @@ -0,0 +1,41 @@ +import rclpy +from rclpy.node import Node + +from nav_msgs.msg import Odometry +from sensor_msgs.msg import MultiDOFJointState + +import tf2_geometry_msgs +from geometry_msgs.msg import Transform, Pose, Vector3, Wrench + +class OdometryMultiDOFJointStateRepublisher(Node): + def __init__(self): + super().__init__('odometry_mdof_state_republisher') + self.odom_sub_ = self.create_subscription(Odometry, '/diff_drive_controller/odom', self.odom_callback, 1) + self.mdof_state_pub_ = self.create_publisher(MultiDOFJointState, '/multi_dof_joint_states', 1) + + def odom_callback(self, odom_msg): + mdof_state_msg = MultiDOFJointState() + mdof_state_msg.joint_names = ["position"] + transform = Transform() + transform.translation.x = odom_msg.pose.pose.position.x + transform.translation.y = odom_msg.pose.pose.position.y + transform.translation.z = odom_msg.pose.pose.position.z + transform.rotation = odom_msg.pose.pose.orientation + mdof_state_msg.transforms = [transform] + mdof_state_msg.twist = [odom_msg.twist.twist] + mdof_state_msg.wrench = [Wrench()] + self.mdof_state_pub_.publish(mdof_state_msg) + +def main(args=None): + rclpy.init(args=args) + + odometry_repub = OdometryMultiDOFJointStateRepublisher() + + rclpy.spin(odometry_repub) + + odometry_repub.destroy_node() + + rclpy.shutdown() + +if __name__ == '__main__': + main() From f81eaa80afe66d48b10703051300158e848cccba Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 22 Mar 2024 14:46:48 +0000 Subject: [PATCH 13/19] Configure MoveItSimpleControllerManager --- src/stretch_re1_pro_config/config/config.yaml | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/stretch_re1_pro_config/config/config.yaml b/src/stretch_re1_pro_config/config/config.yaml index ac93f01..5c7e871 100644 --- a/src/stretch_re1_pro_config/config/config.yaml +++ b/src/stretch_re1_pro_config/config/config.yaml @@ -112,10 +112,9 @@ moveit_params: kinematics: package: "stretch_re1_pro_config" path: "config/moveit/kinematics.yaml" - # NOTE: This is not supported yet - # moveit_simple_controller_manager: - # package: "stretch_re1_pro_config" - # path: "config/moveit/moveit_simple_controller_manager.yaml" + moveit_simple_controller_manager: + package: "stretch_re1_pro_config" + path: "config/moveit/moveit_simple_controller_manager.yaml" servo: package: "stretch_re1_pro_config" path: "config/moveit/stretch_servo.yaml" From 8168771b04aa0fae7a044edbdf5fbb73c32aab10 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 22 Mar 2024 16:02:02 +0000 Subject: [PATCH 14/19] Configure trajectory_execution.control_multi_dof_joint_variables --- src/stretch_re1_pro_config/config/config.yaml | 1 + src/stretch_ros | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/stretch_re1_pro_config/config/config.yaml b/src/stretch_re1_pro_config/config/config.yaml index 5c7e871..b0dcbd6 100644 --- a/src/stretch_re1_pro_config/config/config.yaml +++ b/src/stretch_re1_pro_config/config/config.yaml @@ -145,6 +145,7 @@ moveit_params: allowed_execution_duration_scaling: 2.0 allowed_goal_duration_margin: 5.0 allowed_start_tolerance: 0.01 + control_multi_dof_joint_variables: True # Configuration for launching ros2_control processes. # [Required, if using ros2_control] diff --git a/src/stretch_ros b/src/stretch_ros index cfc3e56..58f033f 160000 --- a/src/stretch_ros +++ b/src/stretch_ros @@ -1 +1 @@ -Subproject commit cfc3e5645391de7a9f3f5afa40366a65820255c3 +Subproject commit 58f033faf12d5983a42f8e0b56a77be7380d1b70 From c2764574a63e9787ed018d397fb686e905c3e126 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 22 Mar 2024 16:23:18 +0000 Subject: [PATCH 15/19] Fix controller names in objectives --- src/stretch_re1_pro_config/objectives/move_to_joint_state.xml | 2 +- src/stretch_re1_pro_config/objectives/move_to_pose.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/stretch_re1_pro_config/objectives/move_to_joint_state.xml b/src/stretch_re1_pro_config/objectives/move_to_joint_state.xml index fdc4ff6..d13a25c 100644 --- a/src/stretch_re1_pro_config/objectives/move_to_joint_state.xml +++ b/src/stretch_re1_pro_config/objectives/move_to_joint_state.xml @@ -4,7 +4,7 @@ - + diff --git a/src/stretch_re1_pro_config/objectives/move_to_pose.xml b/src/stretch_re1_pro_config/objectives/move_to_pose.xml index ce5793b..89aad14 100644 --- a/src/stretch_re1_pro_config/objectives/move_to_pose.xml +++ b/src/stretch_re1_pro_config/objectives/move_to_pose.xml @@ -4,7 +4,7 @@ - + From a2131089b369c7f0963591bea02032ef1c89c74f Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 22 Mar 2024 16:27:05 +0000 Subject: [PATCH 16/19] Activate all controllers by default --- src/stretch_re1_pro_config/config/config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/stretch_re1_pro_config/config/config.yaml b/src/stretch_re1_pro_config/config/config.yaml index b0dcbd6..8ef4c70 100644 --- a/src/stretch_re1_pro_config/config/config.yaml +++ b/src/stretch_re1_pro_config/config/config.yaml @@ -160,11 +160,11 @@ ros2_control: controllers_active_at_startup: - "joint_state_broadcaster" - "diff_drive_controller" + - "stretch_base_controller" + - "stretch_controller" # Load but do not start these controllers so they can be activated later if needed. # [Optional, default=[]] - controllers_inactive_at_startup: - - "stretch_controller" - - "stretch_base_controller" + controllers_inactive_at_startup: [] # Any controllers here will not be spawned by MoveIt Pro. # [Optional, default=[]] controllers_not_managed: [] From dfea888e4f72495ea9f3fc465078bd57d1b36f53 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 22 Mar 2024 16:39:24 +0000 Subject: [PATCH 17/19] Fix odometry repub executable --- .../scripts/repub_odometry_mdof_joint_states.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py b/src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py index 4df3752..364b9f0 100755 --- a/src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py +++ b/src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 import rclpy from rclpy.node import Node From 1d0c6a4684e7a18f220ad25fef6e588ff9fcfcfe Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 4 Apr 2024 13:43:42 +0000 Subject: [PATCH 18/19] Reference issue for future maintenance of mdof state publisher --- .../launch/sim/robot_drivers_to_persist_sim.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/stretch_re1_pro_config/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/stretch_re1_pro_config/launch/sim/robot_drivers_to_persist_sim.launch.py index d062009..7b9d5e0 100644 --- a/src/stretch_re1_pro_config/launch/sim/robot_drivers_to_persist_sim.launch.py +++ b/src/stretch_re1_pro_config/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): output='log', arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'odom']) - # TODO(henningkayser): make mdof_joint_state_publisher a proper MoveGroup capability, or ros2_controller? + # TODO(moveit_studio#7004): make mdof_joint_state_publisher a proper MoveGroup capability, or ros2_controller? odom_to_mdof_joint_state_repub = Node(package='stretch_re1_pro_config', executable='repub_odometry_mdof_joint_states.py', name='odom_to_mdof_joint_state_repub', From 15a6a4f0471f51fd02aac249aca6c2849b2e3bf0 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 4 Apr 2024 13:58:56 +0000 Subject: [PATCH 19/19] Document Odometry republisher --- .../repub_odometry_mdof_joint_states.py | 29 ++++++++++++++++--- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py b/src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py index 364b9f0..ee2594e 100755 --- a/src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py +++ b/src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py @@ -9,12 +9,32 @@ from geometry_msgs.msg import Transform, Pose, Vector3, Wrench class OdometryMultiDOFJointStateRepublisher(Node): - def __init__(self): + """! The Odometry to MultiDOFJointState republisher node. + + Subscribes to Odometry messages, copies the data into a MultiDOFJointState, + which are published to '/multi_dof_joint_states'. + """ + + def __init__(self, odom_topic, mdof_topic): + """! Initialize Odometry subscriber and MultiDOFJointState publisher. + + @param odom_topic topic name to use for Odometry subscription + @param mdof_topic topic name to use for MultiDOFJointState publishing + + @return Instance of the OdometryMultiDOFJointStateRepublisher node + """ + super().__init__('odometry_mdof_state_republisher') - self.odom_sub_ = self.create_subscription(Odometry, '/diff_drive_controller/odom', self.odom_callback, 1) - self.mdof_state_pub_ = self.create_publisher(MultiDOFJointState, '/multi_dof_joint_states', 1) + self.odom_sub_ = self.create_subscription(Odometry, odom_topic, self.odom_callback, 1) + self.mdof_state_pub_ = self.create_publisher(MultiDOFJointState, mdof_topic, 1) def odom_callback(self, odom_msg): + """! Subscription callback to run for incoming Odometry messages + + Each Odometry message is copied into a MultiDOFJointState message and republished + to a different topic. + + """ mdof_state_msg = MultiDOFJointState() mdof_state_msg.joint_names = ["position"] transform = Transform() @@ -30,7 +50,8 @@ def odom_callback(self, odom_msg): def main(args=None): rclpy.init(args=args) - odometry_repub = OdometryMultiDOFJointStateRepublisher() + # TODO(moveit_studio#7004) Expose topic names and joint name to launch config + odometry_repub = OdometryMultiDOFJointStateRepublisher('/diff_drive_controller/odom', '/multi_dof_joint_states') rclpy.spin(odometry_repub)