added stuff for motion_primitive_from_trajectory_controller (ros-controls/ros2_controllers#1837)

Author: mathias31415

ur_moveit_config/config/moveit_controllers.yaml

@@ -11,11 +11,12 @@ moveit_simple_controller_manager:
   controller_names:
     - scaled_joint_trajectory_controller
     - joint_trajectory_controller
+    - motion_primitive_from_trajectory_controller
 
   scaled_joint_trajectory_controller:
     action_ns: follow_joint_trajectory
     type: FollowJointTrajectory
-    default: true
+    default: false
     joints:
       - shoulder_pan_joint
       - shoulder_lift_joint
@@ -36,3 +37,16 @@ moveit_simple_controller_manager:
       - wrist_1_joint
       - wrist_2_joint
       - wrist_3_joint
+
+
+  motion_primitive_from_trajectory_controller:
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint

ur_robot_driver/README_MotionPrimitive.md

@@ -5,20 +5,19 @@ Hardware interface for executing motion primitives on a UR robot using the ROS 2
 
 ![Licence](https://img.shields.io/badge/License-BSD-3-Clause-blue.svg)
 
-# Demo Video with URSim
-[![Play Video](doc/motion_primitive_ur_driver/motion_primitive_demo_video_preview.png)](https://www.youtube.com/watch?v=htUJtfkgr6Q)
-
-# Related packages/ repos
-- [control_msgs](https://github.com/ros-controls/control_msgs/blob/motion_primitives/control_msgs/action/ExecuteMotionPrimitiveSequence.action)
-- [ros2_controllers with motion_primitives_forward_controller](https://github.com/b-robotized-forks/ros2_controllers/tree/motion_primitive_forward_controller/motion_primitives_forward_controller)
-- [Universal_Robots_ROS2_Driver with motion_primitive_ur_driver](https://github.com/b-robotized-forks/Universal_Robots_ROS2_Driver_MotionPrimitive)
-- [Universal_Robots_Client_Library](https://github.com/UniversalRobots/Universal_Robots_Client_Library)
+# Demo Video with motion_primitives_forward_controller
+[![Play Video](doc/motion_primitive_ur_driver/moprim_forward_controller_ur_demo_thumbnail.png)](https://youtu.be/SKz6LFvJmhQ)
 
+# Demo Video with motion_primitives_from_trajectory_controller
+[![Play Video](doc/motion_primitive_ur_driver/moprim_from_traj_controller_ur_demo_thumbnail.png)](https://youtu.be/nsG4sW8BfLI)
 
 # Architecture
-
+**with motion_primitives_forward_controller**
 ![Architecture Overview](doc/motion_primitive_ur_driver/ros2_control_motion_primitives_ur.drawio.png)
 
+**with motion_primitives_from_trajectory_controller**
+![Architecture Overview](doc/motion_primitive_ur_driver/ros2_control_motion_primitives_from_traj_ur.drawio.png)
+
 # Command and State Interfaces
 
 In motion primitives mode, the following state and command interfaces are used to enable communication between the controller and the hardware interface.
@@ -104,42 +103,40 @@ ros2 run ur_client_library start_ursim.sh -m ur10e
 Remote control needs to be enabled:
 https://robodk.com/doc/en/Robots-Universal-Robots-How-enable-Remote-Control-URe.html
 
-## Launch hardware_interface with motion_primitives_ur_driver
-With URSim:
+## With motion_primitives_forward_controller
+**With URSim:**
 ```
 ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur10e robot_ip:=192.168.56.101 launch_rviz:=true headless_mode:=true initial_joint_controller:=motion_primitive_forward_controller
 ```
-With H-KA UR10e:
+**With H-KA UR10e:**
 ```
 ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur10e robot_ip:=192.168.1.102 launch_rviz:=true headless_mode:=true initial_joint_controller:=motion_primitive_forward_controller
 ```
-## (optional) switching control mode
+**(optional) switching control mode**
 ```
 ros2 control switch_controllers --activate motion_primitive_forward_controller --deactivate scaled_joint_trajectory_controller
 ```
-## Send motion primitives
+**Send motion primitives from python script**
 > [!WARNING]
 > Ensure that the robot in your configuration is able to execute these motion primitives without any risk of collision.
 ```
 ros2 run ur_robot_driver send_dummy_motion_primitives_ur10e.py
 ```
 During the execution of the motion primitives, the movement can be stopped by pressing the Enter key in the terminal.
 
+## With motion_primitives_from_trajectory_controller with MoveIt + RViz
+**Start MoveIt + RViz**
+```
+ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur10e launch_rviz:=true
+```
+**Start Controller + HW-Interface with URSim**
+```
+ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur10e robot_ip:=192.168.56.101 initial_joint_controller:=motion_primitive_from_trajectory_controller launch_rviz:=false
+```
+**Start Controller + HW-Interface with H-KA UR10e**
+```
+ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur10e robot_ip:=192.168.1.102 initial_joint_controller:=motion_primitive_from_trajectory_controller launch_rviz:=false
+```
+
 # TODOs/ Improvements
-- if trajectory is finished while `instruction_executer->cancelMotion()` is called --> returns with execution_status ERROR --> no new command can be sent to hw-interface --> need to call `instruction_executer->cancelMotion()` a second time
-- The default `hardware_interface` implementation and the `InstructionExecutor` used to execute motion primitives both rely on a callback function that is triggered when a trajectory is completed. In the current implementation, the callback function of the `ur_driver` is overwritten, meaning that only one of the callback functions can be active at a time. This issue has been addressed by registering the `InstructionExecutor`'s callback when motion primitives mode is activated, and restoring the `hardware_interface`'s callback when the mode is deactivated. To enable this, a method `registerTrajDoneCallback()` was added to the `InstructionExecutor` in the `ur_client_library`:
-```cpp
-  void urcl::InstructionExecutor::registerTrajDoneCallback()
-  {
-    driver_->registerTrajectoryDoneCallback(
-        std::bind(&InstructionExecutor::trajDoneCallback, this, std::placeholders::_1));
-  }
-```
---> Is there a better solution for this?
-
-# Useful sources
-- https://docs.universal-robots.com/Universal_Robots_ROS_Documentation/doc/ur_client_library/doc/architecture/instruction_executor.html
-- https://docs.universal-robots.com/Universal_Robots_ROS_Documentation/doc/ur_client_library/doc/examples/instruction_executor.html
-- https://rtw.b-robotized.com/master/use-cases/ros_workspaces/aliases.html
-- https://control.ros.org/master/doc/ros2_control/ros2controlcli/doc/userdoc.html
-- ...
+- if trajectory is finished while `instruction_executer->cancelMotion()` is called --> returns with execution_status ERROR --> no new command can be sent to hw-interface --> need to call `instruction_executer->cancelMotion()` a second time

ur_robot_driver/config/ur_controllers.yaml

@@ -53,6 +53,9 @@ controller_manager:
     motion_primitive_forward_controller:
       type: motion_primitives_forward_controller/MotionPrimitivesForwardController
 
+    motion_primitive_from_trajectory_controller:
+      type: motion_primitives_from_trajectory_controller/MotionPrimitivesFromTrajectoryController
+
 speed_scaling_state_broadcaster:
   ros__parameters:
     state_publish_rate: 100.0
@@ -223,3 +226,52 @@ motion_primitive_forward_controller:
     state_interfaces:
       - $(var tf_prefix)motion_primitive/execution_status
       - $(var tf_prefix)motion_primitive/ready_for_new_primitive
+
+motion_primitive_from_trajectory_controller:
+  ros__parameters:
+    command_interfaces:
+      - $(var tf_prefix)motion_primitive/motion_type
+      - $(var tf_prefix)motion_primitive/q1
+      - $(var tf_prefix)motion_primitive/q2
+      - $(var tf_prefix)motion_primitive/q3
+      - $(var tf_prefix)motion_primitive/q4
+      - $(var tf_prefix)motion_primitive/q5
+      - $(var tf_prefix)motion_primitive/q6
+      - $(var tf_prefix)motion_primitive/pos_x
+      - $(var tf_prefix)motion_primitive/pos_y
+      - $(var tf_prefix)motion_primitive/pos_z
+      - $(var tf_prefix)motion_primitive/pos_qx
+      - $(var tf_prefix)motion_primitive/pos_qy
+      - $(var tf_prefix)motion_primitive/pos_qz
+      - $(var tf_prefix)motion_primitive/pos_qw
+      - $(var tf_prefix)motion_primitive/pos_via_x
+      - $(var tf_prefix)motion_primitive/pos_via_y
+      - $(var tf_prefix)motion_primitive/pos_via_z
+      - $(var tf_prefix)motion_primitive/pos_via_qx
+      - $(var tf_prefix)motion_primitive/pos_via_qy
+      - $(var tf_prefix)motion_primitive/pos_via_qz
+      - $(var tf_prefix)motion_primitive/pos_via_qw
+      - $(var tf_prefix)motion_primitive/blend_radius
+      - $(var tf_prefix)motion_primitive/velocity
+      - $(var tf_prefix)motion_primitive/acceleration
+      - $(var tf_prefix)motion_primitive/move_time
+    state_interfaces:
+      - $(var tf_prefix)motion_primitive/execution_status
+      - $(var tf_prefix)motion_primitive/ready_for_new_primitive
+    local_joint_order:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    approximate_mode: "RDP_LIN"  # "RDP_LIN" or "RDP_PTP"
+    use_time_not_vel_and_acc: true
+    epsilon_joint_angle: 0.05
+    epsilon_cart_position: 0.01
+    epsilon_cart_angle: 0.05
+    # blend_radius_overwrite: 0.01
+    # joint_vel_overwrite: 0.5
+    # joint_acc_overwrite: 0.5
+    # cart_vel_overwrite: 0.5
+    # cart_acc_overwrite: 0.5

ur_robot_driver/doc/motion_primitive_ur_driver/moprim_forward_controller_ur_demo_thumbnail.png

@@ -194,6 +194,7 @@ def controller_spawner(controllers, active=True):
         "freedrive_mode_controller",
         "tool_contact_controller",
         "motion_primitive_forward_controller",
+        "motion_primitive_from_trajectory_controller",
     ]
     if activate_joint_controller.perform(context) == "true":
         controllers_active.append(initial_joint_controller.perform(context))
@@ -349,6 +350,7 @@ def generate_launch_description():
                 "freedrive_mode_controller",
                 "passthrough_trajectory_controller",
                 "motion_primitive_forward_controller",
+                "motion_primitive_from_trajectory_controller",
             ],
             description="Initially loaded robot controller.",
         )