543 lines
23 KiB
XML
543 lines
23 KiB
XML
<?xml version="1.0" ?>
|
|
<!-- =================================================================================== -->
|
|
<!-- | This document was autogenerated by xacro from /opt/ros/humble/share/ur_description/urdf/ur.urdf.xacro | -->
|
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
|
<!-- =================================================================================== -->
|
|
<robot name="ur5">
|
|
<!--
|
|
Base UR robot series xacro macro.
|
|
|
|
NOTE this is NOT a URDF. It cannot directly be loaded by consumers
|
|
expecting a flattened '.urdf' file. See the top-level '.xacro' for that
|
|
(but note that .xacro must still be processed by the xacro command).
|
|
|
|
This file models the base kinematic chain of a UR robot, which then gets
|
|
parameterised by various configuration files to convert it into a UR3(e),
|
|
UR5(e), UR10(e) or UR16e.
|
|
|
|
NOTE the default kinematic parameters (i.e., link lengths, frame locations,
|
|
offsets, etc) do not correspond to any particular robot. They are defaults
|
|
only. There WILL be non-zero offsets between the Forward Kinematics results
|
|
in TF (i.e., robot_state_publisher) and the values reported by the Teach
|
|
Pendant.
|
|
|
|
For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
|
|
parameter MUST point to a .yaml file containing the appropriate values for
|
|
the targeted robot.
|
|
|
|
If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
|
|
described in the readme of that repository to extract the kinematic
|
|
calibration from the controller and generate the required .yaml file.
|
|
|
|
Main author of the migration to yaml configs Ludovic Delval.
|
|
|
|
Contributors to previous versions (in no particular order)
|
|
|
|
- Denis Stogl
|
|
- Lovro Ivanov
|
|
- Felix Messmer
|
|
- Kelsey Hawkins
|
|
- Wim Meeussen
|
|
- Shaun Edwards
|
|
- Nadia Hammoudeh Garcia
|
|
- Dave Hershberger
|
|
- G. vd. Hoorn
|
|
- Philip Long
|
|
- Dave Coleman
|
|
- Miguel Prada
|
|
- Mathias Luedtke
|
|
- Marcel Schnirring
|
|
- Felix von Drigalski
|
|
- Felix Exner
|
|
- Jimmy Da Silva
|
|
- Ajit Krisshna N L
|
|
- Muhammad Asif Rana
|
|
-->
|
|
<!--
|
|
NOTE the macro defined in this file is NOT part of the public API of this
|
|
package. Users CANNOT rely on this file being available, or stored in
|
|
this location. Nor can they rely on the existence of the macro.
|
|
-->
|
|
<!-- create link fixed to the "world" -->
|
|
<link name="world"/>
|
|
<ros2_control name="ur5" type="system">
|
|
<hardware>
|
|
<plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
|
|
<param name="robot_ip">172.18.0.2</param>
|
|
<param name="script_filename">/opt/ros/humble/share/ur_client_library/resources/external_control.urscript</param>
|
|
<param name="output_recipe_filename">/opt/ros/humble/share/ur_robot_driver/resources/rtde_output_recipe.txt</param>
|
|
<param name="input_recipe_filename">/opt/ros/humble/share/ur_robot_driver/resources/rtde_input_recipe.txt</param>
|
|
<param name="headless_mode">False</param>
|
|
<param name="reverse_port">50001</param>
|
|
<param name="script_sender_port">50002</param>
|
|
<param name="reverse_ip">0.0.0.0</param>
|
|
<param name="script_command_port">50004</param>
|
|
<param name="trajectory_port">50003</param>
|
|
<param name="tf_prefix"></param>
|
|
<param name="non_blocking_read">True</param>
|
|
<param name="servoj_gain">2000</param>
|
|
<param name="servoj_lookahead_time">0.03</param>
|
|
<param name="use_tool_communication">False</param>
|
|
<param name="kinematics/hash">calib_209549117540498681</param>
|
|
<param name="tool_voltage">0</param>
|
|
<param name="tool_parity">0</param>
|
|
<param name="tool_baud_rate">115200</param>
|
|
<param name="tool_stop_bits">1</param>
|
|
<param name="tool_rx_idle_chars">1.5</param>
|
|
<param name="tool_tx_idle_chars">3.5</param>
|
|
<param name="tool_device_name">/tmp/ttyUR</param>
|
|
<param name="tool_tcp_port">54321</param>
|
|
<param name="keep_alive_count">2</param>
|
|
</hardware>
|
|
<joint name="shoulder_pan_joint">
|
|
<command_interface name="position"/>
|
|
<command_interface name="velocity"/>
|
|
<state_interface name="position">
|
|
<!-- initial position for the FakeSystem and simulation -->
|
|
<param name="initial_value">0.0</param>
|
|
</state_interface>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
<joint name="shoulder_lift_joint">
|
|
<command_interface name="position"/>
|
|
<command_interface name="velocity"/>
|
|
<state_interface name="position">
|
|
<!-- initial position for the FakeSystem and simulation -->
|
|
<param name="initial_value">-1.57</param>
|
|
</state_interface>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
<joint name="elbow_joint">
|
|
<command_interface name="position"/>
|
|
<command_interface name="velocity"/>
|
|
<state_interface name="position">
|
|
<!-- initial position for the FakeSystem and simulation -->
|
|
<param name="initial_value">0.0</param>
|
|
</state_interface>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
<joint name="wrist_1_joint">
|
|
<command_interface name="position"/>
|
|
<command_interface name="velocity"/>
|
|
<state_interface name="position">
|
|
<!-- initial position for the FakeSystem and simulation -->
|
|
<param name="initial_value">-1.57</param>
|
|
</state_interface>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
<joint name="wrist_2_joint">
|
|
<command_interface name="position"/>
|
|
<command_interface name="velocity"/>
|
|
<state_interface name="position">
|
|
<!-- initial position for the FakeSystem and simulation -->
|
|
<param name="initial_value">0.0</param>
|
|
</state_interface>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
<joint name="wrist_3_joint">
|
|
<command_interface name="position"/>
|
|
<command_interface name="velocity"/>
|
|
<state_interface name="position">
|
|
<!-- initial position for the FakeSystem and simulation -->
|
|
<param name="initial_value">0.0</param>
|
|
</state_interface>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
<sensor name="tcp_fts_sensor">
|
|
<state_interface name="force.x"/>
|
|
<state_interface name="force.y"/>
|
|
<state_interface name="force.z"/>
|
|
<state_interface name="torque.x"/>
|
|
<state_interface name="torque.y"/>
|
|
<state_interface name="torque.z"/>
|
|
</sensor>
|
|
<sensor name="tcp_pose">
|
|
<state_interface name="position.x"/>
|
|
<state_interface name="position.y"/>
|
|
<state_interface name="position.z"/>
|
|
<state_interface name="orientation.x"/>
|
|
<state_interface name="orientation.y"/>
|
|
<state_interface name="orientation.z"/>
|
|
<state_interface name="orientation.w"/>
|
|
</sensor>
|
|
<!-- NOTE The following are joints used only for testing with fake hardware and will change in the future -->
|
|
<gpio name="speed_scaling">
|
|
<state_interface name="speed_scaling_factor"/>
|
|
<param name="initial_speed_scaling_factor">1</param>
|
|
<command_interface name="target_speed_fraction_cmd"/>
|
|
<command_interface name="target_speed_fraction_async_success"/>
|
|
</gpio>
|
|
<gpio name="gpio">
|
|
<command_interface name="standard_digital_output_cmd_0"/>
|
|
<command_interface name="standard_digital_output_cmd_1"/>
|
|
<command_interface name="standard_digital_output_cmd_2"/>
|
|
<command_interface name="standard_digital_output_cmd_3"/>
|
|
<command_interface name="standard_digital_output_cmd_4"/>
|
|
<command_interface name="standard_digital_output_cmd_5"/>
|
|
<command_interface name="standard_digital_output_cmd_6"/>
|
|
<command_interface name="standard_digital_output_cmd_7"/>
|
|
<command_interface name="standard_digital_output_cmd_8"/>
|
|
<command_interface name="standard_digital_output_cmd_9"/>
|
|
<command_interface name="standard_digital_output_cmd_10"/>
|
|
<command_interface name="standard_digital_output_cmd_11"/>
|
|
<command_interface name="standard_digital_output_cmd_12"/>
|
|
<command_interface name="standard_digital_output_cmd_13"/>
|
|
<command_interface name="standard_digital_output_cmd_14"/>
|
|
<command_interface name="standard_digital_output_cmd_15"/>
|
|
<command_interface name="standard_digital_output_cmd_16"/>
|
|
<command_interface name="standard_digital_output_cmd_17"/>
|
|
<command_interface name="standard_analog_output_cmd_0"/>
|
|
<command_interface name="standard_analog_output_cmd_1"/>
|
|
<command_interface name="analog_output_domain_cmd"/>
|
|
<command_interface name="tool_voltage_cmd"/>
|
|
<command_interface name="io_async_success"/>
|
|
<state_interface name="digital_output_0"/>
|
|
<state_interface name="digital_output_1"/>
|
|
<state_interface name="digital_output_2"/>
|
|
<state_interface name="digital_output_3"/>
|
|
<state_interface name="digital_output_4"/>
|
|
<state_interface name="digital_output_5"/>
|
|
<state_interface name="digital_output_6"/>
|
|
<state_interface name="digital_output_7"/>
|
|
<state_interface name="digital_output_8"/>
|
|
<state_interface name="digital_output_9"/>
|
|
<state_interface name="digital_output_10"/>
|
|
<state_interface name="digital_output_11"/>
|
|
<state_interface name="digital_output_12"/>
|
|
<state_interface name="digital_output_13"/>
|
|
<state_interface name="digital_output_14"/>
|
|
<state_interface name="digital_output_15"/>
|
|
<state_interface name="digital_output_16"/>
|
|
<state_interface name="digital_output_17"/>
|
|
<state_interface name="digital_input_0"/>
|
|
<state_interface name="digital_input_1"/>
|
|
<state_interface name="digital_input_2"/>
|
|
<state_interface name="digital_input_3"/>
|
|
<state_interface name="digital_input_4"/>
|
|
<state_interface name="digital_input_5"/>
|
|
<state_interface name="digital_input_6"/>
|
|
<state_interface name="digital_input_7"/>
|
|
<state_interface name="digital_input_8"/>
|
|
<state_interface name="digital_input_9"/>
|
|
<state_interface name="digital_input_10"/>
|
|
<state_interface name="digital_input_11"/>
|
|
<state_interface name="digital_input_12"/>
|
|
<state_interface name="digital_input_13"/>
|
|
<state_interface name="digital_input_14"/>
|
|
<state_interface name="digital_input_15"/>
|
|
<state_interface name="digital_input_16"/>
|
|
<state_interface name="digital_input_17"/>
|
|
<state_interface name="standard_analog_output_0"/>
|
|
<state_interface name="standard_analog_output_1"/>
|
|
<state_interface name="standard_analog_input_0"/>
|
|
<state_interface name="standard_analog_input_1"/>
|
|
<state_interface name="analog_io_type_0"/>
|
|
<state_interface name="analog_io_type_1"/>
|
|
<state_interface name="analog_io_type_2"/>
|
|
<state_interface name="analog_io_type_3"/>
|
|
<state_interface name="tool_mode"/>
|
|
<state_interface name="tool_output_voltage"/>
|
|
<state_interface name="tool_output_current"/>
|
|
<state_interface name="tool_temperature"/>
|
|
<state_interface name="tool_analog_input_0"/>
|
|
<state_interface name="tool_analog_input_1"/>
|
|
<state_interface name="tool_analog_input_type_0"/>
|
|
<state_interface name="tool_analog_input_type_1"/>
|
|
<state_interface name="robot_mode"/>
|
|
<state_interface name="robot_status_bit_0"/>
|
|
<state_interface name="robot_status_bit_1"/>
|
|
<state_interface name="robot_status_bit_2"/>
|
|
<state_interface name="robot_status_bit_3"/>
|
|
<state_interface name="safety_mode"/>
|
|
<state_interface name="safety_status_bit_0"/>
|
|
<state_interface name="safety_status_bit_1"/>
|
|
<state_interface name="safety_status_bit_2"/>
|
|
<state_interface name="safety_status_bit_3"/>
|
|
<state_interface name="safety_status_bit_4"/>
|
|
<state_interface name="safety_status_bit_5"/>
|
|
<state_interface name="safety_status_bit_6"/>
|
|
<state_interface name="safety_status_bit_7"/>
|
|
<state_interface name="safety_status_bit_8"/>
|
|
<state_interface name="safety_status_bit_9"/>
|
|
<state_interface name="safety_status_bit_10"/>
|
|
<state_interface name="program_running"/>
|
|
</gpio>
|
|
<gpio name="payload">
|
|
<command_interface name="mass"/>
|
|
<command_interface name="cog.x"/>
|
|
<command_interface name="cog.y"/>
|
|
<command_interface name="cog.z"/>
|
|
<command_interface name="payload_async_success"/>
|
|
</gpio>
|
|
<gpio name="resend_robot_program">
|
|
<command_interface name="resend_robot_program_cmd"/>
|
|
<command_interface name="resend_robot_program_async_success"/>
|
|
</gpio>
|
|
<gpio name="hand_back_control">
|
|
<command_interface name="hand_back_control_cmd"/>
|
|
<command_interface name="hand_back_control_async_success"/>
|
|
</gpio>
|
|
<gpio name="zero_ftsensor">
|
|
<command_interface name="zero_ftsensor_cmd"/>
|
|
<command_interface name="zero_ftsensor_async_success"/>
|
|
</gpio>
|
|
<gpio name="system_interface">
|
|
<state_interface name="initialized"/>
|
|
</gpio>
|
|
<gpio name="get_robot_software_version">
|
|
<state_interface name="get_version_major"/>
|
|
<state_interface name="get_version_minor"/>
|
|
<state_interface name="get_version_build"/>
|
|
<state_interface name="get_version_bugfix"/>
|
|
</gpio>
|
|
</ros2_control>
|
|
<!-- Add URDF transmission elements (for ros_control) -->
|
|
<!--<xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />-->
|
|
<!-- Placeholder for ros2_control transmission which don't yet exist -->
|
|
<!-- links - main serial chain -->
|
|
<link name="base_link"/>
|
|
<link name="base_link_inertia">
|
|
<visual>
|
|
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/visual/base.dae"/>
|
|
</geometry>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/collision/base.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<mass value="4.0"/>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="shoulder_link">
|
|
<visual>
|
|
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/visual/shoulder.dae"/>
|
|
</geometry>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/collision/shoulder.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<mass value="3.7"/>
|
|
<origin rpy="0 0 0" xyz="0.0 -0.00193 -0.02561"/>
|
|
<inertia ixx="0.014972358333333331" ixy="0" ixz="0" iyy="0.014972358333333331" iyz="0" izz="0.01040625"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="upper_arm_link">
|
|
<visual>
|
|
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.13585"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/visual/upperarm.dae"/>
|
|
</geometry>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.13585"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/collision/upperarm.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<mass value="8.393"/>
|
|
<origin rpy="0 1.570796326794897 0" xyz="-0.2125 0.0 0.11336"/>
|
|
<inertia ixx="0.13388583541666665" ixy="0" ixz="0" iyy="0.13388583541666665" iyz="0" izz="0.0151074"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="forearm_link">
|
|
<visual>
|
|
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0165"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/visual/forearm.dae"/>
|
|
</geometry>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0165"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/collision/forearm.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<mass value="2.33"/>
|
|
<origin rpy="0 1.570796326794897 0" xyz="-0.24225 0.0 0.0265"/>
|
|
<inertia ixx="0.031216803515624995" ixy="0" ixz="0" iyy="0.031216803515624995" iyz="0" izz="0.004095"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="wrist_1_link">
|
|
<visual>
|
|
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.093"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/visual/wrist1.dae"/>
|
|
</geometry>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.093"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/collision/wrist1.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<mass value="1.219"/>
|
|
<origin rpy="0 0 0" xyz="0.0 -0.01634 -0.0018"/>
|
|
<inertia ixx="0.002013889583333333" ixy="0" ixz="0" iyy="0.002013889583333333" iyz="0" izz="0.0021942"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="wrist_2_link">
|
|
<visual>
|
|
<origin rpy="0 0 0" xyz="0 0 -0.095"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/visual/wrist2.dae"/>
|
|
</geometry>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="0 0 0" xyz="0 0 -0.095"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/collision/wrist2.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<mass value="1.219"/>
|
|
<origin rpy="0 0 0" xyz="0.0 0.01634 -0.0018"/>
|
|
<inertia ixx="0.0018310395833333333" ixy="0" ixz="0" iyy="0.0018310395833333333" iyz="0" izz="0.0021942"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="wrist_3_link">
|
|
<visual>
|
|
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.0818"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/visual/wrist3.dae"/>
|
|
</geometry>
|
|
</visual>
|
|
<collision>
|
|
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.0818"/>
|
|
<geometry>
|
|
<mesh filename="package://ur_description/meshes/ur5/collision/wrist3.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<mass value="0.1879"/>
|
|
<origin rpy="0 0 0" xyz="0.0 0.0 -0.001159"/>
|
|
<inertia ixx="8.062475833333332e-05" ixy="0" ixz="0" iyy="8.062475833333332e-05" iyz="0" izz="0.0001321171875"/>
|
|
</inertial>
|
|
</link>
|
|
<!-- base_joint fixes base_link to the environment -->
|
|
<joint name="base_joint" type="fixed">
|
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
<parent link="world"/>
|
|
<child link="base_link"/>
|
|
</joint>
|
|
<!-- joints - main serial chain -->
|
|
<joint name="base_link-base_link_inertia" type="fixed">
|
|
<parent link="base_link"/>
|
|
<child link="base_link_inertia"/>
|
|
<!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
|
|
frames of the robot/controller have X+ pointing backwards.
|
|
Use the joint between 'base_link' and 'base_link_inertia' (a dummy
|
|
link/frame) to introduce the necessary rotation over Z (of pi rad).
|
|
-->
|
|
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
|
</joint>
|
|
<joint name="shoulder_pan_joint" type="revolute">
|
|
<parent link="base_link_inertia"/>
|
|
<child link="shoulder_link"/>
|
|
<origin rpy="0 0 0" xyz="0 0 0.089159"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit effort="150.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
|
<safety_controller k_position="20" k_velocity="0.0" soft_lower_limit="-6.133185307179586" soft_upper_limit="6.133185307179586"/>
|
|
<dynamics damping="0" friction="0"/>
|
|
</joint>
|
|
<joint name="shoulder_lift_joint" type="revolute">
|
|
<parent link="shoulder_link"/>
|
|
<child link="upper_arm_link"/>
|
|
<origin rpy="1.570796327 0 0" xyz="0 0 0"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit effort="150.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
|
<safety_controller k_position="20" k_velocity="0.0" soft_lower_limit="-6.133185307179586" soft_upper_limit="6.133185307179586"/>
|
|
<dynamics damping="0" friction="0"/>
|
|
</joint>
|
|
<joint name="elbow_joint" type="revolute">
|
|
<parent link="upper_arm_link"/>
|
|
<child link="forearm_link"/>
|
|
<origin rpy="0 0 0" xyz="-0.425 0 0"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
|
|
<safety_controller k_position="20" k_velocity="0.0" soft_lower_limit="-2.991592653589793" soft_upper_limit="2.991592653589793"/>
|
|
<dynamics damping="0" friction="0"/>
|
|
</joint>
|
|
<joint name="wrist_1_joint" type="revolute">
|
|
<parent link="forearm_link"/>
|
|
<child link="wrist_1_link"/>
|
|
<origin rpy="0 0 0" xyz="-0.39225 0 0.10915"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
|
<safety_controller k_position="20" k_velocity="0.0" soft_lower_limit="-6.133185307179586" soft_upper_limit="6.133185307179586"/>
|
|
<dynamics damping="0" friction="0"/>
|
|
</joint>
|
|
<joint name="wrist_2_joint" type="revolute">
|
|
<parent link="wrist_1_link"/>
|
|
<child link="wrist_2_link"/>
|
|
<origin rpy="1.570796327 0 0" xyz="0 -0.09465 -1.941303950897609e-11"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
|
<safety_controller k_position="20" k_velocity="0.0" soft_lower_limit="-6.133185307179586" soft_upper_limit="6.133185307179586"/>
|
|
<dynamics damping="0" friction="0"/>
|
|
</joint>
|
|
<joint name="wrist_3_joint" type="revolute">
|
|
<parent link="wrist_2_link"/>
|
|
<child link="wrist_3_link"/>
|
|
<origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.0823 -1.688001216681175e-11"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
|
<safety_controller k_position="20" k_velocity="0.0" soft_lower_limit="-6.133185307179586" soft_upper_limit="6.133185307179586"/>
|
|
<dynamics damping="0" friction="0"/>
|
|
</joint>
|
|
<link name="ft_frame"/>
|
|
<joint name="wrist_3_link-ft_frame" type="fixed">
|
|
<parent link="wrist_3_link"/>
|
|
<child link="ft_frame"/>
|
|
<origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
|
|
</joint>
|
|
<!-- ROS-Industrial 'base' frame - base_link to UR 'Base' Coordinates transform -->
|
|
<link name="base"/>
|
|
<joint name="base_link-base_fixed_joint" type="fixed">
|
|
<!-- Note the rotation over Z of pi radians - as base_link is REP-103
|
|
aligned (i.e., has X+ forward, Y+ left and Z+ up), this is needed
|
|
to correctly align 'base' with the 'Base' coordinate system of
|
|
the UR controller.
|
|
-->
|
|
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
|
<parent link="base_link"/>
|
|
<child link="base"/>
|
|
</joint>
|
|
<!-- ROS-Industrial 'flange' frame - attachment point for EEF models -->
|
|
<link name="flange"/>
|
|
<joint name="wrist_3-flange" type="fixed">
|
|
<parent link="wrist_3_link"/>
|
|
<child link="flange"/>
|
|
<origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
|
|
</joint>
|
|
<!-- ROS-Industrial 'tool0' frame - all-zeros tool frame -->
|
|
<link name="tool0"/>
|
|
<joint name="flange-tool0" type="fixed">
|
|
<!-- default toolframe - X+ left, Y+ up, Z+ front -->
|
|
<origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
|
|
<parent link="flange"/>
|
|
<child link="tool0"/>
|
|
</joint>
|
|
</robot>
|