Chainable diff drive support

.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

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()

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: []

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

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

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

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

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

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?

src/stretch_re1_pro_config/description/stretch_base.ros2_control.xacro

@@ -6,10 +6,37 @@
         <ros2_control name="${name}" type="system">
             <hardware>
                 <plugin>fake_components/GenericSystem</plugin>
+                <param name="calculate_dynamics">true</param>
             </hardware>
-            <joint name="position">
-                <!-- <param name="initial_position">0.0 0.0 0.0</param> -->
-                <command_interface name="position" />
+
+            <!-- Virtual position joint for supporting MoveIt's planar joint type in joint trajectories -->
+            <joint name="position/x">
+                <param name="initial_position">0.0</param>
+                <command_interface name="velocity" />
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="position/y">
+                <param name="initial_position">0.0</param>
+                <command_interface name="velocity" />
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="position/theta">
+                <param name="initial_position">0.0</param>
+                <command_interface name="velocity" />
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+            </joint>
+
+            <!-- Wheel joints used for diff drive controller support -->
+            <joint name="joint_left_wheel">
+                <command_interface name="velocity"/>
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_right_wheel">
+                <command_interface name="velocity"/>
                 <state_interface name="position"/>
                 <state_interface name="velocity"/>
             </joint>

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])