diff --git a/.gitmodules b/.gitmodules index cf210e0..189f4a7 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,11 @@ [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 +[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 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_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/config/config.yaml b/src/stretch_re1_pro_config/config/config.yaml index 003d098..8ef4c70 100644 --- a/src/stretch_re1_pro_config/config/config.yaml +++ b/src/stretch_re1_pro_config/config/config.yaml @@ -106,12 +106,15 @@ 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" + 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" @@ -120,16 +123,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 @@ -142,14 +145,7 @@ moveit_params: allowed_execution_duration_scaling: 2.0 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" + control_multi_dof_joint_variables: True # Configuration for launching ros2_control processes. # [Required, if using ros2_control] @@ -163,11 +159,12 @@ ros2_control: # [Optional, default=[]] controllers_active_at_startup: - "joint_state_broadcaster" - - "servo_controller" + - "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" + controllers_inactive_at_startup: [] # 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_controller_manager.yaml b/src/stretch_re1_pro_config/config/moveit/moveit_simple_controller_manager.yaml new file mode 100644 index 0000000..26e2ca3 --- /dev/null +++ b/src/stretch_re1_pro_config/config/moveit/moveit_simple_controller_manager.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/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 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? 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 + + + + + + + + + + + + + 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..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 @@ -36,6 +36,12 @@ 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]) + # 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', + output='log') + + return LaunchDescription([static_tf, odom_to_mdof_joint_state_repub]) 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..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 @@ -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..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,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 @@ - - - - - - - - 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..ee2594e --- /dev/null +++ b/src/stretch_re1_pro_config/scripts/repub_odometry_mdof_joint_states.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +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): + """! 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, 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() + 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) + + # 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) + + odometry_repub.destroy_node() + + rclpy.shutdown() + +if __name__ == '__main__': + main() 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: [] diff --git a/src/stretch_ros b/src/stretch_ros index 364285a..58f033f 160000 --- a/src/stretch_ros +++ b/src/stretch_ros @@ -1 +1 @@ -Subproject commit 364285a51ef4ae237fe76e3615be77a7e75d7f51 +Subproject commit 58f033faf12d5983a42f8e0b56a77be7380d1b70