Forward trajectory controller (backport of #944)

* Add trajectory passthrough controller (#944)

This adds a controller that allows sending a complete trajectory to the robot for execution.

---

Co-authored-by: Felix Exner <[email protected]>

ur_bringup/config/ur_controllers.yaml

@@ -24,35 +24,51 @@ controller_manager:
     forward_position_controller:
       type: position_controllers/JointGroupPositionController
 
+    tcp_pose_broadcaster:
+      type: pose_broadcaster/PoseBroadcaster
+
+    passthrough_trajectory_controller:
+      type: ur_controllers/PassthroughTrajectoryController
+
+    ur_configuration_controller:
+      type: ur_controllers/URConfigurationController
 
 speed_scaling_state_broadcaster:
   ros__parameters:
     state_publish_rate: 100.0
+    tf_prefix: "$(var tf_prefix)"
+
+io_and_status_controller:
+  ros__parameters:
+    tf_prefix: "$(var tf_prefix)"
 
+ur_configuration_controller:
+  ros__parameters:
+    tf_prefix: "$(var tf_prefix)"
 
 force_torque_sensor_broadcaster:
   ros__parameters:
-    sensor_name: tcp_fts_sensor
+    sensor_name: $(var tf_prefix)tcp_fts_sensor
     state_interface_names:
       - force.x
       - force.y
       - force.z
       - torque.x
       - torque.y
       - torque.z
-    frame_id: tool0
+    frame_id: $(var tf_prefix)tool0
     topic_name: ft_data
 
 
 joint_trajectory_controller:
   ros__parameters:
     joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
     command_interfaces:
       - position
     state_interfaces:
@@ -64,23 +80,23 @@ joint_trajectory_controller:
     constraints:
       stopped_velocity_tolerance: 0.2
       goal_time: 0.0
-      shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
-      shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
-      elbow_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
 
 
 scaled_joint_trajectory_controller:
   ros__parameters:
     joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
     command_interfaces:
       - position
     state_interfaces:
@@ -92,30 +108,53 @@ scaled_joint_trajectory_controller:
     constraints:
       stopped_velocity_tolerance: 0.2
       goal_time: 0.0
-      shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
-      shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
-      elbow_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+    speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor
+
+passthrough_trajectory_controller:
+  ros__parameters:
+    tf_prefix: "$(var tf_prefix)"
+    joints:
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
+    state_interfaces:
+      - position
+      - velocity
+    speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor
 
 forward_velocity_controller:
   ros__parameters:
     joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
     interface_name: velocity
 
 forward_position_controller:
   ros__parameters:
     joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
+
+tcp_pose_broadcaster:
+  ros__parameters:
+    frame_id: $(var tf_prefix)base
+    pose_name: $(var tf_prefix)tcp_pose
+    tf:
+      child_frame_id: $(var tf_prefix)tool0_controller

ur_controllers/CMakeLists.txt

@@ -19,6 +19,10 @@ find_package(std_srvs REQUIRED)
 find_package(ur_dashboard_msgs REQUIRED)
 find_package(ur_msgs REQUIRED)
 find_package(generate_parameter_library REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(action_msgs REQUIRED)
+
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   angles
@@ -34,6 +38,9 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   ur_dashboard_msgs
   ur_msgs
   generate_parameter_library
+  control_msgs
+  trajectory_msgs
+  action_msgs
 )
 
 include_directories(include)
@@ -54,6 +61,11 @@ generate_parameter_library(
   src/scaled_joint_trajectory_controller_parameters.yaml
 )
 
+generate_parameter_library(
+  passthrough_trajectory_controller_parameters
+  src/passthrough_trajectory_controller_parameters.yaml
+)
+
 generate_parameter_library(
   ur_configuration_controller_parameters
   src/ur_configuration_controller_parameters.yaml
@@ -63,6 +75,7 @@ add_library(${PROJECT_NAME} SHARED
   src/scaled_joint_trajectory_controller.cpp
   src/speed_scaling_state_broadcaster.cpp
   src/gpio_controller.cpp
+  src/passthrough_trajectory_controller.cpp
   src/ur_configuration_controller.cpp)
 
 target_include_directories(${PROJECT_NAME} PRIVATE
@@ -72,6 +85,7 @@ target_link_libraries(${PROJECT_NAME}
   gpio_controller_parameters
   speed_scaling_state_broadcaster_parameters
   scaled_joint_trajectory_controller_parameters
+  passthrough_trajectory_controller_parameters
   ur_configuration_controller_parameters
 )
 ament_target_dependencies(${PROJECT_NAME}

ur_controllers/controller_plugins.xml

@@ -14,6 +14,11 @@
       This controller publishes the Tool IO.
     </description>
   </class>
+  <class name="ur_controllers/PassthroughTrajectoryController" type="ur_controllers::PassthroughTrajectoryController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      This controller forwards a joint-based trajectory to the robot controller for interpolation.
+    </description>
+  </class>
   <class name="ur_controllers/URConfigurationController" type="ur_controllers::URConfigurationController" base_class_type="controller_interface::ControllerInterface">
     <description>
       Controller used to get and change the configuration of the robot

ur_controllers/doc/index.rst

@@ -135,3 +135,132 @@ Advertised services
 * ``~/set_speed_slider [ur_msgs/srv/SetSpeedSliderFraction]``: Set the value of the speed slider.
 * ``~/zero_ftsensor [std_srvs/srv/Trigger]``: Zeroes the reported wrench of the force torque
   sensor.
+
+.. _passthrough_trajectory_controller:
+
+ur_controllers/PassthroughTrajectoryController
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This controller uses a ``control_msgs/FollowJointTrajectory`` action but instead of interpolating
+the trajectory on the ROS pc it forwards the complete trajectory to the robot controller for
+interpolation and execution. This way, the realtime requirements for the control PC can be
+massively decreased, since the robot always "knows" what to do next. That means that you should be
+able to run a stable driver connection also without a real-time patched kernel.
+
+Interpolation depends on the robot controller's implementation, but in conjunction with the
+ur_robot_driver it defaults to mimicking ros2_control's spline interpolation. So, any trajectory
+planned e.g. with MoveIt! will be executed following the trajectory exactly.
+
+A trajectory sent to the controller's action server will be forwarded to the robot controller and
+executed there. Once all setpoints are transferred to the robot, the controller goes into a waiting
+state where it waits for the trajectory to be finished. While waiting, the controller tracks the
+time spent on the trajectory to ensure the robot isn't stuck during execution.
+
+This controller also supports **speed scaling** such that and scaling down of the trajectory done
+by the robot, for example due to safety settings on the robot or simply because a slower execution
+is configured on the teach pendant. This will be considered, during execution monitoring, so the
+controller basically tracks the scaled time instead of the real time.
+
+.. note::
+
+   When using this controller with the URSim simulator execution times can be slightly larger than
+   the expected time depending on the simulation host's resources. This effect will not be present
+   when using a real UR arm.
+
+.. note::
+
+   This controller can currently only be used with URSim or a real UR robot. Neither mock hardware
+   nor gazebo support this type of trajectory interfaces at the time being.
+
+Tolerances
+""""""""""
+
+Currently, the trajectory passthrough controller only supports goal tolerances and goal time
+tolerances passed in the action directly. Please make sure that the tolerances are completely
+filled with all joint names.
+
+A **goal time tolerance** of ``0.0`` means that no goal time tolerance is set and the action will
+not fail when execution takes too long.
+
+Action interface / usage
+""""""""""""""""""""""""
+
+To use this controller, publish a goal to the ``~/follow_joint_trajectory`` action interface
+similar to the `joint_trajectory_controller <https://control.ros.org/master/doc/ros2_controllers/joint_trajectory_controller/doc/userdoc.html>`_.
+
+Currently, the controller doesn't support replacing a running trajectory action. While a trajectory
+is being executed, goals will be rejected until the action has finished. If you want to replace it,
+first cancel the running action and then send a new one.
+
+Parameters
+""""""""""
+
+The trajectory passthrough controller uses the following parameters:
+
++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+
+| Parameter name                   | Type         | Default value                          | Description                                                                                                      |
+|                                  |              |                                        |                                                                                                                  |
++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+
+| ``joints`` (required)            | string_array | <empty>                                | Joint names to  listen to                                                                                        |
++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+
+| ``state_interfaces`` (required)  | string_array | <empty>                                | State interfaces provided by the hardware for all joints. Subset of ``["position", "velocity", "acceleration"]`` |
++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+
+| ``speed_scaling_interface_name`` | string       | ``speed_scaling/speed_scaling_factor`` | Fully qualified name of the speed scaling interface name.                                                        |
++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+
+| ``tf_prefix``                    | string       | <empty>                                | Urdf prefix of the corresponding arm                                                                             |
++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+
+
+Interfaces
+""""""""""
+
+In order to use this, the hardware has to export a command interface for passthrough operations for each joint. It always has
+to export position, velocity and acceleration interfaces in order to be able to project the full
+JointTrajectory definition. This is why there are separate fields used, as for passthrough mode
+accelerations might be relevant also for robots that don't support commanding accelerations
+directly to their joints.
+
+.. code:: xml
+
+   <gpio name="${tf_prefix}trajectory_passthrough">
+     <command_interface name="setpoint_positions_0"/>
+     <command_interface name="setpoint_positions_1"/>
+     <command_interface name="setpoint_positions_2"/>
+     <command_interface name="setpoint_positions_3"/>
+     <command_interface name="setpoint_positions_4"/>
+     <command_interface name="setpoint_positions_5"/>
+     <command_interface name="setpoint_velocities_0"/>
+     <command_interface name="setpoint_velocities_1"/>
+     <command_interface name="setpoint_velocities_2"/>
+     <command_interface name="setpoint_velocities_3"/>
+     <command_interface name="setpoint_velocities_4"/>
+     <command_interface name="setpoint_velocities_5"/>
+     <command_interface name="setpoint_accelerations_0"/>
+     <command_interface name="setpoint_accelerations_1"/>
+     <command_interface name="setpoint_accelerations_2"/>
+     <command_interface name="setpoint_accelerations_3"/>
+     <command_interface name="setpoint_accelerations_4"/>
+     <command_interface name="setpoint_accelerations_5"/>
+     <command_interface name="transfer_state"/>
+     <command_interface name="time_from_start"/>
+     <command_interface name="abort"/>
+   </gpio>
+
+.. note::
+
+   The hardware component has to take care that the passthrough command interfaces cannot be
+   activated in parallel to the streaming command interfaces.
+
+Implementation details / dataflow
+"""""""""""""""""""""""""""""""""
+
+* A trajectory passed to the controller will be sent to the hardware component one by one.
+* The controller will send one setpoint and then wait for the hardware to acknowledge that it can
+  take a new setpoint.
+* This happens until all setpoints have been transferred to the hardware. Then, the controller goes
+  into a waiting state where it monitors execution time and waits for the hardware to finish
+  execution.
+* If execution takes longer than anticipated, a warning will be printed.
+* If execution finished taking longer than expected (plus the goal time tolerance), the action will fail.
+* When the hardware reports that execution has been aborted (The ``passthrough_trajectory_abort``
+  command interface), the action will be aborted.
+* When the action is preempted, execution on the hardware is preempted.