diff --git a/ur5_description/urdf/robot.urdf.xacro b/ur5_description/urdf/model.urdf.xacro similarity index 100% rename from ur5_description/urdf/robot.urdf.xacro rename to ur5_description/urdf/model.urdf.xacro diff --git a/ur5_description/urdf/robot.urdf.pretty.xacro b/ur5_description/urdf/robot.urdf.pretty.xacro deleted file mode 100644 index 019ed7eb13acedaec265ba708af31c7285dc11b9..0000000000000000000000000000000000000000 --- a/ur5_description/urdf/robot.urdf.pretty.xacro +++ /dev/null @@ -1,289 +0,0 @@ -<?xml version="1.0"?> -<!-- -DH for UR5: -a = [0.00000, -0.42500, -0.39225, 0.00000, 0.00000, 0.0000] -d = [0.089159, 0.00000, 0.00000, 0.10915, 0.09465, 0.0823] -alpha = [ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 ] -q_home_offset = [0, -1.570796327, 0, -1.570796327, 0, 0] -joint_direction = [-1, -1, 1, 1, 1, 1] -mass = [3.7000, 8.3930, 2.2750, 1.2190, 1.2190, 0.1879] -center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0, 0.0265], [0, -0.0018, 0.01634], [0, 0.0018,0.01634], [0, 0, -0.001159] ] ---> -<robot name="universal_robot"> - - <property name="pi" value="3.14159265" /> - -<!-- Inertia parameters --> - <property name="base_mass" value="4.0" /> <!-- Invented number, only matters for simulator --> - <property name="shoulder_mass" value="3.7000" /> - <property name="upper_arm_mass" value="8.3930" /> - <property name="forearm_mass" value="2.2750" /> - <property name="wrist_1_mass" value="1.2190" /> - <property name="wrist_2_mass" value="1.2190" /> - <property name="wrist_3_mass" value="0.1879" /> - - <property name="shoulder_cog" value="0.0 0.00193 -0.02561" /> - <property name="upper_arm_cog" value="0.0 -0.024201 0.2125" /> <!-- 0.11336 - 0.089159 = --> - <property name="forearm_cog" value="0.0 0.0265 0.11993" /> <!-- 0.119 is not half of 0.39225, is this really correct? --> - <property name="wrist_1_cog" value="0.0 0.110949 0.01634" /> <!-- 0.0018 + 0.10915 = 0.110949 --> - <property name="wrist_2_cog" value="0.0 0.0018 0.11099" /> <!-- 0.01634 + 0.09465 = 0.11099--> - <property name="wrist_3_cog" value="0.0 0.001159 0.0" /> - -<!-- Kinematic model --> - <property name="shoulder_height" value="0.089159" /> - <property name="shoulder_offset" value="0.13585" /> <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 --> - <property name="upper_arm_length" value="0.42500" /> - <property name="elbow_offset" value="0.1197" /> <!-- CAD measured --> - <property name="forearm_length" value="0.39225" /> - <property name="wrist_1_length" value="0.093" /> <!-- CAD measured --> - <property name="wrist_2_length" value="0.09465" /> <!-- In CAD this distance is 0.930, but in the spec it is 0.09465 --> - <property name="wrist_3_length" value="0.0823" /> - - <property name="shoulder_radius" value="0.060" /> <!-- manually measured --> - <property name="upper_arm_radius" value="0.054" /> <!-- manually measured --> - <property name="elbow_radius" value="0.060" /> <!-- manually measured --> - <property name="forearm_radius" value="0.040" /> <!-- manually measured --> - <property name="wrist_radius" value="0.045" /> <!-- manually measured --> - -<!-- Collision model --> - <property name="base_collision_length" value="0.160" /> <!-- manually measured --> - <property name="shoulder_collision_length" value="0.200" /> <!-- manually measured --> - <property name="shoulder_collision_offset" value="0.035" /> <!-- manually measured --> - <property name="elbow_collision_length" value="0.200" /> <!-- manually measured --> - <property name="elbow_collision_offset" value="0.035" /> <!-- manually measured --> - - - - - <link name="base_link" > - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/Base.dae" /> - </geometry> - <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 ${3.0 * pi / 4.0}" /> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/Base.dae" /> - </geometry> - <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 ${3.0 * pi / 4.0}" /> - </collision> - <inertial> - <mass value="${base_mass}" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - - <joint name="shoulder_pan_joint" type="revolute"> - <parent link="base_link" /> - <child link = "shoulder_link" /> - <origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" /> - <axis xyz="0.0 0.0 1.0" /> - <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/> - <dynamics damping="0.1" friction="0.1"/> - </joint> - - <link name="shoulder_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/Shoulder.dae" /> - </geometry> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/Shoulder.dae" /> - </geometry> - </collision> - <inertial> - <mass value="${shoulder_mass}" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - <origin xyz="${shoulder_cog}" rpy="0.0 0.0 0.0" /> - </inertial> - </link> - - <joint name="shoulder_lift_joint" type="revolute"> - <parent link="shoulder_link" /> - <child link = "upper_arm_link" /> - <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" /> - <axis xyz="0.0 1.0 0.0" /> - <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/> - <dynamics damping="0.1" friction="0.1"/> - </joint> - - <link name="upper_arm_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/UpperArm.dae" /> - </geometry> - <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/UpperArm.dae" /> - </geometry> - <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /> - </collision> - <inertial> - <mass value="${upper_arm_mass}" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - <origin xyz="${upper_arm_cog}" rpy="0.0 0.0 0.0" /> - </inertial> - </link> - - <joint name="elbow_joint" type="revolute"> - <parent link="upper_arm_link" /> - <child link = "forearm_link" /> - <origin xyz="0.0 ${-elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" /> - <axis xyz="0.0 1.0 0.0" /> - <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/> - <dynamics damping="0.1" friction="0.1"/> - </joint> - - <link name="forearm_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/Forearm.dae" /> - </geometry> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/Forearm.dae" /> - </geometry> - </collision> - <inertial> - <mass value="${forearm_mass}" /> - <origin xyz="${forearm_cog}" rpy="0.0 0.0 0.0" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - - <joint name="wrist_1_joint" type="revolute"> - <parent link="forearm_link" /> - <child link = "wrist_1_link" /> - <origin xyz="0.0 0.0 ${forearm_length}" rpy="0.0 ${pi / 2.0} 0.0" /> - <axis xyz="0.0 1.0 0.0" /> - <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/> - <dynamics damping="0.1" friction="0.1"/> - </joint> - - <link name="wrist_1_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/Wrist1.dae" /> - </geometry> - <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 ${pi / 2.0} 0.0" /> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/Wrist1.dae" /> - </geometry> - <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 ${pi / 2.0} 0.0" /> - </collision> - <inertial> - <mass value="${wrist_1_mass}" /> - <origin xyz="${wrist_1_cog}" rpy="0.0 0.0 0.0" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - - <joint name="wrist_2_joint" type="revolute"> - <parent link="wrist_1_link" /> - <child link = "wrist_2_link" /> - <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0.0 0.0" /> - <axis xyz="0.0 0.0 1.0" /> - <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/> - <dynamics damping="0.1" friction="0.1"/> - </joint> - - <link name="wrist_2_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/Wrist2.dae" /> - </geometry> - <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" /> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/Wrist2.dae" /> - </geometry> - <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" /> - </collision> - <inertial> - <mass value="${wrist_2_mass}" /> - <origin xyz="${wrist_2_cog}" rpy="0.0 0.0 0.0" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - - <joint name="wrist_3_joint" type="revolute"> - <parent link="wrist_2_link" /> - <child link = "wrist_3_link" /> - <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" /> - <axis xyz="0.0 1.0 0.0" /> - <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/> - <dynamics damping="0.1" friction="0.1"/> - </joint> - - <link name="wrist_3_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/Wrist3.dae" /> - </geometry> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/Wrist3.dae" /> - </geometry> - </collision> - <inertial> - <mass value="${wrist_3_mass}" /> - <origin xyz="${wrist_3_cog}" rpy="0.0 0.0 0.0" /> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - - <joint name="ee_fixed_joint" type="fixed"> - <parent link="wrist_3_link" /> - <child link = "ee_link" /> - <origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 ${pi/2.0}" /> - </joint> - - <link name="ee_link" /> - - - - <!-- Extra links for collision model --> - <joint name="shoulder_collision_joint" type="fixed"> - <parent link="upper_arm_link" /> - <child link = "shoulder_collision_link" /> - </joint> - <link name="shoulder_collision_link"> - <collision> - <geometry> - <cylinder length="${shoulder_collision_length}" radius="${shoulder_radius}"/> - </geometry> - <origin xyz="0.0 ${-shoulder_collision_offset} 0.0" rpy="${-pi / 2.0} 0.0 0.0" /> - </collision> - </link> - - <joint name="elbow_collision_joint" type="fixed"> - <parent link="upper_arm_link" /> - <child link = "elbow_collision_link" /> - </joint> - <link name="elbow_collision_link"> - <collision> - <geometry> - <cylinder length="${elbow_collision_length}" radius="${elbow_radius}"/> - </geometry> - <origin xyz="0.0 ${-elbow_collision_offset} ${upper_arm_length}" rpy="${pi / 2.0} 0.0 0.0" /> - </collision> - </link> - - <gazebo reference="universal_robot"> - <material>Gazebo/Blue</material> - </gazebo> - -</robot> - - - diff --git a/ur5_description/urdf/robot.urdf.pretty.xml b/ur5_description/urdf/robot.urdf.pretty.xml deleted file mode 100644 index 44f307c82c4d6b6430e7c2be9883f7a718397a2f..0000000000000000000000000000000000000000 --- a/ur5_description/urdf/robot.urdf.pretty.xml +++ /dev/null @@ -1,248 +0,0 @@ -<?xml version="1.0" ?> -<!-- =================================================================================== --> -<!-- | This document was autogenerated by xacro from robot.urdf.pretty.xacro | --> -<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | --> -<!-- =================================================================================== --> -<!-- -DH for UR5: -a = [0.00000, -0.42500, -0.39225, 0.00000, 0.00000, 0.0000] -d = [0.089159, 0.00000, 0.00000, 0.10915, 0.09465, 0.0823] -alpha = [ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 ] -q_home_offset = [0, -1.570796327, 0, -1.570796327, 0, 0] -joint_direction = [-1, -1, 1, 1, 1, 1] -mass = [3.7000, 8.3930, 2.2750, 1.2190, 1.2190, 0.1879] -center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0, 0.0265], [0, -0.0018, 0.01634], [0, 0.0018,0.01634], [0, 0, -0.001159] ] ---> -<robot name="universal_robot"> - <!-- Inertia parameters --> - <!-- Invented number, only matters for simulator --> - <!-- 0.11336 - 0.089159 = --> - <!-- 0.119 is not half of 0.39225, is this really correct? --> - <!-- 0.0018 + 0.10915 = 0.110949 --> - <!-- 0.01634 + 0.09465 = 0.11099--> - <!-- Kinematic model --> - <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 --> - <!-- CAD measured --> - <!-- CAD measured --> - <!-- In CAD this distance is 0.930, but in the spec it is 0.09465 --> - <!-- manually measured --> - <!-- manually measured --> - <!-- manually measured --> - <!-- manually measured --> - <!-- manually measured --> - <!-- Collision model --> - <!-- manually measured --> - <!-- manually measured --> - <!-- manually measured --> - <!-- manually measured --> - <!-- manually measured --> - <link name="base_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/Base.dae"/> - </geometry> - <origin rpy="0.0 0.0 2.3561944875" xyz="0.0 0.0 0.003"/> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/Base.dae"/> - </geometry> - <origin rpy="0.0 0.0 2.3561944875" xyz="0.0 0.0 0.003"/> - </collision> - <inertial> - <mass value="4.0"/> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - <joint name="shoulder_pan_joint" type="revolute"> - <parent link="base_link"/> - <child link="shoulder_link"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/> - <axis xyz="0.0 0.0 1.0"/> - <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/> - <dynamics damping="0.1" friction="0.1"/> - </joint> - <link name="shoulder_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/Shoulder.dae"/> - </geometry> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/Shoulder.dae"/> - </geometry> - </collision> - <inertial> - <mass value="3.7"/> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00193 -0.02561"/> - </inertial> - </link> - <joint name="shoulder_lift_joint" type="revolute"> - <parent link="shoulder_link"/> - <child link="upper_arm_link"/> - <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.13585 0.0"/> - <axis xyz="0.0 1.0 0.0"/> - <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/> - <dynamics damping="0.1" friction="0.1"/> - </joint> - <link name="upper_arm_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/UpperArm.dae"/> - </geometry> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/UpperArm.dae"/> - </geometry> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/> - </collision> - <inertial> - <mass value="8.393"/> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.024201 0.2125"/> - </inertial> - </link> - <joint name="elbow_joint" type="revolute"> - <parent link="upper_arm_link"/> - <child link="forearm_link"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/> - <axis xyz="0.0 1.0 0.0"/> - <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/> - <dynamics damping="0.1" friction="0.1"/> - </joint> - <link name="forearm_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/Forearm.dae"/> - </geometry> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/Forearm.dae"/> - </geometry> - </collision> - <inertial> - <mass value="2.275"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0265 0.11993"/> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - <joint name="wrist_1_joint" type="revolute"> - <parent link="forearm_link"/> - <child link="wrist_1_link"/> - <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.0 0.39225"/> - <axis xyz="0.0 1.0 0.0"/> - <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/> - <dynamics damping="0.1" friction="0.1"/> - </joint> - <link name="wrist_1_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/Wrist1.dae"/> - </geometry> - <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.093 0.0"/> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/Wrist1.dae"/> - </geometry> - <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.093 0.0"/> - </collision> - <inertial> - <mass value="1.219"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.110949 0.01634"/> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - <joint name="wrist_2_joint" type="revolute"> - <parent link="wrist_1_link"/> - <child link="wrist_2_link"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/> - <axis xyz="0.0 0.0 1.0"/> - <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/> - <dynamics damping="0.1" friction="0.1"/> - </joint> - <link name="wrist_2_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/Wrist2.dae"/> - </geometry> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/Wrist2.dae"/> - </geometry> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/> - </collision> - <inertial> - <mass value="1.219"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0018 0.11099"/> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - <joint name="wrist_3_joint" type="revolute"> - <parent link="wrist_2_link"/> - <child link="wrist_3_link"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/> - <axis xyz="0.0 1.0 0.0"/> - <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/> - <dynamics damping="0.1" friction="0.1"/> - </joint> - <link name="wrist_3_link"> - <visual> - <geometry> - <mesh filename="package://ur5_description/meshes/Wrist3.dae"/> - </geometry> - </visual> - <collision> - <geometry> - <mesh filename="package://ur5_description/meshes/Wrist3.dae"/> - </geometry> - </collision> - <inertial> - <mass value="0.1879"/> - <origin rpy="0.0 0.0 0.0" xyz="0.0 0.001159 0.0"/> - <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> - </inertial> - </link> - <joint name="ee_fixed_joint" type="fixed"> - <parent link="wrist_3_link"/> - <child link="ee_link"/> - <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0823 0.0"/> - </joint> - <link name="ee_link"/> - <!-- Extra links for collision model --> - <joint name="shoulder_collision_joint" type="fixed"> - <parent link="upper_arm_link"/> - <child link="shoulder_collision_link"/> - </joint> - <link name="shoulder_collision_link"> - <collision> - <geometry> - <cylinder length="0.2" radius="0.06"/> - </geometry> - <origin rpy="-1.570796325 0.0 0.0" xyz="0.0 -0.035 0.0"/> - </collision> - </link> - <joint name="elbow_collision_joint" type="fixed"> - <parent link="upper_arm_link"/> - <child link="elbow_collision_link"/> - </joint> - <link name="elbow_collision_link"> - <collision> - <geometry> - <cylinder length="0.2" radius="0.06"/> - </geometry> - <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.035 0.425"/> - </collision> - </link> - <gazebo reference="universal_robot"> - <material>Gazebo/Blue</material> - </gazebo> -</robot> -