Chainable base controller support

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

stretch_moveit_config/config/gripper.ros2_control.xacro

@@ -5,7 +5,7 @@
 
         <ros2_control name="${name}" type="system">
             <hardware>
-                <plugin>fake_components/GenericSystem</plugin>
+                <plugin>mock_components/GenericSystem</plugin>
             </hardware>
             <joint name="joint_gripper_finger_left">
                 <param name="initial_position">0.0</param>
@@ -23,4 +23,4 @@
 
     </xacro:macro>
 
-</robot>
+</robot>

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

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

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

stretch_moveit_config/config/stretch_base.ros2_control.xacro

@@ -6,10 +6,37 @@
         <ros2_control name="${name}" type="system">
             <hardware>
                 <plugin>mock_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>

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

stretch_moveit_config/package.xml

@@ -22,6 +22,7 @@
   <run_depend>moveit_planners_ompl</run_depend>
   <run_depend>moveit_ros_move_group</run_depend>
   <run_depend>moveit_ros_visualization</run_depend>
+  <run_depend>picknik_diff_drive_controller</run_depend>
   <!-- TODO(JafarAbdi): uncomment once MSA is ported -->
   <!-- run_depend>moveit_setup_assistant</run_depend -->
   <run_depend>robot_state_publisher</run_depend>