Dual arm tutorial for ROS2

my_dual_robot_cell/my_dual_robot_cell_control/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.8)
+project(my_dual_robot_cell_control)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY launch config urdf
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

my_dual_robot_cell/my_dual_robot_cell_control/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2022, Universal Robots A/S
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

my_dual_robot_cell/my_dual_robot_cell_control/config/alice_calibration.yaml

@@ -0,0 +1,44 @@
+kinematics:
+  shoulder:
+    x: 0
+    y: 0
+    z: 0.15188497379686153
+    roll: -0
+    pitch: 0
+    yaw: 6.0977137363254793e-05
+  upper_arm:
+    x: -5.6533207234432926e-06
+    y: 0
+    z: 0
+    roll: 1.570844251867735
+    pitch: 0
+    yaw: 2.541818406001163e-05
+  forearm:
+    x: -0.24357369892909492
+    y: 0
+    z: 0
+    roll: 3.1408819727448281
+    pitch: 3.1414601607728438
+    yaw: -3.1415265155090113
+  wrist_1:
+    x: -0.21292841478821867
+    y: 0.00048506402454531479
+    z: 0.11301075109662011
+    roll: 3.1373004863304383
+    pitch: 3.1363886827765697
+    yaw: 3.1415791419793657
+  wrist_2:
+    x: 4.074182080180165e-05
+    y: -0.085310893242290478
+    z: -6.4346805950626749e-05
+    roll: 1.5715505891323607
+    pitch: 0
+    yaw: -1.1414933636491948e-05
+  wrist_3:
+    x: -7.319426521397714e-05
+    y: 0.08232894752301384
+    z: -5.8521744863255091e-05
+    roll: 1.5700854986261004
+    pitch: 3.1415926535897931
+    yaw: -3.1415870300000677
+  hash: calib_3443727564195709335

my_dual_robot_cell/my_dual_robot_cell_control/config/bob_calibration.yaml

@@ -0,0 +1,44 @@
+kinematics:
+  shoulder:
+    x: 0
+    y: 0
+    z: 0.15192390485501492
+    roll: -0
+    pitch: 0
+    yaw: 7.5207672895458863e-05
+  upper_arm:
+    x: 1.1565754681822927e-07
+    y: 0
+    z: 0
+    roll: 1.5701230398196422
+    pitch: 0
+    yaw: -3.745754755790643e-05
+  forearm:
+    x: -0.24342623565759611
+    y: 0
+    z: 0
+    roll: 0.0062667292139209178
+    pitch: -0.0013839232587116385
+    yaw: 3.5263175677186283e-05
+  wrist_1:
+    x: -0.21319918045377464
+    y: -0.00042101985050556432
+    z: 0.11194857450023041
+    roll: 0.0037608148871306722
+    pitch: 0.00014433964301166516
+    yaw: -5.7788592498697277e-05
+  wrist_2:
+    x: 3.3301180434358473e-05
+    y: -0.085304702494341758
+    z: -2.7251181339165215e-05
+    roll: 1.5711157837487961
+    pitch: 0
+    yaw: -3.3083428877200004e-05
+  wrist_3:
+    x: -4.675513506427485e-05
+    y: 0.082256832990903556
+    z: -5.7204020078096673e-05
+    roll: 1.5701008950748028
+    pitch: 3.1415926535897931
+    yaw: 3.1415605336065866
+  hash: calib_10683367749926797482

my_dual_robot_cell/my_dual_robot_cell_control/config/combined_controllers.yaml

