← Source browser

src/custom-ur-descriptions/cms_robot_description/urdf/ur_with_zivid_epick.xacro

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">

<!--Chain: map -> UR -> flange -> wrist_extension -> te_robotside -> te_toolside -> EPick gripper -->

  <!-- robot name parameter -->
  <xacro:arg name="name" default="ur5e"/>

  <!-- import main macros -->
  <xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
  <xacro:include filename="$(find ur_robot_driver)/urdf/ur.ros2_control.xacro"/>
  <xacro:include filename="$(find cms_robot_description)/urdf/zivid_camera_mount.xacro"/>
  <xacro:include filename="$(find cms_robot_description)/urdf/wrist_extension.xacro"/>
  <xacro:include filename="$(find cms_robot_description)/urdf/te_robotside.xacro"/>
  <xacro:include filename="$(find cms_robot_description)/urdf/te_toolside.xacro"/>
  <xacro:include filename="$(find cms_robot_description)/urdf/te_coupling.xacro"/>
  <xacro:include filename="$(find epick_config)/urdf/epick_overlay.xacro"/>

  <!-- possible 'ur_type' values: ur3, ur5e, ur5, ur10, ur10e, ur16e, ur20 -->
  <xacro:arg name="ur_type" default="ur5e"/>
  <!--. -->
  <!-- Parameters from upstream UR description -->
  <xacro:arg name="tf_prefix" default="" />
  <xacro:arg name="joint_limit_params" default="$(find ur_description)/config/$(arg ur_type)/joint_limits.yaml"/>
  <xacro:arg name="kinematics_params" default="$(find ur_description)/config/$(arg ur_type)/default_kinematics.yaml"/>
  <xacro:arg name="physical_params" default="$(find ur_description)/config/$(arg ur_type)/physical_parameters.yaml"/>
  <xacro:arg name="visual_params" default="$(find ur_description)/config/$(arg ur_type)/visual_parameters.yaml"/>
  <xacro:arg name="safety_limits" default="false"/>
  <xacro:arg name="safety_pos_margin" default="0.15"/>
  <xacro:arg name="safety_k_position" default="20"/>

  <!-- ros2_control related parameters -->
  <xacro:arg name="headless_mode" default="false" />
  <xacro:arg name="robot_ip" default="0.0.0.0" />
  <xacro:arg name="script_filename" default=""/>
  <xacro:arg name="output_recipe_filename" default=""/>
  <xacro:arg name="input_recipe_filename" default=""/>
  <xacro:arg name="reverse_ip" default="0.0.0.0"/>
  <xacro:arg name="script_command_port" default="50004"/>
  <xacro:arg name="reverse_port" default="50001"/>
  <xacro:arg name="script_sender_port" default="50002"/>
  <xacro:arg name="trajectory_port" default="50003"/>
  <!-- Keep-alive: number of 20ms cycles without response before disconnect (10 = 200ms timeout) -->
  <xacro:arg name="robot_receive_timeout" default="0.2"/>

  <!-- tool communication related parameters -->
  <xacro:arg name="use_tool_communication" default="false" />
  <xacro:arg name="tool_voltage" default="0" />
  <xacro:arg name="tool_parity" default="0" />
  <xacro:arg name="tool_baud_rate" default="115200" />
  <xacro:arg name="tool_stop_bits" default="1" />
  <xacro:arg name="tool_rx_idle_chars" default="1.5" />
  <xacro:arg name="tool_tx_idle_chars" default="3.5" />
  <xacro:arg name="tool_device_name" default="/tmp/ttyUR" />
  <xacro:arg name="tool_tcp_port" default="54321" />

  <!-- Simulation parameters -->
  <xacro:arg name="mock_sensor_commands" default="false" />
  <xacro:arg name="use_mock_hardware" default="false" />

  <!-- initial position for simulations (Fake Hardware, Gazebo, Ignition) -->
  <xacro:arg name="initial_positions_file" default="$(find cms_moveit_config)/config/epick/initial_positions.yaml"/>
  <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>

  <!-- create link fixed to the "map" -->
  <link name="map" />

  <!-- Kinematic model (Jazzy: no hardware params here) -->
  <xacro:ur_robot
    name="$(arg name)"
    tf_prefix="$(arg tf_prefix)"
    parent="map"
    joint_limits_parameters_file="$(arg joint_limit_params)"
    kinematics_parameters_file="$(arg kinematics_params)"
    physical_parameters_file="$(arg physical_params)"
    visual_parameters_file="$(arg visual_params)"
    safety_limits="$(arg safety_limits)"
    safety_pos_margin="$(arg safety_pos_margin)"
    safety_k_position="$(arg safety_k_position)"
    >
    <origin xyz="0 0 0" rpy="0 0 0" />
  </xacro:ur_robot>

  <!-- ros2_control hardware interface (Jazzy: separate from kinematic macro) -->
  <xacro:ur_ros2_control
    name="$(arg name)"
    tf_prefix="$(arg tf_prefix)"
    kinematics_parameters_file="$(arg kinematics_params)"
    robot_ip="$(arg robot_ip)"
    script_filename="$(arg script_filename)"
    input_recipe_filename="$(arg input_recipe_filename)"
    output_recipe_filename="$(arg output_recipe_filename)"
    use_mock_hardware="$(arg use_mock_hardware)"
    mock_sensor_commands="$(arg mock_sensor_commands)"
    headless_mode="$(arg headless_mode)"
    initial_positions="${xacro.load_yaml(initial_positions_file)}"
    use_tool_communication="$(arg use_tool_communication)"
    tool_voltage="$(arg tool_voltage)"
    tool_parity="$(arg tool_parity)"
    tool_baud_rate="$(arg tool_baud_rate)"
    tool_stop_bits="$(arg tool_stop_bits)"
    tool_rx_idle_chars="$(arg tool_rx_idle_chars)"
    tool_tx_idle_chars="$(arg tool_tx_idle_chars)"
    tool_device_name="$(arg tool_device_name)"
    tool_tcp_port="$(arg tool_tcp_port)"
    reverse_ip="$(arg reverse_ip)"
    script_command_port="$(arg script_command_port)"
    reverse_port="$(arg reverse_port)"
    script_sender_port="$(arg script_sender_port)"
    trajectory_port="$(arg trajectory_port)"
    robot_receive_timeout="$(arg robot_receive_timeout)"
  />

  <!-- Modular Zivid: Arm Mount + Camera (with calibrated camera position) -->
  <!-- Using tool0 instead of flange - Zivid calibration uses tool0 frame -->
  <xacro:zivid_camera_mount
    parent_link="$(arg tf_prefix)tool0"
    xyz="0 0 0.005"
    rpy="0 0 0"
    zivid_model="ZIVID_2_PLUS_MR60"/>

  <!-- Wrist extension (EX-50-35): bolts to flange, extends 35mm along tool-Z -->
  <xacro:wrist_extension
    parent_link="$(arg tf_prefix)tool0"
    xyz="0 0 0.016"
    rpy="1.5708 0 3.14159"/>

  <!-- Tool exchanger robot-side plate (Wingman WM1) at the tip of the extension -->
  <xacro:te_robotside
    parent_link="wrist_extension"
    xyz="0 0.047 0"
    rpy="3.14159 0 0"/>

  <!-- Tool exchanger tool-side plate (Wingman WM1) — comes with each gripper -->
  <xacro:te_toolside
    parent_link="te_robotside"
    xyz="0 -0.011 0"
    rpy="0 0 0"/>

  <!-- Cup dimensions come from suction_cups.yaml; launch forwards the profile via the environment. -->
  <xacro:property name="cups_yaml_path" value="$(find epick_config)/config/suction_cups.yaml"/>
  <xacro:property name="cups_config" value="${xacro.load_yaml(cups_yaml_path)}"/>
  <xacro:arg name="cup_profile" default="$(optenv BEAMBOT_EPICK_CUP_PROFILE 3mm_dia_external_air)"/>
  <xacro:property name="cup_profile_name" value="$(arg cup_profile)"/>
  <xacro:property name="cup" value="${cups_config['cups'][cup_profile_name]}"/>

  <!-- Tool-changer coupling (W-010-B, 16.9mm axial) on the tool-side plate.
       Mounts on te_toolside's outward (-Y) face; coupling axis (mesh Z) aligned
       to te_toolside -Y via the rpy below. -->
  <xacro:te_coupling
    parent_link="te_toolside"
    xyz="0 -0.012 0"
    rpy="1.5708 0 0"/>

  <!-- Epick Gripper — mounted on the coupling far face.
       origin_rpy yaw=pi: the cylindrical coupling/spacer hid a 180 deg yaw flip
       of epick_tip vs the validated pre-coupling frame; this restores it so the
       IK target matches and wrist_3 doesn't swing the camera into wrist_1. -->
  <xacro:epick_overlay use_mock_hardware="$(arg use_mock_hardware)" parent="te_coupling"
                  origin_xyz="0 0 0.0099"
                  origin_rpy="0 0 3.14159"
                  extension_length="${cup['extension_length']}"
                  extension_radius="${cup['extension_radius']}"
                  suction_cup_height="${cup['suction_cup_height']}"
                  suction_cup_radius="${cup['suction_cup_radius']}"/>
</robot>