Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Dual arm tutorial for ROS2 #3

Draft
wants to merge 16 commits into
base: main
Choose a base branch
from
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions my_dual_robot_cell/my_dual_robot_cell_control/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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()
29 changes: 29 additions & 0 deletions my_dual_robot_cell/my_dual_robot_cell_control/LICENSE
Original file line number Diff line number Diff line change
@@ -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.
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
@@ -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
Comment on lines +3 to +7
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Only one joint_state_broadcaster should be sufficient.

Suggested change
alice_joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
bob_joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
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
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
controller_manager:
ros__parameters:
update_rate: 125 # Hz
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could we please default to an e-series robot?

Suggested change
update_rate: 125 # Hz
update_rate: 500 # Hz

Loading