@@ -0,0 +1,244 @@
+controller_manager:
+  ros__parameters:
+    alice_joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    bob_joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    alice_io_and_status_controller:
+      type: ur_controllers/GPIOController
+
+    bob_io_and_status_controller:
+      type: ur_controllers/GPIOController
+
+    alice_speed_scaling_state_broadcaster:
+      type: ur_controllers/SpeedScalingStateBroadcaster
+
+    bob_speed_scaling_state_broadcaster:
+      type: ur_controllers/SpeedScalingStateBroadcaster
+
+    alice_force_torque_sensor_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
+    bob_force_torque_sensor_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
+    alice_joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    bob_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    alice_scaled_joint_trajectory_controller:
+      type: ur_controllers/ScaledJointTrajectoryController
+
+    bob_scaled_joint_trajectory_controller:
+      type: ur_controllers/ScaledJointTrajectoryController
+
+    forward_velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    alice_forward_position_controller:
+      type: position_controllers/JointGroupPositionController
+
+    bob_forward_position_controller:
+      type: position_controllers/JointGroupPositionController
+
+
+alice_speed_scaling_state_broadcaster:
+  ros__parameters:
+    state_publish_rate: 100.0
+    tf_prefix: "alice_"
+
+bob_speed_scaling_state_broadcaster:
+  ros__parameters:
+    state_publish_rate: 100.0
+    tf_prefix: "bob_"
+
+alice_io_and_status_controller:
+  ros__parameters:
+    tf_prefix: "alice_"
+
+bob_io_and_status_controller:
+  ros__parameters:
+    tf_prefix: "bob_"
+
+alice_force_torque_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: alice_tcp_fts_sensor
+    state_interface_names:
+      - force.x
+      - force.y
+      - force.z
+      - torque.x
+      - torque.y
+      - torque.z
+    frame_id: bob_tool0
+    topic_name: alice_ft_data
+
+bob_force_torque_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: bob_tcp_fts_sensor
+    state_interface_names:
+      - force.x
+      - force.y
+      - force.z
+      - torque.x
+      - torque.y
+      - torque.z
+    frame_id: bob_tool0
+    topic_name: bob_ft_data
+
+
+alice_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - alice_shoulder_pan_joint
+      - alice_shoulder_lift_joint
+      - alice_elbow_joint
+      - alice_wrist_1_joint
+      - alice_wrist_2_joint
+      - alice_wrist_3_joint
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.2
+      goal_time: 0.0
+      alice_shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+      alice_shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+      alice_elbow_joint: { trajectory: 0.2, goal: 0.1 }
+      alice_wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+      alice_wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+      alice_wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+
+bob_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - bob_shoulder_pan_joint
+      - bob_shoulder_lift_joint
+      - bob_elbow_joint
+      - bob_wrist_1_joint
+      - bob_wrist_2_joint
+      - bob_wrist_3_joint
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.2
+      goal_time: 0.0
+      bob_shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+      bob_shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+      bob_elbow_joint: { trajectory: 0.2, goal: 0.1 }
+      bob_wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+      bob_wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+      bob_wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+
+alice_scaled_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - alice_shoulder_pan_joint
+      - alice_shoulder_lift_joint
+      - alice_elbow_joint
+      - alice_wrist_1_joint
+      - alice_wrist_2_joint
+      - alice_wrist_3_joint
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.2
+      goal_time: 0.0
+      alice_shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+      alice_shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+      alice_elbow_joint: { trajectory: 0.2, goal: 0.1 }
+      alice_wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+      alice_wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+      alice_wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+    speed_scaling_interface_name: alice_speed_scaling/speed_scaling_factor
+
+bob_scaled_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - bob_shoulder_pan_joint
+      - bob_shoulder_lift_joint
+      - bob_elbow_joint
+      - bob_wrist_1_joint
+      - bob_wrist_2_joint
+      - bob_wrist_3_joint
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.2
+      goal_time: 0.0
+      bob_shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+      bob_shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+      bob_elbow_joint: { trajectory: 0.2, goal: 0.1 }
+      bob_wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+      bob_wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+      bob_wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+    speed_scaling_interface_name: bob_speed_scaling/speed_scaling_factor
+
+alice_forward_velocity_controller:
+  ros__parameters:
+    joints:
+      - alice_shoulder_pan_joint
+      - alice_shoulder_lift_joint
+      - alice_elbow_joint
+      - alice_wrist_1_joint
+      - alice_wrist_2_joint
+      - alice_wrist_3_joint
+    interface_name: velocity
+
+bob_forward_velocity_controller:
+  ros__parameters:
+    joints:
+      - bob_shoulder_pan_joint
+      - bob_shoulder_lift_joint
+      - bob_elbow_joint
+      - bob_wrist_1_joint
+      - bob_wrist_2_joint
+      - bob_wrist_3_joint
+    interface_name: velocity
+
+alice_forward_position_controller:
+  ros__parameters:
+    joints:
+      - alice_shoulder_pan_joint
+      - alice_shoulder_lift_joint
+      - alice_elbow_joint
+      - alice_wrist_1_joint
+      - alice_wrist_2_joint
+      - alice_wrist_3_joint
+
+bob_forward_position_controller:
+  ros__parameters:
+    joints:
+      - bob_shoulder_pan_joint
+      - bob_shoulder_lift_joint
+      - bob_elbow_joint
+      - bob_wrist_1_joint
+      - bob_wrist_2_joint
+      - bob_wrist_3_joint

my_dual_robot_cell/my_dual_robot_cell_control/config/update_rate.yaml

@@ -0,0 +1,3 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 125  # Hz